Factor for generic state priors for specific types. More...

#include <Factor_GenericPrior.h>

Inheritance diagram for ov_init::Factor_GenericPrior:

Public Member Functions
bool	Evaluate (double const const parameters, double residuals, double *jacobians) const override
	Error residual and Jacobian calculation. More...

	Factor_GenericPrior (const Eigen::MatrixXd &x_lin_, const std::vector< std::string > &x_type_, const Eigen::MatrixXd &prior_Info, const Eigen::MatrixXd &prior_grad)
	Default constructor. More...

virtual	~Factor_GenericPrior ()

Public Attributes
Eigen::MatrixXd	b
	Constant term inside the cost s.t. sqrtI^T * b = marginal gradient (can be zero) More...

Eigen::MatrixXd	sqrtI
	The square-root of the information s.t. sqrtI^T * sqrtI = marginal information. More...

Eigen::MatrixXd	x_lin
	State estimates at the time of marginalization to linearize the problem. More...

std::vector< std::string >	x_type
	State type for each variable in x_lin. Can be [quat, quat_yaw, vec3, vec8]. More...

Detailed Description

Factor for generic state priors for specific types.

This is a general factor which handles state priors which have non-zero linear errors. In general a unitary factor will have zero error when it is created, thus this extra term can be ignored. But if performing marginalization, this can be non-zero. See the following paper Section 3.2 Eq. 25-35 https://journals.sagepub.com/doi/full/10.1177/0278364919835021

We have the following minimization problem:

$\textrm{argmin} ||A * (x - x_{lin}) + b||^2$

In general we have the following after marginalization:

$(A^T*A) = Inf_{prior}$ (the prior information)
$A^T*b = grad_{prior}$ (the prior gradient)

For example, consider we have the following system were we wish to remove the xm states. This is the problem of state marginalization.

$[ Arr Arm ] [ xr ] = [ - gr ]$

$[ Amr Amm ] [ xm ] = [ - gm ]$

We wish to marginalize the xm states which are correlated with the other states $ xr $ . The Jacobian (and thus information matrix A) is computed at the current best guess $x_{lin}$ . We can define the following optimal subcost form which only involves the $ xr $ states as:

$cost^2 = (xr - xr_{lin})^T*(A^T*A)*(xr - xr_{lin}) + b^T*A*(xr - xr_{lin}) + b^b$

where we have:

$A = sqrt(Arr - Arm*Amm^{-1}*Amr)$

$b = A^-1 * (gr - Arm*Amm^{-1}*gm)$

Definition at line 72 of file Factor_GenericPrior.h.

Constructor & Destructor Documentation

◆ Factor_GenericPrior()

Factor_GenericPrior::Factor_GenericPrior	(	const Eigen::MatrixXd &	x_lin_,
		const std::vector< std::string > &	x_type_,
		const Eigen::MatrixXd &	prior_Info,
		const Eigen::MatrixXd &	prior_grad
	)

Default constructor.

Definition at line 28 of file Factor_GenericPrior.cpp.

◆ ~Factor_GenericPrior()

virtual ov_init::Factor_GenericPrior::~Factor_GenericPrior ( )

inlinevirtual

Definition at line 92 of file Factor_GenericPrior.h.

Member Function Documentation

◆ Evaluate()

bool Factor_GenericPrior::Evaluate	(	double const const	parameters,
		double *	residuals,
		double **	jacobians
	)		const

override

Error residual and Jacobian calculation.

Definition at line 94 of file Factor_GenericPrior.cpp.

Member Data Documentation

◆ b

Eigen::MatrixXd ov_init::Factor_GenericPrior::b

Constant term inside the cost s.t. sqrtI^T * b = marginal gradient (can be zero)

Definition at line 84 of file Factor_GenericPrior.h.

◆ sqrtI

Eigen::MatrixXd ov_init::Factor_GenericPrior::sqrtI

The square-root of the information s.t. sqrtI^T * sqrtI = marginal information.

Definition at line 81 of file Factor_GenericPrior.h.

◆ x_lin

Eigen::MatrixXd ov_init::Factor_GenericPrior::x_lin

State estimates at the time of marginalization to linearize the problem.

Definition at line 75 of file Factor_GenericPrior.h.

◆ x_type

std::vector<std::string> ov_init::Factor_GenericPrior::x_type

State type for each variable in x_lin. Can be [quat, quat_yaw, vec3, vec8].

Definition at line 78 of file Factor_GenericPrior.h.

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

Public Member Functions

Public Attributes