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 (at) google.com (Keir Mierle) 30 // sameeragarwal (at) 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: 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. 211 template<bool col1, bool col2, bool col3, bool col4> 212 void 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 258 TEST(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 283 TEST(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 307 class 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 375 TEST(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
