1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
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 "ceres/trust_region_minimizer.h"
37
38 #include <cmath>
39
40 #include "ceres/autodiff_cost_function.h"
41 #include "ceres/cost_function.h"
42 #include "ceres/dense_qr_solver.h"
43 #include "ceres/dense_sparse_matrix.h"
44 #include "ceres/evaluator.h"
45 #include "ceres/internal/port.h"
46 #include "ceres/linear_solver.h"
47 #include "ceres/minimizer.h"
48 #include "ceres/problem.h"
49 #include "ceres/trust_region_strategy.h"
50 #include "gtest/gtest.h"
51
52 namespace ceres {
53 namespace internal {
54
55 // Templated Evaluator for Powell's function. The template parameters
56 // indicate which of the four variables/columns of the jacobian are
57 // active. This is equivalent to constructing a problem and using the
58 // SubsetLocalParameterization. This allows us to test the support for
59 // the Evaluator::Plus operation besides checking for the basic
60 // performance of the trust region algorithm.
61 template <bool col1, bool col2, bool col3, bool col4>
62 class PowellEvaluator2 : public Evaluator {
63 public:
64 // clang-format off
PowellEvaluator2()65 PowellEvaluator2()
66 : num_active_cols_(
67 (col1 ? 1 : 0) +
68 (col2 ? 1 : 0) +
69 (col3 ? 1 : 0) +
70 (col4 ? 1 : 0)) {
71 VLOG(1) << "Columns: "
72 << col1 << " "
73 << col2 << " "
74 << col3 << " "
75 << col4;
76 }
77 // clang-format on
78
~PowellEvaluator2()79 virtual ~PowellEvaluator2() {}
80
81 // Implementation of Evaluator interface.
CreateJacobian() const82 SparseMatrix* CreateJacobian() const final {
83 CHECK(col1 || col2 || col3 || col4);
84 DenseSparseMatrix* dense_jacobian =
85 new DenseSparseMatrix(NumResiduals(), NumEffectiveParameters());
86 dense_jacobian->SetZero();
87 return dense_jacobian;
88 }
89
Evaluate(const Evaluator::EvaluateOptions & evaluate_options,const double * state,double * cost,double * residuals,double * gradient,SparseMatrix * jacobian)90 bool Evaluate(const Evaluator::EvaluateOptions& evaluate_options,
91 const double* state,
92 double* cost,
93 double* residuals,
94 double* gradient,
95 SparseMatrix* jacobian) final {
96 const double x1 = state[0];
97 const double x2 = state[1];
98 const double x3 = state[2];
99 const double x4 = state[3];
100
101 VLOG(1) << "State: "
102 << "x1=" << x1 << ", "
103 << "x2=" << x2 << ", "
104 << "x3=" << x3 << ", "
105 << "x4=" << x4 << ".";
106
107 const double f1 = x1 + 10.0 * x2;
108 const double f2 = sqrt(5.0) * (x3 - x4);
109 const double f3 = pow(x2 - 2.0 * x3, 2.0);
110 const double f4 = sqrt(10.0) * pow(x1 - x4, 2.0);
111
112 VLOG(1) << "Function: "
113 << "f1=" << f1 << ", "
114 << "f2=" << f2 << ", "
115 << "f3=" << f3 << ", "
116 << "f4=" << f4 << ".";
117
118 *cost = (f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4) / 2.0;
119
120 VLOG(1) << "Cost: " << *cost;
121
122 if (residuals != NULL) {
123 residuals[0] = f1;
124 residuals[1] = f2;
125 residuals[2] = f3;
126 residuals[3] = f4;
127 }
128
129 if (jacobian != NULL) {
130 DenseSparseMatrix* dense_jacobian;
131 dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian);
132 dense_jacobian->SetZero();
133
134 ColMajorMatrixRef jacobian_matrix = dense_jacobian->mutable_matrix();
135 CHECK_EQ(jacobian_matrix.cols(), num_active_cols_);
136
137 int column_index = 0;
138 if (col1) {
139 // clang-format off
140 jacobian_matrix.col(column_index++) <<
141 1.0,
142 0.0,
143 0.0,
144 sqrt(10.0) * 2.0 * (x1 - x4) * (1.0 - x4);
145 // clang-format on
146 }
147 if (col2) {
148 // clang-format off
149 jacobian_matrix.col(column_index++) <<
150 10.0,
151 0.0,
152 2.0*(x2 - 2.0*x3)*(1.0 - 2.0*x3),
153 0.0;
154 // clang-format on
155 }
156
157 if (col3) {
158 // clang-format off
159 jacobian_matrix.col(column_index++) <<
160 0.0,
161 sqrt(5.0),
162 2.0*(x2 - 2.0*x3)*(x2 - 2.0),
163 0.0;
164 // clang-format on
165 }
166
167 if (col4) {
168 // clang-format off
169 jacobian_matrix.col(column_index++) <<
170 0.0,
171 -sqrt(5.0),
172 0.0,
173 sqrt(10.0) * 2.0 * (x1 - x4) * (x1 - 1.0);
174 // clang-format on
175 }
176 VLOG(1) << "\n" << jacobian_matrix;
177 }
178
179 if (gradient != NULL) {
180 int column_index = 0;
181 if (col1) {
182 gradient[column_index++] = f1 + f4 * sqrt(10.0) * 2.0 * (x1 - x4);
183 }
184
185 if (col2) {
186 gradient[column_index++] = f1 * 10.0 + f3 * 2.0 * (x2 - 2.0 * x3);
187 }
188
189 if (col3) {
190 gradient[column_index++] =
191 f2 * sqrt(5.0) + f3 * (2.0 * 2.0 * (2.0 * x3 - x2));
192 }
193
194 if (col4) {
195 gradient[column_index++] =
196 -f2 * sqrt(5.0) + f4 * sqrt(10.0) * 2.0 * (x4 - x1);
197 }
198 }
199
200 return true;
201 }
202
Plus(const double * state,const double * delta,double * state_plus_delta) const203 bool Plus(const double* state,
204 const double* delta,
205 double* state_plus_delta) const final {
206 int delta_index = 0;
207 state_plus_delta[0] = (col1 ? state[0] + delta[delta_index++] : state[0]);
208 state_plus_delta[1] = (col2 ? state[1] + delta[delta_index++] : state[1]);
209 state_plus_delta[2] = (col3 ? state[2] + delta[delta_index++] : state[2]);
210 state_plus_delta[3] = (col4 ? state[3] + delta[delta_index++] : state[3]);
211 return true;
212 }
213
NumEffectiveParameters() const214 int NumEffectiveParameters() const final { return num_active_cols_; }
NumParameters() const215 int NumParameters() const final { return 4; }
NumResiduals() const216 int NumResiduals() const final { return 4; }
217
218 private:
219 const int num_active_cols_;
220 };
221
222 // Templated function to hold a subset of the columns fixed and check
223 // if the solver converges to the optimal values or not.
224 template <bool col1, bool col2, bool col3, bool col4>
IsTrustRegionSolveSuccessful(TrustRegionStrategyType strategy_type)225 void IsTrustRegionSolveSuccessful(TrustRegionStrategyType strategy_type) {
226 Solver::Options solver_options;
227 LinearSolver::Options linear_solver_options;
228 DenseQRSolver linear_solver(linear_solver_options);
229
230 double parameters[4] = {3, -1, 0, 1.0};
231
232 // If the column is inactive, then set its value to the optimal
233 // value.
234 parameters[0] = (col1 ? parameters[0] : 0.0);
235 parameters[1] = (col2 ? parameters[1] : 0.0);
236 parameters[2] = (col3 ? parameters[2] : 0.0);
237 parameters[3] = (col4 ? parameters[3] : 0.0);
238
239 Minimizer::Options minimizer_options(solver_options);
240 minimizer_options.gradient_tolerance = 1e-26;
241 minimizer_options.function_tolerance = 1e-26;
242 minimizer_options.parameter_tolerance = 1e-26;
243 minimizer_options.evaluator.reset(
244 new PowellEvaluator2<col1, col2, col3, col4>);
245 minimizer_options.jacobian.reset(
246 minimizer_options.evaluator->CreateJacobian());
247
248 TrustRegionStrategy::Options trust_region_strategy_options;
249 trust_region_strategy_options.trust_region_strategy_type = strategy_type;
250 trust_region_strategy_options.linear_solver = &linear_solver;
251 trust_region_strategy_options.initial_radius = 1e4;
252 trust_region_strategy_options.max_radius = 1e20;
253 trust_region_strategy_options.min_lm_diagonal = 1e-6;
254 trust_region_strategy_options.max_lm_diagonal = 1e32;
255 minimizer_options.trust_region_strategy.reset(
256 TrustRegionStrategy::Create(trust_region_strategy_options));
257
258 TrustRegionMinimizer minimizer;
259 Solver::Summary summary;
260 minimizer.Minimize(minimizer_options, parameters, &summary);
261
262 // The minimum is at x1 = x2 = x3 = x4 = 0.
263 EXPECT_NEAR(0.0, parameters[0], 0.001);
264 EXPECT_NEAR(0.0, parameters[1], 0.001);
265 EXPECT_NEAR(0.0, parameters[2], 0.001);
266 EXPECT_NEAR(0.0, parameters[3], 0.001);
267 }
268
TEST(TrustRegionMinimizer,PowellsSingularFunctionUsingLevenbergMarquardt)269 TEST(TrustRegionMinimizer, PowellsSingularFunctionUsingLevenbergMarquardt) {
270 // This case is excluded because this has a local minimum and does
271 // not find the optimum. This should not affect the correctness of
272 // this test since we are testing all the other 14 combinations of
273 // column activations.
274 //
275 // IsSolveSuccessful<true, true, false, true>();
276
277 const TrustRegionStrategyType kStrategy = LEVENBERG_MARQUARDT;
278 // clang-format off
279 IsTrustRegionSolveSuccessful<true, true, true, true >(kStrategy);
280 IsTrustRegionSolveSuccessful<true, true, true, false>(kStrategy);
281 IsTrustRegionSolveSuccessful<true, false, true, true >(kStrategy);
282 IsTrustRegionSolveSuccessful<false, true, true, true >(kStrategy);
283 IsTrustRegionSolveSuccessful<true, true, false, false>(kStrategy);
284 IsTrustRegionSolveSuccessful<true, false, true, false>(kStrategy);
285 IsTrustRegionSolveSuccessful<false, true, true, false>(kStrategy);
286 IsTrustRegionSolveSuccessful<true, false, false, true >(kStrategy);
287 IsTrustRegionSolveSuccessful<false, true, false, true >(kStrategy);
288 IsTrustRegionSolveSuccessful<false, false, true, true >(kStrategy);
289 IsTrustRegionSolveSuccessful<true, false, false, false>(kStrategy);
290 IsTrustRegionSolveSuccessful<false, true, false, false>(kStrategy);
291 IsTrustRegionSolveSuccessful<false, false, true, false>(kStrategy);
292 IsTrustRegionSolveSuccessful<false, false, false, true >(kStrategy);
293 // clang-format on
294 }
295
TEST(TrustRegionMinimizer,PowellsSingularFunctionUsingDogleg)296 TEST(TrustRegionMinimizer, PowellsSingularFunctionUsingDogleg) {
297 // The following two cases are excluded because they encounter a
298 // local minimum.
299 //
300 // IsTrustRegionSolveSuccessful<true, true, false, true >(kStrategy);
301 // IsTrustRegionSolveSuccessful<true, true, true, true >(kStrategy);
302
303 const TrustRegionStrategyType kStrategy = DOGLEG;
304 // clang-format off
305 IsTrustRegionSolveSuccessful<true, true, true, false>(kStrategy);
306 IsTrustRegionSolveSuccessful<true, false, true, true >(kStrategy);
307 IsTrustRegionSolveSuccessful<false, true, true, true >(kStrategy);
308 IsTrustRegionSolveSuccessful<true, true, false, false>(kStrategy);
309 IsTrustRegionSolveSuccessful<true, false, true, false>(kStrategy);
310 IsTrustRegionSolveSuccessful<false, true, true, false>(kStrategy);
311 IsTrustRegionSolveSuccessful<true, false, false, true >(kStrategy);
312 IsTrustRegionSolveSuccessful<false, true, false, true >(kStrategy);
313 IsTrustRegionSolveSuccessful<false, false, true, true >(kStrategy);
314 IsTrustRegionSolveSuccessful<true, false, false, false>(kStrategy);
315 IsTrustRegionSolveSuccessful<false, true, false, false>(kStrategy);
316 IsTrustRegionSolveSuccessful<false, false, true, false>(kStrategy);
317 IsTrustRegionSolveSuccessful<false, false, false, true >(kStrategy);
318 // clang-format on
319 }
320
321 class CurveCostFunction : public CostFunction {
322 public:
CurveCostFunction(int num_vertices,double target_length)323 CurveCostFunction(int num_vertices, double target_length)
324 : num_vertices_(num_vertices), target_length_(target_length) {
325 set_num_residuals(1);
326 for (int i = 0; i < num_vertices_; ++i) {
327 mutable_parameter_block_sizes()->push_back(2);
328 }
329 }
330
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const331 bool Evaluate(double const* const* parameters,
332 double* residuals,
333 double** jacobians) const {
334 residuals[0] = target_length_;
335
336 for (int i = 0; i < num_vertices_; ++i) {
337 int prev = (num_vertices_ + i - 1) % num_vertices_;
338 double length = 0.0;
339 for (int dim = 0; dim < 2; dim++) {
340 const double diff = parameters[prev][dim] - parameters[i][dim];
341 length += diff * diff;
342 }
343 residuals[0] -= sqrt(length);
344 }
345
346 if (jacobians == NULL) {
347 return true;
348 }
349
350 for (int i = 0; i < num_vertices_; ++i) {
351 if (jacobians[i] != NULL) {
352 int prev = (num_vertices_ + i - 1) % num_vertices_;
353 int next = (i + 1) % num_vertices_;
354
355 double u[2], v[2];
356 double norm_u = 0., norm_v = 0.;
357 for (int dim = 0; dim < 2; dim++) {
358 u[dim] = parameters[i][dim] - parameters[prev][dim];
359 norm_u += u[dim] * u[dim];
360 v[dim] = parameters[next][dim] - parameters[i][dim];
361 norm_v += v[dim] * v[dim];
362 }
363
364 norm_u = sqrt(norm_u);
365 norm_v = sqrt(norm_v);
366
367 for (int dim = 0; dim < 2; dim++) {
368 jacobians[i][dim] = 0.;
369
370 if (norm_u > std::numeric_limits<double>::min()) {
371 jacobians[i][dim] -= u[dim] / norm_u;
372 }
373
374 if (norm_v > std::numeric_limits<double>::min()) {
375 jacobians[i][dim] += v[dim] / norm_v;
376 }
377 }
378 }
379 }
380
381 return true;
382 }
383
384 private:
385 int num_vertices_;
386 double target_length_;
387 };
388
TEST(TrustRegionMinimizer,JacobiScalingTest)389 TEST(TrustRegionMinimizer, JacobiScalingTest) {
390 int N = 6;
391 std::vector<double*> y(N);
392 const double pi = 3.1415926535897932384626433;
393 for (int i = 0; i < N; i++) {
394 double theta = i * 2. * pi / static_cast<double>(N);
395 y[i] = new double[2];
396 y[i][0] = cos(theta);
397 y[i][1] = sin(theta);
398 }
399
400 Problem problem;
401 problem.AddResidualBlock(new CurveCostFunction(N, 10.), NULL, y);
402 Solver::Options options;
403 options.linear_solver_type = ceres::DENSE_QR;
404 Solver::Summary summary;
405 Solve(options, &problem, &summary);
406 EXPECT_LE(summary.final_cost, 1e-10);
407
408 for (int i = 0; i < N; i++) {
409 delete[] y[i];
410 }
411 }
412
413 struct ExpCostFunctor {
414 template <typename T>
operator ()ceres::internal::ExpCostFunctor415 bool operator()(const T* const x, T* residual) const {
416 residual[0] = T(10.0) - exp(x[0]);
417 return true;
418 }
419
Createceres::internal::ExpCostFunctor420 static CostFunction* Create() {
421 return new AutoDiffCostFunction<ExpCostFunctor, 1, 1>(new ExpCostFunctor);
422 }
423 };
424
TEST(TrustRegionMinimizer,GradientToleranceConvergenceUpdatesStep)425 TEST(TrustRegionMinimizer, GradientToleranceConvergenceUpdatesStep) {
426 double x = 5;
427 Problem problem;
428 problem.AddResidualBlock(ExpCostFunctor::Create(), NULL, &x);
429 problem.SetParameterLowerBound(&x, 0, 3.0);
430 Solver::Options options;
431 Solver::Summary summary;
432 Solve(options, &problem, &summary);
433 EXPECT_NEAR(3.0, x, 1e-12);
434 const double expected_final_cost = 0.5 * pow(10.0 - exp(3.0), 2);
435 EXPECT_NEAR(expected_final_cost, summary.final_cost, 1e-12);
436 }
437
438 } // namespace internal
439 } // namespace ceres
440