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: moll.markus@arcor.de (Markus Moll) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <limits> 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/dense_qr_solver.h" 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/dogleg_strategy.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_solver.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/trust_region_strategy.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gtest/gtest.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace { 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass Fixture : public testing::Test { 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong protected: 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<DenseSparseMatrix> jacobian_; 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector residual_; 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector x_; 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::Options options_; 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A test problem where 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// J^T J = Q diag([1 2 4 8 16 32]) Q^T 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// where Q is a randomly chosen orthonormal basis of R^6. 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The residual is chosen so that the minimum of the quadratic function is 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// at (1, 1, 1, 1, 1, 1). It is therefore at a distance of sqrt(6) ~ 2.45 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// from the origin. 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass DoglegStrategyFixtureEllipse : public Fixture { 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong protected: 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void SetUp() { 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix basis(6, 6); 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The following lines exceed 80 characters for better readability. 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong basis << -0.1046920933796121, -0.7449367449921986, -0.4190744502875876, -0.4480450716142566, 0.2375351607929440, -0.0363053418882862, 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.4064975684355914, 0.2681113508511354, -0.7463625494601520, -0.0803264850508117, -0.4463149623021321, 0.0130224954867195, 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -0.5514387729089798, 0.1026621026168657, -0.5008316122125011, 0.5738122212666414, 0.2974664724007106, 0.1296020877535158, 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.5037835370947156, 0.2668479925183712, -0.1051754618492798, -0.0272739396578799, 0.7947481647088278, -0.1776623363955670, 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -0.4005458426625444, 0.2939330589634109, -0.0682629380550051, -0.2895448882503687, -0.0457239396341685, -0.8139899477847840, 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong -0.3247764582762654, 0.4528151365941945, -0.0276683863102816, -0.6155994592510784, 0.1489240599972848, 0.5362574892189350; 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector Ddiag(6); 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ddiag << 1.0, 2.0, 4.0, 8.0, 16.0, 32.0; 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix sqrtD = Ddiag.array().sqrt().matrix().asDiagonal(); 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix jacobian = sqrtD * basis; 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian_.reset(new DenseSparseMatrix(jacobian)); 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector minimum(6); 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong minimum << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_ = -jacobian * minimum; 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.resize(6); 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.setZero(); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options_.min_lm_diagonal = 1.0; 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options_.max_lm_diagonal = 1.0; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A test problem where 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// J^T J = diag([1 2 4 8 16 32]) . 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The residual is chosen so that the minimum of the quadratic function is 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// at (0, 0, 1, 0, 0, 0). It is therefore at a distance of 1 from the origin. 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The gradient at the origin points towards the global minimum. 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass DoglegStrategyFixtureValley : public Fixture { 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong protected: 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void SetUp() { 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector Ddiag(6); 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Ddiag << 1.0, 2.0, 4.0, 8.0, 16.0, 32.0; 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix jacobian = Ddiag.asDiagonal(); 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian_.reset(new DenseSparseMatrix(jacobian)); 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector minimum(6); 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong minimum << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_ = -jacobian * minimum; 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.resize(6); 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.setZero(); 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options_.min_lm_diagonal = 1.0; 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options_.max_lm_diagonal = 1.0; 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kTolerance = 1e-14; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kToleranceLoose = 1e-5; 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst double kEpsilon = std::numeric_limits<double>::epsilon(); 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The DoglegStrategy must never return a step that is longer than the current 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// trust region radius. 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DoglegStrategyFixtureEllipse, TrustRegionObeyedTraditional) { 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearSolver> linear_solver( 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new DenseQRSolver(LinearSolver::Options())); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.linear_solver = linear_solver.get(); 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The global minimum is at (1, 1, ..., 1), so the distance to it is 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // sqrt(6.0). By restricting the trust region to a radius of 2.0, 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // we test if the trust region is actually obeyed. 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.dogleg_type = TRADITIONAL_DOGLEG; 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.initial_radius = 2.0; 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.max_radius = 2.0; 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DoglegStrategy strategy(options_); 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::PerSolveOptions pso; 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian_.get(), 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_.data(), 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.data()); 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_LE(x_.norm(), options_.initial_radius * (1.0 + 4.0 * kEpsilon)); 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DoglegStrategyFixtureEllipse, TrustRegionObeyedSubspace) { 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearSolver> linear_solver( 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new DenseQRSolver(LinearSolver::Options())); 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.linear_solver = linear_solver.get(); 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.dogleg_type = SUBSPACE_DOGLEG; 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.initial_radius = 2.0; 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.max_radius = 2.0; 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DoglegStrategy strategy(options_); 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::PerSolveOptions pso; 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian_.get(), 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_.data(), 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.data()); 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 16779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_LE(x_.norm(), options_.initial_radius * (1.0 + 4.0 * kEpsilon)); 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DoglegStrategyFixtureEllipse, CorrectGaussNewtonStep) { 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearSolver> linear_solver( 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new DenseQRSolver(LinearSolver::Options())); 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.linear_solver = linear_solver.get(); 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.dogleg_type = SUBSPACE_DOGLEG; 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.initial_radius = 10.0; 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.max_radius = 10.0; 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DoglegStrategy strategy(options_); 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::PerSolveOptions pso; 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian_.get(), 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_.data(), 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.data()); 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 18779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(0), 1.0, kToleranceLoose); 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(1), 1.0, kToleranceLoose); 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(2), 1.0, kToleranceLoose); 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(3), 1.0, kToleranceLoose); 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(4), 1.0, kToleranceLoose); 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(5), 1.0, kToleranceLoose); 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test if the subspace basis is a valid orthonormal basis of the space spanned 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// by the gradient and the Gauss-Newton point. 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DoglegStrategyFixtureEllipse, ValidSubspaceBasis) { 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearSolver> linear_solver( 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new DenseQRSolver(LinearSolver::Options())); 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.linear_solver = linear_solver.get(); 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.dogleg_type = SUBSPACE_DOGLEG; 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.initial_radius = 2.0; 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.max_radius = 2.0; 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DoglegStrategy strategy(options_); 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::PerSolveOptions pso; 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling strategy.ComputeStep(pso, jacobian_.get(), residual_.data(), x_.data()); 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Check if the basis is orthonormal. 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Matrix basis = strategy.subspace_basis(); 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(basis.col(0).norm(), 1.0, kTolerance); 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(basis.col(1).norm(), 1.0, kTolerance); 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(basis.col(0).dot(basis.col(1)), 0.0, kTolerance); 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Check if the gradient projects onto itself. 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vector gradient = strategy.gradient(); 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR((gradient - basis*(basis.transpose()*gradient)).norm(), 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.0, 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kTolerance); 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Check if the Gauss-Newton point projects onto itself. 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Vector gn = strategy.gauss_newton_step(); 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR((gn - basis*(basis.transpose()*gn)).norm(), 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 0.0, 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kTolerance); 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test if the step is correct if the gradient and the Gauss-Newton step point 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// in the same direction and the Gauss-Newton step is outside the trust region, 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// i.e. the trust region is active. 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DoglegStrategyFixtureValley, CorrectStepLocalOptimumAlongGradient) { 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearSolver> linear_solver( 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new DenseQRSolver(LinearSolver::Options())); 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.linear_solver = linear_solver.get(); 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.dogleg_type = SUBSPACE_DOGLEG; 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.initial_radius = 0.25; 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.max_radius = 0.25; 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DoglegStrategy strategy(options_); 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::PerSolveOptions pso; 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian_.get(), 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_.data(), 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.data()); 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 24979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(0), 0.0, kToleranceLoose); 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(1), 0.0, kToleranceLoose); 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(2), options_.initial_radius, kToleranceLoose); 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(3), 0.0, kToleranceLoose); 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(4), 0.0, kToleranceLoose); 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(5), 0.0, kToleranceLoose); 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Test if the step is correct if the gradient and the Gauss-Newton step point 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// in the same direction and the Gauss-Newton step is inside the trust region, 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// i.e. the trust region is inactive. 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(DoglegStrategyFixtureValley, CorrectStepGlobalOptimumAlongGradient) { 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearSolver> linear_solver( 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new DenseQRSolver(LinearSolver::Options())); 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.linear_solver = linear_solver.get(); 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.dogleg_type = SUBSPACE_DOGLEG; 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.initial_radius = 2.0; 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.max_radius = 2.0; 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DoglegStrategy strategy(options_); 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::PerSolveOptions pso; 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong jacobian_.get(), 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_.data(), 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_.data()); 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 27779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(0), 0.0, kToleranceLoose); 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(1), 0.0, kToleranceLoose); 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(2), 1.0, kToleranceLoose); 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(3), 0.0, kToleranceLoose); 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(4), 0.0, kToleranceLoose); 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(x_(5), 0.0, kToleranceLoose); 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 288