1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: moll.markus (at) arcor.de (Markus Moll) 30 31 #include <limits> 32 #include "ceres/internal/eigen.h" 33 #include "ceres/internal/scoped_ptr.h" 34 #include "ceres/dense_qr_solver.h" 35 #include "ceres/dogleg_strategy.h" 36 #include "ceres/linear_solver.h" 37 #include "ceres/trust_region_strategy.h" 38 #include "glog/logging.h" 39 #include "gtest/gtest.h" 40 41 namespace ceres { 42 namespace internal { 43 namespace { 44 45 class Fixture : public testing::Test { 46 protected: 47 scoped_ptr<DenseSparseMatrix> jacobian_; 48 Vector residual_; 49 Vector x_; 50 TrustRegionStrategy::Options options_; 51 }; 52 53 // A test problem where 54 // 55 // J^T J = Q diag([1 2 4 8 16 32]) Q^T 56 // 57 // where Q is a randomly chosen orthonormal basis of R^6. 58 // The residual is chosen so that the minimum of the quadratic function is 59 // at (1, 1, 1, 1, 1, 1). It is therefore at a distance of sqrt(6) ~ 2.45 60 // from the origin. 61 class DoglegStrategyFixtureEllipse : public Fixture { 62 protected: 63 virtual void SetUp() { 64 Matrix basis(6, 6); 65 // The following lines exceed 80 characters for better readability. 66 basis << -0.1046920933796121, -0.7449367449921986, -0.4190744502875876, -0.4480450716142566, 0.2375351607929440, -0.0363053418882862, 67 0.4064975684355914, 0.2681113508511354, -0.7463625494601520, -0.0803264850508117, -0.4463149623021321, 0.0130224954867195, 68 -0.5514387729089798, 0.1026621026168657, -0.5008316122125011, 0.5738122212666414, 0.2974664724007106, 0.1296020877535158, 69 0.5037835370947156, 0.2668479925183712, -0.1051754618492798, -0.0272739396578799, 0.7947481647088278, -0.1776623363955670, 70 -0.4005458426625444, 0.2939330589634109, -0.0682629380550051, -0.2895448882503687, -0.0457239396341685, -0.8139899477847840, 71 -0.3247764582762654, 0.4528151365941945, -0.0276683863102816, -0.6155994592510784, 0.1489240599972848, 0.5362574892189350; 72 73 Vector Ddiag(6); 74 Ddiag << 1.0, 2.0, 4.0, 8.0, 16.0, 32.0; 75 76 Matrix sqrtD = Ddiag.array().sqrt().matrix().asDiagonal(); 77 Matrix jacobian = sqrtD * basis; 78 jacobian_.reset(new DenseSparseMatrix(jacobian)); 79 80 Vector minimum(6); 81 minimum << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 82 residual_ = -jacobian * minimum; 83 84 x_.resize(6); 85 x_.setZero(); 86 87 options_.min_lm_diagonal = 1.0; 88 options_.max_lm_diagonal = 1.0; 89 } 90 }; 91 92 // A test problem where 93 // 94 // J^T J = diag([1 2 4 8 16 32]) . 95 // 96 // The residual is chosen so that the minimum of the quadratic function is 97 // at (0, 0, 1, 0, 0, 0). It is therefore at a distance of 1 from the origin. 98 // The gradient at the origin points towards the global minimum. 99 class DoglegStrategyFixtureValley : public Fixture { 100 protected: 101 virtual void SetUp() { 102 Vector Ddiag(6); 103 Ddiag << 1.0, 2.0, 4.0, 8.0, 16.0, 32.0; 104 105 Matrix jacobian = Ddiag.asDiagonal(); 106 jacobian_.reset(new DenseSparseMatrix(jacobian)); 107 108 Vector minimum(6); 109 minimum << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; 110 residual_ = -jacobian * minimum; 111 112 x_.resize(6); 113 x_.setZero(); 114 115 options_.min_lm_diagonal = 1.0; 116 options_.max_lm_diagonal = 1.0; 117 } 118 }; 119 120 const double kTolerance = 1e-14; 121 const double kToleranceLoose = 1e-5; 122 const double kEpsilon = std::numeric_limits<double>::epsilon(); 123 124 } // namespace 125 126 // The DoglegStrategy must never return a step that is longer than the current 127 // trust region radius. 128 TEST_F(DoglegStrategyFixtureEllipse, TrustRegionObeyedTraditional) { 129 scoped_ptr<LinearSolver> linear_solver( 130 new DenseQRSolver(LinearSolver::Options())); 131 options_.linear_solver = linear_solver.get(); 132 // The global minimum is at (1, 1, ..., 1), so the distance to it is 133 // sqrt(6.0). By restricting the trust region to a radius of 2.0, 134 // we test if the trust region is actually obeyed. 135 options_.dogleg_type = TRADITIONAL_DOGLEG; 136 options_.initial_radius = 2.0; 137 options_.max_radius = 2.0; 138 139 DoglegStrategy strategy(options_); 140 TrustRegionStrategy::PerSolveOptions pso; 141 142 TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 143 jacobian_.get(), 144 residual_.data(), 145 x_.data()); 146 147 EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 148 EXPECT_LE(x_.norm(), options_.initial_radius * (1.0 + 4.0 * kEpsilon)); 149 } 150 151 TEST_F(DoglegStrategyFixtureEllipse, TrustRegionObeyedSubspace) { 152 scoped_ptr<LinearSolver> linear_solver( 153 new DenseQRSolver(LinearSolver::Options())); 154 options_.linear_solver = linear_solver.get(); 155 options_.dogleg_type = SUBSPACE_DOGLEG; 156 options_.initial_radius = 2.0; 157 options_.max_radius = 2.0; 158 159 DoglegStrategy strategy(options_); 160 TrustRegionStrategy::PerSolveOptions pso; 161 162 TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 163 jacobian_.get(), 164 residual_.data(), 165 x_.data()); 166 167 EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 168 EXPECT_LE(x_.norm(), options_.initial_radius * (1.0 + 4.0 * kEpsilon)); 169 } 170 171 TEST_F(DoglegStrategyFixtureEllipse, CorrectGaussNewtonStep) { 172 scoped_ptr<LinearSolver> linear_solver( 173 new DenseQRSolver(LinearSolver::Options())); 174 options_.linear_solver = linear_solver.get(); 175 options_.dogleg_type = SUBSPACE_DOGLEG; 176 options_.initial_radius = 10.0; 177 options_.max_radius = 10.0; 178 179 DoglegStrategy strategy(options_); 180 TrustRegionStrategy::PerSolveOptions pso; 181 182 TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 183 jacobian_.get(), 184 residual_.data(), 185 x_.data()); 186 187 EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 188 EXPECT_NEAR(x_(0), 1.0, kToleranceLoose); 189 EXPECT_NEAR(x_(1), 1.0, kToleranceLoose); 190 EXPECT_NEAR(x_(2), 1.0, kToleranceLoose); 191 EXPECT_NEAR(x_(3), 1.0, kToleranceLoose); 192 EXPECT_NEAR(x_(4), 1.0, kToleranceLoose); 193 EXPECT_NEAR(x_(5), 1.0, kToleranceLoose); 194 } 195 196 // Test if the subspace basis is a valid orthonormal basis of the space spanned 197 // by the gradient and the Gauss-Newton point. 198 TEST_F(DoglegStrategyFixtureEllipse, ValidSubspaceBasis) { 199 scoped_ptr<LinearSolver> linear_solver( 200 new DenseQRSolver(LinearSolver::Options())); 201 options_.linear_solver = linear_solver.get(); 202 options_.dogleg_type = SUBSPACE_DOGLEG; 203 options_.initial_radius = 2.0; 204 options_.max_radius = 2.0; 205 206 DoglegStrategy strategy(options_); 207 TrustRegionStrategy::PerSolveOptions pso; 208 209 strategy.ComputeStep(pso, jacobian_.get(), residual_.data(), x_.data()); 210 211 // Check if the basis is orthonormal. 212 const Matrix basis = strategy.subspace_basis(); 213 EXPECT_NEAR(basis.col(0).norm(), 1.0, kTolerance); 214 EXPECT_NEAR(basis.col(1).norm(), 1.0, kTolerance); 215 EXPECT_NEAR(basis.col(0).dot(basis.col(1)), 0.0, kTolerance); 216 217 // Check if the gradient projects onto itself. 218 const Vector gradient = strategy.gradient(); 219 EXPECT_NEAR((gradient - basis*(basis.transpose()*gradient)).norm(), 220 0.0, 221 kTolerance); 222 223 // Check if the Gauss-Newton point projects onto itself. 224 const Vector gn = strategy.gauss_newton_step(); 225 EXPECT_NEAR((gn - basis*(basis.transpose()*gn)).norm(), 226 0.0, 227 kTolerance); 228 } 229 230 // Test if the step is correct if the gradient and the Gauss-Newton step point 231 // in the same direction and the Gauss-Newton step is outside the trust region, 232 // i.e. the trust region is active. 233 TEST_F(DoglegStrategyFixtureValley, CorrectStepLocalOptimumAlongGradient) { 234 scoped_ptr<LinearSolver> linear_solver( 235 new DenseQRSolver(LinearSolver::Options())); 236 options_.linear_solver = linear_solver.get(); 237 options_.dogleg_type = SUBSPACE_DOGLEG; 238 options_.initial_radius = 0.25; 239 options_.max_radius = 0.25; 240 241 DoglegStrategy strategy(options_); 242 TrustRegionStrategy::PerSolveOptions pso; 243 244 TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 245 jacobian_.get(), 246 residual_.data(), 247 x_.data()); 248 249 EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 250 EXPECT_NEAR(x_(0), 0.0, kToleranceLoose); 251 EXPECT_NEAR(x_(1), 0.0, kToleranceLoose); 252 EXPECT_NEAR(x_(2), options_.initial_radius, kToleranceLoose); 253 EXPECT_NEAR(x_(3), 0.0, kToleranceLoose); 254 EXPECT_NEAR(x_(4), 0.0, kToleranceLoose); 255 EXPECT_NEAR(x_(5), 0.0, kToleranceLoose); 256 } 257 258 // Test if the step is correct if the gradient and the Gauss-Newton step point 259 // in the same direction and the Gauss-Newton step is inside the trust region, 260 // i.e. the trust region is inactive. 261 TEST_F(DoglegStrategyFixtureValley, CorrectStepGlobalOptimumAlongGradient) { 262 scoped_ptr<LinearSolver> linear_solver( 263 new DenseQRSolver(LinearSolver::Options())); 264 options_.linear_solver = linear_solver.get(); 265 options_.dogleg_type = SUBSPACE_DOGLEG; 266 options_.initial_radius = 2.0; 267 options_.max_radius = 2.0; 268 269 DoglegStrategy strategy(options_); 270 TrustRegionStrategy::PerSolveOptions pso; 271 272 TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 273 jacobian_.get(), 274 residual_.data(), 275 x_.data()); 276 277 EXPECT_NE(summary.termination_type, LINEAR_SOLVER_FAILURE); 278 EXPECT_NEAR(x_(0), 0.0, kToleranceLoose); 279 EXPECT_NEAR(x_(1), 0.0, kToleranceLoose); 280 EXPECT_NEAR(x_(2), 1.0, kToleranceLoose); 281 EXPECT_NEAR(x_(3), 0.0, kToleranceLoose); 282 EXPECT_NEAR(x_(4), 0.0, kToleranceLoose); 283 EXPECT_NEAR(x_(5), 0.0, kToleranceLoose); 284 } 285 286 } // namespace internal 287 } // namespace ceres 288
