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
49namespace ceres {
50namespace 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.
58template <bool col1, bool col2, bool col3, bool col4>
59class PowellEvaluator2 : public Evaluator {
60 public:
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
74  virtual ~PowellEvaluator2() {}
75
76  // Implementation of Evaluator interface.
77  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
85  virtual bool Evaluate(const Evaluator::EvaluateOptions& evaluate_options,
86                        const double* state,
87                        double* cost,
88                        double* residuals,
89                        double* gradient,
90                        SparseMatrix* jacobian) {
91    const double x1 = state[0];
92    const double x2 = state[1];
93    const double x3 = state[2];
94    const double x4 = state[3];
95
96    VLOG(1) << "State: "
97            << "x1=" << x1 << ", "
98            << "x2=" << x2 << ", "
99            << "x3=" << x3 << ", "
100            << "x4=" << x4 << ".";
101
102    const double f1 = x1 + 10.0 * x2;
103    const double f2 = sqrt(5.0) * (x3 - x4);
104    const double f3 = pow(x2 - 2.0 * x3, 2.0);
105    const double f4 = sqrt(10.0) * pow(x1 - x4, 2.0);
106
107    VLOG(1) << "Function: "
108            << "f1=" << f1 << ", "
109            << "f2=" << f2 << ", "
110            << "f3=" << f3 << ", "
111            << "f4=" << f4 << ".";
112
113    *cost = (f1*f1 + f2*f2 + f3*f3 + f4*f4) / 2.0;
114
115    VLOG(1) << "Cost: " << *cost;
116
117    if (residuals != NULL) {
118      residuals[0] = f1;
119      residuals[1] = f2;
120      residuals[2] = f3;
121      residuals[3] = f4;
122    }
123
124    if (jacobian != NULL) {
125      DenseSparseMatrix* dense_jacobian;
126      dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian);
127      dense_jacobian->SetZero();
128
129      ColMajorMatrixRef jacobian_matrix = dense_jacobian->mutable_matrix();
130      CHECK_EQ(jacobian_matrix.cols(), num_active_cols_);
131
132      int column_index = 0;
133      if (col1) {
134        jacobian_matrix.col(column_index++) <<
135            1.0,
136            0.0,
137            0.0,
138            sqrt(10.0) * 2.0 * (x1 - x4) * (1.0 - x4);
139      }
140      if (col2) {
141        jacobian_matrix.col(column_index++) <<
142            10.0,
143            0.0,
144            2.0*(x2 - 2.0*x3)*(1.0 - 2.0*x3),
145            0.0;
146      }
147
148      if (col3) {
149        jacobian_matrix.col(column_index++) <<
150            0.0,
151            sqrt(5.0),
152            2.0*(x2 - 2.0*x3)*(x2 - 2.0),
153            0.0;
154      }
155
156      if (col4) {
157        jacobian_matrix.col(column_index++) <<
158            0.0,
159            -sqrt(5.0),
160            0.0,
161            sqrt(10.0) * 2.0 * (x1 - x4) * (x1 - 1.0);
162      }
163      VLOG(1) << "\n" << jacobian_matrix;
164    }
165
166    if (gradient != NULL) {
167      int column_index = 0;
168      if (col1) {
169        gradient[column_index++] = f1  + f4 * sqrt(10.0) * 2.0 * (x1 - x4);
170      }
171
172      if (col2) {
173        gradient[column_index++] = f1 * 10.0 + f3 * 2.0 * (x2 - 2.0 * x3);
174      }
175
176      if (col3) {
177        gradient[column_index++] =
178            f2 * sqrt(5.0) + f3 * (2.0 * 2.0 * (2.0 * x3 - x2));
179      }
180
181      if (col4) {
182        gradient[column_index++] =
183            -f2 * sqrt(5.0) + f4 * sqrt(10.0) * 2.0 * (x4 - x1);
184      }
185    }
186
187    return true;
188  }
189
190  virtual bool Plus(const double* state,
191                    const double* delta,
192                    double* state_plus_delta) const {
193    int delta_index = 0;
194    state_plus_delta[0] = (col1  ? state[0] + delta[delta_index++] : state[0]);
195    state_plus_delta[1] = (col2  ? state[1] + delta[delta_index++] : state[1]);
196    state_plus_delta[2] = (col3  ? state[2] + delta[delta_index++] : state[2]);
197    state_plus_delta[3] = (col4  ? state[3] + delta[delta_index++] : state[3]);
198    return true;
199  }
200
201  virtual int NumEffectiveParameters() const { return num_active_cols_; }
202  virtual int NumParameters()          const { return 4; }
203  virtual int NumResiduals()           const { return 4; }
204
205 private:
206  const int num_active_cols_;
207};
208
209// Templated function to hold a subset of the columns fixed and check
210// if the solver converges to the optimal values or not.
211template<bool col1, bool col2, bool col3, bool col4>
212void IsTrustRegionSolveSuccessful(TrustRegionStrategyType strategy_type) {
213  Solver::Options solver_options;
214  LinearSolver::Options linear_solver_options;
215  DenseQRSolver linear_solver(linear_solver_options);
216
217  double parameters[4] = { 3, -1, 0, 1.0 };
218
219  // If the column is inactive, then set its value to the optimal
220  // value.
221  parameters[0] = (col1 ? parameters[0] : 0.0);
222  parameters[1] = (col2 ? parameters[1] : 0.0);
223  parameters[2] = (col3 ? parameters[2] : 0.0);
224  parameters[3] = (col4 ? parameters[3] : 0.0);
225
226  PowellEvaluator2<col1, col2, col3, col4> powell_evaluator;
227  scoped_ptr<SparseMatrix> jacobian(powell_evaluator.CreateJacobian());
228
229  Minimizer::Options minimizer_options(solver_options);
230  minimizer_options.gradient_tolerance = 1e-26;
231  minimizer_options.function_tolerance = 1e-26;
232  minimizer_options.parameter_tolerance = 1e-26;
233  minimizer_options.evaluator = &powell_evaluator;
234  minimizer_options.jacobian = jacobian.get();
235
236  TrustRegionStrategy::Options trust_region_strategy_options;
237  trust_region_strategy_options.trust_region_strategy_type = strategy_type;
238  trust_region_strategy_options.linear_solver = &linear_solver;
239  trust_region_strategy_options.initial_radius = 1e4;
240  trust_region_strategy_options.max_radius = 1e20;
241  trust_region_strategy_options.min_lm_diagonal = 1e-6;
242  trust_region_strategy_options.max_lm_diagonal = 1e32;
243  scoped_ptr<TrustRegionStrategy> strategy(
244      TrustRegionStrategy::Create(trust_region_strategy_options));
245  minimizer_options.trust_region_strategy = strategy.get();
246
247  TrustRegionMinimizer minimizer;
248  Solver::Summary summary;
249  minimizer.Minimize(minimizer_options, parameters, &summary);
250
251  // The minimum is at x1 = x2 = x3 = x4 = 0.
252  EXPECT_NEAR(0.0, parameters[0], 0.001);
253  EXPECT_NEAR(0.0, parameters[1], 0.001);
254  EXPECT_NEAR(0.0, parameters[2], 0.001);
255  EXPECT_NEAR(0.0, parameters[3], 0.001);
256};
257
258TEST(TrustRegionMinimizer, PowellsSingularFunctionUsingLevenbergMarquardt) {
259  // This case is excluded because this has a local minimum and does
260  // not find the optimum. This should not affect the correctness of
261  // this test since we are testing all the other 14 combinations of
262  // column activations.
263  //
264  //   IsSolveSuccessful<true, true, false, true>();
265
266  const TrustRegionStrategyType kStrategy = LEVENBERG_MARQUARDT;
267  IsTrustRegionSolveSuccessful<true,  true,  true,  true >(kStrategy);
268  IsTrustRegionSolveSuccessful<true,  true,  true,  false>(kStrategy);
269  IsTrustRegionSolveSuccessful<true,  false, true,  true >(kStrategy);
270  IsTrustRegionSolveSuccessful<false, true,  true,  true >(kStrategy);
271  IsTrustRegionSolveSuccessful<true,  true,  false, false>(kStrategy);
272  IsTrustRegionSolveSuccessful<true,  false, true,  false>(kStrategy);
273  IsTrustRegionSolveSuccessful<false, true,  true,  false>(kStrategy);
274  IsTrustRegionSolveSuccessful<true,  false, false, true >(kStrategy);
275  IsTrustRegionSolveSuccessful<false, true,  false, true >(kStrategy);
276  IsTrustRegionSolveSuccessful<false, false, true,  true >(kStrategy);
277  IsTrustRegionSolveSuccessful<true,  false, false, false>(kStrategy);
278  IsTrustRegionSolveSuccessful<false, true,  false, false>(kStrategy);
279  IsTrustRegionSolveSuccessful<false, false, true,  false>(kStrategy);
280  IsTrustRegionSolveSuccessful<false, false, false, true >(kStrategy);
281}
282
283TEST(TrustRegionMinimizer, PowellsSingularFunctionUsingDogleg) {
284  // The following two cases are excluded because they encounter a
285  // local minimum.
286  //
287  //  IsTrustRegionSolveSuccessful<true, true, false, true >(kStrategy);
288  //  IsTrustRegionSolveSuccessful<true,  true,  true,  true >(kStrategy);
289
290  const TrustRegionStrategyType kStrategy = DOGLEG;
291  IsTrustRegionSolveSuccessful<true,  true,  true,  false>(kStrategy);
292  IsTrustRegionSolveSuccessful<true,  false, true,  true >(kStrategy);
293  IsTrustRegionSolveSuccessful<false, true,  true,  true >(kStrategy);
294  IsTrustRegionSolveSuccessful<true,  true,  false, false>(kStrategy);
295  IsTrustRegionSolveSuccessful<true,  false, true,  false>(kStrategy);
296  IsTrustRegionSolveSuccessful<false, true,  true,  false>(kStrategy);
297  IsTrustRegionSolveSuccessful<true,  false, false, true >(kStrategy);
298  IsTrustRegionSolveSuccessful<false, true,  false, true >(kStrategy);
299  IsTrustRegionSolveSuccessful<false, false, true,  true >(kStrategy);
300  IsTrustRegionSolveSuccessful<true,  false, false, false>(kStrategy);
301  IsTrustRegionSolveSuccessful<false, true,  false, false>(kStrategy);
302  IsTrustRegionSolveSuccessful<false, false, true,  false>(kStrategy);
303  IsTrustRegionSolveSuccessful<false, false, false, true >(kStrategy);
304}
305
306
307class CurveCostFunction : public CostFunction {
308 public:
309  CurveCostFunction(int num_vertices, double target_length)
310      : num_vertices_(num_vertices), target_length_(target_length) {
311    set_num_residuals(1);
312    for (int i = 0; i < num_vertices_; ++i) {
313      mutable_parameter_block_sizes()->push_back(2);
314    }
315  }
316
317  bool Evaluate(double const* const* parameters,
318                double* residuals,
319                double** jacobians) const {
320    residuals[0] = target_length_;
321
322    for (int i = 0; i < num_vertices_; ++i) {
323      int prev = (num_vertices_ + i - 1) % num_vertices_;
324      double length = 0.0;
325      for (int dim = 0; dim < 2; dim++) {
326        const double diff = parameters[prev][dim] - parameters[i][dim];
327        length += diff * diff;
328      }
329      residuals[0] -= sqrt(length);
330    }
331
332    if (jacobians == NULL) {
333      return true;
334    }
335
336    for (int i = 0; i < num_vertices_; ++i) {
337      if (jacobians[i] != NULL) {
338        int prev = (num_vertices_ + i - 1) % num_vertices_;
339        int next = (i + 1) % num_vertices_;
340
341        double u[2], v[2];
342        double norm_u = 0., norm_v = 0.;
343        for (int dim = 0; dim < 2; dim++) {
344          u[dim] = parameters[i][dim] - parameters[prev][dim];
345          norm_u += u[dim] * u[dim];
346          v[dim] = parameters[next][dim] - parameters[i][dim];
347          norm_v += v[dim] * v[dim];
348        }
349
350        norm_u = sqrt(norm_u);
351        norm_v = sqrt(norm_v);
352
353        for (int dim = 0; dim < 2; dim++) {
354          jacobians[i][dim] = 0.;
355
356          if (norm_u > std::numeric_limits< double >::min()) {
357            jacobians[i][dim] -= u[dim] / norm_u;
358          }
359
360          if (norm_v > std::numeric_limits< double >::min()) {
361            jacobians[i][dim] += v[dim] / norm_v;
362          }
363        }
364      }
365    }
366
367    return true;
368  }
369
370 private:
371  int     num_vertices_;
372  double  target_length_;
373};
374
375TEST(TrustRegionMinimizer, JacobiScalingTest) {
376  int N = 6;
377  std::vector< double* > y(N);
378  const double pi = 3.1415926535897932384626433;
379  for (int i = 0; i < N; i++) {
380    double theta = i * 2. * pi/ static_cast< double >(N);
381    y[i] = new double[2];
382    y[i][0] = cos(theta);
383    y[i][1] = sin(theta);
384  }
385
386  Problem problem;
387  problem.AddResidualBlock(new CurveCostFunction(N, 10.), NULL, y);
388  Solver::Options options;
389  options.linear_solver_type = ceres::DENSE_QR;
390  Solver::Summary summary;
391  Solve(options, &problem, &summary);
392  EXPECT_LE(summary.final_cost, 1e-10);
393
394  for (int i = 0; i < N; i++) {
395    delete []y[i];
396  }
397}
398
399}  // namespace internal
400}  // namespace ceres
401