mrob::FGraphSolve Class Reference

#include <factor_graph_solve.hpp>

Inheritance diagram for mrob::FGraphSolve:

Collaboration diagram for mrob::FGraphSolve:

Public Types
enum	matrixMethod { ADJ =0, SCHUR }
enum	optimMethod { GN =0, LM, LM_ELLIPS }

Public Member Functions
	FGraphSolve (matrixMethod method=ADJ)
uint_t	solve (optimMethod method=GN, uint_t maxIters=20, matData_t lambda=1e-6, matData_t solutionTolerance=1e-2)
matData_t	chi2 (bool evaluateResidualsFlag=true)
std::vector< MatX >	get_estimated_state ()
void	set_build_matrix_method (matrixMethod method)
matrixMethod	get_build_matrix_method ()
SMatCol	get_information_matrix ()
SMatCol	get_adjacency_matrix ()
SMatCol	get_W_matrix ()
MatX1	get_vector_b ()
MatX1	get_chi2_array ()
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 ()

Protected Member Functions
void	build_problem (bool useLambda=false)
void	build_adjacency ()
void	build_info_adjacency ()
void	build_info_EF ()
void	build_schur ()
void	optimize_gauss_newton (bool useLambda=false)
uint_t	optimize_levenberg_marquardt (uint_t maxIters)
void	update_nodes ()
void	synchronize_nodes_state ()
void	synchronize_nodes_auxiliary_state ()
void	build_index_nodes_matrix ()

Protected Attributes
matrixMethod	matrixMethod_
optimMethod	optimMethod_
factor_id_t	N_
factor_id_t	M_
std::unordered_map< factor_id_t, factor_id_t >	indNodesMatrix_
SMatRow	A_
SMatRow	W_
MatX1	r_
SMatCol	L_
MatX1	b_
MatX1	dx_
matData_t	lambda_
matData_t	solutionTolerance_
MatX1	diagL_
SMatCol	hessianEF_
MatX1	gradientEF_
bool	buildAdjacencyFlag_
TimeProfiling	time_profiles_
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_

Detailed Description

Class FGraphSolve creates all the required matrices for solving the LSQ problem. The problem takes the following form:

x* = argmin {C(x)} = argmin {1/2 sum ||r_i(x,z)||2_W} = argmin 1/2||r||2_W.

last term in vectorized form.

By convention, the residuals r_i are ALWAYS formulated as follows:

| r(x) = h(x) - z |

With this arrangement, the linearized factor substracts the residual (r) to the first order term of the nonlinear observation function: ||h(x)-z||2_W = ||h(x0) + J dx - z ||2_W = ||J dx + r||2_W

When optimizing the linearized LSQ: dC 1 d – = - —(sum r' W r) = J' W (J dx + r) = 0 dx 2 dx

=> dx = -(J'WJ)^(-1) J'W r

This convention will be followed by all factors in this library, otherwise the optimization will not work properly.

Different options are provided:

• ADJ: Adjacency matrix (plus indirect construction of Information)
• SCHUR (TODO): Diagonal and information from Schur complement

Routines provide different optimization methods:

• Gauss-Newton (GN) using Cholesky LDLT with minimum degree ordering
• Levenberg–Marquardt (LM) (Nocedal Ch.10) using spherical trust region alg. (Nocedal 4.1) to estimate a "good" lambda. Bertsekas p.105 proposes a similar heuristic approach for the trust region, which we convert to lambda estimation (we follow Bertsekas' notation in code).
• LM_Ellipsoid implementation. Slightly different than LM-Spherical on how to condition the information matrix.

Member Enumeration Documentation

matrixMethod

enum mrob::FGraphSolve::matrixMethod

This enums all matrix building methods available For now we only do build adjacency

optimMethod

enum mrob::FGraphSolve::optimMethod

This enums optimization methods available:

• Gauss Newton
• Levenberg Marquardt (trust-region-like for lambda adjustment) TODO LM elliptical?

Member Function Documentation

build_adjacency()

void FGraphSolve::build_adjacency ( )

protected

This protected method creates an Adjacency matrix, iterating over all factors in the FG and creates a block diagonal matrix W with each factors information. As a result, residuals, Jacobians and chi2 values are up to date

build_index_nodes_matrix()

void FGraphSolve::build_index_nodes_matrix ( )

protected

This function build the node index that will be used for creating the adjacency matrix and the information matrix.

It maps the node Id to the column index in the matrix, such that non-consecutive nodes can be bookkeep for a fast access

build_info_adjacency()

void FGraphSolve::build_info_adjacency ( )

protected

From the adjacency matrix it creates the information matrix as L = A^T * W * A The residuals are also calculated as b = A^T * W *r

L_ dx = b_ corresponds to the normal equation A'*W*A dx = A'*W*r only store the lower part of the information matrix (symmetric)

XXX: In terms of speed, using the selfadjointview does not improve, Eigen stores a temporary object and then copy only the upper part.

build_info_EF()

void FGraphSolve::build_info_EF ( )

protected

Builds the information matrix directly from Eigen Factors. It follows a different approach than build adjacency, it will only create a Hessian and Jacobian when at least one EF is present.

build_problem()

void FGraphSolve::build_problem ( bool  useLambda = false )

protected

build problem creates an information matrix L, W and a vector b

It chooses from building the information from the adjacency matrix, directly building info or schur (TODO)

If bool useLambda is true, it also stores a vector D2 containing the diagonal of the information matrix L

chi2()

matData_t FGraphSolve::chi2

(

bool

evaluateResidualsFlag = true

)

Evaluates the current solution chi2.

Variable relinearizeProblemFlag:

• (default) true: Recalculates residuals.
• false: Uses the previous calculated residuals

get_adjacency_matrix()

SMatCol mrob::FGraphSolve::get_adjacency_matrix ( )

inline

Returns a copy to the Adjacency matrix. There is a conversion (implies copy) from Row to Col-convention (which is what np.array needs) TODO If true, it re-evaluates the problem

get_chi2_array()

MatX1 FGraphSolve::get_chi2_array

(

)

Returns a vector of chi2 values for each of the factors.

get_estimated_state()

std::vector< MatX > FGraphSolve::get_estimated_state

(

)

Returns a Reference to the solution vector of all variables, vectors, matrices, etc.

get_information_matrix()

SMatCol mrob::FGraphSolve::get_information_matrix ( )

inline

Returns a copy to the information matrix. pybind does not allow to pass by reference, so there is a copy anyway CHeck out more here: https://pybind11.readthedocs.io/en/stable/advanced/cast/eigen.html TODO If true, it re-evaluates the problem

get_vector_b()

MatX1 mrob::FGraphSolve::get_vector_b ( )

inline

Returns a copy to the processed residuals in state space b = A'Wr. TODO If true, it re-evaluates the problem

get_W_matrix()

SMatCol mrob::FGraphSolve::get_W_matrix ( )

inline

Returns a copy to the W matrix. There is a conversion (implies copy) from Row to Col-convention (which is what np.array needs) TODO If true, it re-evaluates the problem

optimize_gauss_newton()

void FGraphSolve::optimize_gauss_newton ( bool  useLambda = false )

protected

Once the matrix L is generated, it solves the linearized LSQ by using the Gauss-Newton algorithm

Input useLambda (default false) builds the GN problem with lambda factor on the diagonal L = A'*A + lambda * I

optimize_levenberg_marquardt()

uint_t FGraphSolve::optimize_levenberg_marquardt ( uint_t  maxIters )

protected

It generates the information matrix as L' = L + lambda * I

TODO, the preconditioning could be adapted to expected values, such as w < pi and v < avg L' = L + lambda * D2

Iteratively updates the solution given the right estimation of lambda Parameters are necessary to be specified in advance, o.w. it would use default values.

input maxIters, before returning a result

output: number of iterations it took to converge. 0 when incorrect solution

set_build_matrix_method()

void mrob::FGraphSolve::set_build_matrix_method ( matrixMethod  method )

inline

Functions to set the matrix method building

solve()

uint_t FGraphSolve::solve	(	optimMethod	method = GN,
		uint_t	maxIters = 20,
		matData_t	lambda = 1e-6,
		matData_t	solutionTolerance = 1e-2
	)

Solve call the corresponding routine on the class parameters or ultimately on the function input, by default optim method is Gauss Newton

Return: number of iterations Failed to converge = 0 iterations

2800 2D nodes on M3500 Time profile :13.902 % build Adjacency matrix, 34.344 % build Information, 48.0506 % build Cholesky, 2.3075 % solve forward and back substitution, 1.3959 % update values,

synchronize_nodes_auxiliary_state()

void FGraphSolve::synchronize_nodes_auxiliary_state ( )

protected

Synchronize auxiliary state variables in all nodes exactly value the current state. This function is used when we will update a solution but it needs verification, so we book-keep at auxiliary.

synchronize_nodes_state()

void FGraphSolve::synchronize_nodes_state ( )

protected

Synchronize state variable in all nodes exactly value the current state.

Usually this function un-does an incorrect update of the state.

update_nodes()

void FGraphSolve::update_nodes ( )

protected

Function that updates all nodes with the current solution, this must be called after solving the problem

---

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

• src/FGraph/mrob/factor_graph_solve.hpp
• src/FGraph/factor_graph_solve.cpp