59 lines
2.4 KiB
Plaintext
59 lines
2.4 KiB
Plaintext
/**
|
|
|
|
|
|
@page dev-index Covariance Index Internals
|
|
|
|
|
|
@section dev-index-types Type System
|
|
|
|
The type system that the ov_msckf leverages was inspired by graph-based optimization frameworks such as [Georgia Tech Smoothing and Mapping library (GTSAM)](https://github.com/borglab/gtsam).
|
|
As compared to manually knowing where in the covariance state elements are, the interaction is abstracted away from the user and is replaced by a straight forward way to access the desired elements.
|
|
The ov_msckf::State is owner of these types and thus after creation one should not delete these externally.
|
|
|
|
|
|
@code{.cpp}
|
|
class Type {
|
|
protected:
|
|
// Current best estimate
|
|
Eigen::MatrixXd _value;
|
|
// Location of error state in covariance
|
|
int _id = -1;
|
|
// Dimension of error state
|
|
int _size = -1;
|
|
};
|
|
@endcode
|
|
|
|
|
|
A type is defined by its location in its covariance, its current estimate and its error state size.
|
|
The current value does not have to be a vector, but could be a matrix in the case of an SO(3) rotation group type object.
|
|
The error state needs to be a vector and thus a type will need to define the mapping between this error state and its manifold representation.
|
|
|
|
|
|
|
|
|
|
@section dev-index-update Type-based EKF Update
|
|
|
|
|
|
To show the power of this type-based indexing system, we will go through how we compute the EKF update.
|
|
The specific method we will be looking at is the @ref ov_msckf::StateHelper::EKFUpdate() which takes in the state, vector of types, Jacobian, residual, and measurement covariance.
|
|
As compared to passing a Jacobian matrix that is the full size of state wide, we instead leverage this type system by passing a "small" Jacobian that has only the state elements that the measurements are a function of.
|
|
|
|
|
|
For example, if we have a global 3D SLAM feature update (implemented in @ref ov_msckf::UpdaterSLAM) our Jacobian will only be a function of the newest clone and the feature.
|
|
This means that our Jacobian is only of size 9 as compared to the size our state.
|
|
In addition to the matrix containing the Jacobian elements, we need to know what order this Jacobian is in, thus we pass a vector of types which correspond to the state elements our Jacobian is a function of.
|
|
Thus in our example, it would contain two types: our newest clone @ref ov_type::PoseJPL and current landmark feature @ref ov_type::Landmark with our Jacobian being the type size of the pose plus the type size of the landmark in width.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
*/ |