1
// Ceres Solver - A fast non-linear least squares minimizer
2
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3
// http://code.google.com/p/ceres-solver/
5
// Redistribution and use in source and binary forms, with or without
6
// modification, are permitted provided that the following conditions are met:
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.
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.
29
// Author: sameeragarwal@google.com (Sameer Agarwal)
31
// Implementation of a simple LM algorithm which uses the step sizing
32
// rule of "Methods for Nonlinear Least Squares" by K. Madsen,
33
// H.B. Nielsen and O. Tingleff. Available to download from
35
// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
37
// The basic algorithm described in this note is an exact step
38
// algorithm that depends on the Newton(LM) step being solved exactly
39
// in each iteration. When a suitable iterative solver is available to
40
// solve the Newton(LM) step, the algorithm will automatically switch
41
// to an inexact step solution method. This trades some slowdown in
42
// convergence for significant savings in solve time and memory
43
// usage. Our implementation of the Truncated Newton algorithm follows
44
// the discussion and recommendataions in "Stephen G. Nash, A Survey
45
// of Truncated Newton Methods, Journal of Computational and Applied
46
// Mathematics, 124(1-2), 45-59, 2000.
48
#include "ceres/levenberg_marquardt.h"
57
#include <glog/logging.h>
59
#include "ceres/array_utils.h"
60
#include "ceres/evaluator.h"
61
#include "ceres/file.h"
62
#include "ceres/linear_least_squares_problems.h"
63
#include "ceres/linear_solver.h"
64
#include "ceres/matrix_proto.h"
65
#include "ceres/sparse_matrix.h"
66
#include "ceres/stringprintf.h"
67
#include "ceres/internal/eigen.h"
68
#include "ceres/internal/scoped_ptr.h"
69
#include "ceres/types.h"
75
// Numbers for clamping the size of the LM diagonal. The size of these
76
// numbers is heuristic. We will probably be adjusting them in the
77
// future based on more numerical experience. With jacobi scaling
78
// enabled, these numbers should be all but redundant.
79
const double kMinLevenbergMarquardtDiagonal = 1e-6;
80
const double kMaxLevenbergMarquardtDiagonal = 1e32;
82
// Small constant for various floating point issues.
83
const double kEpsilon = 1e-12;
85
// Number of times the linear solver should be retried in case of
86
// numerical failure. The retries are done by exponentially scaling up
87
// mu at each retry. This leads to stronger and stronger
88
// regularization making the linear least squares problem better
89
// conditioned at each retry.
90
const int kMaxLinearSolverRetries = 5;
92
// D = 1/sqrt(diag(J^T * J))
93
void EstimateScale(const SparseMatrix& jacobian, double* D) {
95
jacobian.SquaredColumnNorm(D);
96
for (int i = 0; i < jacobian.num_cols(); ++i) {
97
D[i] = 1.0 / (kEpsilon + sqrt(D[i]));
102
void LevenbergMarquardtDiagonal(const SparseMatrix& jacobian,
105
jacobian.SquaredColumnNorm(D);
106
for (int i = 0; i < jacobian.num_cols(); ++i) {
107
D[i] = min(max(D[i], kMinLevenbergMarquardtDiagonal),
108
kMaxLevenbergMarquardtDiagonal);
112
bool RunCallback(IterationCallback* callback,
113
const IterationSummary& iteration_summary,
114
Solver::Summary* summary) {
115
const CallbackReturnType status = (*callback)(iteration_summary);
117
case SOLVER_TERMINATE_SUCCESSFULLY:
118
summary->termination_type = USER_SUCCESS;
119
VLOG(1) << "Terminating on USER_SUCCESS.";
122
summary->termination_type = USER_ABORT;
123
VLOG(1) << "Terminating on USER_ABORT.";
125
case SOLVER_CONTINUE:
128
LOG(FATAL) << "Unknown status returned by callback: "
135
LevenbergMarquardt::~LevenbergMarquardt() {}
137
void LevenbergMarquardt::Minimize(const Minimizer::Options& options,
138
Evaluator* evaluator,
139
LinearSolver* linear_solver,
140
const double* initial_parameters,
141
double* final_parameters,
142
Solver::Summary* summary) {
143
time_t start_time = time(NULL);
144
const int num_parameters = evaluator->NumParameters();
145
const int num_effective_parameters = evaluator->NumEffectiveParameters();
146
const int num_residuals = evaluator->NumResiduals();
148
summary->termination_type = NO_CONVERGENCE;
149
summary->num_successful_steps = 0;
150
summary->num_unsuccessful_steps = 0;
152
// Allocate the various vectors needed by the algorithm.
153
memcpy(final_parameters, initial_parameters,
154
num_parameters * sizeof(*initial_parameters));
156
VectorRef x(final_parameters, num_parameters);
157
Vector x_new(num_parameters);
159
Vector lm_step(num_effective_parameters);
160
Vector gradient(num_effective_parameters);
161
Vector scaled_gradient(num_effective_parameters);
162
// Jacobi scaling vector
163
Vector scale(num_effective_parameters);
165
Vector f_model(num_residuals);
166
Vector f(num_residuals);
167
Vector f_new(num_residuals);
168
Vector D(num_parameters);
169
Vector muD(num_parameters);
171
// Ask the Evaluator to create the jacobian matrix. The sparsity
172
// pattern of this matrix is going to remain constant, so we only do
173
// this once and then re-use this matrix for all subsequent Jacobian
175
scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
177
double x_norm = x.norm();
183
// Do initial cost and Jacobian evaluation.
184
if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) {
185
LOG(WARNING) << "Failed to compute residuals and Jacobian. "
187
summary->termination_type = NUMERICAL_FAILURE;
191
if (options.jacobi_scaling) {
192
EstimateScale(*jacobian, scale.data());
193
jacobian->ScaleColumns(scale.data());
198
// This is a poor way to do this computation. Even if fixed_cost is
199
// zero, because we are subtracting two possibly large numbers, we
200
// are depending on exact cancellation to give us a zero here. But
201
// initial_cost and cost have been computed by two different
202
// evaluators. One which runs on the whole problem (in
203
// solver_impl.cc) in single threaded mode and another which runs
204
// here on the reduced problem, so fixed_cost can (and does) contain
205
// some numerical garbage with a relative magnitude of 1e-14.
207
// The right way to do this, would be to compute the fixed cost on
208
// just the set of residual blocks which are held constant and were
209
// removed from the original problem when the reduced problem was
211
summary->fixed_cost = summary->initial_cost - cost;
213
double model_cost = f.squaredNorm() / 2.0;
214
double total_cost = summary->fixed_cost + cost;
216
scaled_gradient.setZero();
217
jacobian->LeftMultiply(f.data(), scaled_gradient.data());
218
gradient = scaled_gradient.array() / scale.array();
220
double gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();
221
// We need the max here to guard againt the gradient being zero.
222
const double gradient_max_norm_0 = max(gradient_max_norm, kEpsilon);
223
double gradient_tolerance = options.gradient_tolerance * gradient_max_norm_0;
225
double mu = options.tau;
228
double actual_cost_change = 0.0;
229
double step_norm = 0.0;
230
double relative_decrease = 0.0;
232
// Insane steps are steps which are not sane, i.e. there is some
233
// numerical kookiness going on with them. There are various reasons
234
// for this kookiness, some easier to diagnose then others. From the
235
// point of view of the non-linear solver, they are steps which
236
// cannot be used. We return with NUMERICAL_FAILURE after
237
// kMaxLinearSolverRetries consecutive insane steps.
238
bool step_is_sane = false;
239
int num_consecutive_insane_steps = 0;
241
// Whether the step resulted in a sufficient decrease in the
242
// objective function when compared to the decrease in the value of
244
bool step_is_successful = false;
246
// Parse the iterations for which to dump the linear problem.
247
vector<int> iterations_to_dump = options.lsqp_iterations_to_dump;
248
sort(iterations_to_dump.begin(), iterations_to_dump.end());
250
IterationSummary iteration_summary;
251
iteration_summary.iteration = iteration;
252
iteration_summary.step_is_successful = false;
253
iteration_summary.cost = total_cost;
254
iteration_summary.cost_change = actual_cost_change;
255
iteration_summary.gradient_max_norm = gradient_max_norm;
256
iteration_summary.step_norm = step_norm;
257
iteration_summary.relative_decrease = relative_decrease;
258
iteration_summary.mu = mu;
259
iteration_summary.eta = options.eta;
260
iteration_summary.linear_solver_iterations = 0;
261
iteration_summary.linear_solver_time_sec = 0.0;
262
iteration_summary.iteration_time_sec = (time(NULL) - start_time);
263
if (options.logging_type >= PER_MINIMIZER_ITERATION) {
264
summary->iterations.push_back(iteration_summary);
267
// Check if the starting point is an optimum.
268
VLOG(2) << "Gradient max norm: " << gradient_max_norm
269
<< " tolerance: " << gradient_tolerance
270
<< " ratio: " << gradient_max_norm / gradient_max_norm_0
271
<< " tolerance: " << options.gradient_tolerance;
272
if (gradient_max_norm <= gradient_tolerance) {
273
summary->termination_type = GRADIENT_TOLERANCE;
274
VLOG(1) << "Terminating on GRADIENT_TOLERANCE. "
275
<< "Relative gradient max norm: "
276
<< gradient_max_norm / gradient_max_norm_0
277
<< " <= " << options.gradient_tolerance;
281
// Call the various callbacks.
282
for (int i = 0; i < options.callbacks.size(); ++i) {
283
if (!RunCallback(options.callbacks[i], iteration_summary, summary)) {
288
// We only need the LM diagonal if we are actually going to do at
289
// least one iteration of the optimization. So we wait to do it
291
LevenbergMarquardtDiagonal(*jacobian, D.data());
293
while ((iteration < options.max_num_iterations) &&
294
(time(NULL) - start_time) <= options.max_solver_time_sec) {
295
time_t iteration_start_time = time(NULL);
296
step_is_sane = false;
297
step_is_successful = false;
299
IterationSummary iteration_summary;
300
// The while loop here is just to provide an easily breakable
301
// control structure. We are guaranteed to always exit this loop
302
// at the end of one iteration or before.
304
muD = (mu * D).array().sqrt();
305
LinearSolver::PerSolveOptions solve_options;
306
solve_options.D = muD.data();
307
solve_options.q_tolerance = options.eta;
308
// Disable r_tolerance checking. Since we only care about
309
// termination via the q_tolerance. As Nash and Sofer show,
310
// r_tolerance based termination is essentially useless in
311
// Truncated Newton methods.
312
solve_options.r_tolerance = -1.0;
314
// Invalidate the output array lm_step, so that we can detect if
315
// the linear solver generated numerical garbage. This is known
316
// to happen for the DENSE_QR and then DENSE_SCHUR solver when
317
// the Jacobin is severly rank deficient and mu is too small.
318
InvalidateArray(num_effective_parameters, lm_step.data());
319
const time_t linear_solver_start_time = time(NULL);
320
LinearSolver::Summary linear_solver_summary =
321
linear_solver->Solve(jacobian.get(),
325
iteration_summary.linear_solver_time_sec =
326
(time(NULL) - linear_solver_start_time);
327
iteration_summary.linear_solver_iterations =
328
linear_solver_summary.num_iterations;
330
if (binary_search(iterations_to_dump.begin(),
331
iterations_to_dump.end(),
333
CHECK(DumpLinearLeastSquaresProblem(options.lsqp_dump_directory,
335
options.lsqp_dump_format_type,
340
options.num_eliminate_blocks))
341
<< "Tried writing linear least squares problem: "
342
<< options.lsqp_dump_directory
346
// We ignore the case where the linear solver did not converge,
347
// since the partial solution computed by it still maybe of use,
348
// and there is no reason to ignore it, especially since we
349
// spent so much time computing it.
350
if ((linear_solver_summary.termination_type != TOLERANCE) &&
351
(linear_solver_summary.termination_type != MAX_ITERATIONS)) {
352
VLOG(1) << "Linear solver failure: retrying with a higher mu";
356
if (!IsArrayValid(num_effective_parameters, lm_step.data())) {
357
LOG(WARNING) << "Linear solver failure. Failed to compute a finite "
358
<< "step. Terminating. Please report this to the Ceres "
359
<< "Solver developers.";
360
summary->termination_type = NUMERICAL_FAILURE;
364
step_norm = (lm_step.array() * scale.array()).matrix().norm();
366
// Check step length based convergence. If the step length is
367
// too small, then we are done.
368
const double step_size_tolerance = options.parameter_tolerance *
369
(x_norm + options.parameter_tolerance);
371
VLOG(2) << "Step size: " << step_norm
372
<< " tolerance: " << step_size_tolerance
373
<< " ratio: " << step_norm / step_size_tolerance
374
<< " tolerance: " << options.parameter_tolerance;
375
if (step_norm <= options.parameter_tolerance *
376
(x_norm + options.parameter_tolerance)) {
377
summary->termination_type = PARAMETER_TOLERANCE;
378
VLOG(1) << "Terminating on PARAMETER_TOLERANCE."
379
<< "Relative step size: " << step_norm / step_size_tolerance
380
<< " <= " << options.parameter_tolerance;
384
Vector delta = -(lm_step.array() * scale.array()).matrix();
385
if (!evaluator->Plus(x.data(), delta.data(), x_new.data())) {
386
LOG(WARNING) << "Failed to compute Plus(x, delta, x_plus_delta). "
388
summary->termination_type = NUMERICAL_FAILURE;
392
double cost_new = 0.0;
393
if (!evaluator->Evaluate(x_new.data(), &cost_new, NULL, NULL)) {
394
LOG(WARNING) << "Failed to compute the value of the objective "
395
<< "function. Terminating.";
396
summary->termination_type = NUMERICAL_FAILURE;
401
jacobian->RightMultiply(lm_step.data(), f_model.data());
402
const double model_cost_new =
403
(f.segment(0, num_residuals) - f_model).squaredNorm() / 2;
405
actual_cost_change = cost - cost_new;
406
double model_cost_change = model_cost - model_cost_new;
408
VLOG(2) << "[Model cost] current: " << model_cost
409
<< " new : " << model_cost_new
410
<< " change: " << model_cost_change;
412
VLOG(2) << "[Nonlinear cost] current: " << cost
413
<< " new : " << cost_new
414
<< " change: " << actual_cost_change
415
<< " relative change: " << fabs(actual_cost_change) / cost
416
<< " tolerance: " << options.function_tolerance;
418
// In exact arithmetic model_cost_change should never be
419
// negative. But due to numerical precision issues, we may end up
420
// with a small negative number. model_cost_change which are
421
// negative and large in absolute value are indicative of a
422
// numerical failure in the solver.
423
if (model_cost_change < -kEpsilon) {
424
VLOG(1) << "Model cost change is negative.\n"
425
<< "Current : " << model_cost
426
<< " new : " << model_cost_new
427
<< " change: " << model_cost_change << "\n";
431
// If we have reached this far, then we are willing to trust the
432
// numerical quality of the step.
434
num_consecutive_insane_steps = 0;
436
// Check function value based convergence.
437
if (fabs(actual_cost_change) < options.function_tolerance * cost) {
438
VLOG(1) << "Termination on FUNCTION_TOLERANCE."
439
<< " Relative cost change: " << fabs(actual_cost_change) / cost
440
<< " tolerance: " << options.function_tolerance;
441
summary->termination_type = FUNCTION_TOLERANCE;
445
// Clamp model_cost_change at kEpsilon from below.
446
if (model_cost_change < kEpsilon) {
447
VLOG(1) << "Clamping model cost change " << model_cost_change
448
<< " to " << kEpsilon;
449
model_cost_change = kEpsilon;
452
relative_decrease = actual_cost_change / model_cost_change;
453
VLOG(2) << "actual_cost_change / model_cost_change = "
454
<< relative_decrease;
456
if (relative_decrease < options.min_relative_decrease) {
457
VLOG(2) << "Unsuccessful step.";
461
VLOG(2) << "Successful step.";
463
++summary->num_successful_steps;
467
if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) {
468
LOG(WARNING) << "Failed to compute residuals and jacobian. "
470
summary->termination_type = NUMERICAL_FAILURE;
474
if (options.jacobi_scaling) {
475
jacobian->ScaleColumns(scale.data());
478
model_cost = f.squaredNorm() / 2.0;
479
LevenbergMarquardtDiagonal(*jacobian, D.data());
480
scaled_gradient.setZero();
481
jacobian->LeftMultiply(f.data(), scaled_gradient.data());
482
gradient = scaled_gradient.array() / scale.array();
483
gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();
485
// Check gradient based convergence.
486
VLOG(2) << "Gradient max norm: " << gradient_max_norm
487
<< " tolerance: " << gradient_tolerance
488
<< " ratio: " << gradient_max_norm / gradient_max_norm_0
489
<< " tolerance: " << options.gradient_tolerance;
490
if (gradient_max_norm <= gradient_tolerance) {
491
summary->termination_type = GRADIENT_TOLERANCE;
492
VLOG(1) << "Terminating on GRADIENT_TOLERANCE. "
493
<< "Relative gradient max norm: "
494
<< gradient_max_norm / gradient_max_norm_0
495
<< " <= " << options.gradient_tolerance
500
mu = mu * max(1.0 / 3.0, 1 - pow(2 * relative_decrease - 1, 3));
501
mu = std::max(options.min_mu, mu);
503
step_is_successful = true;
508
++num_consecutive_insane_steps;
511
if (num_consecutive_insane_steps == kMaxLinearSolverRetries) {
512
summary->termination_type = NUMERICAL_FAILURE;
513
VLOG(1) << "Too many consecutive retries; ending with numerical fail.";
515
if (!options.crash_and_dump_lsqp_on_failure) {
519
// Dump debugging information to disk.
520
CHECK(options.lsqp_dump_format_type == TEXTFILE ||
521
options.lsqp_dump_format_type == PROTOBUF)
522
<< "Dumping the linear least squares problem on crash "
523
<< "requires Solver::Options::lsqp_dump_format_type to be "
524
<< "PROTOBUF or TEXTFILE.";
526
if (DumpLinearLeastSquaresProblem(options.lsqp_dump_directory,
528
options.lsqp_dump_format_type,
533
options.num_eliminate_blocks)) {
534
LOG(FATAL) << "Linear least squares problem saved to: "
535
<< options.lsqp_dump_directory
536
<< ". Please provide this to the Ceres developers for "
537
<< " debugging along with the v=2 log.";
539
LOG(FATAL) << "Tried writing linear least squares problem: "
540
<< options.lsqp_dump_directory
545
if (!step_is_successful) {
546
// Either the step did not lead to a decrease in cost or there
547
// was numerical failure. In either case we will scale mu up and
548
// retry. If it was a numerical failure, we hope that the
549
// stronger regularization will make the linear system better
550
// conditioned. If it was numerically sane, but there was no
551
// decrease in cost, then increasing mu reduces the size of the
552
// trust region and we look for a decrease closer to the
553
// linearization point.
554
++summary->num_unsuccessful_steps;
561
total_cost = summary->fixed_cost + cost;
563
iteration_summary.iteration = iteration;
564
iteration_summary.step_is_successful = step_is_successful;
565
iteration_summary.cost = total_cost;
566
iteration_summary.cost_change = actual_cost_change;
567
iteration_summary.gradient_max_norm = gradient_max_norm;
568
iteration_summary.step_norm = step_norm;
569
iteration_summary.relative_decrease = relative_decrease;
570
iteration_summary.mu = mu;
571
iteration_summary.eta = options.eta;
572
iteration_summary.iteration_time_sec = (time(NULL) - iteration_start_time);
574
if (options.logging_type >= PER_MINIMIZER_ITERATION) {
575
summary->iterations.push_back(iteration_summary);
578
// Call the various callbacks.
579
for (int i = 0; i < options.callbacks.size(); ++i) {
580
if (!RunCallback(options.callbacks[i], iteration_summary, summary)) {
587
} // namespace internal
added
removed
internal/ceres/levenberg_marquardt.cc
