LevenbergMarquardtOptimizer.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
24 #include <gtsam/nonlinear/Values.h>
28 #include <gtsam/base/Vector.h>
29 #ifdef GTSAM_USE_BOOST_FEATURES
30 #include <gtsam/base/timing.h>
31 #endif
32 
33 #include <cmath>
34 #include <fstream>
35 #include <iostream>
36 #include <iomanip>
37 #include <limits>
38 #include <string>
39 
40 using namespace std;
41 
42 namespace gtsam {
43 
44 typedef internal::LevenbergMarquardtState State;
45 
46 /* ************************************************************************* */
47 LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph,
48  const Values& initialValues,
51  graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues),
52  params.lambdaInitial, params.lambdaFactor))),
53  params_(LevenbergMarquardtParams::EnsureHasOrdering(params, graph)) {}
54 
56  const Values& initialValues,
57  const Ordering& ordering,
60  graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues),
61  params.lambdaInitial, params.lambdaFactor))),
62  params_(LevenbergMarquardtParams::ReplaceOrdering(params, ordering)) {}
63 
64 /* ************************************************************************* */
66  // use chrono to measure time in microseconds
67  startTime_ = std::chrono::high_resolution_clock::now();
68 }
69 
70 /* ************************************************************************* */
72  auto currentState = static_cast<const State*>(state_.get());
73  return currentState->lambda;
74 }
75 
76 /* ************************************************************************* */
78  auto currentState = static_cast<const State*>(state_.get());
79  return currentState->totalNumberInnerIterations;
80 }
81 
82 /* ************************************************************************* */
84  return graph_.linearize(state_->values);
85 }
86 
87 /* ************************************************************************* */
89  const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal) const {
90  gttic(damp);
91  auto currentState = static_cast<const State*>(state_.get());
92 
94  std::cout << "building damped system with lambda " << currentState->lambda << std::endl;
95 
97  return currentState->buildDampedSystem(linear, sqrtHessianDiagonal);
98  else
99  return currentState->buildDampedSystem(linear);
100 }
101 
102 /* ************************************************************************* */
103 // Log current error/lambda to file
104 inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){
105  auto currentState = static_cast<const State*>(state_.get());
106 
107  if (!params_.logFile.empty()) {
108  ofstream os(params_.logFile.c_str(), ios::app);
109  // use chrono to measure time in microseconds
110  auto currentTime = std::chrono::high_resolution_clock::now();
111  // Get the time spent in seconds and print it
112  double timeSpent = std::chrono::duration_cast<std::chrono::microseconds>(currentTime - startTime_).count() / 1e6;
113  os << /*inner iterations*/ currentState->totalNumberInnerIterations << ","
114  << timeSpent << ","
115  << /*current error*/ currentError << "," << currentState->lambda << ","
116  << /*outer iterations*/ currentState->iterations << endl;
117  }
118 }
119 
120 /* ************************************************************************* */
122  const VectorValues& sqrtHessianDiagonal) {
123  auto currentState = static_cast<const State*>(state_.get());
125 
126 #ifdef GTSAM_USE_BOOST_FEATURES
127 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
128  boost::timer::cpu_timer lamda_iteration_timer;
129  lamda_iteration_timer.start();
130 #else
131  boost::timer lamda_iteration_timer;
132  lamda_iteration_timer.restart();
133 #endif
134 #else
135  auto start = std::chrono::high_resolution_clock::now();
136 #endif
137 
138  if (verbose)
139  cout << "trying lambda = " << currentState->lambda << endl;
140 
141  // Build damped system for this lambda (adds prior factors that make it like gradient descent)
142  auto dampedSystem = buildDampedSystem(linear, sqrtHessianDiagonal);
143 
144  // Try solving
145  double modelFidelity = 0.0;
146  bool step_is_successful = false;
147  bool stopSearchingLambda = false;
148  double newError = numeric_limits<double>::infinity();
149  double costChange = 0.0;
150  Values newValues;
152 
153  bool systemSolvedSuccessfully;
154  try {
155  // ============ Solve is where most computation happens !! =================
156  delta = solve(dampedSystem, params_);
157  systemSolvedSuccessfully = true;
158  } catch (const IndeterminantLinearSystemException&) {
159  systemSolvedSuccessfully = false;
160  }
161 
162  if (systemSolvedSuccessfully) {
163  if (verbose)
164  cout << "linear delta norm = " << delta.norm() << endl;
166  delta.print("delta");
167 
168  // Compute the old linearized error as it is not the same
169  // as the nonlinear error when robust noise models are used.
170  double oldLinearizedError = linear.error(VectorValues::Zero(delta));
171  double newlinearizedError = linear.error(delta);
172 
173  // cost change in the linearized system (old - new)
174  double linearizedCostChange = oldLinearizedError - newlinearizedError;
175  if (verbose)
176  cout << "newlinearizedError = " << newlinearizedError
177  << " linearizedCostChange = " << linearizedCostChange << endl;
178 
179  if (linearizedCostChange >= 0) { // step is valid
180  // update values
181  gttic(retract);
182  // ============ This is where the solution is updated ====================
183  newValues = currentState->values.retract(delta);
184  // =======================================================================
185  gttoc(retract);
186 
187  // compute new error
188  gttic(compute_error);
189  if (verbose)
190  cout << "calculating error:" << endl;
191  newError = graph_.error(newValues);
192  gttoc(compute_error);
193 
194  if (verbose)
195  cout << "old error (" << currentState->error << ") new (tentative) error (" << newError
196  << ")" << endl;
197 
198  // cost change in the original, nonlinear system (old - new)
199  costChange = currentState->error - newError;
200 
201  if (linearizedCostChange > std::numeric_limits<double>::epsilon() * oldLinearizedError) {
202  // the (linear) error has to decrease to satisfy this condition
203  // fidelity of linearized model VS original system between
204  modelFidelity = costChange / linearizedCostChange;
205  // if we decrease the error in the nonlinear system and modelFidelity is above threshold
206  step_is_successful = modelFidelity > params_.minModelFidelity;
207  if (verbose)
208  cout << "modelFidelity: " << modelFidelity << endl;
209  } // else we consider the step non successful and we either increase lambda or stop if error
210  // change is small
211 
212  double minAbsoluteTolerance = params_.relativeErrorTol * currentState->error;
213  // if the change is small we terminate
214  if (std::abs(costChange) < minAbsoluteTolerance) {
215  if (verbose)
216  cout << "abs(costChange)=" << std::abs(costChange)
217  << " minAbsoluteTolerance=" << minAbsoluteTolerance
218  << " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl;
219  stopSearchingLambda = true;
220  }
221  }
222  } // if (systemSolvedSuccessfully)
223 
225 #ifdef GTSAM_USE_BOOST_FEATURES
226 // do timing
227 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
228  double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall;
229 #else
230  double iterationTime = lamda_iteration_timer.elapsed();
231 #endif
232 #else
233  auto end = std::chrono::high_resolution_clock::now();
234  double iterationTime = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / 1e6;
235 #endif
236  if (currentState->iterations == 0) {
237  cout << "iter cost cost_change lambda success iter_time" << endl;
238  }
239  cout << setw(4) << currentState->iterations << " " << setw(12) << newError << " " << setw(12) << setprecision(2)
240  << costChange << " " << setw(10) << setprecision(2) << currentState->lambda << " " << setw(6)
241  << systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << iterationTime << endl;
242  }
243  if (step_is_successful) {
244  // we have successfully decreased the cost and we have good modelFidelity
245  // NOTE(frank): As we return immediately after this, we move the newValues
246  // TODO(frank): make Values actually support move. Does not seem to happen now.
247  state_ = currentState->decreaseLambda(params_, modelFidelity, std::move(newValues), newError);
248  return true;
249  } else if (!stopSearchingLambda) { // we failed to solved the system or had no decrease in cost
250  if (verbose)
251  cout << "increasing lambda" << endl;
252  State* modifiedState = static_cast<State*>(state_.get());
253  modifiedState->increaseLambda(params_); // TODO(frank): make this functional with Values move
254 
255  // check if lambda is too big
256  if (modifiedState->lambda >= params_.lambdaUpperBound) {
259  cout << "Warning: Levenberg-Marquardt giving up because "
260  "cannot decrease error with maximum lambda" << endl;
261  return true;
262  } else {
263  return false; // only case where we will keep trying
264  }
265  } else { // the change in the cost is very small and it is not worth trying bigger lambdas
266  if (verbose)
267  cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl;
268  return true;
269  }
270 }
271 
272 /* ************************************************************************* */
274  auto currentState = static_cast<const State*>(state_.get());
275 
276  gttic(LM_iterate);
277 
278  // Linearize graph
280  cout << "linearizing = " << endl;
282 
283  if(currentState->totalNumberInnerIterations==0) { // write initial error
284  writeLogFile(currentState->error);
285 
287  cout << "Initial error: " << currentState->error
288  << ", values: " << currentState->values.size() << std::endl;
289  }
290  }
291 
292  // Only calculate diagonal of Hessian (expensive) once per outer iteration, if we need it
293  VectorValues sqrtHessianDiagonal;
294  if (params_.diagonalDamping) {
295  sqrtHessianDiagonal = linear->hessianDiagonal();
296  for (auto& [key, value] : sqrtHessianDiagonal) {
297  value = value.cwiseMax(params_.minDiagonal).cwiseMin(params_.maxDiagonal).cwiseSqrt();
298  }
299  }
300 
301  // Keep increasing lambda until we make make progress
302  while (!tryLambda(*linear, sqrtHessianDiagonal)) {
303  auto newState = static_cast<const State*>(state_.get());
304  writeLogFile(newState->error);
305  }
306 
307  return linear;
308 }
309 
310 } /* namespace gtsam */
311 
gttoc
#define gttoc(label)
Definition: timing.h:296
timing.h
Timing utilities.
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
Vector.h
typedef and functions to augment Eigen's VectorXd
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::NonlinearOptimizer::solve
virtual VectorValues solve(const GaussianFactorGraph &gfg, const NonlinearOptimizerParams &params) const
Definition: NonlinearOptimizer.cpp:132
gtsam::LevenbergMarquardtOptimizer::iterate
GaussianFactorGraph::shared_ptr iterate() override
Definition: LevenbergMarquardtOptimizer.cpp:273
timer
static BenchTimer timer
Definition: benchmark-blocking-sizes.cpp:31
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::internal::DoglegState
Definition: DoglegOptimizer.cpp:54
gtsam::LevenbergMarquardtParams::minDiagonal
double minDiagonal
when using diagonal damping saturates the minimum diagonal entries (default: 1e-6)
Definition: LevenbergMarquardtParams.h:58
gtsam::LevenbergMarquardtOptimizer::lambda
double lambda() const
Access the current damping value.
Definition: LevenbergMarquardtOptimizer.cpp:71
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::NonlinearOptimizerParams::verbosity
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Definition: NonlinearOptimizerParams.h:46
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
Ordering.h
Variable ordering for the elimination algorithm.
os
ofstream os("timeSchurFactors.csv")
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::LevenbergMarquardtParams::diagonalDamping
bool diagonalDamping
if true, use diagonal of Hessian
Definition: LevenbergMarquardtParams.h:56
gtsam::LevenbergMarquardtParams::TRYDELTA
@ TRYDELTA
Definition: LevenbergMarquardtParams.h:40
LevenbergMarquardtState.h
A LevenbergMarquardtState class containing most of the logic for Levenberg-Marquardt.
gtsam::LevenbergMarquardtOptimizer::startTime_
std::chrono::time_point< std::chrono::high_resolution_clock > startTime_
time when optimization started
Definition: LevenbergMarquardtOptimizer.h:41
gtsam::LevenbergMarquardtOptimizer::params_
const LevenbergMarquardtParams params_
LM parameters.
Definition: LevenbergMarquardtOptimizer.h:38
epsilon
static double epsilon
Definition: testRot3.cpp:37
linearExceptions.h
Exceptions that may be thrown by linear solver components.
gtsam::LevenbergMarquardtParams::logFile
std::string logFile
an optional CSV log file, with [iteration, time, error, lambda]
Definition: LevenbergMarquardtParams.h:55
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::LevenbergMarquardtOptimizer::linearize
virtual GaussianFactorGraph::shared_ptr linearize() const
Definition: LevenbergMarquardtOptimizer.cpp:83
gtsam::LevenbergMarquardtOptimizer::getInnerIterations
int getInnerIterations() const
Access the current number of inner iterations.
Definition: LevenbergMarquardtOptimizer.cpp:77
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::State
internal::DoglegState State
Definition: DoglegOptimizer.cpp:62
gtsam::LevenbergMarquardtOptimizer::tryLambda
bool tryLambda(const GaussianFactorGraph &linear, const VectorValues &sqrtHessianDiagonal)
Definition: LevenbergMarquardtOptimizer.cpp:121
gtsam::LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer
LevenbergMarquardtOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const LevenbergMarquardtParams &params=LevenbergMarquardtParams())
Definition: LevenbergMarquardtOptimizer.cpp:47
gtsam::LevenbergMarquardtParams::TRYLAMBDA
@ TRYLAMBDA
Definition: LevenbergMarquardtParams.h:40
gtsam::VectorValues::Zero
static VectorValues Zero(const VectorValues &other)
Definition: VectorValues.cpp:75
gtsam::LevenbergMarquardtParams::lambdaUpperBound
double lambdaUpperBound
The maximum lambda to try before assuming the optimization has failed (default: 1e5)
Definition: LevenbergMarquardtParams.h:51
gtsam::NonlinearOptimizerParams::TERMINATION
@ TERMINATION
Definition: NonlinearOptimizerParams.h:39
gtsam::LevenbergMarquardtParams::minModelFidelity
double minModelFidelity
Lower bound for the modelFidelity to accept the result of an LM iteration.
Definition: LevenbergMarquardtParams.h:54
gtsam::NonlinearOptimizer::graph_
NonlinearFactorGraph graph_
The graph with nonlinear factors.
Definition: NonlinearOptimizer.h:78
ordering
static enum @1096 ordering
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: SFMdata.h:40
gtsam::NonlinearOptimizer
Definition: NonlinearOptimizer.h:75
error
static double error
Definition: testRot3.cpp:37
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
Definition: GaussianFactorGraph.cpp:71
std
Definition: BFloat16.h:88
gtsam::LevenbergMarquardtParams::SUMMARY
@ SUMMARY
Definition: LevenbergMarquardtParams.h:40
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
Definition: NonlinearOptimizerParams.h:43
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::LevenbergMarquardtOptimizer::buildDampedSystem
GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph &linear, const VectorValues &sqrtHessianDiagonal) const
Definition: LevenbergMarquardtOptimizer.cpp:88
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::IndeterminantLinearSystemException
Definition: linearExceptions.h:94
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::LevenbergMarquardtParams::verbosityLM
VerbosityLM verbosityLM
The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::ver...
Definition: LevenbergMarquardtParams.h:53
gtsam::NonlinearOptimizer::state_
std::unique_ptr< internal::NonlinearOptimizerState > state_
PIMPL'd state.
Definition: NonlinearOptimizer.h:80
gtsam::LevenbergMarquardtParams::maxDiagonal
double maxDiagonal
when using diagonal damping saturates the maximum diagonal entries (default: 1e32)
Definition: LevenbergMarquardtParams.h:59
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::LevenbergMarquardtOptimizer::initTime
void initTime()
Definition: LevenbergMarquardtOptimizer.cpp:65
test_callbacks.value
value
Definition: test_callbacks.py:160
Values.h
A non-templated config holding any types of Manifold-group elements.
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::LevenbergMarquardtParams::DAMPED
@ DAMPED
Definition: LevenbergMarquardtParams.h:40
gtsam::LevenbergMarquardtOptimizer::writeLogFile
void writeLogFile(double currentError)
Definition: LevenbergMarquardtOptimizer.cpp:104


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:07