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: keir@google.com (Keir Mierle)
30 // sameeragarwal@google.com (Sameer Agarwal)
31 //
32 // This tests the TrustRegionMinimizer loop using a direct Evaluator
33 // implementation, rather than having a test that goes through all the
34 // Program and Problem machinery.
35
36 #include <cmath>
37 #include "ceres/cost_function.h"
38 #include "ceres/dense_qr_solver.h"
39 #include "ceres/dense_sparse_matrix.h"
40 #include "ceres/evaluator.h"
41 #include "ceres/internal/port.h"
42 #include "ceres/linear_solver.h"
43 #include "ceres/minimizer.h"
44 #include "ceres/problem.h"
45 #include "ceres/trust_region_minimizer.h"
46 #include "ceres/trust_region_strategy.h"
47 #include "gtest/gtest.h"
48
49 namespace ceres {
50 namespace internal {
51
52 // Templated Evaluator for Powell's function. The template parameters
53 // indicate which of the four variables/columns of the jacobian are
54 // active. This is equivalent to constructing a problem and using the
55 // SubsetLocalParameterization. This allows us to test the support for
56 // the Evaluator::Plus operation besides checking for the basic
57 // performance of the trust region algorithm.
58 template <bool col1, bool col2, bool col3, bool col4>
59 class PowellEvaluator2 : public Evaluator {
60 public:
PowellEvaluator2()61 PowellEvaluator2()
62 : num_active_cols_(
63 (col1 ? 1 : 0) +
64 (col2 ? 1 : 0) +
65 (col3 ? 1 : 0) +
66 (col4 ? 1 : 0)) {
67 VLOG(1) << "Columns: "
68 << col1 << " "
69 << col2 << " "
70 << col3 << " "
71 << col4;
72 }
73
~PowellEvaluator2()74 virtual ~PowellEvaluator2() {}
75
76 // Implementation of Evaluator interface.
CreateJacobian() const77 virtual SparseMatrix* CreateJacobian() const {
78 CHECK(col1 || col2 || col3 || col4);
79 DenseSparseMatrix* dense_jacobian =
80 new DenseSparseMatrix(NumResiduals(), NumEffectiveParameters());
81 dense_jacobian->SetZero();
82 return dense_jacobian;
83 }
84
Evaluate(const double * state,double * cost,double * residuals,double *,SparseMatrix * jacobian)85 virtual bool Evaluate(const double* state,
86 double* cost,
87 double* residuals,
88 double* /* gradient */,
89 SparseMatrix* jacobian) {
90 double x1 = state[0];
91 double x2 = state[1];
92 double x3 = state[2];
93 double x4 = state[3];
94
95 VLOG(1) << "State: "
96 << "x1=" << x1 << ", "
97 << "x2=" << x2 << ", "
98 << "x3=" << x3 << ", "
99 << "x4=" << x4 << ".";
100
101 double f1 = x1 + 10.0 * x2;
102 double f2 = sqrt(5.0) * (x3 - x4);
103 double f3 = pow(x2 - 2.0 * x3, 2.0);
104 double f4 = sqrt(10.0) * pow(x1 - x4, 2.0);
105
106 VLOG(1) << "Function: "
107 << "f1=" << f1 << ", "
108 << "f2=" << f2 << ", "
109 << "f3=" << f3 << ", "
110 << "f4=" << f4 << ".";
111
112 *cost = (f1*f1 + f2*f2 + f3*f3 + f4*f4) / 2.0;
113
114 VLOG(1) << "Cost: " << *cost;
115
116 if (residuals != NULL) {
117 residuals[0] = f1;
118 residuals[1] = f2;
119 residuals[2] = f3;
120 residuals[3] = f4;
121 }
122
123 if (jacobian != NULL) {
124 DenseSparseMatrix* dense_jacobian;
125 dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian);
126 dense_jacobian->SetZero();
127
128 AlignedMatrixRef jacobian_matrix = dense_jacobian->mutable_matrix();
129 CHECK_EQ(jacobian_matrix.cols(), num_active_cols_);
130
131 int column_index = 0;
132 if (col1) {
133 jacobian_matrix.col(column_index++) <<
134 1.0,
135 0.0,
136 0.0,
137 sqrt(10.0) * 2.0 * (x1 - x4) * (1.0 - x4);
138 }
139 if (col2) {
140 jacobian_matrix.col(column_index++) <<
141 10.0,
142 0.0,
143 2.0*(x2 - 2.0*x3)*(1.0 - 2.0*x3),
144 0.0;
145 }
146
147 if (col3) {
148 jacobian_matrix.col(column_index++) <<
149 0.0,
150 sqrt(5.0),
151 2.0*(x2 - 2.0*x3)*(x2 - 2.0),
152 0.0;
153 }
154
155 if (col4) {
156 jacobian_matrix.col(column_index++) <<
157 0.0,
158 -sqrt(5.0),
159 0.0,
160 sqrt(10.0) * 2.0 * (x1 - x4) * (x1 - 1.0);
161 }
162 VLOG(1) << "\n" << jacobian_matrix;
163 }
164 return true;
165 }
166
Plus(const double * state,const double * delta,double * state_plus_delta) const167 virtual bool Plus(const double* state,
168 const double* delta,
169 double* state_plus_delta) const {
170 int delta_index = 0;
171 state_plus_delta[0] = (col1 ? state[0] + delta[delta_index++] : state[0]);
172 state_plus_delta[1] = (col2 ? state[1] + delta[delta_index++] : state[1]);
173 state_plus_delta[2] = (col3 ? state[2] + delta[delta_index++] : state[2]);
174 state_plus_delta[3] = (col4 ? state[3] + delta[delta_index++] : state[3]);
175 return true;
176 }
177
NumEffectiveParameters() const178 virtual int NumEffectiveParameters() const { return num_active_cols_; }
NumParameters() const179 virtual int NumParameters() const { return 4; }
NumResiduals() const180 virtual int NumResiduals() const { return 4; }
181
182 private:
183 const int num_active_cols_;
184 };
185
186 // Templated function to hold a subset of the columns fixed and check
187 // if the solver converges to the optimal values or not.
188 template<bool col1, bool col2, bool col3, bool col4>
IsTrustRegionSolveSuccessful(TrustRegionStrategyType strategy_type)189 void IsTrustRegionSolveSuccessful(TrustRegionStrategyType strategy_type) {
190 Solver::Options solver_options;
191 LinearSolver::Options linear_solver_options;
192 DenseQRSolver linear_solver(linear_solver_options);
193
194 double parameters[4] = { 3, -1, 0, 1.0 };
195
196 // If the column is inactive, then set its value to the optimal
197 // value.
198 parameters[0] = (col1 ? parameters[0] : 0.0);
199 parameters[1] = (col2 ? parameters[1] : 0.0);
200 parameters[2] = (col3 ? parameters[2] : 0.0);
201 parameters[3] = (col4 ? parameters[3] : 0.0);
202
203 PowellEvaluator2<col1, col2, col3, col4> powell_evaluator;
204 scoped_ptr<SparseMatrix> jacobian(powell_evaluator.CreateJacobian());
205
206 Minimizer::Options minimizer_options(solver_options);
207 minimizer_options.gradient_tolerance = 1e-26;
208 minimizer_options.function_tolerance = 1e-26;
209 minimizer_options.parameter_tolerance = 1e-26;
210 minimizer_options.evaluator = &powell_evaluator;
211 minimizer_options.jacobian = jacobian.get();
212
213 TrustRegionStrategy::Options trust_region_strategy_options;
214 trust_region_strategy_options.trust_region_strategy_type = strategy_type;
215 trust_region_strategy_options.linear_solver = &linear_solver;
216 trust_region_strategy_options.initial_radius = 1e4;
217 trust_region_strategy_options.max_radius = 1e20;
218 trust_region_strategy_options.lm_min_diagonal = 1e-6;
219 trust_region_strategy_options.lm_max_diagonal = 1e32;
220 scoped_ptr<TrustRegionStrategy> strategy(
221 TrustRegionStrategy::Create(trust_region_strategy_options));
222 minimizer_options.trust_region_strategy = strategy.get();
223
224 TrustRegionMinimizer minimizer;
225 Solver::Summary summary;
226 minimizer.Minimize(minimizer_options, parameters, &summary);
227
228 // The minimum is at x1 = x2 = x3 = x4 = 0.
229 EXPECT_NEAR(0.0, parameters[0], 0.001);
230 EXPECT_NEAR(0.0, parameters[1], 0.001);
231 EXPECT_NEAR(0.0, parameters[2], 0.001);
232 EXPECT_NEAR(0.0, parameters[3], 0.001);
233 };
234
TEST(TrustRegionMinimizer,PowellsSingularFunctionUsingLevenbergMarquardt)235 TEST(TrustRegionMinimizer, PowellsSingularFunctionUsingLevenbergMarquardt) {
236 // This case is excluded because this has a local minimum and does
237 // not find the optimum. This should not affect the correctness of
238 // this test since we are testing all the other 14 combinations of
239 // column activations.
240 //
241 // IsSolveSuccessful<true, true, false, true>();
242
243 const TrustRegionStrategyType kStrategy = LEVENBERG_MARQUARDT;
244 IsTrustRegionSolveSuccessful<true, true, true, true >(kStrategy);
245 IsTrustRegionSolveSuccessful<true, true, true, false>(kStrategy);
246 IsTrustRegionSolveSuccessful<true, false, true, true >(kStrategy);
247 IsTrustRegionSolveSuccessful<false, true, true, true >(kStrategy);
248 IsTrustRegionSolveSuccessful<true, true, false, false>(kStrategy);
249 IsTrustRegionSolveSuccessful<true, false, true, false>(kStrategy);
250 IsTrustRegionSolveSuccessful<false, true, true, false>(kStrategy);
251 IsTrustRegionSolveSuccessful<true, false, false, true >(kStrategy);
252 IsTrustRegionSolveSuccessful<false, true, false, true >(kStrategy);
253 IsTrustRegionSolveSuccessful<false, false, true, true >(kStrategy);
254 IsTrustRegionSolveSuccessful<true, false, false, false>(kStrategy);
255 IsTrustRegionSolveSuccessful<false, true, false, false>(kStrategy);
256 IsTrustRegionSolveSuccessful<false, false, true, false>(kStrategy);
257 IsTrustRegionSolveSuccessful<false, false, false, true >(kStrategy);
258 }
259
TEST(TrustRegionMinimizer,PowellsSingularFunctionUsingDogleg)260 TEST(TrustRegionMinimizer, PowellsSingularFunctionUsingDogleg) {
261 // The following two cases are excluded because they encounter a local minimum.
262 //
263 // IsTrustRegionSolveSuccessful<true, true, false, true >(kStrategy);
264 // IsTrustRegionSolveSuccessful<true, true, true, true >(kStrategy);
265
266 const TrustRegionStrategyType kStrategy = DOGLEG;
267 IsTrustRegionSolveSuccessful<true, true, true, false>(kStrategy);
268 IsTrustRegionSolveSuccessful<true, false, true, true >(kStrategy);
269 IsTrustRegionSolveSuccessful<false, true, true, true >(kStrategy);
270 IsTrustRegionSolveSuccessful<true, true, false, false>(kStrategy);
271 IsTrustRegionSolveSuccessful<true, false, true, false>(kStrategy);
272 IsTrustRegionSolveSuccessful<false, true, true, false>(kStrategy);
273 IsTrustRegionSolveSuccessful<true, false, false, true >(kStrategy);
274 IsTrustRegionSolveSuccessful<false, true, false, true >(kStrategy);
275 IsTrustRegionSolveSuccessful<false, false, true, true >(kStrategy);
276 IsTrustRegionSolveSuccessful<true, false, false, false>(kStrategy);
277 IsTrustRegionSolveSuccessful<false, true, false, false>(kStrategy);
278 IsTrustRegionSolveSuccessful<false, false, true, false>(kStrategy);
279 IsTrustRegionSolveSuccessful<false, false, false, true >(kStrategy);
280 }
281
282
283 class CurveCostFunction : public CostFunction {
284 public:
CurveCostFunction(int num_vertices,double target_length)285 CurveCostFunction(int num_vertices, double target_length)
286 : num_vertices_(num_vertices), target_length_(target_length) {
287 set_num_residuals(1);
288 for (int i = 0; i < num_vertices_; ++i) {
289 mutable_parameter_block_sizes()->push_back(2);
290 }
291 }
292
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const293 bool Evaluate(double const* const* parameters,
294 double* residuals,
295 double** jacobians) const {
296 residuals[0] = target_length_;
297
298 for (int i = 0; i < num_vertices_; ++i) {
299 int prev = (num_vertices_ + i - 1) % num_vertices_;
300 double length = 0.0;
301 for (int dim = 0; dim < 2; dim++) {
302 const double diff = parameters[prev][dim] - parameters[i][dim];
303 length += diff * diff;
304 }
305 residuals[0] -= sqrt(length);
306 }
307
308 if (jacobians == NULL) {
309 return true;
310 }
311
312 for (int i = 0; i < num_vertices_; ++i) {
313 if (jacobians[i] != NULL) {
314 int prev = (num_vertices_ + i - 1) % num_vertices_;
315 int next = (i + 1) % num_vertices_;
316
317 double u[2], v[2];
318 double norm_u = 0., norm_v = 0.;
319 for (int dim = 0; dim < 2; dim++) {
320 u[dim] = parameters[i][dim] - parameters[prev][dim];
321 norm_u += u[dim] * u[dim];
322 v[dim] = parameters[next][dim] - parameters[i][dim];
323 norm_v += v[dim] * v[dim];
324 }
325
326 norm_u = sqrt(norm_u);
327 norm_v = sqrt(norm_v);
328
329 for (int dim = 0; dim < 2; dim++) {
330 jacobians[i][dim] = 0.;
331
332 if (norm_u > std::numeric_limits< double >::min()) {
333 jacobians[i][dim] -= u[dim] / norm_u;
334 }
335
336 if (norm_v > std::numeric_limits< double >::min()) {
337 jacobians[i][dim] += v[dim] / norm_v;
338 }
339 }
340 }
341 }
342
343 return true;
344 }
345
346 private:
347 int num_vertices_;
348 double target_length_;
349 };
350
TEST(TrustRegionMinimizer,JacobiScalingTest)351 TEST(TrustRegionMinimizer, JacobiScalingTest) {
352 int N = 6;
353 std::vector< double* > y(N);
354 const double pi = 3.1415926535897932384626433;
355 for (int i = 0; i < N; i++) {
356 double theta = i * 2. * pi/ static_cast< double >(N);
357 y[i] = new double[2];
358 y[i][0] = cos(theta);
359 y[i][1] = sin(theta);
360 }
361
362 Problem problem;
363 problem.AddResidualBlock(new CurveCostFunction(N, 10.), NULL, y);
364 Solver::Options options;
365 options.linear_solver_type = ceres::DENSE_QR;
366 Solver::Summary summary;
367 Solve(options, &problem, &summary);
368 EXPECT_LE(summary.final_cost, 1e-10);
369
370 for (int i = 0; i < N; i++) {
371 delete y[i];
372 }
373 }
374
375 } // namespace internal
376 } // namespace ceres
377