Levenberg-Marquardt Solver (Dense matrices version). More...
#include <levenberg_marquardt_dense.h>
Public Types | |
using | Ptr = std::shared_ptr< LevenbergMarquardtDense > |
using | UPtr = std::unique_ptr< LevenbergMarquardtDense > |
![]() | |
using | Ptr = std::shared_ptr< NlpSolverInterface > |
using | UPtr = std::unique_ptr< NlpSolverInterface > |
Public Member Functions | |
void | clear () override |
Clear internal caches. More... | |
NlpSolverInterface::Ptr | getInstance () const override |
Return a newly created instance of the current solver. More... | |
bool | initialize (OptimizationProblemInterface *problem=nullptr) override |
Initialize the solver w.r.t. a given optimization problem. More... | |
bool | isLsqSolver () const override |
Return true if the solver onyl supports costs in lsq form. More... | |
void | setIterations (int iterations) |
Define the number of solver iterations. More... | |
void | setPenaltyWeights (double weight_eq, double weight_ineq, double weight_bounds) |
Define penalty weights (equality constraints, inequality constraints, bounds) More... | |
void | setWeightAdapation (double factor_eq, double factor_ineq, double factor_bounds, double max_eq, double max_ineq, double max_bounds) |
Set parameters for weight adaptation (refer to the class description); set factors to 1 in order to disable adaptation. More... | |
SolverStatus | solve (OptimizationProblemInterface &problem, bool new_structure=true, bool new_run=true, double *obj_value=nullptr) override |
Solve the provided optimization problem. More... | |
![]() | |
virtual | ~NlpSolverInterface () |
Virtual destructor. More... | |
Protected Member Functions | |
void | adaptWeights () |
Perform single weight adapation step. More... | |
void | computeJacobian (OptimizationProblemInterface &problem) |
Compute overall jacobian including constraint approximation. More... | |
void | computeValues (OptimizationProblemInterface &problem) |
Compute overall value vector including constraint approximation. More... | |
void | resetWeights () |
Reset weights to their original values. More... | |
Private Attributes | |
Eigen::VectorXd | _delta |
int | _eq_dim = 0 |
int | _finite_bounds_dim = 0 |
Eigen::MatrixXd | _hessian |
int | _ineq_dim = 0 |
int | _iterations = 10 |
Eigen::MatrixXd | _jacobian |
int | _obj_dim = 0 |
int | _param_dim = 0 |
Eigen::VectorXd | _rhs |
int | _val_dim = 0 |
Eigen::VectorXd | _values |
double | _weight_adapt_factor_bounds = 1 |
double | _weight_adapt_factor_eq = 1 |
double | _weight_adapt_factor_ineq = 1 |
double | _weight_adapt_max_bounds = 500 |
double | _weight_adapt_max_eq = 500 |
double | _weight_adapt_max_ineq = 500 |
double | _weight_bounds = _weight_init_bounds |
double | _weight_eq = _weight_init_eq |
double | _weight_ineq = _weight_init_ineq |
double | _weight_init_bounds = 2 |
double | _weight_init_eq = 2 |
double | _weight_init_ineq = 2 |
Levenberg-Marquardt Solver (Dense matrices version).
This solver implements the Levenberg-Marquardt algorithm also implemented in [1]. The underlying linear system is solved using Eigens Dense Matrix algebra (http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html).
The solver requires the optimization problem to be defined in Least-Squares form. Bounds, inequality and equality constraints are transformed to objective functions with quadratic penalties. For this purpose scalar weights for all 3 approximations are defined in order to configure their influence in the overall objective. Theoretically, the weights should be infinity but this would cause an ill-conditioned Hessian matrix and consequently slow convergence. This solver implementation also provides a simple weight adapation strategy which starts with user-defined weight values (setPenaltyWeights()). In each invocation of the solve() method the weights are scaled by additional factors (see setWeightAdapation()) up to specified maximum values. Hereby, it is weight_new = weight_old * factor. The weights are resetted to their original values in case solve() is called with new_run
== true.
[1] R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, W. Burgard. "g2o: A General Framework for Graph Optimization", ICRA, 2011.
Definition at line 62 of file levenberg_marquardt_dense.h.
using corbo::LevenbergMarquardtDense::Ptr = std::shared_ptr<LevenbergMarquardtDense> |
Definition at line 65 of file levenberg_marquardt_dense.h.
using corbo::LevenbergMarquardtDense::UPtr = std::unique_ptr<LevenbergMarquardtDense> |
Definition at line 66 of file levenberg_marquardt_dense.h.
|
protected |
Perform single weight adapation step.
Definition at line 281 of file levenberg_marquardt_dense.cpp.
|
overridevirtual |
Clear internal caches.
Implements corbo::NlpSolverInterface.
Definition at line 293 of file levenberg_marquardt_dense.cpp.
|
protected |
Compute overall jacobian including constraint approximation.
Definition at line 226 of file levenberg_marquardt_dense.cpp.
|
protected |
Compute overall value vector including constraint approximation.
Definition at line 200 of file levenberg_marquardt_dense.cpp.
|
inlineoverridevirtual |
Return a newly created instance of the current solver.
Implements corbo::NlpSolverInterface.
Definition at line 69 of file levenberg_marquardt_dense.h.
|
overridevirtual |
Initialize the solver w.r.t. a given optimization problem.
The structure of the optimization problem might be still subject to change when calling this method. However, this method can be used to preallocate some memory and to check general features of the provided optimization problem, e.g. if Jacobian and Hessian matrices are available.
[in] | problem | Optimization problem definition with possibly incomplete structure [optional] |
Reimplemented from corbo::NlpSolverInterface.
Definition at line 33 of file levenberg_marquardt_dense.cpp.
|
inlineoverridevirtual |
Return true if the solver onyl supports costs in lsq form.
Implements corbo::NlpSolverInterface.
Definition at line 72 of file levenberg_marquardt_dense.h.
|
protected |
Reset weights to their original values.
Definition at line 274 of file levenberg_marquardt_dense.cpp.
|
inline |
Define the number of solver iterations.
Definition at line 80 of file levenberg_marquardt_dense.h.
void corbo::LevenbergMarquardtDense::setPenaltyWeights | ( | double | weight_eq, |
double | weight_ineq, | ||
double | weight_bounds | ||
) |
Define penalty weights (equality constraints, inequality constraints, bounds)
Definition at line 252 of file levenberg_marquardt_dense.cpp.
void corbo::LevenbergMarquardtDense::setWeightAdapation | ( | double | factor_eq, |
double | factor_ineq, | ||
double | factor_bounds, | ||
double | max_eq, | ||
double | max_ineq, | ||
double | max_bounds | ||
) |
Set parameters for weight adaptation (refer to the class description); set factors to 1 in order to disable adaptation.
Definition at line 263 of file levenberg_marquardt_dense.cpp.
|
overridevirtual |
Solve the provided optimization problem.
[in,out] | problem | Optimization problem definition, pre-solving: object contains the initial parameter set post-solving: object contains the optimized parameter set |
[in] | new_structure | true if one of the dimensions or non-zero patterns has changed. |
[in] | new_run | true if a new optimization run (not just an iteration) is performed, e.g. to reset possible weight adaptation strategies etc. |
[out] | obj_value | Returns the resulting objective function value [optional] |
Implements corbo::NlpSolverInterface.
Definition at line 44 of file levenberg_marquardt_dense.cpp.
|
private |
Definition at line 134 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 126 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 128 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 133 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 127 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 109 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 132 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 125 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 124 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 135 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 129 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 131 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 117 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 115 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 116 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 121 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 119 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 120 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 139 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 137 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 138 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 113 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 111 of file levenberg_marquardt_dense.h.
|
private |
Definition at line 112 of file levenberg_marquardt_dense.h.