Levenberg-Marquardt Solver (Sparse matrices version). More...

#include <levenberg_marquardt_sparse.h>

Inheritance diagram for corbo::LevenbergMarquardtSparse:

Public Types
using	Ptr = std::shared_ptr< LevenbergMarquardtSparse >

using	UPtr = std::unique_ptr< LevenbergMarquardtSparse >

Public Types inherited from corbo::NlpSolverInterface
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...

Public Member Functions inherited from corbo::NlpSolverInterface
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

Detailed Description

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.

Author: Christoph Rösmann (chris.nosp@m.toph.nosp@m..roes.nosp@m.mann.nosp@m.@tu-d.nosp@m.ortm.nosp@m.und.d.nosp@m.e)

Definition at line 68 of file levenberg_marquardt_sparse.h.

Member Typedef Documentation

◆ Ptr

using corbo::LevenbergMarquardtSparse::Ptr = std::shared_ptr<LevenbergMarquardtSparse>

Definition at line 71 of file levenberg_marquardt_sparse.h.

◆ UPtr

using corbo::LevenbergMarquardtSparse::UPtr = std::unique_ptr<LevenbergMarquardtSparse>

Definition at line 72 of file levenberg_marquardt_sparse.h.

Member Function Documentation

◆ adaptWeights()

void corbo::LevenbergMarquardtSparse::adaptWeights ( )

protected

Perform single weight adapation step.

Definition at line 277 of file levenberg_marquardt_sparse.cpp.

◆ clear()

void corbo::LevenbergMarquardtSparse::clear ( )

overridevirtual

Clear internal caches.

Implements corbo::NlpSolverInterface.

Definition at line 289 of file levenberg_marquardt_sparse.cpp.

◆ computeValues()

void corbo::LevenbergMarquardtSparse::computeValues ( OptimizationProblemInterface & problem )

protected

Compute overall value vector including constraint approximation.

Definition at line 222 of file levenberg_marquardt_sparse.cpp.

◆ getInstance()

NlpSolverInterface::Ptr corbo::LevenbergMarquardtSparse::getInstance ( ) const

inlineoverridevirtual

Return a newly created instance of the current solver.

Implements corbo::NlpSolverInterface.

Definition at line 75 of file levenberg_marquardt_sparse.h.

◆ initialize()

bool corbo::LevenbergMarquardtSparse::initialize ( OptimizationProblemInterface * problem = nullptr )

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.

Parameters

[in] problem Optimization problem definition with possibly incomplete structure [optional]

Returns: true if the sovler seems to be suited, false otherwise.

Reimplemented from corbo::NlpSolverInterface.

Definition at line 33 of file levenberg_marquardt_sparse.cpp.

◆ isLsqSolver()

bool corbo::LevenbergMarquardtSparse::isLsqSolver ( ) const

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.

◆ resetWeights()

void corbo::LevenbergMarquardtSparse::resetWeights ( )

protected

Reset weights to their original values.

Definition at line 270 of file levenberg_marquardt_sparse.cpp.

◆ setIterations()

void corbo::LevenbergMarquardtSparse::setIterations ( int iterations )

inline

Define the number of solver iterations.

Definition at line 86 of file levenberg_marquardt_sparse.h.

◆ setPenaltyWeights()

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.

◆ setWeightAdapation()

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.

◆ solve()

SolverStatus corbo::LevenbergMarquardtSparse::solve	(	OptimizationProblemInterface &	problem,
		bool	new_structure = `true`,
		bool	new_run = `true`,
		double *	obj_value = `nullptr`
	)

overridevirtual

Solve the provided optimization problem.

Parameters

[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]

Returns: Solver status as indicated above

Implements corbo::NlpSolverInterface.

Definition at line 44 of file levenberg_marquardt_sparse.cpp.

Member Data Documentation

◆ _delta

Eigen::VectorXd corbo::LevenbergMarquardtSparse::_delta

private

Definition at line 137 of file levenberg_marquardt_sparse.h.

◆ _eq_dim

int corbo::LevenbergMarquardtSparse::_eq_dim = 0

private

Definition at line 129 of file levenberg_marquardt_sparse.h.

◆ _finite_bounds_dim

int corbo::LevenbergMarquardtSparse::_finite_bounds_dim = 0

private

Definition at line 131 of file levenberg_marquardt_sparse.h.

◆ _hessian

Eigen::SparseMatrix<double> corbo::LevenbergMarquardtSparse::_hessian

private

Definition at line 136 of file levenberg_marquardt_sparse.h.

◆ _ineq_dim

int corbo::LevenbergMarquardtSparse::_ineq_dim = 0

private

Definition at line 130 of file levenberg_marquardt_sparse.h.

◆ _iterations

int corbo::LevenbergMarquardtSparse::_iterations = 10

private

Definition at line 112 of file levenberg_marquardt_sparse.h.

◆ _jacobian

Eigen::SparseMatrix<double> corbo::LevenbergMarquardtSparse::_jacobian

private

Definition at line 135 of file levenberg_marquardt_sparse.h.

◆ _obj_dim

int corbo::LevenbergMarquardtSparse::_obj_dim = 0

private

Definition at line 128 of file levenberg_marquardt_sparse.h.

◆ _param_dim

int corbo::LevenbergMarquardtSparse::_param_dim = 0

private

Definition at line 127 of file levenberg_marquardt_sparse.h.

◆ _rhs

Eigen::VectorXd corbo::LevenbergMarquardtSparse::_rhs

private

Definition at line 138 of file levenberg_marquardt_sparse.h.

◆ _sparse_solver

Eigen::SimplicialLLT<Eigen::SparseMatrix<double>, Eigen::Upper> corbo::LevenbergMarquardtSparse::_sparse_solver

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.

◆ _val_dim

int corbo::LevenbergMarquardtSparse::_val_dim = 0

private

Definition at line 132 of file levenberg_marquardt_sparse.h.

◆ _values

Eigen::VectorXd corbo::LevenbergMarquardtSparse::_values

private

Definition at line 134 of file levenberg_marquardt_sparse.h.

◆ _weight_adapt_factor_bounds

double corbo::LevenbergMarquardtSparse::_weight_adapt_factor_bounds = 1

private

Definition at line 120 of file levenberg_marquardt_sparse.h.

◆ _weight_adapt_factor_eq

double corbo::LevenbergMarquardtSparse::_weight_adapt_factor_eq = 1

private

Definition at line 118 of file levenberg_marquardt_sparse.h.

◆ _weight_adapt_factor_ineq

double corbo::LevenbergMarquardtSparse::_weight_adapt_factor_ineq = 1

private

Definition at line 119 of file levenberg_marquardt_sparse.h.

◆ _weight_adapt_max_bounds

double corbo::LevenbergMarquardtSparse::_weight_adapt_max_bounds = 500

private

Definition at line 124 of file levenberg_marquardt_sparse.h.

◆ _weight_adapt_max_eq

double corbo::LevenbergMarquardtSparse::_weight_adapt_max_eq = 500

private

Definition at line 122 of file levenberg_marquardt_sparse.h.

◆ _weight_adapt_max_ineq

double corbo::LevenbergMarquardtSparse::_weight_adapt_max_ineq = 500

private

Definition at line 123 of file levenberg_marquardt_sparse.h.

◆ _weight_bounds

double corbo::LevenbergMarquardtSparse::_weight_bounds = _weight_init_bounds

private

Definition at line 142 of file levenberg_marquardt_sparse.h.

◆ _weight_eq

double corbo::LevenbergMarquardtSparse::_weight_eq = _weight_init_eq

private

Definition at line 140 of file levenberg_marquardt_sparse.h.

◆ _weight_ineq

double corbo::LevenbergMarquardtSparse::_weight_ineq = _weight_init_ineq

private

Definition at line 141 of file levenberg_marquardt_sparse.h.

◆ _weight_init_bounds

double corbo::LevenbergMarquardtSparse::_weight_init_bounds = 2

private

Definition at line 116 of file levenberg_marquardt_sparse.h.

◆ _weight_init_eq

double corbo::LevenbergMarquardtSparse::_weight_init_eq = 2

private

Definition at line 114 of file levenberg_marquardt_sparse.h.

◆ _weight_init_ineq

double corbo::LevenbergMarquardtSparse::_weight_init_ineq = 2

private

Definition at line 115 of file levenberg_marquardt_sparse.h.

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

Public Types

Public Member Functions

Protected Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ Ptr

◆ UPtr

Member Function Documentation

◆ adaptWeights()

◆ clear()

◆ computeValues()

◆ getInstance()

◆ initialize()

◆ isLsqSolver()

◆ resetWeights()

◆ setIterations()

◆ setPenaltyWeights()

◆ setWeightAdapation()

◆ solve()

Member Data Documentation

◆ _delta

◆ _eq_dim

◆ _finite_bounds_dim

◆ _hessian

◆ _ineq_dim

◆ _iterations

◆ _jacobian

◆ _obj_dim

◆ _param_dim

◆ _rhs

◆ _sparse_solver

◆ _val_dim

◆ _values

◆ _weight_adapt_factor_bounds

◆ _weight_adapt_factor_eq

◆ _weight_adapt_factor_ineq

◆ _weight_adapt_max_bounds

◆ _weight_adapt_max_eq

◆ _weight_adapt_max_ineq

◆ _weight_bounds

◆ _weight_eq

◆ _weight_ineq

◆ _weight_init_bounds

◆ _weight_init_eq

◆ _weight_init_ineq