ov_init::Factor_GenericPrior class

Factor for generic state priors for specific types.

Contents

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) \]

Constructors, destructors, conversion operators

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.

Public functions

auto Evaluate(double const*const* parameters, double* residuals, double** jacobians) const -> bool override
Error residual and Jacobian calculation.

Public variables

Eigen::MatrixXd x_lin
State estimates at the time of marginalization to linearize the problem.
std::vector<std::string> x_type
State type for each variable in x_lin. Can be [quat, quat_yaw, vec3, vec8].
Eigen::MatrixXd sqrtI
The square-root of the information s.t. sqrtI^T * sqrtI = marginal information.
Eigen::MatrixXd b
Constant term inside the cost s.t. sqrtI^T * b = marginal gradient (can be zero)