#include <factor_graph_solve_dense.hpp>

Inheritance diagram for mrob::FGraphSolveDense:

Collaboration diagram for mrob::FGraphSolveDense:

Public Member Functions
virtual matData_t	calculate_error () override

virtual void	calculate_gradient_hessian () override

virtual void	update_state () override

virtual void	bookkeep_state () override

virtual void	update_state_from_bookkeep () override

Public Member Functions inherited from mrob::FGraph
factor_id_t	add_factor (std::shared_ptr< Factor > &factor)

factor_id_t	add_eigen_factor (std::shared_ptr< EigenFactor > &factor)

factor_id_t	add_node (std::shared_ptr< Node > &node)

std::shared_ptr< Node > &	get_node (factor_id_t key)

std::shared_ptr< Factor > &	get_factor (factor_id_t key)

std::shared_ptr< EigenFactor > &	get_eigen_factor (factor_id_t key)

void	print (bool complete=false) const

factor_id_t	number_nodes ()

factor_id_t	number_factors ()

uint_t	get_dimension_state ()

uint_t	get_dimension_obs ()

void	save_graph () const

void	load_graph ()

Public Member Functions inherited from mrob::OptimizerDense
	OptimizerDense (matData_t solutionTolerance=1e-4, matData_t lambda=1e-5)

Public Member Functions inherited from mrob::Optimizer
	Optimizer (matData_t solutionTolerance=1e-4, matData_t lambda=1e-5)

uint_t	solve (optimMethod method, uint_t max_iters=1e2, double lambda=1e-5)

Additional Inherited Members
Public Types inherited from mrob::Optimizer
enum	optimMethod { NEWTON_RAPHSON =0, LEVENBERG_MARQUARDT_SPHER, LEVENBERG_MARQUARDT_ELLIP }

Protected Member Functions inherited from mrob::OptimizerDense
uint_t	optimize_newton_raphson_one_iteration (bool useLambda=false)

matData_t	calculate_model_fidelity (matData_t diff_error)

Protected Member Functions inherited from mrob::Optimizer
uint_t	optimize_newton_raphson ()

uint_t	optimize_levenberg_marquardt ()

Protected Attributes inherited from mrob::FGraph
std::deque< std::shared_ptr< Node > >	nodes_

std::deque< std::shared_ptr< Node > >	active_nodes_

std::deque< std::shared_ptr< Factor > >	factors_

std::deque< std::shared_ptr< EigenFactor > >	eigen_factors_

uint_t	stateDim_

uint_t	obsDim_

Protected Attributes inherited from mrob::OptimizerDense
MatX	hessian_

Protected Attributes inherited from mrob::Optimizer
optimMethod	optimization_method_ {}

matData_t	solutionTolerance_

uint_t	max_iters_

MatX1	gradient_

MatX1	dx_

matData_t	lambda_

Detailed Description

Class FGraphSolveDense solve a factor graph problem assuming dense precision matrix.

It inherits from two classes:

FGraph: structure for adding generic factors
Optimizer: Optimization methods given some abstract routines

Member Function Documentation

◆ bookkeep_state()

void FGraphSolveDense::bookkeep_state ( )

overridevirtual

For Levenberg-Marquard This function bookkeeps the current state values This is in case the optimization step does not improve

Implements mrob::Optimizer.

◆ calculate_error()

matData_t FGraphSolveDense::calculate_error ( )

overridevirtual

General abstract functions to implement: Calculate error calculates the current error function

Implements mrob::Optimizer.

◆ calculate_gradient_hessian()

void FGraphSolveDense::calculate_gradient_hessian ( )

overridevirtual

Gradient calculates the gradient and Hessian This function may be called after calculate_error() or not (some cases of LM). For a general purpose it is required that this function is either: 1) self-contained. No assumptions made and recalculated (redundantly) residuals 2) Most of the times will be called after calculate_error. No recalculations are requires except inside the update_bookkeep_state which will have invalid residuals and need update (less prefered option)

Implements mrob::Optimizer.