Marginalization class that handles marginalization (and sparsification) for fixed-lag smoothing. More...

#include <marginalization.hpp>

Public Member Functions
void	preMarginalize (std::shared_ptr< Frame > &frame0, std::shared_ptr< Frame > &frame1, std::shared_ptr< Marginalization > &marginalization_last)
	Select all the variables to keep and to marginalize in the Markov Blanket for fixed lag smoothing. More...

void	preMarginalizeRelative (std::shared_ptr< Frame > &frame0, std::shared_ptr< Frame > &frame1)
	Select all the variables to keep and marginalize to derive the relative pose factor between two frames. More...

bool	sparsifyVIO ()
	Sparsify the dense prior factor in the VIO case. More...

bool	sparsifyVO ()
	Sparsify the dense prior factor in the VO case. More...

void	computeInformationAndGradient (std::vector< std::shared_ptr< MarginalizationBlockInfo >> blocks, Eigen::MatrixXd &A, Eigen::VectorXd &b)
	Compute the information matrix and the gradient for a set of factors. More...

void	rankReveallingDecomposition (Eigen::MatrixXd A, Eigen::MatrixXd &U, Eigen::VectorXd &d)
	Compute the SVD of a given matrix to reveal its rank. More...

bool	computeSchurComplement ()
	Compute the dense prior with the Schur complement on _Ak. More...

bool	computeJacobiansAndResiduals ()
	Compute the jacobian and the residual of the dense prior factor. More...

double	computeEntropy (std::shared_ptr< ALandmark > lmk)
	Compute the Entropy of a given landmark. More...

double	computeMutualInformation (std::shared_ptr< ALandmark > lmk_i, std::shared_ptr< ALandmark > lmk_j)
	Compute the Mutual Information between two landmarks. More...

double	computeOffDiag (std::shared_ptr< ALandmark > lmk_i, std::shared_ptr< ALandmark > lmk_j)
	Approximate the Mutual Information between two landmarks using off diagonal blocks of _Ak. More...

double	computeKLD (Eigen::MatrixXd A_p, Eigen::MatrixXd A_q)
	Compute the KLD between the multivariate Gaussian with their Information Matrix assuming their mean is equal. More...

Public Attributes
int	_m
	Parametric size of the variables to marginalize. More...

int	_n
	Parametric size of the variables to keep. More...

int	_n_full
	Parametric size of the variables to keep after rank reveilling. More...

const double	_eps = 1e-12
	Threshold to consider a null eigen value. More...

std::shared_ptr< Frame >	_frame_to_marg
	Frame to marginalize. More...

std::shared_ptr< Frame >	_frame_to_keep
	Frame to keep. More...

typed_vec_landmarks	_lmk_to_keep
	Set of landmarks to keep. More...

typed_vec_landmarks	_lmk_to_marg
	Set of landmarks to marginalize. More...

std::unordered_map< std::shared_ptr< Frame >, int >	_map_frame_idx
	Map between frames and indices in _Ak. More...

std::unordered_map< std::shared_ptr< ALandmark >, int >	_map_lmk_idx
	Map between landmarks and indices in _Ak. More...

std::unordered_map< std::shared_ptr< Frame >, Eigen::MatrixXd >	_map_frame_inf
	Map between frame and their marginal information matrix. More...

std::vector< std::shared_ptr< MarginalizationBlockInfo > >	_marginalization_blocks
	Vector of Marginalization blocks to derive _Ak. More...

std::unordered_map< std::shared_ptr< ALandmark >, Eigen::Matrix3d >	_map_lmk_inf
	Map between landmarks and info mat of sparse relative prior factors. More...

std::unordered_map< std::shared_ptr< ALandmark >, Eigen::Vector3d >	_map_lmk_prior
	Map between landmarks and priors of sparse prior relative factors. More...

std::shared_ptr< ALandmark >	_lmk_with_prior
	Landmark that has an absolute prior factor. More...

Eigen::Matrix3d	_info_lmk
	Information matrix of the landmark absolute prior. More...

Eigen::Vector3d	_prior_lmk
	Prior of the landmark absolute prior. More...

Eigen::MatrixXd	_Ak
	Information matrix of the subproblem. More...

Eigen::VectorXd	_bk
	Gradient of the subproblem. More...

Eigen::MatrixXd	_Sigma_k
	Covariance of the dense prior. More...

Eigen::MatrixXd	_U
	Eigen vectors that have non null eigen values. More...

Eigen::VectorXd	_Lambda
	Non null Eigen values. More...

Eigen::VectorXd	_Sigma
	Inverse of _Lambda. More...

Eigen::MatrixXd	_marginalization_jacobian
	Jacobian of the dense prior factor. More...

Eigen::VectorXd	_marginalization_residual
	Residual of the dense prior factor. More...

Detailed Description

Marginalization class that handles marginalization (and sparsification) for fixed-lag smoothing.

This class is responsible for selecting the variables to keep and to marginalize, computing the information matrix, computing the Schur complement, and computing the Jacobians and residuals for the marginalization. It also provides methods for sparsifying the dense prior in the case of VIO and VO.

Member Function Documentation

◆ computeEntropy()

double isae::Marginalization::computeEntropy ( std::shared_ptr< ALandmark > lmk )

Compute the Entropy of a given landmark.

◆ computeInformationAndGradient()

void isae::Marginalization::computeInformationAndGradient	(	std::vector< std::shared_ptr< MarginalizationBlockInfo >>	blocks,
		Eigen::MatrixXd &	A,
		Eigen::VectorXd &	b
	)

Compute the information matrix and the gradient for a set of factors.

Parameters

blocks	The vector of factors stored in Marginalization Blocks
A	The information matrix
B	The gradient

◆ computeJacobiansAndResiduals()

bool isae::Marginalization::computeJacobiansAndResiduals ( )

Compute the jacobian and the residual of the dense prior factor.

◆ computeKLD()

double isae::Marginalization::computeKLD	(	Eigen::MatrixXd	A_p,
		Eigen::MatrixXd	A_q
	)

Compute the KLD between the multivariate Gaussian with their Information Matrix assuming their mean is equal.

◆ computeMutualInformation()

double isae::Marginalization::computeMutualInformation	(	std::shared_ptr< ALandmark >	lmk_i,
		std::shared_ptr< ALandmark >	lmk_j
	)

Compute the Mutual Information between two landmarks.

◆ computeOffDiag()

double isae::Marginalization::computeOffDiag	(	std::shared_ptr< ALandmark >	lmk_i,
		std::shared_ptr< ALandmark >	lmk_j
	)

Approximate the Mutual Information between two landmarks using off diagonal blocks of _Ak.

◆ computeSchurComplement()

bool isae::Marginalization::computeSchurComplement ( )

Compute the dense prior with the Schur complement on _Ak.

◆ preMarginalize()

void isae::Marginalization::preMarginalize	(	std::shared_ptr< Frame > &	frame0,
		std::shared_ptr< Frame > &	frame1,
		std::shared_ptr< Marginalization > &	marginalization_last
	)

Select all the variables to keep and to marginalize in the Markov Blanket for fixed lag smoothing.

Parameters

frame0	frame to marginalize
frame1	frame linked to frame0
marginalization_last	previous marginalization scheme

◆ preMarginalizeRelative()

void isae::Marginalization::preMarginalizeRelative	(	std::shared_ptr< Frame > &	frame0,
		std::shared_ptr< Frame > &	frame1
	)

Select all the variables to keep and marginalize to derive the relative pose factor between two frames.

Parameters

frame0	The first frame
frame1	The second frame

◆ rankReveallingDecomposition()

void isae::Marginalization::rankReveallingDecomposition	(	Eigen::MatrixXd	A,
		Eigen::MatrixXd &	U,
		Eigen::VectorXd &	d
	)

Compute the SVD of a given matrix to reveal its rank.

Parameters

A	The input matrix
U	The Eigen vectors of non null eigen values (up to a threshold)
d	Non null Eigen values

◆ sparsifyVIO()

bool isae::Marginalization::sparsifyVIO ( )

Sparsify the dense prior factor in the VIO case.

◆ sparsifyVO()

bool isae::Marginalization::sparsifyVO ( )

Sparsify the dense prior factor in the VO case.

CHOW LIU TREE REORDERING ///

LANDMARK CHAIN ///

Member Data Documentation

◆ _Ak

Eigen::MatrixXd isae::Marginalization::_Ak

Information matrix of the subproblem.

◆ _bk

Eigen::VectorXd isae::Marginalization::_bk

Gradient of the subproblem.

◆ _eps

const double isae::Marginalization::_eps = 1e-12

Threshold to consider a null eigen value.

◆ _frame_to_keep

std::shared_ptr<Frame> isae::Marginalization::_frame_to_keep

Frame to keep.

◆ _frame_to_marg

std::shared_ptr<Frame> isae::Marginalization::_frame_to_marg

Frame to marginalize.

◆ _info_lmk

Eigen::Matrix3d isae::Marginalization::_info_lmk

Information matrix of the landmark absolute prior.

◆ _Lambda

Eigen::VectorXd isae::Marginalization::_Lambda

Non null Eigen values.

◆ _lmk_to_keep

typed_vec_landmarks isae::Marginalization::_lmk_to_keep

Set of landmarks to keep.

◆ _lmk_to_marg

typed_vec_landmarks isae::Marginalization::_lmk_to_marg

Set of landmarks to marginalize.

◆ _lmk_with_prior

std::shared_ptr<ALandmark> isae::Marginalization::_lmk_with_prior

Landmark that has an absolute prior factor.

◆ _m

int isae::Marginalization::_m

Parametric size of the variables to marginalize.

◆ _map_frame_idx

std::unordered_map<std::shared_ptr<Frame>, int> isae::Marginalization::_map_frame_idx

Map between frames and indices in _Ak.

◆ _map_frame_inf

std::unordered_map<std::shared_ptr<Frame>, Eigen::MatrixXd> isae::Marginalization::_map_frame_inf

Map between frame and their marginal information matrix.

◆ _map_lmk_idx

std::unordered_map<std::shared_ptr<ALandmark>, int> isae::Marginalization::_map_lmk_idx

Map between landmarks and indices in _Ak.

◆ _map_lmk_inf

std::unordered_map<std::shared_ptr<ALandmark>, Eigen::Matrix3d> isae::Marginalization::_map_lmk_inf

Map between landmarks and info mat of sparse relative prior factors.

◆ _map_lmk_prior

std::unordered_map<std::shared_ptr<ALandmark>, Eigen::Vector3d> isae::Marginalization::_map_lmk_prior

Map between landmarks and priors of sparse prior relative factors.

◆ _marginalization_blocks

std::vector<std::shared_ptr<MarginalizationBlockInfo> > isae::Marginalization::_marginalization_blocks

Vector of Marginalization blocks to derive _Ak.

◆ _marginalization_jacobian

Eigen::MatrixXd isae::Marginalization::_marginalization_jacobian

Jacobian of the dense prior factor.

◆ _marginalization_residual

Eigen::VectorXd isae::Marginalization::_marginalization_residual

Residual of the dense prior factor.

◆ _n

int isae::Marginalization::_n

Parametric size of the variables to keep.

◆ _n_full

int isae::Marginalization::_n_full

Parametric size of the variables to keep after rank reveilling.

◆ _prior_lmk

Eigen::Vector3d isae::Marginalization::_prior_lmk

Prior of the landmark absolute prior.

◆ _Sigma

Eigen::VectorXd isae::Marginalization::_Sigma

Inverse of _Lambda.

◆ _Sigma_k

Eigen::MatrixXd isae::Marginalization::_Sigma_k

Covariance of the dense prior.

◆ _U

Eigen::MatrixXd isae::Marginalization::_U

Eigen vectors that have non null eigen values.

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

/home/deos/ce.debeunne/colcon_ws/src/SaDVIO/cpp/include/isaeslam/optimizers/marginalization.hpp
/home/deos/ce.debeunne/colcon_ws/src/SaDVIO/cpp/src/optimizers/marginalization.cpp

Public Member Functions

Public Attributes

Detailed Description

Member Function Documentation

◆ computeEntropy()

◆ computeInformationAndGradient()

◆ computeJacobiansAndResiduals()

◆ computeKLD()

◆ computeMutualInformation()

◆ computeOffDiag()

◆ computeSchurComplement()

◆ preMarginalize()

◆ preMarginalizeRelative()

◆ rankReveallingDecomposition()

◆ sparsifyVIO()

◆ sparsifyVO()

Member Data Documentation

◆ _Ak

◆ _bk

◆ _eps

◆ _frame_to_keep

◆ _frame_to_marg

◆ _info_lmk

◆ _Lambda

◆ _lmk_to_keep

◆ _lmk_to_marg

◆ _lmk_with_prior

◆ _m

◆ _map_frame_idx

◆ _map_frame_inf

◆ _map_lmk_idx

◆ _map_lmk_inf

◆ _map_lmk_prior

◆ _marginalization_blocks

◆ _marginalization_jacobian

◆ _marginalization_residual

◆ _n

◆ _n_full

◆ _prior_lmk

◆ _Sigma

◆ _Sigma_k

◆ _U