CpiBase class
Base class for continuous preintegration integrators.
Contents
This is the base class that both continuous-time preintegrators extend. Please take a look at the derived classes CpiV1 and CpiV2 for the actual implementation. Please see the following publication for details on the theory [13] :
Continuous Preintegration Theory for Graph-based Visual-Inertial Navigation Authors: Kevin Eckenhoff, Patrick Geneva, and Guoquan Huang http://udel.edu/~ghuang/papers/tr_
cpi.pdf
The steps to use this preintegration class are as follows:
- call setLinearizationPoints() to set the bias/orientation linearization point
- call feed_
IMU() will all IMU measurements you want to precompound over - access public varibles, to get means, Jacobians, and measurement covariance
Derived classes
Constructors, destructors, conversion operators
- CpiBase(double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_ = false)
- Default constructor.
Public functions
- void setLinearizationPoints(Eigen::Matrix<double, 3, 1> b_w_lin_, Eigen::Matrix<double, 3, 1> b_a_lin_, Eigen::Matrix<double, 4, 1> q_k_lin_ = Eigen::Matrix<double, 4, 1>::Zero(), Eigen::Matrix<double, 3, 1> grav_ = Eigen::Matrix<double, 3, 1>::Zero())
- Set linearization points of the integration.
- void feed_IMU(double t_0, double t_1, Eigen::Matrix<double, 3, 1> w_m_0, Eigen::Matrix<double, 3, 1> a_m_0, Eigen::Matrix<double, 3, 1> w_m_1 = Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 3, 1> a_m_1 = Eigen::Matrix<double, 3, 1>::Zero()) pure virtual
- Main function that will sequentially compute the preintegration measurement.
Public variables
- double DT
- measurement integration time
- Eigen::Matrix<double, 3, 1> alpha_tau
- alpha measurement mean
- Eigen::Matrix<double, 3, 1> beta_tau
- beta measurement mean
- Eigen::Matrix<double, 4, 1> q_k2tau
- orientation measurement mean
- Eigen::Matrix<double, 3, 3> R_k2tau
- orientation measurement mean
- Eigen::Matrix<double, 3, 3> J_q
- orientation Jacobian wrt b_w
- Eigen::Matrix<double, 3, 3> J_a
- alpha Jacobian wrt b_w
- Eigen::Matrix<double, 3, 3> J_b
- beta Jacobian wrt b_w
- Eigen::Matrix<double, 3, 3> H_a
- alpha Jacobian wrt b_a
- Eigen::Matrix<double, 3, 3> H_b
- beta Jacobian wrt b_a
- Eigen::Matrix<double, 3, 1> b_w_lin
- b_w linearization point (gyroscope)
- Eigen::Matrix<double, 3, 1> b_a_lin
- b_a linearization point (accelerometer)
- Eigen::Matrix<double, 4, 1> q_k_lin
- q_k linearization point (only model 2 uses)
- Eigen::Matrix<double, 3, 1> grav
- Global gravity.
- Eigen::Matrix<double, 12, 12> Q_c
- Our continous-time measurement noise matrix (computed from contructor noise values)
- Eigen::Matrix<double, 15, 15> P_meas
- Our final measurement covariance.
Function documentation
ov_core::
Default constructor.
| Parameters | |
|---|---|
| sigma_w | gyroscope white noise density (rad/s/sqrt(hz)) |
| sigma_wb | gyroscope random walk (rad/s^2/sqrt(hz)) |
| sigma_a | accelerometer white noise density (m/s^2/sqrt(hz)) |
| sigma_ab | accelerometer random walk (m/s^3/sqrt(hz)) |
| imu_avg_ | if we want to average the imu measurements (IJRR paper did not do this) |
void ov_core::
Set linearization points of the integration.
| Parameters | |
|---|---|
| b_w_lin_ in | gyroscope bias linearization point |
| b_a_lin_ in | accelerometer bias linearization point |
| q_k_lin_ in | orientation linearization point (only model 2 uses) |
| grav_ in | global gravity at the current timestep |
This function sets the linearization points we are to preintegrate about. For model 2 we will also pass the q_GtoK and current gravity estimate.
void ov_core::
Main function that will sequentially compute the preintegration measurement.
| Parameters | |
|---|---|
| t_0 in | first IMU timestamp |
| t_1 in | second IMU timestamp |
| w_m_0 in | first imu gyroscope measurement |
| a_m_0 in | first imu acceleration measurement |
| w_m_1 in | second imu gyroscope measurement |
| a_m_1 in | second imu acceleration measurement |
This new IMU messages and will precompound our measurements, jacobians, and measurement covariance. Please see both CpiV1 and CpiV2 classes for implementation details on how this works.