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_.lm_min_diagonal = 1.0; 88 options_.lm_max_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_.lm_min_diagonal = 1.0; 116 options_.lm_max_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 sqrt(6.0). 133 // By restricting the trust region to a radius of 2.0, we test if the trust 134 // 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, 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, 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, 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 TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 210 jacobian_.get(), 211 residual_.data(), 212 x_.data()); 213 214 // Check if the basis is orthonormal. 215 const Matrix basis = strategy.subspace_basis(); 216 EXPECT_NEAR(basis.col(0).norm(), 1.0, kTolerance); 217 EXPECT_NEAR(basis.col(1).norm(), 1.0, kTolerance); 218 EXPECT_NEAR(basis.col(0).dot(basis.col(1)), 0.0, kTolerance); 219 220 // Check if the gradient projects onto itself. 221 const Vector gradient = strategy.gradient(); 222 EXPECT_NEAR((gradient - basis*(basis.transpose()*gradient)).norm(), 223 0.0, 224 kTolerance); 225 226 // Check if the Gauss-Newton point projects onto itself. 227 const Vector gn = strategy.gauss_newton_step(); 228 EXPECT_NEAR((gn - basis*(basis.transpose()*gn)).norm(), 229 0.0, 230 kTolerance); 231 } 232 233 // Test if the step is correct if the gradient and the Gauss-Newton step point 234 // in the same direction and the Gauss-Newton step is outside the trust region, 235 // i.e. the trust region is active. 236 TEST_F(DoglegStrategyFixtureValley, CorrectStepLocalOptimumAlongGradient) { 237 scoped_ptr<LinearSolver> linear_solver( 238 new DenseQRSolver(LinearSolver::Options())); 239 options_.linear_solver = linear_solver.get(); 240 options_.dogleg_type = SUBSPACE_DOGLEG; 241 options_.initial_radius = 0.25; 242 options_.max_radius = 0.25; 243 244 DoglegStrategy strategy(options_); 245 TrustRegionStrategy::PerSolveOptions pso; 246 247 TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 248 jacobian_.get(), 249 residual_.data(), 250 x_.data()); 251 252 EXPECT_NE(summary.termination_type, FAILURE); 253 EXPECT_NEAR(x_(0), 0.0, kToleranceLoose); 254 EXPECT_NEAR(x_(1), 0.0, kToleranceLoose); 255 EXPECT_NEAR(x_(2), options_.initial_radius, kToleranceLoose); 256 EXPECT_NEAR(x_(3), 0.0, kToleranceLoose); 257 EXPECT_NEAR(x_(4), 0.0, kToleranceLoose); 258 EXPECT_NEAR(x_(5), 0.0, kToleranceLoose); 259 } 260 261 // Test if the step is correct if the gradient and the Gauss-Newton step point 262 // in the same direction and the Gauss-Newton step is inside the trust region, 263 // i.e. the trust region is inactive. 264 TEST_F(DoglegStrategyFixtureValley, CorrectStepGlobalOptimumAlongGradient) { 265 scoped_ptr<LinearSolver> linear_solver( 266 new DenseQRSolver(LinearSolver::Options())); 267 options_.linear_solver = linear_solver.get(); 268 options_.dogleg_type = SUBSPACE_DOGLEG; 269 options_.initial_radius = 2.0; 270 options_.max_radius = 2.0; 271 272 DoglegStrategy strategy(options_); 273 TrustRegionStrategy::PerSolveOptions pso; 274 275 TrustRegionStrategy::Summary summary = strategy.ComputeStep(pso, 276 jacobian_.get(), 277 residual_.data(), 278 x_.data()); 279 280 EXPECT_NE(summary.termination_type, FAILURE); 281 EXPECT_NEAR(x_(0), 0.0, kToleranceLoose); 282 EXPECT_NEAR(x_(1), 0.0, kToleranceLoose); 283 EXPECT_NEAR(x_(2), 1.0, kToleranceLoose); 284 EXPECT_NEAR(x_(3), 0.0, kToleranceLoose); 285 EXPECT_NEAR(x_(4), 0.0, kToleranceLoose); 286 EXPECT_NEAR(x_(5), 0.0, kToleranceLoose); 287 } 288 289 } // namespace internal 290 } // namespace ceres 291 292
