#include <LevenbergMarquardtOptimizer.h>
Public Types | |
typedef std::shared_ptr< LevenbergMarquardtOptimizer > | shared_ptr |
Public Types inherited from gtsam::NonlinearOptimizer | |
using | shared_ptr = std::shared_ptr< const NonlinearOptimizer > |
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. More... | |
int | getInnerIterations () const |
Access the current number of inner iterations. More... | |
void | print (const std::string &str="") const |
print More... | |
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) |
Public Member Functions inherited from gtsam::NonlinearOptimizer | |
virtual const Values & | optimize () |
const Values & | optimizeSafely () |
double | error () const |
return error in current optimizer state More... | |
size_t | iterations () const |
return number of iterations in current optimizer state More... | |
const Values & | values () const |
return values in current optimizer state More... | |
const NonlinearFactorGraph & | graph () const |
return the graph with nonlinear factors More... | |
virtual | ~NonlinearOptimizer () |
virtual VectorValues | solve (const GaussianFactorGraph &gfg, const NonlinearOptimizerParams ¶ms) const |
Protected Member Functions | |
const NonlinearOptimizerParams & | _params () const override |
void | initTime () |
Protected Member Functions inherited from gtsam::NonlinearOptimizer | |
void | defaultOptimize () |
NonlinearOptimizer (const NonlinearFactorGraph &graph, std::unique_ptr< internal::NonlinearOptimizerState > state) | |
Protected Attributes | |
const LevenbergMarquardtParams | params_ |
LM parameters. More... | |
std::chrono::time_point< std::chrono::high_resolution_clock > | startTime_ |
time when optimization started More... | |
Protected Attributes inherited from gtsam::NonlinearOptimizer | |
NonlinearFactorGraph | graph_ |
The graph with nonlinear factors. More... | |
std::unique_ptr< internal::NonlinearOptimizerState > | state_ |
PIMPL'd state. More... | |
This class performs Levenberg-Marquardt nonlinear optimization
Definition at line 35 of file LevenbergMarquardtOptimizer.h.
typedef std::shared_ptr<LevenbergMarquardtOptimizer> gtsam::LevenbergMarquardtOptimizer::shared_ptr |
Definition at line 46 of file LevenbergMarquardtOptimizer.h.
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 |
Definition at line 47 of file LevenbergMarquardtOptimizer.cpp.
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 |
Definition at line 55 of file LevenbergMarquardtOptimizer.cpp.
|
inlineoverride |
Virtual destructor
Definition at line 74 of file LevenbergMarquardtOptimizer.h.
|
inlineoverrideprotectedvirtual |
Access the parameters (base class version)
Implements gtsam::NonlinearOptimizer.
Definition at line 127 of file LevenbergMarquardtOptimizer.h.
GaussianFactorGraph gtsam::LevenbergMarquardtOptimizer::buildDampedSystem | ( | const GaussianFactorGraph & | linear, |
const VectorValues & | sqrtHessianDiagonal | ||
) | const |
Build a damped system for a specific lambda – for testing only
Definition at line 88 of file LevenbergMarquardtOptimizer.cpp.
int gtsam::LevenbergMarquardtOptimizer::getInnerIterations | ( | ) | const |
Access the current number of inner iterations.
Definition at line 77 of file LevenbergMarquardtOptimizer.cpp.
|
protected |
Definition at line 65 of file LevenbergMarquardtOptimizer.cpp.
|
overridevirtual |
Perform a single iteration, returning GaussianFactorGraph corresponding to the linearized factor graph.
Implements gtsam::NonlinearOptimizer.
Definition at line 273 of file LevenbergMarquardtOptimizer.cpp.
double gtsam::LevenbergMarquardtOptimizer::lambda | ( | ) | const |
Access the current damping value.
Definition at line 71 of file LevenbergMarquardtOptimizer.cpp.
|
virtual |
linearize, can be overwritten
Definition at line 83 of file LevenbergMarquardtOptimizer.cpp.
|
inline |
Read-only access the parameters
Definition at line 106 of file LevenbergMarquardtOptimizer.h.
|
inline |
Definition at line 89 of file LevenbergMarquardtOptimizer.h.
bool gtsam::LevenbergMarquardtOptimizer::tryLambda | ( | const GaussianFactorGraph & | linear, |
const VectorValues & | sqrtHessianDiagonal | ||
) |
Inner loop, changes state, returns true if successful or giving up
Definition at line 121 of file LevenbergMarquardtOptimizer.cpp.
|
inline |
Definition at line 104 of file LevenbergMarquardtOptimizer.cpp.
|
protected |
LM parameters.
Definition at line 38 of file LevenbergMarquardtOptimizer.h.
|
protected |
time when optimization started
Definition at line 41 of file LevenbergMarquardtOptimizer.h.