Base class for continuous preintegration integrators. More...

#include <CpiBase.h>

Inheritance diagram for ov_core::CpiBase:

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

virtual 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())=0
	Main function that will sequentially compute the preintegration measurement. 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, 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...

Detailed Description

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 [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 CpiBase.h.

Constructor & Destructor Documentation

◆ CpiBase()

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

inline

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)

Definition at line 60 of file CpiBase.h.

◆ ~CpiBase()

virtual ov_core::CpiBase::~CpiBase ( )

inlinevirtual

Definition at line 76 of file CpiBase.h.

Member Function Documentation

◆ feed_IMU()

virtual 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

[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

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.

Implemented in ov_core::CpiV2, and ov_core::CpiV1.

◆ setLinearizationPoints()

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()`
	)

inline

Set linearization points of the integration.

Parameters

[in]	b_w_lin_	gyroscope bias linearization point
[in]	b_a_lin_	accelerometer bias linearization point
[in]	q_k_lin_	orientation linearization point (only model 2 uses)
[in]	grav_	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.

Definition at line 88 of file CpiBase.h.

Member Data Documentation

◆ alpha_tau

Eigen::Matrix<double, 3, 1> ov_core::CpiBase::alpha_tau = Eigen::Matrix<double, 3, 1>::Zero()

alpha measurement mean

Definition at line 120 of file CpiBase.h.

◆ b_a_lin

Eigen::Matrix<double, 3, 1> ov_core::CpiBase::b_a_lin

b_a linearization point (accelerometer)

Definition at line 134 of file CpiBase.h.

◆ b_w_lin

Eigen::Matrix<double, 3, 1> ov_core::CpiBase::b_w_lin

b_w linearization point (gyroscope)

Definition at line 133 of file CpiBase.h.

◆ beta_tau

Eigen::Matrix<double, 3, 1> ov_core::CpiBase::beta_tau = Eigen::Matrix<double, 3, 1>::Zero()

beta measurement mean

Definition at line 121 of file CpiBase.h.

◆ DT

double ov_core::CpiBase::DT = 0

measurement integration time

Definition at line 119 of file CpiBase.h.

◆ e_1

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

Definition at line 154 of file CpiBase.h.

◆ e_1x

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

Definition at line 159 of file CpiBase.h.

◆ e_2

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

Definition at line 155 of file CpiBase.h.

◆ e_2x

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

Definition at line 160 of file CpiBase.h.

◆ e_3

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

Definition at line 156 of file CpiBase.h.

◆ e_3x

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

Definition at line 161 of file CpiBase.h.

◆ eye3

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

Definition at line 151 of file CpiBase.h.

◆ grav

Eigen::Matrix<double, 3, 1> ov_core::CpiBase::grav = Eigen::Matrix<double, 3, 1>::Zero()

Global gravity.

Definition at line 138 of file CpiBase.h.

◆ H_a

Eigen::Matrix<double, 3, 3> ov_core::CpiBase::H_a = Eigen::Matrix<double, 3, 3>::Zero()

alpha Jacobian wrt b_a

Definition at line 129 of file CpiBase.h.

◆ H_b

Eigen::Matrix<double, 3, 3> ov_core::CpiBase::H_b = Eigen::Matrix<double, 3, 3>::Zero()

beta Jacobian wrt b_a

Definition at line 130 of file CpiBase.h.

◆ imu_avg

bool ov_core::CpiBase::imu_avg = false

Definition at line 116 of file CpiBase.h.

◆ J_a

Eigen::Matrix<double, 3, 3> ov_core::CpiBase::J_a = Eigen::Matrix<double, 3, 3>::Zero()

alpha Jacobian wrt b_w

Definition at line 127 of file CpiBase.h.

◆ J_b

Eigen::Matrix<double, 3, 3> ov_core::CpiBase::J_b = Eigen::Matrix<double, 3, 3>::Zero()

beta Jacobian wrt b_w

Definition at line 128 of file CpiBase.h.

◆ J_q

Eigen::Matrix<double, 3, 3> ov_core::CpiBase::J_q = Eigen::Matrix<double, 3, 3>::Zero()

orientation Jacobian wrt b_w

Definition at line 126 of file CpiBase.h.

◆ P_meas

Eigen::Matrix<double, 15, 15> ov_core::CpiBase::P_meas = Eigen::Matrix<double, 15, 15>::Zero()

Our final measurement covariance.

Definition at line 144 of file CpiBase.h.

◆ Q_c

Eigen::Matrix<double, 12, 12> ov_core::CpiBase::Q_c = Eigen::Matrix<double, 12, 12>::Zero()

Our continous-time measurement noise matrix (computed from contructor noise values)

Definition at line 141 of file CpiBase.h.

◆ q_k2tau

Eigen::Matrix<double, 4, 1> ov_core::CpiBase::q_k2tau

orientation measurement mean

Definition at line 122 of file CpiBase.h.

◆ q_k_lin

Eigen::Matrix<double, 4, 1> ov_core::CpiBase::q_k_lin

q_k linearization point (only model 2 uses)

Definition at line 135 of file CpiBase.h.

◆ R_k2tau

Eigen::Matrix<double, 3, 3> ov_core::CpiBase::R_k2tau = Eigen::Matrix<double, 3, 3>::Identity()

orientation measurement mean

Definition at line 123 of file CpiBase.h.

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

CpiBase.h

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CpiBase()

◆ ~CpiBase()

Member Function Documentation

◆ feed_IMU()

◆ setLinearizationPoints()

Member Data Documentation

◆ alpha_tau

◆ b_a_lin

◆ b_w_lin

◆ beta_tau

◆ DT

◆ e_1

◆ e_1x

◆ e_2

◆ e_2x

◆ e_3

◆ e_3x

◆ eye3

◆ grav

◆ H_a

◆ H_b

◆ imu_avg

◆ J_a

◆ J_b

◆ J_q

◆ P_meas

◆ Q_c

◆ q_k2tau

◆ q_k_lin

◆ R_k2tau