ov_core::CpiBase class

Base class for continuous preintegration integrators.

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 [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

Derived classes

class CpiV1
Model 1 of continuous preintegration.
class CpiV2
Model 2 of continuous preintegration.

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::CpiBase::CpiBase(double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_ = false)

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::CpiBase::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.

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::CpiBase::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.

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.