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

#include <levenberg_marquardt_dense.h>

Inheritance diagram for corbo::LevenbergMarquardtDense:

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

using	UPtr = std::unique_ptr< LevenbergMarquardtDense >

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

Detailed Description

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.

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 84 of file levenberg_marquardt_dense.h.

Member Typedef Documentation

◆ Ptr

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

Definition at line 109 of file levenberg_marquardt_dense.h.

◆ UPtr

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

Definition at line 110 of file levenberg_marquardt_dense.h.

Member Function Documentation

◆ adaptWeights()

void corbo::LevenbergMarquardtDense::adaptWeights ( )

protected

Perform single weight adapation step.

Definition at line 303 of file levenberg_marquardt_dense.cpp.

◆ clear()

void corbo::LevenbergMarquardtDense::clear ( )

overridevirtual

Clear internal caches.

Implements corbo::NlpSolverInterface.

Definition at line 315 of file levenberg_marquardt_dense.cpp.

◆ computeJacobian()

void corbo::LevenbergMarquardtDense::computeJacobian ( OptimizationProblemInterface & problem )

protected

Compute overall jacobian including constraint approximation.

Definition at line 248 of file levenberg_marquardt_dense.cpp.

◆ computeValues()

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

protected

Compute overall value vector including constraint approximation.

Definition at line 222 of file levenberg_marquardt_dense.cpp.

◆ getInstance()

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

inlineoverridevirtual

Return a newly created instance of the current solver.

Implements corbo::NlpSolverInterface.

Definition at line 113 of file levenberg_marquardt_dense.h.

◆ initialize()

bool corbo::LevenbergMarquardtDense::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 55 of file levenberg_marquardt_dense.cpp.

◆ isLsqSolver()

bool corbo::LevenbergMarquardtDense::isLsqSolver ( ) const

inlineoverridevirtual

Return true if the solver onyl supports costs in lsq form.

Implements corbo::NlpSolverInterface.

Definition at line 116 of file levenberg_marquardt_dense.h.

◆ resetWeights()

void corbo::LevenbergMarquardtDense::resetWeights ( )

protected

Reset weights to their original values.

Definition at line 296 of file levenberg_marquardt_dense.cpp.

◆ setIterations()

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

inline

Define the number of solver iterations.

Definition at line 124 of file levenberg_marquardt_dense.h.

◆ setPenaltyWeights()

void corbo::LevenbergMarquardtDense::setPenaltyWeights	(	double	weight_eq,
		double	weight_ineq,
		double	weight_bounds
	)

Define penalty weights (equality constraints, inequality constraints, bounds)

Definition at line 274 of file levenberg_marquardt_dense.cpp.

◆ setWeightAdapation()

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 285 of file levenberg_marquardt_dense.cpp.

◆ solve()

SolverStatus corbo::LevenbergMarquardtDense::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]