| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2018 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: mierle@gmail.com (Keir Mierle) |
| |
| #include "ceres/evaluation_callback.h" |
| |
| #include <cmath> |
| #include <limits> |
| #include <vector> |
| |
| #include "ceres/autodiff_cost_function.h" |
| #include "ceres/problem.h" |
| #include "ceres/problem_impl.h" |
| #include "ceres/sized_cost_function.h" |
| #include "ceres/solver.h" |
| #include "gtest/gtest.h" |
| |
| namespace ceres { |
| namespace internal { |
| |
| // Use an inline hash function to avoid portability wrangling. Algorithm from |
| // Daniel Bernstein, known as the "djb2" hash. |
| template <typename T> |
| uint64_t Djb2Hash(const T* data, const int size) { |
| uint64_t hash = 5381; |
| const uint8_t* data_as_bytes = reinterpret_cast<const uint8_t*>(data); |
| for (int i = 0; i < sizeof(*data) * size; ++i) { |
| hash = hash * 33 + data_as_bytes[i]; |
| } |
| return hash; |
| } |
| |
| const double kUninitialized = 0; |
| |
| // Generally multiple inheritance is a terrible idea, but in this (test) |
| // case it makes for a relatively elegant test implementation. |
| struct WigglyBowlCostFunctionAndEvaluationCallback : SizedCostFunction<2, 2>, |
| EvaluationCallback { |
| explicit WigglyBowlCostFunctionAndEvaluationCallback(double* parameter) |
| : EvaluationCallback(), |
| user_parameter_block(parameter), |
| prepare_num_calls(0), |
| prepare_requested_jacobians(false), |
| prepare_new_evaluation_point(false), |
| prepare_parameter_hash(kUninitialized), |
| evaluate_num_calls(0), |
| evaluate_last_parameter_hash(kUninitialized) {} |
| |
| virtual ~WigglyBowlCostFunctionAndEvaluationCallback() {} |
| |
| // Evaluation callback interface. This checks that all the preconditions are |
| // met at the point that Ceres calls into it. |
| void PrepareForEvaluation(bool evaluate_jacobians, |
| bool new_evaluation_point) final { |
| // At this point, the incoming parameters are implicitly pushed by Ceres |
| // into the user parameter blocks; in contrast to in Evaluate(). |
| uint64_t incoming_parameter_hash = Djb2Hash(user_parameter_block, 2); |
| |
| // Check: Prepare() & Evaluate() come in pairs, in that order. Before this |
| // call, the number of calls excluding this one should match. |
| EXPECT_EQ(prepare_num_calls, evaluate_num_calls); |
| |
| // Check: new_evaluation_point indicates that the parameter has changed. |
| if (new_evaluation_point) { |
| // If it's a new evaluation point, then the parameter should have |
| // changed. Technically, it's not required that it must change but |
| // in practice it does, and that helps with testing. |
| EXPECT_NE(evaluate_last_parameter_hash, incoming_parameter_hash); |
| EXPECT_NE(prepare_parameter_hash, incoming_parameter_hash); |
| } else { |
| // If this is the same evaluation point as last time, ensure that |
| // the parameters match both from the previous evaluate, the |
| // previous prepare, and the current prepare. |
| EXPECT_EQ(evaluate_last_parameter_hash, prepare_parameter_hash); |
| EXPECT_EQ(evaluate_last_parameter_hash, incoming_parameter_hash); |
| } |
| |
| // Save details for to check at the next call to Evaluate(). |
| prepare_num_calls++; |
| prepare_requested_jacobians = evaluate_jacobians; |
| prepare_new_evaluation_point = new_evaluation_point; |
| prepare_parameter_hash = incoming_parameter_hash; |
| } |
| |
| // Cost function interface. This checks that preconditions that were |
| // set as part of the PrepareForEvaluation() call are met in this one. |
| bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const final { |
| // Cost function implementation of the "Wiggly Bowl" function: |
| // |
| // 1/2 * [(y - a*sin(x))^2 + x^2], |
| // |
| // expressed as a Ceres cost function with two residuals: |
| // |
| // r[0] = y - a*sin(x) |
| // r[1] = x. |
| // |
| // This is harder to optimize than the Rosenbrock function because the |
| // minimizer has to navigate a sine-shaped valley while descending the 1D |
| // parabola formed along the y axis. Note that the "a" needs to be more |
| // than 5 to get a strong enough wiggle effect in the cost surface to |
| // trigger failed iterations in the optimizer. |
| const double a = 10.0; |
| double x = (*parameters)[0]; |
| double y = (*parameters)[1]; |
| residuals[0] = y - a * sin(x); |
| residuals[1] = x; |
| if (jacobians != NULL) { |
| (*jacobians)[2 * 0 + 0] = -a * cos(x); // df1/dx |
| (*jacobians)[2 * 0 + 1] = 1.0; // df1/dy |
| (*jacobians)[2 * 1 + 0] = 1.0; // df2/dx |
| (*jacobians)[2 * 1 + 1] = 0.0; // df2/dy |
| } |
| |
| uint64_t incoming_parameter_hash = Djb2Hash(*parameters, 2); |
| |
| // Check: PrepareForEvaluation() & Evaluate() come in pairs, in that order. |
| EXPECT_EQ(prepare_num_calls, evaluate_num_calls + 1); |
| |
| // Check: if new_evaluation_point indicates that the parameter has |
| // changed, it has changed; otherwise it is the same. |
| if (prepare_new_evaluation_point) { |
| EXPECT_NE(evaluate_last_parameter_hash, incoming_parameter_hash); |
| } else { |
| EXPECT_NE(evaluate_last_parameter_hash, kUninitialized); |
| EXPECT_EQ(evaluate_last_parameter_hash, incoming_parameter_hash); |
| } |
| |
| // Check: Parameter matches value in in parameter blocks during prepare. |
| EXPECT_EQ(prepare_parameter_hash, incoming_parameter_hash); |
| |
| // Check: jacobians are requested if they were in PrepareForEvaluation(). |
| EXPECT_EQ(prepare_requested_jacobians, jacobians != NULL); |
| |
| evaluate_num_calls++; |
| evaluate_last_parameter_hash = incoming_parameter_hash; |
| return true; |
| } |
| |
| // Pointer to the parameter block associated with this cost function. |
| // Contents should get set by Ceres before calls to PrepareForEvaluation() |
| // and Evaluate(). |
| double* user_parameter_block; |
| |
| // Track state: PrepareForEvaluation(). |
| // |
| // These track details from the PrepareForEvaluation() call (hence the |
| // "prepare_" prefix), which are checked for consistency in Evaluate(). |
| int prepare_num_calls; |
| bool prepare_requested_jacobians; |
| bool prepare_new_evaluation_point; |
| uint64_t prepare_parameter_hash; |
| |
| // Track state: Evaluate(). |
| // |
| // These track details from the Evaluate() call (hence the "evaluate_" |
| // prefix), which are then checked for consistency in the calls to |
| // PrepareForEvaluation(). Mutable is reasonable for this case. |
| mutable int evaluate_num_calls; |
| mutable uint64_t evaluate_last_parameter_hash; |
| }; |
| |
| TEST(EvaluationCallback, WithTrustRegionMinimizer) { |
| double parameters[2] = {50.0, 50.0}; |
| const uint64_t original_parameters_hash = Djb2Hash(parameters, 2); |
| |
| WigglyBowlCostFunctionAndEvaluationCallback cost_function(parameters); |
| Problem::Options problem_options; |
| problem_options.evaluation_callback = &cost_function; |
| problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; |
| Problem problem(problem_options); |
| problem.AddResidualBlock(&cost_function, NULL, parameters); |
| |
| Solver::Options options; |
| options.linear_solver_type = DENSE_QR; |
| options.max_num_iterations = 50; |
| |
| // Run the solve. Checking is done inside the cost function / callback. |
| Solver::Summary summary; |
| Solve(options, &problem, &summary); |
| |
| // Ensure that this was a hard cost function (not all steps succeed). |
| EXPECT_GT(summary.num_successful_steps, 10); |
| EXPECT_GT(summary.num_unsuccessful_steps, 10); |
| |
| // Ensure PrepareForEvaluation() is called the appropriate number of times. |
| EXPECT_EQ( |
| cost_function.prepare_num_calls, |
| // Unsuccessful steps are evaluated only once (no jacobians). |
| summary.num_unsuccessful_steps + |
| // Successful steps are evaluated twice: with and without jacobians. |
| 2 * summary.num_successful_steps |
| // Final iteration doesn't re-evaluate the jacobian. |
| // Note: This may be sensitive to tweaks to the TR algorithm; if |
| // this becomes too brittle, remove this EXPECT_EQ() entirely. |
| - 1); |
| |
| // Ensure the callback calls ran a reasonable number of times. |
| EXPECT_GT(cost_function.prepare_num_calls, 0); |
| EXPECT_GT(cost_function.evaluate_num_calls, 0); |
| EXPECT_EQ(cost_function.prepare_num_calls, cost_function.evaluate_num_calls); |
| |
| // Ensure that the parameters did actually change. |
| EXPECT_NE(Djb2Hash(parameters, 2), original_parameters_hash); |
| } |
| |
| // r = 1 - x |
| struct LinearResidual { |
| template <typename T> |
| bool operator()(const T* x, T* residuals) const { |
| residuals[0] = 1.0 - x[0]; |
| return true; |
| } |
| |
| static CostFunction* Create() { |
| return new AutoDiffCostFunction<LinearResidual, 1, 1>(new LinearResidual); |
| }; |
| }; |
| |
| // Increments a counter everytime PrepareForEvaluation is called. |
| class IncrementingEvaluationCallback : public EvaluationCallback { |
| public: |
| void PrepareForEvaluation(bool evaluate_jacobians, |
| bool new_evaluation_point) final { |
| (void)evaluate_jacobians; |
| (void)new_evaluation_point; |
| counter_ += 1.0; |
| } |
| |
| const double counter() const { return counter_; } |
| |
| private: |
| double counter_ = -1; |
| }; |
| |
| // r = IncrementingEvaluationCallback::counter - x |
| struct EvaluationCallbackResidual { |
| explicit EvaluationCallbackResidual( |
| const IncrementingEvaluationCallback& callback) |
| : callback(callback) {} |
| |
| template <typename T> |
| bool operator()(const T* x, T* residuals) const { |
| residuals[0] = callback.counter() - x[0]; |
| return true; |
| } |
| |
| const IncrementingEvaluationCallback& callback; |
| |
| static CostFunction* Create(IncrementingEvaluationCallback& callback) { |
| return new AutoDiffCostFunction<EvaluationCallbackResidual, 1, 1>( |
| new EvaluationCallbackResidual(callback)); |
| }; |
| }; |
| |
| // The following test, constructs a problem with residual blocks all |
| // of whose parameters are constant, so they are evaluated once |
| // outside the Minimizer to compute Solver::Summary::fixed_cost. |
| // |
| // The cost function for this residual block depends on the |
| // IncrementingEvaluationCallback::counter_, by checking the value of |
| // the fixed cost, we can check if the IncrementingEvaluationCallback |
| // was called. |
| TEST(EvaluationCallback, EvaluationCallbackIsCalledBeforeFixedCostIsEvaluated) { |
| double x = 1; |
| double y = 2; |
| std::unique_ptr<IncrementingEvaluationCallback> callback( |
| new IncrementingEvaluationCallback); |
| Problem::Options problem_options; |
| problem_options.evaluation_callback = callback.get(); |
| Problem problem(problem_options); |
| problem.AddResidualBlock(LinearResidual::Create(), nullptr, &x); |
| problem.AddResidualBlock( |
| EvaluationCallbackResidual::Create(*callback), nullptr, &y); |
| problem.SetParameterBlockConstant(&y); |
| |
| Solver::Options options; |
| options.linear_solver_type = DENSE_QR; |
| Solver::Summary summary; |
| Solve(options, &problem, &summary); |
| EXPECT_EQ(summary.fixed_cost, 2.0); |
| EXPECT_EQ(summary.final_cost, summary.fixed_cost); |
| EXPECT_GT(callback->counter(), 0); |
| } |
| |
| static void WithLineSearchMinimizerImpl( |
| LineSearchType line_search, |
| LineSearchDirectionType line_search_direction, |
| LineSearchInterpolationType line_search_interpolation) { |
| double parameters[2] = {50.0, 50.0}; |
| const uint64_t original_parameters_hash = Djb2Hash(parameters, 2); |
| |
| WigglyBowlCostFunctionAndEvaluationCallback cost_function(parameters); |
| Problem::Options problem_options; |
| problem_options.evaluation_callback = &cost_function; |
| problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; |
| Problem problem(problem_options); |
| problem.AddResidualBlock(&cost_function, NULL, parameters); |
| |
| Solver::Options options; |
| options.linear_solver_type = DENSE_QR; |
| options.max_num_iterations = 50; |
| options.minimizer_type = ceres::LINE_SEARCH; |
| |
| options.line_search_type = line_search; |
| options.line_search_direction_type = line_search_direction; |
| options.line_search_interpolation_type = line_search_interpolation; |
| |
| // Run the solve. Checking is done inside the cost function / callback. |
| Solver::Summary summary; |
| Solve(options, &problem, &summary); |
| |
| // Ensure the callback calls ran a reasonable number of times. |
| EXPECT_GT(summary.num_line_search_steps, 10); |
| EXPECT_GT(cost_function.prepare_num_calls, 30); |
| EXPECT_EQ(cost_function.prepare_num_calls, cost_function.evaluate_num_calls); |
| |
| // Ensure that the parameters did actually change. |
| EXPECT_NE(Djb2Hash(parameters, 2), original_parameters_hash); |
| } |
| |
| // Note: These tests omit combinations of Wolfe line search with bisection. |
| // Due to an implementation quirk in Wolfe line search with bisection, there |
| // are calls to re-evaluate an existing point with new_point = true. That |
| // causes the (overly) strict tests to break, since they check the new_point |
| // preconditions in an if-and-only-if way. Strictly speaking, if new_point = |
| // true, the interface does not *require* that the point has changed; only that |
| // if new_point = false, the same point is reused. |
| // |
| // Since the strict checking is useful to verify that there aren't missed |
| // optimizations, omit tests of the Wolfe with bisection cases. |
| |
| // Wolfe with L-BFGS. |
| TEST(EvaluationCallback, WithLineSearchMinimizerWolfeLbfgsCubic) { |
| WithLineSearchMinimizerImpl(WOLFE, LBFGS, CUBIC); |
| } |
| TEST(EvaluationCallback, WithLineSearchMinimizerWolfeLbfgsQuadratic) { |
| WithLineSearchMinimizerImpl(WOLFE, LBFGS, QUADRATIC); |
| } |
| |
| // Wolfe with full BFGS. |
| TEST(EvaluationCallback, WithLineSearchMinimizerWolfeBfgsCubic) { |
| WithLineSearchMinimizerImpl(WOLFE, BFGS, CUBIC); |
| } |
| |
| TEST(EvaluationCallback, WithLineSearchMinimizerWolfeBfgsQuadratic) { |
| WithLineSearchMinimizerImpl(WOLFE, BFGS, QUADRATIC); |
| } |
| |
| // Armijo with nonlinear conjugate gradient. |
| TEST(EvaluationCallback, WithLineSearchMinimizerArmijoCubic) { |
| WithLineSearchMinimizerImpl(ARMIJO, NONLINEAR_CONJUGATE_GRADIENT, CUBIC); |
| } |
| |
| TEST(EvaluationCallback, WithLineSearchMinimizerArmijoBisection) { |
| WithLineSearchMinimizerImpl(ARMIJO, NONLINEAR_CONJUGATE_GRADIENT, BISECTION); |
| } |
| |
| TEST(EvaluationCallback, WithLineSearchMinimizerArmijoQuadratic) { |
| WithLineSearchMinimizerImpl(ARMIJO, NONLINEAR_CONJUGATE_GRADIENT, QUADRATIC); |
| } |
| |
| } // namespace internal |
| } // namespace ceres |