Model 2 of continuous preintegration. More...

#include <CpiV2.h>

Inheritance diagram for ov_core::CpiV2:

Public Member Functions
	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) More...

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

virtual	~CpiV2 ()

Public Member Functions inherited from ov_core::CpiBase
	CpiBase (double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_=false)
	Default constructor. More...

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. More...

virtual	~CpiBase ()

Public Attributes
Eigen::Matrix< double, 3, 3 >	O_a = Eigen::Matrix<double, 3, 3>::Zero()

Eigen::Matrix< double, 3, 3 >	O_b = Eigen::Matrix<double, 3, 3>::Zero()

bool	state_transition_jacobians = true
	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. More...

Public Attributes inherited from ov_core::CpiBase
Eigen::Matrix< double, 3, 1 >	alpha_tau = Eigen::Matrix<double, 3, 1>::Zero()
	alpha measurement mean More...

Eigen::Matrix< double, 3, 1 >	b_a_lin
	b_a linearization point (accelerometer) More...

Eigen::Matrix< double, 3, 1 >	b_w_lin
	b_w linearization point (gyroscope) More...

Eigen::Matrix< double, 3, 1 >	beta_tau = Eigen::Matrix<double, 3, 1>::Zero()
	beta measurement mean More...

double	DT = 0
	measurement integration time More...

Eigen::Matrix< double, 3, 1 >	e_1

Eigen::Matrix< double, 3, 3 >	e_1x

Eigen::Matrix< double, 3, 1 >	e_2

Eigen::Matrix< double, 3, 3 >	e_2x

Eigen::Matrix< double, 3, 1 >	e_3

Eigen::Matrix< double, 3, 3 >	e_3x

Eigen::Matrix< double, 3, 3 >	eye3 = Eigen::Matrix<double, 3, 3>::Identity()

Eigen::Matrix< double, 3, 1 >	grav = Eigen::Matrix<double, 3, 1>::Zero()
	Global gravity. More...

Eigen::Matrix< double, 3, 3 >	H_a = Eigen::Matrix<double, 3, 3>::Zero()
	alpha Jacobian wrt b_a More...

Eigen::Matrix< double, 3, 3 >	H_b = Eigen::Matrix<double, 3, 3>::Zero()
	beta Jacobian wrt b_a More...

bool	imu_avg = false

Eigen::Matrix< double, 3, 3 >	J_a = Eigen::Matrix<double, 3, 3>::Zero()
	alpha Jacobian wrt b_w More...

Eigen::Matrix< double, 3, 3 >	J_b = Eigen::Matrix<double, 3, 3>::Zero()
	beta Jacobian wrt b_w More...

Eigen::Matrix< double, 3, 3 >	J_q = Eigen::Matrix<double, 3, 3>::Zero()
	orientation Jacobian wrt b_w More...

Eigen::Matrix< double, 15, 15 >	P_meas = Eigen::Matrix<double, 15, 15>::Zero()
	Our final measurement covariance. More...

Eigen::Matrix< double, 12, 12 >	Q_c = Eigen::Matrix<double, 12, 12>::Zero()
	Our continous-time measurement noise matrix (computed from contructor noise values) More...

Eigen::Matrix< double, 4, 1 >	q_k2tau
	orientation measurement mean More...

Eigen::Matrix< double, 4, 1 >	q_k_lin
	q_k linearization point (only model 2 uses) More...

Eigen::Matrix< double, 3, 3 >	R_k2tau = Eigen::Matrix<double, 3, 3>::Identity()
	orientation measurement mean More...

Private Attributes
Eigen::Matrix< double, 21, 21 >	Discrete_J_b = Eigen::Matrix<double, 21, 21>::Identity()

Eigen::Matrix< double, 21, 21 >	P_big = Eigen::Matrix<double, 21, 21>::Zero()

Detailed Description

Model 2 of continuous preintegration.

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

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

Definition at line 49 of file CpiV2.h.

Constructor & Destructor Documentation

◆ CpiV2()

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

inline

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)

Definition at line 79 of file CpiV2.h.

◆ ~CpiV2()

virtual ov_core::CpiV2::~CpiV2 ( )

inlinevirtual

Definition at line 82 of file CpiV2.h.

Member Function Documentation

◆ feed_IMU()

void 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

[in]	t_0	first IMU timestamp
[in]	t_1	second IMU timestamp
[in]	w_m_0	first imu gyroscope measurement
[in]	a_m_0	first imu acceleration measurement
[in]	w_m_1	second imu gyroscope measurement
[in]	a_m_1	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.

Implements ov_core::CpiBase.

Definition at line 33 of file CpiV2.cpp.

Member Data Documentation

◆ Discrete_J_b

Eigen::Matrix<double, 21, 21> ov_core::CpiV2::Discrete_J_b = Eigen::Matrix<double, 21, 21>::Identity()

private

Definition at line 56 of file CpiV2.h.

◆ O_a

Eigen::Matrix<double, 3, 3> ov_core::CpiV2::O_a = Eigen::Matrix<double, 3, 3>::Zero()

Definition at line 68 of file CpiV2.h.

◆ O_b

Eigen::Matrix<double, 3, 3> ov_core::CpiV2::O_b = Eigen::Matrix<double, 3, 3>::Zero()

Definition at line 69 of file CpiV2.h.

◆ P_big

Eigen::Matrix<double, 21, 21> ov_core::CpiV2::P_big = Eigen::Matrix<double, 21, 21>::Zero()

private

Definition at line 53 of file CpiV2.h.

◆ state_transition_jacobians

bool ov_core::CpiV2::state_transition_jacobians = true

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.

Definition at line 65 of file CpiV2.h.

The documentation for this class was generated from the following files:

Public Member Functions

Public Attributes

Private Attributes