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
41namespace ceres {
42namespace internal {
43namespace {
44
45class 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.
61class 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.
99class 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
120const double kTolerance = 1e-14;
121const double kToleranceLoose = 1e-5;
122const 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.
128TEST_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
151TEST_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
171TEST_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.
198TEST_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.
233TEST_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.
261TEST_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