GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
#include <LevenbergMarquardtOptimizer.h>
Public Types | |
typedef std::shared_ptr< LevenbergMarquardtOptimizer > | shared_ptr |
Public Member Functions | |
Constructors/Destructor | |
LevenbergMarquardtOptimizer (const NonlinearFactorGraph &graph, const Values &initialValues, const LevenbergMarquardtParams ¶ms=LevenbergMarquardtParams()) | |
LevenbergMarquardtOptimizer (const NonlinearFactorGraph &graph, const Values &initialValues, const Ordering &ordering, const LevenbergMarquardtParams ¶ms=LevenbergMarquardtParams()) | |
~LevenbergMarquardtOptimizer () override | |
Standard interface | |
double | lambda () const |
Access the current damping value. | |
int | getInnerIterations () const |
Access the current number of inner iterations. | |
void | print (const std::string &str="") const |
print | |
Advanced interface | |
GaussianFactorGraph::shared_ptr | iterate () override |
const LevenbergMarquardtParams & | params () const |
void | writeLogFile (double currentError) |
virtual GaussianFactorGraph::shared_ptr | linearize () const |
GaussianFactorGraph | buildDampedSystem (const GaussianFactorGraph &linear, const VectorValues &sqrtHessianDiagonal) const |
bool | tryLambda (const GaussianFactorGraph &linear, const VectorValues &sqrtHessianDiagonal) |
Standard interface | |
virtual const Values & | optimize () |
const Values & | optimizeSafely () |
double | error () const |
return error in current optimizer state | |
size_t | iterations () const |
return number of iterations in current optimizer state | |
const Values & | values () const |
return values in current optimizer state | |
const NonlinearFactorGraph & | graph () const |
return the graph with nonlinear factors | |
Advanced interface | |
virtual VectorValues | solve (const GaussianFactorGraph &gfg, const NonlinearOptimizerParams ¶ms) const |
Protected Member Functions | |
void | initTime () |
const NonlinearOptimizerParams & | _params () const override |
void | defaultOptimize () |
Protected Attributes | |
const LevenbergMarquardtParams | params_ |
LM parameters. | |
std::chrono::time_point< std::chrono::high_resolution_clock > | startTime_ |
time when optimization started | |
NonlinearFactorGraph | graph_ |
The graph with nonlinear factors. | |
std::unique_ptr< internal::NonlinearOptimizerState > | state_ |
PIMPL'd state. | |
This class performs Levenberg-Marquardt nonlinear optimization
gtsam::LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer | ( | const NonlinearFactorGraph & | graph, |
const Values & | initialValues, | ||
const LevenbergMarquardtParams & | params = LevenbergMarquardtParams() |
||
) |
Standard constructor, requires a nonlinear factor graph, initial variable assignments, and optimization parameters. For convenience this version takes plain objects instead of shared pointers, but internally copies the objects.
graph | The nonlinear factor graph to optimize |
initialValues | The initial variable assignments |
params | The optimization parameters |
gtsam::LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer | ( | const NonlinearFactorGraph & | graph, |
const Values & | initialValues, | ||
const Ordering & | ordering, | ||
const LevenbergMarquardtParams & | params = LevenbergMarquardtParams() |
||
) |
Standard constructor, requires a nonlinear factor graph, initial variable assignments, and optimization parameters. For convenience this version takes plain objects instead of shared pointers, but internally copies the objects.
graph | The nonlinear factor graph to optimize |
initialValues | The initial variable assignments |
|
inlineoverride |
Virtual destructor
|
inlineoverrideprotectedvirtual |
Access the parameters (base class version)
Implements gtsam::NonlinearOptimizer.
GaussianFactorGraph gtsam::LevenbergMarquardtOptimizer::buildDampedSystem | ( | const GaussianFactorGraph & | linear, |
const VectorValues & | sqrtHessianDiagonal | ||
) | const |
Build a damped system for a specific lambda – for testing only
|
protectedinherited |
A default implementation of the optimization loop, which calls iterate() until checkConvergence returns true.
|
overridevirtual |
Perform a single iteration, returning GaussianFactorGraph corresponding to the linearized factor graph.
Implements gtsam::NonlinearOptimizer.
|
virtual |
linearize, can be overwritten
|
inlinevirtualinherited |
Optimize for the maximum-likelihood estimate, returning a the optimized variable assignments.
This function simply calls iterate() in a loop, checking for convergence with check_convergence(). For fine-grain control over the optimization process, you may call iterate() and check_convergence() yourself, and if needed modify the optimization state between iterations.
Reimplemented in gtsam::NonlinearConjugateGradientOptimizer.
|
inherited |
Optimize, but return empty result if any uncaught exception is thrown Intended for MATLAB. In C++, use above and catch exceptions. No message is printed: it is up to the caller to check the result
optimizer | a non-linear optimizer |
|
inline |
Read-only access the parameters
|
virtualinherited |
Default function to do linear solve, i.e. optimize a GaussianFactorGraph
bool gtsam::LevenbergMarquardtOptimizer::tryLambda | ( | const GaussianFactorGraph & | linear, |
const VectorValues & | sqrtHessianDiagonal | ||
) |
Inner loop, changes state, returns true if successful or giving up