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:/
We have the following minimization problem:
In general we have the following after marginalization:
- (the prior information)
- (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.
We wish to marginalize the xm states which are correlated with the other states . The Jacobian (and thus information matrix A) is computed at the current best guess . We can define the following optimal subcost form which only involves the states as:
where we have:
- 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.
- auto Evaluate(double const*const* parameters, double* residuals, double** jacobians) const -> bool override
- Error residual and Jacobian calculation.
- 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)