GTSAM  4.0.2
C++ library for smoothing and mapping (SAM)
Public Types | Protected Member Functions | Protected Attributes | List of all members
gtsam::LevenbergMarquardtOptimizer Class Reference

#include <LevenbergMarquardtOptimizer.h>

Inheritance diagram for gtsam::LevenbergMarquardtOptimizer:
Inheritance graph
[legend]
Collaboration diagram for gtsam::LevenbergMarquardtOptimizer:
Collaboration graph
[legend]

Public Types

typedef std::shared_ptr< LevenbergMarquardtOptimizershared_ptr
 

Public Member Functions

Constructors/Destructor
 LevenbergMarquardtOptimizer (const NonlinearFactorGraph &graph, const Values &initialValues, const LevenbergMarquardtParams &params=LevenbergMarquardtParams())
 
 LevenbergMarquardtOptimizer (const NonlinearFactorGraph &graph, const Values &initialValues, const Ordering &ordering, const LevenbergMarquardtParams &params=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 LevenbergMarquardtParamsparams () 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 Valuesoptimize ()
 
const ValuesoptimizeSafely ()
 
double error () const
 return error in current optimizer state
 
size_t iterations () const
 return number of iterations in current optimizer state
 
const Valuesvalues () const
 return values in current optimizer state
 
const NonlinearFactorGraphgraph () const
 return the graph with nonlinear factors
 
Advanced interface
virtual VectorValues solve (const GaussianFactorGraph &gfg, const NonlinearOptimizerParams &params) 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::NonlinearOptimizerStatestate_
 PIMPL'd state.
 

Detailed Description

This class performs Levenberg-Marquardt nonlinear optimization

Constructor & Destructor Documentation

◆ LevenbergMarquardtOptimizer() [1/2]

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.

Parameters
graphThe nonlinear factor graph to optimize
initialValuesThe initial variable assignments
paramsThe optimization parameters

◆ LevenbergMarquardtOptimizer() [2/2]

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.

Parameters
graphThe nonlinear factor graph to optimize
initialValuesThe initial variable assignments

◆ ~LevenbergMarquardtOptimizer()

gtsam::LevenbergMarquardtOptimizer::~LevenbergMarquardtOptimizer ( )
inlineoverride

Virtual destructor

Member Function Documentation

◆ _params()

const NonlinearOptimizerParams& gtsam::LevenbergMarquardtOptimizer::_params ( ) const
inlineoverrideprotectedvirtual

Access the parameters (base class version)

Implements gtsam::NonlinearOptimizer.

◆ buildDampedSystem()

GaussianFactorGraph gtsam::LevenbergMarquardtOptimizer::buildDampedSystem ( const GaussianFactorGraph linear,
const VectorValues sqrtHessianDiagonal 
) const

Build a damped system for a specific lambda – for testing only

◆ defaultOptimize()

void gtsam::NonlinearOptimizer::defaultOptimize ( )
protectedinherited

A default implementation of the optimization loop, which calls iterate() until checkConvergence returns true.

◆ iterate()

GaussianFactorGraph::shared_ptr gtsam::LevenbergMarquardtOptimizer::iterate ( )
overridevirtual

Perform a single iteration, returning GaussianFactorGraph corresponding to the linearized factor graph.

Implements gtsam::NonlinearOptimizer.

◆ linearize()

virtual GaussianFactorGraph::shared_ptr gtsam::LevenbergMarquardtOptimizer::linearize ( ) const
virtual

linearize, can be overwritten

◆ optimize()

virtual const Values& gtsam::NonlinearOptimizer::optimize ( )
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.

◆ optimizeSafely()

const Values& gtsam::NonlinearOptimizer::optimizeSafely ( )
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

Parameters
optimizera non-linear optimizer

◆ params()

const LevenbergMarquardtParams& gtsam::LevenbergMarquardtOptimizer::params ( ) const
inline

Read-only access the parameters

◆ solve()

virtual VectorValues gtsam::NonlinearOptimizer::solve ( const GaussianFactorGraph gfg,
const NonlinearOptimizerParams params 
) const
virtualinherited

Default function to do linear solve, i.e. optimize a GaussianFactorGraph

◆ tryLambda()

bool gtsam::LevenbergMarquardtOptimizer::tryLambda ( const GaussianFactorGraph linear,
const VectorValues sqrtHessianDiagonal 
)

Inner loop, changes state, returns true if successful or giving up


The documentation for this class was generated from the following file: