Levenberg-Marquardt Solver (Sparse matrices version). More...
#include <levenberg_marquardt_sparse.h>
Public Types | |
using | Ptr = std::shared_ptr< LevenbergMarquardtSparse > |
using | UPtr = std::unique_ptr< LevenbergMarquardtSparse > |
![]() | |
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 | 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::SparseMatrix< double > | _hessian |
int | _ineq_dim = 0 |
int | _iterations = 10 |
Eigen::SparseMatrix< double > | _jacobian |
int | _obj_dim = 0 |
int | _param_dim = 0 |
Eigen::VectorXd | _rhs |
Eigen::SimplicialLLT< Eigen::SparseMatrix< double >, Eigen::Upper > | _sparse_solver |
Eigens sparse solver wrapper. Check http://eigen.tuxfamily.org/dox/group__TopicSparseSystems.html for further information and different solvers. The second template parameter specifies, whether the upper or lower triangular part should be used. If CHOLMOD was found by CMake, the cholmod supernodal llt version is used rathar than Eigens simplicial ldlt. Define FORCE_EIGEN_SOLVER if the default LDLT should sill be used even if CHOLMOD is found. More... | |
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 (Sparse 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 68 of file levenberg_marquardt_sparse.h.
using corbo::LevenbergMarquardtSparse::Ptr = std::shared_ptr<LevenbergMarquardtSparse> |
Definition at line 71 of file levenberg_marquardt_sparse.h.
using corbo::LevenbergMarquardtSparse::UPtr = std::unique_ptr<LevenbergMarquardtSparse> |
Definition at line 72 of file levenberg_marquardt_sparse.h.
|
protected |
Perform single weight adapation step.
Definition at line 277 of file levenberg_marquardt_sparse.cpp.
|
overridevirtual |
Clear internal caches.
Implements corbo::NlpSolverInterface.
Definition at line 289 of file levenberg_marquardt_sparse.cpp.
|
protected |
Compute overall value vector including constraint approximation.
Definition at line 222 of file levenberg_marquardt_sparse.cpp.
|
inlineoverridevirtual |
Return a newly created instance of the current solver.
Implements corbo::NlpSolverInterface.
Definition at line 75 of file levenberg_marquardt_sparse.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_sparse.cpp.
|
inlineoverridevirtual |
Return true if the solver onyl supports costs in lsq form.
Implements corbo::NlpSolverInterface.
Definition at line 78 of file levenberg_marquardt_sparse.h.
|
protected |
Reset weights to their original values.
Definition at line 270 of file levenberg_marquardt_sparse.cpp.
|
inline |
Define the number of solver iterations.
Definition at line 86 of file levenberg_marquardt_sparse.h.
void corbo::LevenbergMarquardtSparse::setPenaltyWeights | ( | double | weight_eq, |
double | weight_ineq, | ||
double | weight_bounds | ||
) |
Define penalty weights (equality constraints, inequality constraints, bounds)
Definition at line 248 of file levenberg_marquardt_sparse.cpp.
void corbo::LevenbergMarquardtSparse::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 259 of file levenberg_marquardt_sparse.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_sparse.cpp.
|
private |
Definition at line 137 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 129 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 131 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 136 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 130 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 112 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 135 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 128 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 127 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 138 of file levenberg_marquardt_sparse.h.
|
private |
Eigens sparse solver wrapper. Check http://eigen.tuxfamily.org/dox/group__TopicSparseSystems.html for further information and different solvers. The second template parameter specifies, whether the upper or lower triangular part should be used. If CHOLMOD was found by CMake, the cholmod supernodal llt version is used rathar than Eigens simplicial ldlt. Define FORCE_EIGEN_SOLVER
if the default LDLT should sill be used even if CHOLMOD is found.
Definition at line 155 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 132 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 134 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 120 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 118 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 119 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 124 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 122 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 123 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 142 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 140 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 141 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 116 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 114 of file levenberg_marquardt_sparse.h.
|
private |
Definition at line 115 of file levenberg_marquardt_sparse.h.