ov_core::CpiV2 class

Model 2 of continuous preintegration.

This model is the "piecewise constant local acceleration assumption." Please see the following publication for details on the theory [4] :

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:

  1. call setLinearizationPoints() to set the bias/orientation linearization point
  2. call feed_IMU() will all IMU measurements you want to precompound over
  3. access public varibles, to get means, Jacobians, and measurement covariance

Base classes

class CpiBase
Base class for continuous preintegration integrators.

Constructors, destructors, conversion operators

CpiV2(double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_ = false)
Default constructor for our Model 2 preintegration (piecewise constant local acceleration assumption)

Public functions

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()) virtual
Our precompound function for Model 2.

Public variables

bool state_transition_jacobians
If we want to use analytical jacobians or not. In the paper we just numerically integrated the jacobians If set to false, we use a closed form version similar to model 1.

Function documentation

ov_core::CpiV2::CpiV2(double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_ = false)

Default constructor for our Model 2 preintegration (piecewise constant local acceleration assumption)

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::CpiV2::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()) virtual

Our precompound function for Model 2.

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

We will first analytically integrate our meansurement. We can numerically or analytically integrate our bias jacobians. Then we perform numerical integration for our measurement covariance.