internal/ceres/trust_region_minimizer_test.cc

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