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#include "ceres/internal/eigen.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/levenberg_marquardt_strategy.h" 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_solver.h" 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/trust_region_strategy.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gmock/gmock.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gmock/mock-log.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gtest/gtest.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::AllOf; 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::AnyNumber; 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::HasSubstr; 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::ScopedMockLog; 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::_; 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kTolerance = 1e-16; 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Linear solver that takes as input a vector and checks that the 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// caller passes the same vector as LinearSolver::PerSolveOptions.D. 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass RegularizationCheckingLinearSolver : public DenseSparseMatrixSolver { 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RegularizationCheckingLinearSolver(const int num_cols, const double* diagonal) 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : num_cols_(num_cols), 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong diagonal_(diagonal) { 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual ~RegularizationCheckingLinearSolver() {} 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual LinearSolver::Summary SolveImpl( 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DenseSparseMatrix* A, 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* b, 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const LinearSolver::PerSolveOptions& per_solve_options, 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x) { 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(per_solve_options.D); 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cols_; ++i) { 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(per_solve_options.D[i], diagonal_[i], kTolerance) 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << i << " " << per_solve_options.D[i] << " " << diagonal_[i]; 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return LinearSolver::Summary(); 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_cols_; 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* diagonal_; 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(LevenbergMarquardtStrategy, AcceptRejectStepRadiusScaling) { 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::Options options; 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options.initial_radius = 2.0; 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options.max_radius = 20.0; 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.min_lm_diagonal = 1e-8; 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_lm_diagonal = 1e8; 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // We need a non-null pointer here, so anything should do. 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearSolver> linear_solver( 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new RegularizationCheckingLinearSolver(0, NULL)); 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options.linear_solver = linear_solver.get(); 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LevenbergMarquardtStrategy lms(options); 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), options.initial_radius); 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lms.StepRejected(0.0); 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), 1.0); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lms.StepRejected(-1.0); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), 0.25); 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lms.StepAccepted(1.0); 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), 0.25 * 3.0); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lms.StepAccepted(1.0); 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), 0.25 * 3.0 * 3.0); 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lms.StepAccepted(0.25); 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), 0.25 * 3.0 * 3.0 / 1.125); 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lms.StepAccepted(1.0); 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), 0.25 * 3.0 * 3.0 / 1.125 * 3.0); 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lms.StepAccepted(1.0); 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), 0.25 * 3.0 * 3.0 / 1.125 * 3.0 * 3.0); 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lms.StepAccepted(1.0); 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(lms.Radius(), options.max_radius); 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST(LevenbergMarquardtStrategy, CorrectDiagonalToLinearSolver) { 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix jacobian(2, 3); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian.setZero(); 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian(0, 0) = 0.0; 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian(0, 1) = 1.0; 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian(1, 1) = 1.0; 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian(0, 2) = 100.0; 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double residual = 1.0; 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double x[3]; 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DenseSparseMatrix dsm(jacobian); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::Options options; 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options.initial_radius = 2.0; 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options.max_radius = 20.0; 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.min_lm_diagonal = 1e-2; 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_lm_diagonal = 1e2; 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double diagonal[3]; 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling diagonal[0] = options.min_lm_diagonal; 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong diagonal[1] = 2.0; 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling diagonal[2] = options.max_lm_diagonal; 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong diagonal[i] = sqrt(diagonal[i] / options.initial_radius); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RegularizationCheckingLinearSolver linear_solver(3, diagonal); 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options.linear_solver = &linear_solver; 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LevenbergMarquardtStrategy lms(options); 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::PerSolveOptions pso; 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong { 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ScopedMockLog log; 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_CALL(log, Log(_, _, _)).Times(AnyNumber()); 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_CALL(log, Log(WARNING, _, 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HasSubstr("Failed to compute a finite step."))); 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling TrustRegionStrategy::Summary summary = 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling lms.ComputeStep(pso, &dsm, &residual, x); 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_EQ(summary.termination_type, LINEAR_SOLVER_FAILURE); 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 159