| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2016 Google Inc. All rights reserved. |
| // http://ceres-solver.org/ |
| // |
| // Redistribution and use in source and binary forms, with or without |
| // modification, are permitted provided that the following conditions are met: |
| // |
| // * Redistributions of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // * Redistributions in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // * Neither the name of Google Inc. nor the names of its contributors may be |
| // used to endorse or promote products derived from this software without |
| // specific prior written permission. |
| // |
| // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // POSSIBILITY OF SUCH DAMAGE. |
| // |
| // Authors: wjr@google.com (William Rucklidge), |
| // keir@google.com (Keir Mierle), |
| // dgossow@google.com (David Gossow) |
| |
| #include "ceres/gradient_checker.h" |
| |
| #include <algorithm> |
| #include <cmath> |
| #include <numeric> |
| #include <string> |
| #include <vector> |
| |
| #include "ceres/is_close.h" |
| #include "ceres/stringprintf.h" |
| #include "ceres/types.h" |
| |
| namespace ceres { |
| |
| using internal::IsClose; |
| using internal::StringAppendF; |
| using internal::StringPrintf; |
| using std::string; |
| using std::vector; |
| |
| namespace { |
| // Evaluate the cost function and transform the returned Jacobians to |
| // the local space of the respective local parameterizations. |
| bool EvaluateCostFunction( |
| const ceres::CostFunction* function, |
| double const* const * parameters, |
| const std::vector<const ceres::LocalParameterization*>& |
| local_parameterizations, |
| Vector* residuals, |
| std::vector<Matrix>* jacobians, |
| std::vector<Matrix>* local_jacobians) { |
| CHECK_NOTNULL(residuals); |
| CHECK_NOTNULL(jacobians); |
| CHECK_NOTNULL(local_jacobians); |
| |
| const vector<int32>& block_sizes = function->parameter_block_sizes(); |
| const int num_parameter_blocks = block_sizes.size(); |
| |
| // Allocate Jacobian matrices in local space. |
| local_jacobians->resize(num_parameter_blocks); |
| vector<double*> local_jacobian_data(num_parameter_blocks); |
| for (int i = 0; i < num_parameter_blocks; ++i) { |
| int block_size = block_sizes.at(i); |
| if (local_parameterizations.at(i) != NULL) { |
| block_size = local_parameterizations.at(i)->LocalSize(); |
| } |
| local_jacobians->at(i).resize(function->num_residuals(), block_size); |
| local_jacobians->at(i).setZero(); |
| local_jacobian_data.at(i) = local_jacobians->at(i).data(); |
| } |
| |
| // Allocate Jacobian matrices in global space. |
| jacobians->resize(num_parameter_blocks); |
| vector<double*> jacobian_data(num_parameter_blocks); |
| for (int i = 0; i < num_parameter_blocks; ++i) { |
| jacobians->at(i).resize(function->num_residuals(), block_sizes.at(i)); |
| jacobians->at(i).setZero(); |
| jacobian_data.at(i) = jacobians->at(i).data(); |
| } |
| |
| // Compute residuals & jacobians. |
| CHECK_NE(0, function->num_residuals()); |
| residuals->resize(function->num_residuals()); |
| residuals->setZero(); |
| if (!function->Evaluate(parameters, residuals->data(), |
| jacobian_data.data())) { |
| return false; |
| } |
| |
| // Convert Jacobians from global to local space. |
| for (size_t i = 0; i < local_jacobians->size(); ++i) { |
| if (local_parameterizations.at(i) == NULL) { |
| local_jacobians->at(i) = jacobians->at(i); |
| } else { |
| int global_size = local_parameterizations.at(i)->GlobalSize(); |
| int local_size = local_parameterizations.at(i)->LocalSize(); |
| CHECK_EQ(jacobians->at(i).cols(), global_size); |
| Matrix global_J_local(global_size, local_size); |
| local_parameterizations.at(i)->ComputeJacobian( |
| parameters[i], global_J_local.data()); |
| local_jacobians->at(i) = jacobians->at(i) * global_J_local; |
| } |
| } |
| return true; |
| } |
| } // namespace |
| |
| GradientChecker::GradientChecker( |
| const CostFunction* function, |
| const vector<const LocalParameterization*>* local_parameterizations, |
| const NumericDiffOptions& options) : |
| function_(function) { |
| CHECK_NOTNULL(function); |
| if (local_parameterizations != NULL) { |
| local_parameterizations_ = *local_parameterizations; |
| } else { |
| local_parameterizations_.resize(function->parameter_block_sizes().size(), |
| NULL); |
| } |
| DynamicNumericDiffCostFunction<CostFunction, CENTRAL>* |
| finite_diff_cost_function = |
| new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>( |
| function, DO_NOT_TAKE_OWNERSHIP, options); |
| finite_diff_cost_function_.reset(finite_diff_cost_function); |
| |
| const vector<int32>& parameter_block_sizes = |
| function->parameter_block_sizes(); |
| const int num_parameter_blocks = parameter_block_sizes.size(); |
| for (int i = 0; i < num_parameter_blocks; ++i) { |
| finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]); |
| } |
| finite_diff_cost_function->SetNumResiduals(function->num_residuals()); |
| } |
| |
| bool GradientChecker::Probe(double const* const * parameters, |
| double relative_precision, |
| ProbeResults* results_param) const { |
| int num_residuals = function_->num_residuals(); |
| |
| // Make sure that we have a place to store results, no matter if the user has |
| // provided an output argument. |
| ProbeResults* results; |
| ProbeResults results_local; |
| if (results_param != NULL) { |
| results = results_param; |
| results->residuals.resize(0); |
| results->jacobians.clear(); |
| results->numeric_jacobians.clear(); |
| results->local_jacobians.clear(); |
| results->local_numeric_jacobians.clear(); |
| results->error_log.clear(); |
| } else { |
| results = &results_local; |
| } |
| results->maximum_relative_error = 0.0; |
| results->return_value = true; |
| |
| // Evaluate the derivative using the user supplied code. |
| vector<Matrix>& jacobians = results->jacobians; |
| vector<Matrix>& local_jacobians = results->local_jacobians; |
| if (!EvaluateCostFunction(function_, parameters, local_parameterizations_, |
| &results->residuals, &jacobians, &local_jacobians)) { |
| results->error_log = "Function evaluation with Jacobians failed."; |
| results->return_value = false; |
| } |
| |
| // Evaluate the derivative using numeric derivatives. |
| vector<Matrix>& numeric_jacobians = results->numeric_jacobians; |
| vector<Matrix>& local_numeric_jacobians = results->local_numeric_jacobians; |
| Vector finite_diff_residuals; |
| if (!EvaluateCostFunction(finite_diff_cost_function_.get(), parameters, |
| local_parameterizations_, &finite_diff_residuals, |
| &numeric_jacobians, &local_numeric_jacobians)) { |
| results->error_log += "\nFunction evaluation with numerical " |
| "differentiation failed."; |
| results->return_value = false; |
| } |
| |
| if (!results->return_value) { |
| return false; |
| } |
| |
| for (int i = 0; i < num_residuals; ++i) { |
| if (!IsClose( |
| results->residuals[i], |
| finite_diff_residuals[i], |
| relative_precision, |
| NULL, |
| NULL)) { |
| results->error_log = "Function evaluation with and without Jacobians " |
| "resulted in different residuals."; |
| LOG(INFO) << results->residuals.transpose(); |
| LOG(INFO) << finite_diff_residuals.transpose(); |
| return false; |
| } |
| } |
| |
| // See if any elements have relative error larger than the threshold. |
| int num_bad_jacobian_components = 0; |
| double& worst_relative_error = results->maximum_relative_error; |
| worst_relative_error = 0; |
| |
| // Accumulate the error message for all the jacobians, since it won't get |
| // output if there are no bad jacobian components. |
| string error_log; |
| for (int k = 0; k < function_->parameter_block_sizes().size(); k++) { |
| StringAppendF(&error_log, |
| "========== " |
| "Jacobian for " "block %d: (%ld by %ld)) " |
| "==========\n", |
| k, |
| static_cast<long>(local_jacobians[k].rows()), |
| static_cast<long>(local_jacobians[k].cols())); |
| // The funny spacing creates appropriately aligned column headers. |
| error_log += |
| " block row col user dx/dy num diff dx/dy " |
| "abs error relative error parameter residual\n"; |
| |
| for (int i = 0; i < local_jacobians[k].rows(); i++) { |
| for (int j = 0; j < local_jacobians[k].cols(); j++) { |
| double term_jacobian = local_jacobians[k](i, j); |
| double finite_jacobian = local_numeric_jacobians[k](i, j); |
| double relative_error, absolute_error; |
| bool bad_jacobian_entry = |
| !IsClose(term_jacobian, |
| finite_jacobian, |
| relative_precision, |
| &relative_error, |
| &absolute_error); |
| worst_relative_error = std::max(worst_relative_error, relative_error); |
| |
| StringAppendF(&error_log, |
| "%6d %4d %4d %17g %17g %17g %17g %17g %17g", |
| k, i, j, |
| term_jacobian, finite_jacobian, |
| absolute_error, relative_error, |
| parameters[k][j], |
| results->residuals[i]); |
| |
| if (bad_jacobian_entry) { |
| num_bad_jacobian_components++; |
| StringAppendF( |
| &error_log, |
| " ------ (%d,%d,%d) Relative error worse than %g", |
| k, i, j, relative_precision); |
| } |
| error_log += "\n"; |
| } |
| } |
| } |
| |
| // Since there were some bad errors, dump comprehensive debug info. |
| if (num_bad_jacobian_components) { |
| string header = StringPrintf("\nDetected %d bad Jacobian component(s). " |
| "Worst relative error was %g.\n", |
| num_bad_jacobian_components, |
| worst_relative_error); |
| results->error_log = header + "\n" + error_log; |
| return false; |
| } |
| return true; |
| } |
| |
| } // namespace ceres |