10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_INTERNAL_DOGLEG_STRATEGY_H_ 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_INTERNAL_DOGLEG_STRATEGY_H_ 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_solver.h" 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/trust_region_strategy.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Dogleg step computation and trust region sizing strategy based on 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// on "Methods for Nonlinear Least Squares" by K. Madsen, H.B. Nielsen 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and O. Tingleff. Available to download from 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// One minor modification is that instead of computing the pure 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Gauss-Newton step, we compute a regularized version of it. This is 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// because the Jacobian is often rank-deficient and in such cases 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// using a direct solver leads to numerical failure. 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// If SUBSPACE is passed as the type argument to the constructor, the 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// DoglegStrategy follows the approach by Shultz, Schnabel, Byrd. 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// This finds the exact optimum over the two-dimensional subspace 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// spanned by the two Dogleg vectors. 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass DoglegStrategy : public TrustRegionStrategy { 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling explicit DoglegStrategy(const TrustRegionStrategy::Options& options); 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual ~DoglegStrategy() {} 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // TrustRegionStrategy interface 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual Summary ComputeStep(const PerSolveOptions& per_solve_options, 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong SparseMatrix* jacobian, 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* residuals, 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* step); 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void StepAccepted(double step_quality); 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void StepRejected(double step_quality); 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void StepIsInvalid(); 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual double Radius() const; 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // These functions are predominantly for testing. 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector gradient() const { return gradient_; } 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector gauss_newton_step() const { return gauss_newton_step_; } 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix subspace_basis() const { return subspace_basis_; } 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector subspace_g() const { return subspace_g_; } 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix subspace_B() const { return subspace_B_; } 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef Eigen::Matrix<double, 2, 1, Eigen::DontAlign> Vector2d; 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef Eigen::Matrix<double, 2, 2, Eigen::DontAlign> Matrix2d; 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LinearSolver::Summary ComputeGaussNewtonStep( 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const PerSolveOptions& per_solve_options, 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling SparseMatrix* jacobian, 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* residuals); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void ComputeCauchyPoint(SparseMatrix* jacobian); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void ComputeGradient(SparseMatrix* jacobian, const double* residuals); 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void ComputeTraditionalDoglegStep(double* step); 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool ComputeSubspaceModel(SparseMatrix* jacobian); 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void ComputeSubspaceDoglegStep(double* step); 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const; 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector MakePolynomialForBoundaryConstrainedProblem() const; 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector2d ComputeSubspaceStepFromRoot(double lambda) const; 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double EvaluateSubspaceModel(const Vector2d& x) const; 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearSolver* linear_solver_; 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double radius_; 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double max_radius_; 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double min_diagonal_; 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double max_diagonal_; 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // mu is used to scale the diagonal matrix used to make the 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Gauss-Newton solve full rank. In each solve, the strategy starts 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // out with mu = min_mu, and tries values upto max_mu. If the user 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // reports an invalid step, the value of mu_ is increased so that 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the next solve starts with a stronger regularization. 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If a successful step is reported, then the value of mu_ is 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // decreased with a lower bound of min_mu_. 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double mu_; 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double min_mu_; 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double max_mu_; 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double mu_increase_factor_; 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double increase_threshold_; 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double decrease_threshold_; 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector diagonal_; // sqrt(diag(J^T J)) 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector lm_diagonal_; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector gradient_; 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector gauss_newton_step_; 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // cauchy_step = alpha * gradient 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double alpha_; 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double dogleg_step_norm_; 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // When, ComputeStep is called, reuse_ indicates whether the 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Gauss-Newton and Cauchy steps from the last call to ComputeStep 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // can be reused or not. 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If the user called StepAccepted, then it is expected that the 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // user has recomputed the Jacobian matrix and new Gauss-Newton 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // solve is needed and reuse is set to false. 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If the user called StepRejected, then it is expected that the 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // user wants to solve the trust region problem with the same matrix 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // but a different trust region radius and the Gauss-Newton and 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Cauchy steps can be reused to compute the Dogleg, thus reuse is 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // set to true. 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If the user called StepIsInvalid, then there was a numerical 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // problem with the step computed in the last call to ComputeStep, 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // and the regularization used to do the Gauss-Newton solve is 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // increased and a new solve should be done when ComputeStep is 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // called again, thus reuse is set to false. 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool reuse_; 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The dogleg type determines how the minimum of the local 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // quadratic model is found. 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DoglegType dogleg_type_; 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If the type is SUBSPACE_DOGLEG, the two-dimensional 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // model 1/2 x^T B x + g^T x has to be computed and stored. 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool subspace_is_one_dimensional_; 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix subspace_basis_; 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector2d subspace_g_; 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix2d subspace_B_; 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_INTERNAL_DOGLEG_STRATEGY_H_ 166