diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index 311f8ae02..b82f5ba46 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst @@ -873,6 +873,56 @@ Numeric Differentiation & LocalParameterization and the Jacobian will be affected appropriately. +:class:`GradientChecker` +================================ + +.. class:: GradientChecker + + This class compares the Jacobians returned by a cost function against + derivatives estimated using finite differencing. It is meant as a tool for + unit testing, giving you more fine-grained control than the check_gradients + option in the solver options. + + The condition enforced is that + + .. math:: \forall{i,j}: \frac{J_{ij} - J'_{ij}}{max_{ij}(J_{ij} - J'_{ij})} < r + + where :math:`J_{ij}` is the jacobian as computed by the supplied cost + function (by the user) multiplied by the local parameterization Jacobian, + :math:`J'_{ij}` is the jacobian as computed by finite differences, + multiplied by the local parameterization Jacobian as well, and :math:`r` + is the relative precision. + + Usage: + + .. code-block:: c++ + + // my_cost_function takes two parameter blocks. The first has a local + // parameterization associated with it. + CostFunction* my_cost_function = ... + LocalParameterization* my_parameterization = ... + NumericDiffOptions numeric_diff_options; + + std::vector local_parameterizations; + local_parameterizations.push_back(my_parameterization); + local_parameterizations.push_back(NULL); + + std::vector parameter1; + std::vector parameter2; + // Fill parameter 1 & 2 with test data... + + std::vector parameter_blocks; + parameter_blocks.push_back(parameter1.data()); + parameter_blocks.push_back(parameter2.data()); + + GradientChecker gradient_checker(my_cost_function, + local_parameterizations, numeric_diff_options); + GradientCheckResults results; + if (!gradient_checker.Probe(parameter_blocks.data(), 1e-9, &results) { + LOG(ERROR) << "An error has occured:\n" << results.error_log; + } + + :class:`NormalPrior` ==================== diff --git a/docs/source/nnls_solving.rst b/docs/source/nnls_solving.rst index 4eb1fad4f..ccf43a54b 100644 --- a/docs/source/nnls_solving.rst +++ b/docs/source/nnls_solving.rst @@ -1559,8 +1559,8 @@ elimination group [LiSaad]_. differences. This is expensive since it involves computing the derivative by normal means (e.g. user specified, autodiff, etc), then also computing it using finite differences. The results are - compared, and if they differ substantially, details are printed to - the log. + compared, and if they differ substantially, the optimization fails + and the details are stored in the solver summary. .. member:: double Solver::Options::gradient_check_relative_precision diff --git a/include/ceres/gradient_checker.h b/include/ceres/gradient_checker.h index 28304159b..6d285daf1 100644 --- a/include/ceres/gradient_checker.h +++ b/include/ceres/gradient_checker.h @@ -27,194 +27,121 @@ // POSSIBILITY OF SUCH DAMAGE. // Copyright 2007 Google Inc. All Rights Reserved. // -// Author: wjr@google.com (William Rucklidge) -// -// This file contains a class that exercises a cost function, to make sure -// that it is computing reasonable derivatives. It compares the Jacobians -// computed by the cost function with those obtained by finite -// differences. +// Authors: wjr@google.com (William Rucklidge), +// keir@google.com (Keir Mierle), +// dgossow@google.com (David Gossow) #ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_ #define CERES_PUBLIC_GRADIENT_CHECKER_H_ -#include -#include #include +#include +#include "ceres/cost_function.h" +#include "ceres/dynamic_numeric_diff_cost_function.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" #include "ceres/internal/macros.h" #include "ceres/internal/scoped_ptr.h" -#include "ceres/numeric_diff_cost_function.h" +#include "ceres/local_parameterization.h" #include "glog/logging.h" namespace ceres { -// An object that exercises a cost function, to compare the answers that it -// gives with derivatives estimated using finite differencing. +// GradientChecker compares the Jacobians returned by a cost function against +// derivatives estimated using finite differencing. // -// The only likely usage of this is for testing. +// The condition enforced is that +// +// (J_actual(i, j) - J_numeric(i, j)) +// ------------------------------------ < relative_precision +// max(J_actual(i, j), J_numeric(i, j)) +// +// where J_actual(i, j) is the jacobian as computed by the supplied cost +// function (by the user) multiplied by the local parameterization Jacobian +// and J_numeric is the jacobian as computed by finite differences, multiplied +// by the local parameterization Jacobian as well. // // How to use: Fill in an array of pointers to parameter blocks for your -// CostFunction, and then call Probe(). Check that the return value is -// 'true'. See prober_test.cc for an example. -// -// This is templated similarly to NumericDiffCostFunction, as it internally -// uses that. -template +// CostFunction, and then call Probe(). Check that the return value is 'true'. class GradientChecker { public: - // Here we stash some results from the probe, for later - // inspection. - struct GradientCheckResults { - // Computed cost. - Vector cost; + // This will not take ownership of the cost function or local + // parameterizations. + // + // function: The cost function to probe. + // local_parameterization: A vector of local parameterizations for each + // parameter. May be NULL or contain NULL pointers to indicate that the + // respective parameter does not have a local parameterization. + // options: Options to use for numerical differentiation. + GradientChecker( + const CostFunction* function, + const std::vector* local_parameterizations, + const NumericDiffOptions& options); - // The sizes of these matrices are dictated by the cost function's - // parameter and residual block sizes. Each vector's length will - // term->parameter_block_sizes().size(), and each matrix is the - // Jacobian of the residual with respect to the corresponding parameter - // block. + // Contains results from a call to Probe for later inspection. + struct ProbeResults { + // The return value of the cost function. + bool return_value; + + // Computed residual vector. + Vector residuals; + + // The sizes of the Jacobians below are dictated by the cost function's + // parameter block size and residual block sizes. If a parameter block + // has a local parameterization associated with it, the size of the "local" + // Jacobian will be determined by the local parameterization dimension and + // residual block size, otherwise it will be identical to the regular + // Jacobian. // Derivatives as computed by the cost function. - std::vector term_jacobians; + std::vector jacobians; - // Derivatives as computed by finite differencing. - std::vector finite_difference_jacobians; + // Derivatives as computed by the cost function in local space. + std::vector local_jacobians; - // Infinity-norm of term_jacobians - finite_difference_jacobians. - double error_jacobians; + // Derivatives as computed by nuerical differentiation in local space. + std::vector numeric_jacobians; + + // Derivatives as computed by nuerical differentiation in local space. + std::vector local_numeric_jacobians; + + // Contains the maximum relative error found in the local Jacobians. + double maximum_relative_error; + + // If an error was detected, this will contain a detailed description of + // that error. + std::string error_log; }; - // Checks the Jacobian computed by a cost function. + // Call the cost function, compute alternative Jacobians using finite + // differencing and compare results. If local parameterizations are given, + // the Jacobians will be multiplied by the local parameterization Jacobians + // before performing the check, which effectively means that all errors along + // the null space of the local parameterization will be ignored. + // Returns false if the Jacobians don't match, the cost function return false, + // or if the cost function returns different residual when called with a + // Jacobian output argument vs. calling it without. Otherwise returns true. // - // probe_point: The parameter values at which to probe. - // error_tolerance: A threshold for the infinity-norm difference - // between the Jacobians. If the Jacobians differ by more than - // this amount, then the probe fails. - // - // term: The cost function to test. Not retained after this call returns. - // - // results: On return, the two Jacobians (and other information) - // will be stored here. May be NULL. + // parameters: The parameter values at which to probe. + // relative_precision: A threshold for the relative difference between the + // Jacobians. If the Jacobians differ by more than this amount, then the + // probe fails. + // results: On return, the Jacobians (and other information) will be stored + // here. May be NULL. // // Returns true if no problems are detected and the difference between the // Jacobians is less than error_tolerance. - static bool Probe(double const* const* probe_point, - double error_tolerance, - CostFunctionToProbe *term, - GradientCheckResults* results) { - CHECK_NOTNULL(probe_point); - CHECK_NOTNULL(term); - LOG(INFO) << "-------------------- Starting Probe() --------------------"; - - // We need a GradientCheckeresults, whether or not they supplied one. - internal::scoped_ptr owned_results; - if (results == NULL) { - owned_results.reset(new GradientCheckResults); - results = owned_results.get(); - } - - // Do a consistency check between the term and the template parameters. - CHECK_EQ(M, term->num_residuals()); - const int num_residuals = M; - const std::vector& block_sizes = term->parameter_block_sizes(); - const int num_blocks = block_sizes.size(); - - CHECK_LE(num_blocks, 5) << "Unable to test functions that take more " - << "than 5 parameter blocks"; - if (N0) { - CHECK_EQ(N0, block_sizes[0]); - CHECK_GE(num_blocks, 1); - } else { - CHECK_LT(num_blocks, 1); - } - if (N1) { - CHECK_EQ(N1, block_sizes[1]); - CHECK_GE(num_blocks, 2); - } else { - CHECK_LT(num_blocks, 2); - } - if (N2) { - CHECK_EQ(N2, block_sizes[2]); - CHECK_GE(num_blocks, 3); - } else { - CHECK_LT(num_blocks, 3); - } - if (N3) { - CHECK_EQ(N3, block_sizes[3]); - CHECK_GE(num_blocks, 4); - } else { - CHECK_LT(num_blocks, 4); - } - if (N4) { - CHECK_EQ(N4, block_sizes[4]); - CHECK_GE(num_blocks, 5); - } else { - CHECK_LT(num_blocks, 5); - } - - results->term_jacobians.clear(); - results->term_jacobians.resize(num_blocks); - results->finite_difference_jacobians.clear(); - results->finite_difference_jacobians.resize(num_blocks); - - internal::FixedArray term_jacobian_pointers(num_blocks); - internal::FixedArray - finite_difference_jacobian_pointers(num_blocks); - for (int i = 0; i < num_blocks; i++) { - results->term_jacobians[i].resize(num_residuals, block_sizes[i]); - term_jacobian_pointers[i] = results->term_jacobians[i].data(); - results->finite_difference_jacobians[i].resize( - num_residuals, block_sizes[i]); - finite_difference_jacobian_pointers[i] = - results->finite_difference_jacobians[i].data(); - } - results->cost.resize(num_residuals, 1); - - CHECK(term->Evaluate(probe_point, results->cost.data(), - term_jacobian_pointers.get())); - NumericDiffCostFunction - numeric_term(term, DO_NOT_TAKE_OWNERSHIP); - CHECK(numeric_term.Evaluate(probe_point, results->cost.data(), - finite_difference_jacobian_pointers.get())); - - results->error_jacobians = 0; - for (int i = 0; i < num_blocks; i++) { - Matrix jacobian_difference = results->term_jacobians[i] - - results->finite_difference_jacobians[i]; - results->error_jacobians = - std::max(results->error_jacobians, - jacobian_difference.lpNorm()); - } - - LOG(INFO) << "========== term-computed derivatives =========="; - for (int i = 0; i < num_blocks; i++) { - LOG(INFO) << "term_computed block " << i; - LOG(INFO) << "\n" << results->term_jacobians[i]; - } - - LOG(INFO) << "========== finite-difference derivatives =========="; - for (int i = 0; i < num_blocks; i++) { - LOG(INFO) << "finite_difference block " << i; - LOG(INFO) << "\n" << results->finite_difference_jacobians[i]; - } - - LOG(INFO) << "========== difference =========="; - for (int i = 0; i < num_blocks; i++) { - LOG(INFO) << "difference block " << i; - LOG(INFO) << (results->term_jacobians[i] - - results->finite_difference_jacobians[i]); - } - - LOG(INFO) << "||difference|| = " << results->error_jacobians; - - return results->error_jacobians < error_tolerance; - } + bool Probe(double const* const* parameters, + double relative_precision, + ProbeResults* results) const; private: CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker); + + std::vector local_parameterizations_; + const CostFunction* function_; + internal::scoped_ptr finite_diff_cost_function_; }; } // namespace ceres diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 5d24a8a8d..74b0799ba 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -63,10 +63,12 @@ set(CERES_INTERNAL_SRC dynamic_compressed_row_sparse_matrix.cc evaluator.cc file.cc + gradient_checker.cc gradient_checking_cost_function.cc gradient_problem.cc gradient_problem_solver.cc implicit_schur_complement.cc + is_close.cc iterative_schur_complement_solver.cc levenberg_marquardt_strategy.cc lapack.cc @@ -309,6 +311,7 @@ if (BUILD_TESTING AND GFLAGS) ceres_test(graph) ceres_test(graph_algorithms) ceres_test(householder_vector) + ceres_test(is_close) ceres_test(implicit_schur_complement) ceres_test(iterative_schur_complement_solver) ceres_test(jet) diff --git a/internal/ceres/gradient_checker.cc b/internal/ceres/gradient_checker.cc new file mode 100644 index 000000000..219027cfa --- /dev/null +++ b/internal/ceres/gradient_checker.cc @@ -0,0 +1,277 @@ +// 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 "ceres/is_close.h" + +#include +#include +#include +#include +#include + +#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& + local_parameterizations, + Vector* residuals, + std::vector* jacobians, + std::vector* local_jacobians) { + CHECK_NOTNULL(residuals); + CHECK_NOTNULL(jacobians); + CHECK_NOTNULL(local_jacobians); + + const vector& 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 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 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* 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* + finite_diff_cost_function = + new DynamicNumericDiffCostFunction( + function, DO_NOT_TAKE_OWNERSHIP, options); + finite_diff_cost_function_.reset(finite_diff_cost_function); + + const vector& 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& jacobians = results->jacobians; + vector& 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& numeric_jacobians = results->numeric_jacobians; + vector& 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(local_jacobians[k].rows()), + static_cast(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 diff --git a/internal/ceres/gradient_checker_test.cc b/internal/ceres/gradient_checker_test.cc index 474ead555..d2ce6e394 100644 --- a/internal/ceres/gradient_checker_test.cc +++ b/internal/ceres/gradient_checker_test.cc @@ -38,6 +38,8 @@ #include "ceres/cost_function.h" #include "ceres/random.h" +#include "ceres/solver.h" +#include "ceres/problem.h" #include "glog/logging.h" #include "gtest/gtest.h" @@ -55,7 +57,7 @@ using std::vector; // version, they are both block vectors, of course. class GoodTestTerm : public CostFunction { public: - GoodTestTerm(int arity, int const *dim) : arity_(arity) { + GoodTestTerm(int arity, int const *dim) : arity_(arity), return_value_(true) { // Make 'arity' random vectors. a_.resize(arity_); for (int j = 0; j < arity_; ++j) { @@ -74,6 +76,9 @@ class GoodTestTerm : public CostFunction { bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { + if (!return_value_) { + return false; + } // Compute a . x. double ax = 0; for (int j = 0; j < arity_; ++j) { @@ -101,7 +106,12 @@ class GoodTestTerm : public CostFunction { return true; } + void SetReturnValue(bool return_value) { + return_value_ = return_value; + } + private: + bool return_value_; int arity_; vector > a_; // our vectors. }; @@ -159,37 +169,399 @@ class BadTestTerm : public CostFunction { vector > a_; // our vectors. }; +const double kTolerance = 1e-6; + +void CheckDimensions( + const GradientChecker::ProbeResults& results, + const std::vector& parameter_sizes, + const std::vector& local_parameter_sizes, int residual_size) { + CHECK_EQ(parameter_sizes.size(), local_parameter_sizes.size()); + int num_parameters = parameter_sizes.size(); + ASSERT_EQ(residual_size, results.residuals.size()); + ASSERT_EQ(num_parameters, results.local_jacobians.size()); + ASSERT_EQ(num_parameters, results.local_numeric_jacobians.size()); + ASSERT_EQ(num_parameters, results.jacobians.size()); + ASSERT_EQ(num_parameters, results.numeric_jacobians.size()); + for (int i = 0; i < num_parameters; ++i) { + EXPECT_EQ(residual_size, results.local_jacobians.at(i).rows()); + EXPECT_EQ(local_parameter_sizes[i], results.local_jacobians.at(i).cols()); + EXPECT_EQ(residual_size, results.local_numeric_jacobians.at(i).rows()); + EXPECT_EQ(local_parameter_sizes[i], results.local_numeric_jacobians.at(i).cols()); + EXPECT_EQ(residual_size, results.jacobians.at(i).rows()); + EXPECT_EQ(parameter_sizes[i], results.jacobians.at(i).cols()); + EXPECT_EQ(residual_size, results.numeric_jacobians.at(i).rows()); + EXPECT_EQ(parameter_sizes[i], results.numeric_jacobians.at(i).cols()); + } +} + TEST(GradientChecker, SmokeTest) { srand(5); // Test with 3 blocks of size 2, 3 and 4. - int const arity = 3; - int const dim[arity] = { 2, 3, 4 }; + int const num_parameters = 3; + std::vector parameter_sizes(3); + parameter_sizes[0] = 2; + parameter_sizes[1] = 3; + parameter_sizes[2] = 4; // Make a random set of blocks. - FixedArray parameters(arity); - for (int j = 0; j < arity; ++j) { - parameters[j] = new double[dim[j]]; - for (int u = 0; u < dim[j]; ++u) { + FixedArray parameters(num_parameters); + for (int j = 0; j < num_parameters; ++j) { + parameters[j] = new double[parameter_sizes[j]]; + for (int u = 0; u < parameter_sizes[j]; ++u) { parameters[j][u] = 2.0 * RandDouble() - 1.0; } } - // Make a term and probe it. - GoodTestTerm good_term(arity, dim); - typedef GradientChecker GoodTermGradientChecker; - EXPECT_TRUE(GoodTermGradientChecker::Probe( - parameters.get(), 1e-6, &good_term, NULL)); + NumericDiffOptions numeric_diff_options; + GradientChecker::ProbeResults results; - BadTestTerm bad_term(arity, dim); - typedef GradientChecker BadTermGradientChecker; - EXPECT_FALSE(BadTermGradientChecker::Probe( - parameters.get(), 1e-6, &bad_term, NULL)); + // Test that Probe returns true for correct Jacobians. + GoodTestTerm good_term(num_parameters, parameter_sizes.data()); + GradientChecker good_gradient_checker(&good_term, NULL, numeric_diff_options); + EXPECT_TRUE(good_gradient_checker.Probe(parameters.get(), kTolerance, NULL)); + EXPECT_TRUE(good_gradient_checker.Probe(parameters.get(), kTolerance, + &results)) + << results.error_log; - for (int j = 0; j < arity; j++) { + // Check that results contain sensible data. + ASSERT_EQ(results.return_value, true); + ASSERT_EQ(results.residuals.size(), 1); + CheckDimensions(results, parameter_sizes, parameter_sizes, 1); + EXPECT_GE(results.maximum_relative_error, 0.0); + EXPECT_TRUE(results.error_log.empty()); + + // Test that if the cost function return false, Probe should return false. + good_term.SetReturnValue(false); + EXPECT_FALSE(good_gradient_checker.Probe(parameters.get(), kTolerance, NULL)); + EXPECT_FALSE(good_gradient_checker.Probe(parameters.get(), kTolerance, + &results)) + << results.error_log; + + // Check that results contain sensible data. + ASSERT_EQ(results.return_value, false); + ASSERT_EQ(results.residuals.size(), 1); + CheckDimensions(results, parameter_sizes, parameter_sizes, 1); + for (int i = 0; i < num_parameters; ++i) { + EXPECT_EQ(results.local_jacobians.at(i).norm(), 0); + EXPECT_EQ(results.local_numeric_jacobians.at(i).norm(), 0); + } + EXPECT_EQ(results.maximum_relative_error, 0.0); + EXPECT_FALSE(results.error_log.empty()); + + // Test that Probe returns false for incorrect Jacobians. + BadTestTerm bad_term(num_parameters, parameter_sizes.data()); + GradientChecker bad_gradient_checker(&bad_term, NULL, numeric_diff_options); + EXPECT_FALSE(bad_gradient_checker.Probe(parameters.get(), kTolerance, NULL)); + EXPECT_FALSE(bad_gradient_checker.Probe(parameters.get(), kTolerance, + &results)); + + // Check that results contain sensible data. + ASSERT_EQ(results.return_value, true); + ASSERT_EQ(results.residuals.size(), 1); + CheckDimensions(results, parameter_sizes, parameter_sizes, 1); + EXPECT_GT(results.maximum_relative_error, kTolerance); + EXPECT_FALSE(results.error_log.empty()); + + // Setting a high threshold should make the test pass. + EXPECT_TRUE(bad_gradient_checker.Probe(parameters.get(), 1.0, &results)); + + // Check that results contain sensible data. + ASSERT_EQ(results.return_value, true); + ASSERT_EQ(results.residuals.size(), 1); + CheckDimensions(results, parameter_sizes, parameter_sizes, 1); + EXPECT_GT(results.maximum_relative_error, 0.0); + EXPECT_TRUE(results.error_log.empty()); + + for (int j = 0; j < num_parameters; j++) { delete[] parameters[j]; } } + +/** + * Helper cost function that multiplies the parameters by the given jacobians + * and adds a constant offset. + */ +class LinearCostFunction : public CostFunction { + public: + explicit LinearCostFunction(const Vector& residuals_offset) + : residuals_offset_(residuals_offset) { + set_num_residuals(residuals_offset_.size()); + } + + virtual bool Evaluate(double const* const* parameter_ptrs, double* residuals_ptr, + double** residual_J_params) const { + CHECK_GE(residual_J_params_.size(), 0.0); + VectorRef residuals(residuals_ptr, residual_J_params_[0].rows()); + residuals = residuals_offset_; + + for (size_t i = 0; i < residual_J_params_.size(); ++i) { + const Matrix& residual_J_param = residual_J_params_[i]; + int parameter_size = residual_J_param.cols(); + ConstVectorRef param(parameter_ptrs[i], parameter_size); + + // Compute residual. + residuals += residual_J_param * param; + + // Return Jacobian. + if (residual_J_params != NULL && residual_J_params[i] != NULL) { + Eigen::Map residual_J_param_out(residual_J_params[i], + residual_J_param.rows(), + residual_J_param.cols()); + if (jacobian_offsets_.count(i) != 0) { + residual_J_param_out = residual_J_param + jacobian_offsets_.at(i); + } else { + residual_J_param_out = residual_J_param; + } + } + } + return true; + } + + void AddParameter(const Matrix& residual_J_param) { + CHECK_EQ(num_residuals(), residual_J_param.rows()); + residual_J_params_.push_back(residual_J_param); + mutable_parameter_block_sizes()->push_back(residual_J_param.cols()); + } + + /// Add offset to the given Jacobian before returning it from Evaluate(), + /// thus introducing an error in the comutation. + void SetJacobianOffset(size_t index, Matrix offset) { + CHECK_LT(index, residual_J_params_.size()); + CHECK_EQ(residual_J_params_[index].rows(), offset.rows()); + CHECK_EQ(residual_J_params_[index].cols(), offset.cols()); + jacobian_offsets_[index] = offset; + } + + private: + std::vector residual_J_params_; + std::map jacobian_offsets_; + Vector residuals_offset_; +}; + +/** + * Helper local parameterization that multiplies the delta vector by the given + * jacobian and adds it to the parameter. + */ +class MatrixParameterization : public LocalParameterization { + public: + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + VectorRef(x_plus_delta, GlobalSize()) = + ConstVectorRef(x, GlobalSize()) + + (global_J_local * ConstVectorRef(delta, LocalSize())); + return true; + } + + virtual bool ComputeJacobian(const double* /*x*/, double* jacobian) const { + MatrixRef(jacobian, GlobalSize(), LocalSize()) = global_J_local; + return true; + } + + virtual int GlobalSize() const { return global_J_local.rows(); } + virtual int LocalSize() const { return global_J_local.cols(); } + + Matrix global_J_local; +}; + +TEST(GradientChecker, TestCorrectnessWithLocalParameterizations) { + // Create cost function. + Eigen::Vector3d residual_offset(100.0, 200.0, 300.0); + LinearCostFunction cost_function(residual_offset); + Eigen::Matrix j0; + j0.row(0) << 1.0, 2.0, 3.0; + j0.row(1) << 4.0, 5.0, 6.0; + j0.row(2) << 7.0, 8.0, 9.0; + Eigen::Matrix j1; + j1.row(0) << 10.0, 11.0; + j1.row(1) << 12.0, 13.0; + j1.row(2) << 14.0, 15.0; + + Eigen::Vector3d param0(1.0, 2.0, 3.0); + Eigen::Vector2d param1(4.0, 5.0); + + int const arity = 2; + const int dim[2] = {3, 2}; + + cost_function.AddParameter(j0); + cost_function.AddParameter(j1); + + int const num_parameters = 2; + std::vector parameter_sizes(2); + parameter_sizes[0] = 3; + parameter_sizes[1] = 2; + std::vector local_parameter_sizes(2); + local_parameter_sizes[0] = 2; + local_parameter_sizes[1] = 2; + + // Test cost function for correctness. + Eigen::Matrix j1_out; + Eigen::Matrix j2_out; + Eigen::VectorXd residual(3); + std::vector parameters(2); + parameters[0] = param0.data(); + parameters[1] = param1.data(); + std::vector jacobians(2); + jacobians[0] = j1_out.data(); + jacobians[1] = j2_out.data(); + cost_function.Evaluate(parameters.data(), residual.data(), jacobians.data()); + + Matrix residual_expected = residual_offset + j0 * param0 + j1 * param1; + + EXPECT_TRUE(j1_out == j0); + EXPECT_TRUE(j2_out == j1); + EXPECT_TRUE(residual.isApprox(residual_expected, kTolerance)); + + // Create local parameterization. + Eigen::Matrix global_J_local; + global_J_local.row(0) << 1.5, 2.5; + global_J_local.row(1) << 3.5, 4.5; + global_J_local.row(2) << 5.5, 6.5; + + MatrixParameterization parameterization; + parameterization.global_J_local = global_J_local; + + // Test local parameterization for correctness. + Eigen::Vector3d x(7.0, 8.0, 9.0); + Eigen::Vector2d delta(10.0, 11.0); + + Eigen::Matrix global_J_local_out; + parameterization.ComputeJacobian(x.data(), global_J_local_out.data()); + EXPECT_TRUE(global_J_local_out == global_J_local); + + Eigen::Vector3d x_plus_delta; + parameterization.Plus(x.data(), delta.data(), x_plus_delta.data()); + Eigen::Vector3d x_plus_delta_expected = x + (global_J_local * delta); + EXPECT_TRUE(x_plus_delta.isApprox(x_plus_delta_expected, kTolerance)); + + // Now test GradientChecker. + std::vector parameterizations(2); + parameterizations[0] = ¶meterization; + parameterizations[1] = NULL; + NumericDiffOptions numeric_diff_options; + GradientChecker::ProbeResults results; + GradientChecker gradient_checker(&cost_function, ¶meterizations, + numeric_diff_options); + + Problem::Options problem_options; + problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; + problem_options.local_parameterization_ownership = DO_NOT_TAKE_OWNERSHIP; + Problem problem(problem_options); + Eigen::Vector3d param0_solver; + Eigen::Vector2d param1_solver; + problem.AddParameterBlock(param0_solver.data(), 3, ¶meterization); + problem.AddParameterBlock(param1_solver.data(), 2); + problem.AddResidualBlock(&cost_function, NULL, param0_solver.data(), + param1_solver.data()); + Solver::Options solver_options; + solver_options.check_gradients = true; + solver_options.initial_trust_region_radius = 1e10; + Solver solver; + Solver::Summary summary; + + // First test case: everything is correct. + EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, NULL)); + EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, &results)) + << results.error_log; + + // Check that results contain correct data. + ASSERT_EQ(results.return_value, true); + ASSERT_TRUE(results.residuals == residual); + CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3); + EXPECT_TRUE(results.local_jacobians.at(0) == j0 * global_J_local); + EXPECT_TRUE(results.local_jacobians.at(1) == j1); + EXPECT_TRUE(results.local_numeric_jacobians.at(0).isApprox( + j0 * global_J_local, kTolerance)); + EXPECT_TRUE(results.local_numeric_jacobians.at(1).isApprox( + j1, kTolerance)); + EXPECT_TRUE(results.jacobians.at(0) == j0); + EXPECT_TRUE(results.jacobians.at(1) == j1); + EXPECT_TRUE(results.numeric_jacobians.at(0).isApprox( + j0, kTolerance)); + EXPECT_TRUE(results.numeric_jacobians.at(1).isApprox( + j1, kTolerance)); + EXPECT_GE(results.maximum_relative_error, 0.0); + EXPECT_TRUE(results.error_log.empty()); + + // Test interaction with the 'check_gradients' option in Solver. + param0_solver = param0; + param1_solver = param1; + solver.Solve(solver_options, &problem, &summary); + EXPECT_EQ(CONVERGENCE, summary.termination_type); + EXPECT_LE(summary.final_cost, 1e-12); + + // Second test case: Mess up reported derivatives with respect to 3rd + // component of 1st parameter. Check should fail. + Eigen::Matrix j0_offset; + j0_offset.setZero(); + j0_offset.col(2).setConstant(0.001); + cost_function.SetJacobianOffset(0, j0_offset); + EXPECT_FALSE(gradient_checker.Probe(parameters.data(), kTolerance, NULL)); + EXPECT_FALSE(gradient_checker.Probe(parameters.data(), kTolerance, &results)) + << results.error_log; + + // Check that results contain correct data. + ASSERT_EQ(results.return_value, true); + ASSERT_TRUE(results.residuals == residual); + CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3); + ASSERT_EQ(results.local_jacobians.size(), 2); + ASSERT_EQ(results.local_numeric_jacobians.size(), 2); + EXPECT_TRUE(results.local_jacobians.at(0) == (j0 + j0_offset) * global_J_local); + EXPECT_TRUE(results.local_jacobians.at(1) == j1); + EXPECT_TRUE( + results.local_numeric_jacobians.at(0).isApprox(j0 * global_J_local, + kTolerance)); + EXPECT_TRUE(results.local_numeric_jacobians.at(1).isApprox(j1, kTolerance)); + EXPECT_TRUE(results.jacobians.at(0) == j0 + j0_offset); + EXPECT_TRUE(results.jacobians.at(1) == j1); + EXPECT_TRUE(results.numeric_jacobians.at(0).isApprox(j0, kTolerance)); + EXPECT_TRUE(results.numeric_jacobians.at(1).isApprox(j1, kTolerance)); + EXPECT_GT(results.maximum_relative_error, 0.0); + EXPECT_FALSE(results.error_log.empty()); + + // Test interaction with the 'check_gradients' option in Solver. + param0_solver = param0; + param1_solver = param1; + solver.Solve(solver_options, &problem, &summary); + EXPECT_EQ(FAILURE, summary.termination_type); + + // Now, zero out the local parameterization Jacobian of the 1st parameter + // with respect to the 3rd component. This makes the combination of + // cost function and local parameterization return correct values again. + parameterization.global_J_local.row(2).setZero(); + + // Verify that the gradient checker does not treat this as an error. + EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, &results)) + << results.error_log; + + // Check that results contain correct data. + ASSERT_EQ(results.return_value, true); + ASSERT_TRUE(results.residuals == residual); + CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3); + ASSERT_EQ(results.local_jacobians.size(), 2); + ASSERT_EQ(results.local_numeric_jacobians.size(), 2); + EXPECT_TRUE(results.local_jacobians.at(0) == + (j0 + j0_offset) * parameterization.global_J_local); + EXPECT_TRUE(results.local_jacobians.at(1) == j1); + EXPECT_TRUE(results.local_numeric_jacobians.at(0).isApprox( + j0 * parameterization.global_J_local, kTolerance)); + EXPECT_TRUE(results.local_numeric_jacobians.at(1).isApprox(j1, kTolerance)); + EXPECT_TRUE(results.jacobians.at(0) == j0 + j0_offset); + EXPECT_TRUE(results.jacobians.at(1) == j1); + EXPECT_TRUE(results.numeric_jacobians.at(0).isApprox(j0, kTolerance)); + EXPECT_TRUE(results.numeric_jacobians.at(1).isApprox(j1, kTolerance)); + EXPECT_GE(results.maximum_relative_error, 0.0); + EXPECT_TRUE(results.error_log.empty()); + + // Test interaction with the 'check_gradients' option in Solver. + param0_solver = param0; + param1_solver = param1; + solver.Solve(solver_options, &problem, &summary); + EXPECT_EQ(CONVERGENCE, summary.termination_type); + EXPECT_LE(summary.final_cost, 1e-12); +} + } // namespace internal } // namespace ceres diff --git a/internal/ceres/gradient_checking_cost_function.cc b/internal/ceres/gradient_checking_cost_function.cc index 580fd260e..6f0c59b61 100644 --- a/internal/ceres/gradient_checking_cost_function.cc +++ b/internal/ceres/gradient_checking_cost_function.cc @@ -26,7 +26,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. // -// Author: keir@google.com (Keir Mierle) +// Authors: keir@google.com (Keir Mierle), +// dgossow@google.com (David Gossow) #include "ceres/gradient_checking_cost_function.h" @@ -36,7 +37,7 @@ #include #include -#include "ceres/cost_function.h" +#include "ceres/gradient_checker.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/parameter_block.h" @@ -59,55 +60,25 @@ using std::vector; namespace { -// True if x and y have an absolute relative difference less than -// relative_precision and false otherwise. Stores the relative and absolute -// difference in relative/absolute_error if non-NULL. -bool IsClose(double x, double y, double relative_precision, - double *relative_error, - double *absolute_error) { - double local_absolute_error; - double local_relative_error; - if (!absolute_error) { - absolute_error = &local_absolute_error; - } - if (!relative_error) { - relative_error = &local_relative_error; - } - *absolute_error = abs(x - y); - *relative_error = *absolute_error / max(abs(x), abs(y)); - if (x == 0 || y == 0) { - // If x or y is exactly zero, then relative difference doesn't have any - // meaning. Take the absolute difference instead. - *relative_error = *absolute_error; - } - return abs(*relative_error) < abs(relative_precision); -} - class GradientCheckingCostFunction : public CostFunction { public: - GradientCheckingCostFunction(const CostFunction* function, - const NumericDiffOptions& options, - double relative_precision, - const string& extra_info) + GradientCheckingCostFunction( + const CostFunction* function, + const std::vector* local_parameterizations, + const NumericDiffOptions& options, + double relative_precision, + const string& extra_info, + GradientCheckingIterationCallback* callback) : function_(function), + gradient_checker_(function, local_parameterizations, options), relative_precision_(relative_precision), - extra_info_(extra_info) { - DynamicNumericDiffCostFunction* - finite_diff_cost_function = - new DynamicNumericDiffCostFunction( - function, - DO_NOT_TAKE_OWNERSHIP, - options); - + extra_info_(extra_info), + callback_(callback) { + CHECK_NOTNULL(callback_); const vector& parameter_block_sizes = function->parameter_block_sizes(); - for (int i = 0; i < parameter_block_sizes.size(); ++i) { - finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]); - } *mutable_parameter_block_sizes() = parameter_block_sizes; set_num_residuals(function->num_residuals()); - finite_diff_cost_function->SetNumResiduals(num_residuals()); - finite_diff_cost_function_.reset(finite_diff_cost_function); } virtual ~GradientCheckingCostFunction() { } @@ -120,133 +91,90 @@ class GradientCheckingCostFunction : public CostFunction { return function_->Evaluate(parameters, residuals, NULL); } - int num_residuals = function_->num_residuals(); + GradientChecker::ProbeResults results; + bool okay = gradient_checker_.Probe(parameters, + relative_precision_, + &results); - // Make space for the jacobians of the two methods. - const vector& block_sizes = function_->parameter_block_sizes(); - vector term_jacobians(block_sizes.size()); - vector finite_difference_jacobians(block_sizes.size()); - vector term_jacobian_pointers(block_sizes.size()); - vector finite_difference_jacobian_pointers(block_sizes.size()); - for (int i = 0; i < block_sizes.size(); i++) { - term_jacobians[i].resize(num_residuals, block_sizes[i]); - term_jacobian_pointers[i] = term_jacobians[i].data(); - finite_difference_jacobians[i].resize(num_residuals, block_sizes[i]); - finite_difference_jacobian_pointers[i] = - finite_difference_jacobians[i].data(); - } - - // Evaluate the derivative using the user supplied code. - if (!function_->Evaluate(parameters, - residuals, - &term_jacobian_pointers[0])) { - LOG(WARNING) << "Function evaluation failed."; + // If the cost function returned false, there's nothing we can say about + // the gradients. + if (results.return_value == false) { return false; } - // Evaluate the derivative using numeric derivatives. - finite_diff_cost_function_->Evaluate( - parameters, - residuals, - &finite_difference_jacobian_pointers[0]); + // Copy the residuals. + const int num_residuals = function_->num_residuals(); + MatrixRef(residuals, num_residuals, 1) = results.residuals; - // See if any elements have relative error larger than the threshold. - int num_bad_jacobian_components = 0; - double 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 m; + // Copy the original jacobian blocks into the jacobians array. + const vector& block_sizes = function_->parameter_block_sizes(); for (int k = 0; k < block_sizes.size(); k++) { - // Copy the original jacobian blocks into the jacobians array. if (jacobians[k] != NULL) { MatrixRef(jacobians[k], - term_jacobians[k].rows(), - term_jacobians[k].cols()) = term_jacobians[k]; - } - - StringAppendF(&m, - "========== " - "Jacobian for " "block %d: (%ld by %ld)) " - "==========\n", - k, - static_cast(term_jacobians[k].rows()), - static_cast(term_jacobians[k].cols())); - // The funny spacing creates appropriately aligned column headers. - m += " block row col user dx/dy num diff dx/dy " - "abs error relative error parameter residual\n"; - - for (int i = 0; i < term_jacobians[k].rows(); i++) { - for (int j = 0; j < term_jacobians[k].cols(); j++) { - double term_jacobian = term_jacobians[k](i, j); - double finite_jacobian = finite_difference_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 = max(worst_relative_error, relative_error); - - StringAppendF(&m, "%6d %4d %4d %17g %17g %17g %17g %17g %17g", - k, i, j, - term_jacobian, finite_jacobian, - absolute_error, relative_error, - parameters[k][j], - residuals[i]); - - if (bad_jacobian_entry) { - num_bad_jacobian_components++; - StringAppendF( - &m, " ------ (%d,%d,%d) Relative error worse than %g", - k, i, j, relative_precision_); - } - m += "\n"; - } + results.jacobians[k].rows(), + results.jacobians[k].cols()) = results.jacobians[k]; } } - // Since there were some bad errors, dump comprehensive debug info. - if (num_bad_jacobian_components) { - string header = StringPrintf("Detected %d bad jacobian component(s). " - "Worst relative error was %g.\n", - num_bad_jacobian_components, - worst_relative_error); - if (!extra_info_.empty()) { - header += "Extra info for this residual: " + extra_info_ + "\n"; - } - LOG(WARNING) << "\n" << header << m; + if (!okay) { + std::string error_log = "Gradient Error detected!\nExtra info for " + "this residual: " + extra_info_ + "\n" + results.error_log; + callback_->SetGradientErrorDetected(error_log); } return true; } private: const CostFunction* function_; - internal::scoped_ptr finite_diff_cost_function_; + GradientChecker gradient_checker_; double relative_precision_; string extra_info_; + GradientCheckingIterationCallback* callback_; }; } // namespace -CostFunction *CreateGradientCheckingCostFunction( - const CostFunction *cost_function, +GradientCheckingIterationCallback::GradientCheckingIterationCallback() + : gradient_error_detected_(false) { +} + +CallbackReturnType GradientCheckingIterationCallback::operator()( + const IterationSummary& summary) { + if (gradient_error_detected_) { + LOG(ERROR)<< "Gradient error detected. Terminating solver."; + return SOLVER_ABORT; + } + return SOLVER_CONTINUE; +} +void GradientCheckingIterationCallback::SetGradientErrorDetected( + std::string& error_log) { + gradient_error_detected_ = true; + error_log_ += "\n" + error_log; +} + +CostFunction* CreateGradientCheckingCostFunction( + const CostFunction* cost_function, + const std::vector* local_parameterizations, double relative_step_size, double relative_precision, - const string& extra_info) { + const std::string& extra_info, + GradientCheckingIterationCallback* callback) { NumericDiffOptions numeric_diff_options; numeric_diff_options.relative_step_size = relative_step_size; return new GradientCheckingCostFunction(cost_function, + local_parameterizations, numeric_diff_options, - relative_precision, - extra_info); + relative_precision, extra_info, + callback); } -ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, - double relative_step_size, - double relative_precision) { +ProblemImpl* CreateGradientCheckingProblemImpl( + ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision, + GradientCheckingIterationCallback* callback) { + CHECK_NOTNULL(callback); // We create new CostFunctions by wrapping the original CostFunction // in a gradient checking CostFunction. So its okay for the // ProblemImpl to take ownership of it and destroy it. The @@ -260,6 +188,9 @@ ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, gradient_checking_problem_options.local_parameterization_ownership = DO_NOT_TAKE_OWNERSHIP; + NumericDiffOptions numeric_diff_options; + numeric_diff_options.relative_step_size = relative_step_size; + ProblemImpl* gradient_checking_problem_impl = new ProblemImpl( gradient_checking_problem_options); @@ -294,19 +225,26 @@ ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, string extra_info = StringPrintf( "Residual block id %d; depends on parameters [", i); vector parameter_blocks; + vector local_parameterizations; + parameter_blocks.reserve(residual_block->NumParameterBlocks()); + local_parameterizations.reserve(residual_block->NumParameterBlocks()); for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; parameter_blocks.push_back(parameter_block->mutable_user_state()); StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state()); extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]"; + local_parameterizations.push_back(problem_impl->GetParameterization( + parameter_block->mutable_user_state())); } // Wrap the original CostFunction in a GradientCheckingCostFunction. CostFunction* gradient_checking_cost_function = - CreateGradientCheckingCostFunction(residual_block->cost_function(), - relative_step_size, - relative_precision, - extra_info); + new GradientCheckingCostFunction(residual_block->cost_function(), + &local_parameterizations, + numeric_diff_options, + relative_precision, + extra_info, + callback); // The const_cast is necessary because // ProblemImpl::AddResidualBlock can potentially take ownership of diff --git a/internal/ceres/gradient_checking_cost_function.h b/internal/ceres/gradient_checking_cost_function.h index cf92cb72b..d8bbfedb2 100644 --- a/internal/ceres/gradient_checking_cost_function.h +++ b/internal/ceres/gradient_checking_cost_function.h @@ -26,7 +26,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. // -// Author: keir@google.com (Keir Mierle) +// Authors: keir@google.com (Keir Mierle), +// dgossow@google.com (David Gossow) #ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ #define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ @@ -34,50 +35,72 @@ #include #include "ceres/cost_function.h" +#include "ceres/iteration_callback.h" +#include "ceres/local_parameterization.h" namespace ceres { namespace internal { class ProblemImpl; -// Creates a CostFunction that checks the jacobians that cost_function computes -// with finite differences. Bad results are logged; required precision is -// controlled by relative_precision and the numeric differentiation step size is -// controlled with relative_step_size. See solver.h for a better explanation of -// relative_step_size. Caller owns result. -// -// The condition enforced is that -// -// (J_actual(i, j) - J_numeric(i, j)) -// ------------------------------------ < relative_precision -// max(J_actual(i, j), J_numeric(i, j)) -// -// where J_actual(i, j) is the jacobian as computed by the supplied cost -// function (by the user) and J_numeric is the jacobian as computed by finite -// differences. -// -// Note: This is quite inefficient and is intended only for debugging. +// Callback that collects information about gradient checking errors, and +// will abort the solve as soon as an error occurs. +class GradientCheckingIterationCallback : public IterationCallback { + public: + GradientCheckingIterationCallback(); + + // Will return SOLVER_CONTINUE until a gradient error has been detected, + // then return SOLVER_ABORT. + virtual CallbackReturnType operator()(const IterationSummary& summary); + + // Notify this that a gradient error has occured. + void SetGradientErrorDetected(std::string& error_log); + + bool gradient_error_detected() const { return gradient_error_detected_; } + const std::string& error_log() const { return error_log_; } + private: + bool gradient_error_detected_; + std::string error_log_; +}; + +// Creates a CostFunction that checks the Jacobians that cost_function computes +// with finite differences. This API is only intended for unit tests that intend +// to check the functionality of the GradientCheckingCostFunction +// implementation directly. CostFunction* CreateGradientCheckingCostFunction( const CostFunction* cost_function, + const std::vector* local_parameterizations, double relative_step_size, double relative_precision, - const std::string& extra_info); + const std::string& extra_info, + GradientCheckingIterationCallback* callback); -// Create a new ProblemImpl object from the input problem_impl, where -// each CostFunctions in problem_impl are wrapped inside a -// GradientCheckingCostFunctions. This gives us a ProblemImpl object -// which checks its derivatives against estimates from numeric -// differentiation everytime a ResidualBlock is evaluated. +// Create a new ProblemImpl object from the input problem_impl, where all +// cost functions are wrapped so that each time their Evaluate method is called, +// an additional check is performed that compares the Jacobians computed by +// the original cost function with alternative Jacobians computed using +// numerical differentiation. If local parameterizations are given for any +// parameters, the Jacobians will be compared in the local space instead of the +// ambient space. For details on the gradient checking procedure, see the +// documentation of the GradientChecker class. If an error is detected in any +// iteration, the respective cost function will notify the +// GradientCheckingIterationCallback. +// +// The caller owns the returned ProblemImpl object. +// +// Note: This is quite inefficient and is intended only for debugging. // // relative_step_size and relative_precision are parameters to control // the numeric differentiation and the relative tolerance between the // jacobian computed by the CostFunctions in problem_impl and -// jacobians obtained by numerically differentiating them. For more -// details see the documentation for -// CreateGradientCheckingCostFunction above. -ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, - double relative_step_size, - double relative_precision); +// jacobians obtained by numerically differentiating them. See the +// documentation of 'numeric_derivative_relative_step_size' in solver.h for a +// better explanation. +ProblemImpl* CreateGradientCheckingProblemImpl( + ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision, + GradientCheckingIterationCallback* callback); } // namespace internal } // namespace ceres diff --git a/internal/ceres/gradient_checking_cost_function_test.cc b/internal/ceres/gradient_checking_cost_function_test.cc index 54c616619..e7238c669 100644 --- a/internal/ceres/gradient_checking_cost_function_test.cc +++ b/internal/ceres/gradient_checking_cost_function_test.cc @@ -162,11 +162,12 @@ TEST(GradientCheckingCostFunction, ResidualsAndJacobiansArePreservedTest) { const double kRelativePrecision = 1e-4; TestTerm<-1, -1> term(arity, dim); + GradientCheckingIterationCallback callback; scoped_ptr gradient_checking_cost_function( - CreateGradientCheckingCostFunction(&term, + CreateGradientCheckingCostFunction(&term, NULL, kRelativeStepSize, kRelativePrecision, - "Ignored.")); + "Ignored.", &callback)); term.Evaluate(¶meters[0], &original_residual, &original_jacobians[0]); @@ -218,39 +219,35 @@ TEST(GradientCheckingCostFunction, SmokeTest) { LOG(INFO) << "Bad gradient"; { TestTerm<1, 2> term(arity, dim); + GradientCheckingIterationCallback callback; scoped_ptr gradient_checking_cost_function( - CreateGradientCheckingCostFunction(&term, + CreateGradientCheckingCostFunction(&term, NULL, kRelativeStepSize, kRelativePrecision, - "Fuzzy bananas")); - - ScopedMockLog log; - EXPECT_CALL(log, Log(_, _, _)).Times(AnyNumber()); - EXPECT_CALL(log, Log(WARNING, _, - AllOf(HasSubstr("(1,0,2) Relative error worse than"), - HasSubstr("Fuzzy bananas")))); - - gradient_checking_cost_function->Evaluate(¶meters[0], - &residual, - &jacobians[0]); + "Fuzzy banana", &callback)); + EXPECT_TRUE( + gradient_checking_cost_function->Evaluate(¶meters[0], &residual, + &jacobians[0])); + EXPECT_TRUE(callback.gradient_error_detected()); + EXPECT_TRUE(callback.error_log().find("Fuzzy banana") != std::string::npos); + EXPECT_TRUE(callback.error_log().find("(1,0,2) Relative error worse than") + != std::string::npos); } // The gradient is correct, so no errors are reported. LOG(INFO) << "Good gradient"; { TestTerm<-1, -1> term(arity, dim); + GradientCheckingIterationCallback callback; scoped_ptr gradient_checking_cost_function( - CreateGradientCheckingCostFunction(&term, + CreateGradientCheckingCostFunction(&term, NULL, kRelativeStepSize, kRelativePrecision, - "Ignored.")); - - ScopedMockLog log; - EXPECT_CALL(log, Log(_, _, _)).Times(0); - - gradient_checking_cost_function->Evaluate(¶meters[0], - &residual, - &jacobians[0]); + "Fuzzy banana", &callback)); + EXPECT_TRUE( + gradient_checking_cost_function->Evaluate(¶meters[0], &residual, + &jacobians[0])); + EXPECT_FALSE(callback.gradient_error_detected()); } for (int j = 0; j < arity; j++) { @@ -362,8 +359,9 @@ TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) { problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), NULL, z, x, y); + GradientCheckingIterationCallback callback; scoped_ptr gradient_checking_problem_impl( - CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0)); + CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback)); // The dimensions of the two problems match. EXPECT_EQ(problem_impl.NumParameterBlocks(), @@ -383,7 +381,7 @@ TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) { // Since we added the ParameterBlocks and ResidualBlocks explicitly, // they should be in the same order in the two programs. It is // possible that may change due to implementation changes to - // Program. This is not exepected to be the case and writing code to + // Program. This is not expected to be the case and writing code to // anticipate that possibility not worth the extra complexity in // this test. for (int i = 0; i < program.parameter_blocks().size(); ++i) { diff --git a/internal/ceres/is_close.cc b/internal/ceres/is_close.cc new file mode 100644 index 000000000..a91a17454 --- /dev/null +++ b/internal/ceres/is_close.cc @@ -0,0 +1,59 @@ +// 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: keir@google.com (Keir Mierle), dgossow@google.com (David Gossow) + +#include "ceres/is_close.h" + +#include +#include + +namespace ceres { +namespace internal { +bool IsClose(double x, double y, double relative_precision, + double *relative_error, + double *absolute_error) { + double local_absolute_error; + double local_relative_error; + if (!absolute_error) { + absolute_error = &local_absolute_error; + } + if (!relative_error) { + relative_error = &local_relative_error; + } + *absolute_error = std::fabs(x - y); + *relative_error = *absolute_error / std::max(std::fabs(x), std::fabs(y)); + if (x == 0 || y == 0) { + // If x or y is exactly zero, then relative difference doesn't have any + // meaning. Take the absolute difference instead. + *relative_error = *absolute_error; + } + return *relative_error < std::fabs(relative_precision); +} +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/is_close.h b/internal/ceres/is_close.h new file mode 100644 index 000000000..7789448c8 --- /dev/null +++ b/internal/ceres/is_close.h @@ -0,0 +1,51 @@ +// 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: keir@google.com (Keir Mierle), dgossow@google.com (David Gossow) +// +// Utility routine for comparing two values. + +#ifndef CERES_INTERNAL_IS_CLOSE_H_ +#define CERES_INTERNAL_IS_CLOSE_H_ + +namespace ceres { +namespace internal { +// Returns true if x and y have a relative (unsigned) difference less than +// relative_precision and false otherwise. Stores the relative and absolute +// difference in relative/absolute_error if non-NULL. If one of the two values +// is exactly zero, the absolute difference will be compared, and relative_error +// will be set to the absolute difference. +bool IsClose(double x, + double y, + double relative_precision, + double *relative_error, + double *absolute_error); +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_IS_CLOSE_H_ diff --git a/internal/ceres/is_close_test.cc b/internal/ceres/is_close_test.cc new file mode 100644 index 000000000..8f7aabab5 --- /dev/null +++ b/internal/ceres/is_close_test.cc @@ -0,0 +1,177 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 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. +// +// Author: dgossow@google.com (David Gossow) +// +// This file contains tests for the IsClose function. + +#include "ceres/is_close.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +const double kTolerance = 1e-9; + +TEST(IsClose, BothParametersPositive) { + double relative_error = -1; + double absolute_error = -1; + + // Test cases where both values are positive. + EXPECT_TRUE(IsClose(9.9, 10.0, 0.011, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.01, kTolerance); + EXPECT_NEAR(absolute_error, 0.1, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_TRUE(IsClose(10.0, 9.9, 0.011, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.01, kTolerance); + EXPECT_NEAR(absolute_error, 0.1, kTolerance); + relative_error = -1; + absolute_error = -1; + + EXPECT_FALSE(IsClose(9.9, 10.0, 0.009, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.01, kTolerance); + EXPECT_NEAR(absolute_error, 0.1, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_FALSE(IsClose(10.0, 9.9, 0.009, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.01, kTolerance); + EXPECT_NEAR(absolute_error, 0.1, kTolerance); +} + +TEST(IsClose, BothParametersNegative) { + double relative_error = -1; + double absolute_error = -1; + + // Test cases where both values are negative. + EXPECT_TRUE(IsClose(-9.9, -10.0, 0.011, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.01, kTolerance); + EXPECT_NEAR(absolute_error, 0.1, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_TRUE(IsClose(-10.0, -9.9, 0.011, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.01, kTolerance); + EXPECT_NEAR(absolute_error, 0.1, kTolerance); + relative_error = -1; + absolute_error = -1; + + EXPECT_FALSE(IsClose(-9.9, -10.0, 0.009, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.01, kTolerance); + EXPECT_NEAR(absolute_error, 0.1, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_FALSE(IsClose(-10.0, -9.9, 0.009, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.01, kTolerance); + EXPECT_NEAR(absolute_error, 0.1, kTolerance); +} + +TEST(IsClose, ParametersHaveMixedSigns) { + double relative_error = -1; + double absolute_error = -1; + + // Test cases with mixed signs. + EXPECT_FALSE(IsClose(-0.1, 0.1, 1.99, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 2.0, kTolerance); + EXPECT_NEAR(absolute_error, 0.2, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_TRUE(IsClose(-0.1, 0.1, 2.01, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 2.0, kTolerance); + EXPECT_NEAR(absolute_error, 0.2, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_FALSE(IsClose(0.1, -0.1, 1.99, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 2.0, kTolerance); + EXPECT_NEAR(absolute_error, 0.2, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_TRUE(IsClose(0.1, -0.1, 2.01, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 2.0, kTolerance); + EXPECT_NEAR(absolute_error, 0.2, kTolerance); +} + +TEST(IsClose, OneParameterZero) { + double relative_error = -1; + double absolute_error = -1; + + // Test cases where one of the values is zero. + EXPECT_TRUE(IsClose(0.0, 10.0, 10.1, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 10.0, kTolerance); + EXPECT_NEAR(absolute_error, 10.0, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_TRUE(IsClose(10.0, 0.0, 10.1, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 10.0, kTolerance); + EXPECT_NEAR(absolute_error, 10.0, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_TRUE(IsClose(0.0, -10.0, 10.1, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 10.0, kTolerance); + EXPECT_NEAR(absolute_error, 10.0, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_TRUE(IsClose(-10.0, 0.0, 10.1, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 10.0, kTolerance); + EXPECT_NEAR(absolute_error, 10.0, kTolerance); + relative_error = -1; + absolute_error = -1; + + EXPECT_FALSE(IsClose(0, 10.0, 9.9, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 10.0, kTolerance); + EXPECT_NEAR(absolute_error, 10.0, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_FALSE(IsClose(10.0, 0.0, 9.9, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 10.0, kTolerance); + EXPECT_NEAR(absolute_error, 10.0, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_FALSE(IsClose(0, -10.0, 9.9, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 10.0, kTolerance); + EXPECT_NEAR(absolute_error, 10.0, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_FALSE(IsClose(-10.0, 0.0, 9.9, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 10.0, kTolerance); + EXPECT_NEAR(absolute_error, 10.0, kTolerance); +} + +TEST(IsClose, BothParametersZero) { + double relative_error = -1; + double absolute_error = -1; + EXPECT_TRUE(IsClose(0.0, 0.0, 0.1, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.0, kTolerance); + EXPECT_NEAR(absolute_error, 0.0, kTolerance); + relative_error = -1; + absolute_error = -1; + EXPECT_FALSE(IsClose(0.0, 0.0, 0.0, &relative_error, &absolute_error)); + EXPECT_NEAR(relative_error, 0.0, kTolerance); + EXPECT_NEAR(absolute_error, 0.0, kTolerance); +} +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc index 7f6e82981..d980ba2b7 100644 --- a/internal/ceres/rotation_test.cc +++ b/internal/ceres/rotation_test.cc @@ -32,6 +32,7 @@ #include #include #include "ceres/internal/eigen.h" +#include "ceres/is_close.h" #include "ceres/internal/port.h" #include "ceres/jet.h" #include "ceres/rotation.h" @@ -701,12 +702,7 @@ J4 MakeJ4(double a, double v0, double v1, double v2, double v3) { bool IsClose(double x, double y) { EXPECT_FALSE(IsNaN(x)); EXPECT_FALSE(IsNaN(y)); - double absdiff = fabs(x - y); - if (x == 0 || y == 0) { - return absdiff <= kTolerance; - } - double reldiff = absdiff / max(fabs(x), fabs(y)); - return reldiff <= kTolerance; + return internal::IsClose(x, y, kTolerance, NULL, NULL); } template diff --git a/internal/ceres/solver.cc b/internal/ceres/solver.cc index 21d3c6869..5cc0a6681 100644 --- a/internal/ceres/solver.cc +++ b/internal/ceres/solver.cc @@ -496,21 +496,28 @@ void Solver::Solve(const Solver::Options& options, // values provided by the user. program->SetParameterBlockStatePtrsToUserStatePtrs(); + // If gradient_checking is enabled, wrap all cost functions in a + // gradient checker and install a callback that terminates if any gradient + // error is detected. scoped_ptr gradient_checking_problem; + internal::GradientCheckingIterationCallback gradient_checking_callback; + Solver::Options modified_options = options; if (options.check_gradients) { + modified_options.callbacks.push_back(&gradient_checking_callback); gradient_checking_problem.reset( CreateGradientCheckingProblemImpl( problem_impl, options.numeric_derivative_relative_step_size, - options.gradient_check_relative_precision)); + options.gradient_check_relative_precision, + &gradient_checking_callback)); problem_impl = gradient_checking_problem.get(); program = problem_impl->mutable_program(); } scoped_ptr preprocessor( - Preprocessor::Create(options.minimizer_type)); + Preprocessor::Create(modified_options.minimizer_type)); PreprocessedProblem pp; - const bool status = preprocessor->Preprocess(options, problem_impl, &pp); + const bool status = preprocessor->Preprocess(modified_options, problem_impl, &pp); summary->fixed_cost = pp.fixed_cost; summary->preprocessor_time_in_seconds = WallTimeInSeconds() - start_time; @@ -535,6 +542,13 @@ void Solver::Solve(const Solver::Options& options, summary->postprocessor_time_in_seconds = WallTimeInSeconds() - postprocessor_start_time; + // If the gradient checker reported an error, we want to report FAILURE + // instead of USER_FAILURE and provide the error log. + if (gradient_checking_callback.gradient_error_detected()) { + summary->termination_type = FAILURE; + summary->message = gradient_checking_callback.error_log(); + } + summary->total_time_in_seconds = WallTimeInSeconds() - start_time; }