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@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:
SetUp()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:
SetUp()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.
TEST_F(DoglegStrategyFixtureEllipse,TrustRegionObeyedTraditional)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 
TEST_F(DoglegStrategyFixtureEllipse,TrustRegionObeyedSubspace)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 
TEST_F(DoglegStrategyFixtureEllipse,CorrectGaussNewtonStep)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.
TEST_F(DoglegStrategyFixtureEllipse,ValidSubspaceBasis)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.
TEST_F(DoglegStrategyFixtureValley,CorrectStepLocalOptimumAlongGradient)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.
TEST_F(DoglegStrategyFixtureValley,CorrectStepGlobalOptimumAlongGradient)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