| // 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: keir@google.com (Keir Mierle) |
| |
| #include "ceres/gradient_checking_cost_function.h" |
| |
| #include <cmath> |
| #include <vector> |
| #include "ceres/cost_function.h" |
| #include "ceres/internal/scoped_ptr.h" |
| #include "ceres/local_parameterization.h" |
| #include "ceres/loss_function.h" |
| #include "ceres/parameter_block.h" |
| #include "ceres/problem_impl.h" |
| #include "ceres/program.h" |
| #include "ceres/random.h" |
| #include "ceres/residual_block.h" |
| #include "ceres/sized_cost_function.h" |
| #include "ceres/types.h" |
| #include "glog/logging.h" |
| #include "gmock/gmock.h" |
| #include "gmock/mock-log.h" |
| #include "gtest/gtest.h" |
| |
| namespace ceres { |
| namespace internal { |
| |
| using std::vector; |
| using testing::AllOf; |
| using testing::AnyNumber; |
| using testing::HasSubstr; |
| using testing::ScopedMockLog; |
| using testing::_; |
| |
| // Pick a (non-quadratic) function whose derivative are easy: |
| // |
| // f = exp(- a' x). |
| // df = - f a. |
| // |
| // where 'a' is a vector of the same size as 'x'. In the block |
| // version, they are both block vectors, of course. |
| template<int bad_block = 1, int bad_variable = 2> |
| class TestTerm : public CostFunction { |
| public: |
| // The constructor of this function needs to know the number |
| // of blocks desired, and the size of each block. |
| TestTerm(int arity, int const *dim) : arity_(arity) { |
| // Make 'arity' random vectors. |
| a_.resize(arity_); |
| for (int j = 0; j < arity_; ++j) { |
| a_[j].resize(dim[j]); |
| for (int u = 0; u < dim[j]; ++u) { |
| a_[j][u] = 2.0 * RandDouble() - 1.0; |
| } |
| } |
| |
| for (int i = 0; i < arity_; i++) { |
| mutable_parameter_block_sizes()->push_back(dim[i]); |
| } |
| set_num_residuals(1); |
| } |
| |
| bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| // Compute a . x. |
| double ax = 0; |
| for (int j = 0; j < arity_; ++j) { |
| for (int u = 0; u < parameter_block_sizes()[j]; ++u) { |
| ax += a_[j][u] * parameters[j][u]; |
| } |
| } |
| |
| // This is the cost, but also appears as a factor |
| // in the derivatives. |
| double f = *residuals = exp(-ax); |
| |
| // Accumulate 1st order derivatives. |
| if (jacobians) { |
| for (int j = 0; j < arity_; ++j) { |
| if (jacobians[j]) { |
| for (int u = 0; u < parameter_block_sizes()[j]; ++u) { |
| // See comments before class. |
| jacobians[j][u] = - f * a_[j][u]; |
| |
| if (bad_block == j && bad_variable == u) { |
| // Whoopsiedoopsie! Deliberately introduce a faulty jacobian entry |
| // like what happens when users make an error in their jacobian |
| // computations. This should get detected. |
| LOG(INFO) << "Poisoning jacobian for parameter block " << j |
| << ", row 0, column " << u; |
| jacobians[j][u] += 500; |
| } |
| } |
| } |
| } |
| } |
| |
| return true; |
| } |
| |
| private: |
| int arity_; |
| vector<vector<double> > a_; |
| }; |
| |
| TEST(GradientCheckingCostFunction, ResidualsAndJacobiansArePreservedTest) { |
| srand(5); |
| |
| // Test with 3 blocks of size 2, 3 and 4. |
| int const arity = 3; |
| int const dim[arity] = { 2, 3, 4 }; |
| |
| // Make a random set of blocks. |
| vector<double*> parameters(arity); |
| for (int j = 0; j < arity; ++j) { |
| parameters[j] = new double[dim[j]]; |
| for (int u = 0; u < dim[j]; ++u) { |
| parameters[j][u] = 2.0 * RandDouble() - 1.0; |
| } |
| } |
| |
| double original_residual; |
| double residual; |
| vector<double*> original_jacobians(arity); |
| vector<double*> jacobians(arity); |
| |
| for (int j = 0; j < arity; ++j) { |
| // Since residual is one dimensional the jacobians have the same |
| // size as the parameter blocks. |
| jacobians[j] = new double[dim[j]]; |
| original_jacobians[j] = new double[dim[j]]; |
| } |
| |
| const double kRelativeStepSize = 1e-6; |
| const double kRelativePrecision = 1e-4; |
| |
| TestTerm<-1, -1> term(arity, dim); |
| GradientCheckingIterationCallback callback; |
| scoped_ptr<CostFunction> gradient_checking_cost_function( |
| CreateGradientCheckingCostFunction(&term, NULL, |
| kRelativeStepSize, |
| kRelativePrecision, |
| "Ignored.", &callback)); |
| term.Evaluate(¶meters[0], |
| &original_residual, |
| &original_jacobians[0]); |
| |
| gradient_checking_cost_function->Evaluate(¶meters[0], |
| &residual, |
| &jacobians[0]); |
| EXPECT_EQ(original_residual, residual); |
| |
| for (int j = 0; j < arity; j++) { |
| for (int k = 0; k < dim[j]; ++k) { |
| EXPECT_EQ(original_jacobians[j][k], jacobians[j][k]); |
| } |
| |
| delete[] parameters[j]; |
| delete[] jacobians[j]; |
| delete[] original_jacobians[j]; |
| } |
| } |
| |
| TEST(GradientCheckingCostFunction, SmokeTest) { |
| srand(5); |
| |
| // Test with 3 blocks of size 2, 3 and 4. |
| int const arity = 3; |
| int const dim[arity] = { 2, 3, 4 }; |
| |
| // Make a random set of blocks. |
| vector<double*> parameters(arity); |
| for (int j = 0; j < arity; ++j) { |
| parameters[j] = new double[dim[j]]; |
| for (int u = 0; u < dim[j]; ++u) { |
| parameters[j][u] = 2.0 * RandDouble() - 1.0; |
| } |
| } |
| |
| double residual; |
| vector<double*> jacobians(arity); |
| for (int j = 0; j < arity; ++j) { |
| // Since residual is one dimensional the jacobians have the same size as the |
| // parameter blocks. |
| jacobians[j] = new double[dim[j]]; |
| } |
| |
| const double kRelativeStepSize = 1e-6; |
| const double kRelativePrecision = 1e-4; |
| |
| // Should have one term that's bad, causing everything to get dumped. |
| LOG(INFO) << "Bad gradient"; |
| { |
| TestTerm<1, 2> term(arity, dim); |
| GradientCheckingIterationCallback callback; |
| scoped_ptr<CostFunction> gradient_checking_cost_function( |
| CreateGradientCheckingCostFunction(&term, NULL, |
| kRelativeStepSize, |
| kRelativePrecision, |
| "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<CostFunction> gradient_checking_cost_function( |
| CreateGradientCheckingCostFunction(&term, NULL, |
| kRelativeStepSize, |
| kRelativePrecision, |
| "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++) { |
| delete[] parameters[j]; |
| delete[] jacobians[j]; |
| } |
| } |
| |
| // The following three classes are for the purposes of defining |
| // function signatures. They have dummy Evaluate functions. |
| |
| // Trivial cost function that accepts a single argument. |
| class UnaryCostFunction : public CostFunction { |
| public: |
| UnaryCostFunction(int num_residuals, int32 parameter_block_size) { |
| set_num_residuals(num_residuals); |
| mutable_parameter_block_sizes()->push_back(parameter_block_size); |
| } |
| virtual ~UnaryCostFunction() {} |
| |
| virtual bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| for (int i = 0; i < num_residuals(); ++i) { |
| residuals[i] = 1; |
| } |
| return true; |
| } |
| }; |
| |
| // Trivial cost function that accepts two arguments. |
| class BinaryCostFunction: public CostFunction { |
| public: |
| BinaryCostFunction(int num_residuals, |
| int32 parameter_block1_size, |
| int32 parameter_block2_size) { |
| set_num_residuals(num_residuals); |
| mutable_parameter_block_sizes()->push_back(parameter_block1_size); |
| mutable_parameter_block_sizes()->push_back(parameter_block2_size); |
| } |
| |
| virtual bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| for (int i = 0; i < num_residuals(); ++i) { |
| residuals[i] = 2; |
| } |
| return true; |
| } |
| }; |
| |
| // Trivial cost function that accepts three arguments. |
| class TernaryCostFunction: public CostFunction { |
| public: |
| TernaryCostFunction(int num_residuals, |
| int32 parameter_block1_size, |
| int32 parameter_block2_size, |
| int32 parameter_block3_size) { |
| set_num_residuals(num_residuals); |
| mutable_parameter_block_sizes()->push_back(parameter_block1_size); |
| mutable_parameter_block_sizes()->push_back(parameter_block2_size); |
| mutable_parameter_block_sizes()->push_back(parameter_block3_size); |
| } |
| |
| virtual bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| for (int i = 0; i < num_residuals(); ++i) { |
| residuals[i] = 3; |
| } |
| return true; |
| } |
| }; |
| |
| // Verify that the two ParameterBlocks are formed from the same user |
| // array and have the same LocalParameterization object. |
| void ParameterBlocksAreEquivalent(const ParameterBlock* left, |
| const ParameterBlock* right) { |
| CHECK_NOTNULL(left); |
| CHECK_NOTNULL(right); |
| EXPECT_EQ(left->user_state(), right->user_state()); |
| EXPECT_EQ(left->Size(), right->Size()); |
| EXPECT_EQ(left->Size(), right->Size()); |
| EXPECT_EQ(left->LocalSize(), right->LocalSize()); |
| EXPECT_EQ(left->local_parameterization(), right->local_parameterization()); |
| EXPECT_EQ(left->IsConstant(), right->IsConstant()); |
| } |
| |
| TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) { |
| // Parameter blocks with arbitrarily chosen initial values. |
| double x[] = {1.0, 2.0, 3.0}; |
| double y[] = {4.0, 5.0, 6.0, 7.0}; |
| double z[] = {8.0, 9.0, 10.0, 11.0, 12.0}; |
| double w[] = {13.0, 14.0, 15.0, 16.0}; |
| |
| ProblemImpl problem_impl; |
| problem_impl.AddParameterBlock(x, 3); |
| problem_impl.AddParameterBlock(y, 4); |
| problem_impl.SetParameterBlockConstant(y); |
| problem_impl.AddParameterBlock(z, 5); |
| problem_impl.AddParameterBlock(w, 4, new QuaternionParameterization); |
| problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); |
| problem_impl.AddResidualBlock(new BinaryCostFunction(6, 5, 4) , |
| NULL, z, y); |
| problem_impl.AddResidualBlock(new BinaryCostFunction(3, 3, 5), |
| new TrivialLoss, x, z); |
| problem_impl.AddResidualBlock(new BinaryCostFunction(7, 5, 3), |
| NULL, z, x); |
| problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), |
| NULL, z, x, y); |
| |
| GradientCheckingIterationCallback callback; |
| scoped_ptr<ProblemImpl> gradient_checking_problem_impl( |
| CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback)); |
| |
| // The dimensions of the two problems match. |
| EXPECT_EQ(problem_impl.NumParameterBlocks(), |
| gradient_checking_problem_impl->NumParameterBlocks()); |
| EXPECT_EQ(problem_impl.NumResidualBlocks(), |
| gradient_checking_problem_impl->NumResidualBlocks()); |
| |
| EXPECT_EQ(problem_impl.NumParameters(), |
| gradient_checking_problem_impl->NumParameters()); |
| EXPECT_EQ(problem_impl.NumResiduals(), |
| gradient_checking_problem_impl->NumResiduals()); |
| |
| const Program& program = problem_impl.program(); |
| const Program& gradient_checking_program = |
| gradient_checking_problem_impl->program(); |
| |
| // 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 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) { |
| ParameterBlocksAreEquivalent( |
| program.parameter_blocks()[i], |
| gradient_checking_program.parameter_blocks()[i]); |
| } |
| |
| for (int i = 0; i < program.residual_blocks().size(); ++i) { |
| // Compare the sizes of the two ResidualBlocks. |
| const ResidualBlock* original_residual_block = |
| program.residual_blocks()[i]; |
| const ResidualBlock* new_residual_block = |
| gradient_checking_program.residual_blocks()[i]; |
| EXPECT_EQ(original_residual_block->NumParameterBlocks(), |
| new_residual_block->NumParameterBlocks()); |
| EXPECT_EQ(original_residual_block->NumResiduals(), |
| new_residual_block->NumResiduals()); |
| EXPECT_EQ(original_residual_block->NumScratchDoublesForEvaluate(), |
| new_residual_block->NumScratchDoublesForEvaluate()); |
| |
| // Verify that the ParameterBlocks for the two residuals are equivalent. |
| for (int j = 0; j < original_residual_block->NumParameterBlocks(); ++j) { |
| ParameterBlocksAreEquivalent( |
| original_residual_block->parameter_blocks()[j], |
| new_residual_block->parameter_blocks()[j]); |
| } |
| } |
| } |
| |
| } // namespace internal |
| } // namespace ceres |