Detailed Description

IMU preintegration using the SE_2(3) Lie-group structure of NavState.

Unlike ManifoldPreintegration, each increment is applied with the group exponential map. In Legacy factor-error mode, factors using this backend evaluate their residual with the SE_2(3) logarithm.

Inheritance diagram for gtsam::LieGroupPreintegration:

Public Member Functions
	LieGroupPreintegration (const std::shared_ptr< Params > &params, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
	Construct an empty preintegrator with parameters and a bias linearization point.
Vector9	biasCorrectedDelta (const imuBias::ConstantBias &bias, OptionalJacobian< 9, 6 > H={}) const override
	Return Brossard's right-corrected delta in PIM (R,p,v) ordering.
Public Member Functions inherited from gtsam::ManifoldPreintegration
	ManifoldPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
	Constructor, initializes the variables in the base class.
void	resetIntegration () override
NavState	deltaXij () const override
Rot3	deltaRij () const override
Vector3	deltaPij () const override
Vector3	deltaVij () const override
Vector9	preintegrated () const
	Return the preintegrated measurements as NavState tangent coordinates.
Matrix3	delRdelBiasOmega () const
Matrix3	delPdelBiasAcc () const
Matrix3	delPdelBiasOmega () const
Matrix3	delVdelBiasAcc () const
Matrix3	delVdelBiasOmega () const
bool	equals (const ManifoldPreintegration &other, double tol) const
void	update (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 A, Matrix93 B, Matrix93 *C) override
	Update preintegrated measurements and get derivatives It takes measured quantities in the j frame Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose NOTE(frank): implementation is different in two versions.
Vector9	biasCorrectedDelta (const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override
	Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU measurements so far NOTE(frank): implementation is different in two versions.
virtual std::shared_ptr< ManifoldPreintegration >	clone () const
	Dummy clone for MATLAB.
Public Member Functions inherited from gtsam::PreintegrationBase
	PreintegrationBase (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
	Constructor, initializes the variables in the base class.
void	resetIntegrationAndSetBias (const Bias &biasHat)
bool	matchesParamsWith (const PreintegrationBase &other) const
	check parameters equality: checks whether shared pointer points to same Params object.
const std::shared_ptr< Params > &	params () const
	shared pointer to params
Params &	p () const
	const reference to params
const imuBias::ConstantBias &	biasHat () const
double	deltaTij () const
virtual Vector3	so3TangentAt (double t) const
	Return the SO(3) tangent vector at local time t in [0, deltaTij].
Matrix	deskewPoints (ConstMatrixView points, const Vector3 &velocity_i=Vector3::Zero()) const
	Deskew endpoint-exclusive ordered point batches over the integration interval.
Matrix	deskewPointsAtTimes (ConstMatrixView points, const Vector &times, const Vector3 &velocity_i=Vector3::Zero()) const
	Deskew stacked point batches at explicit times and initial velocity.
Vector6	biasHatVector () const
std::pair< Vector3, Vector3 >	correctMeasurementsBySensorPose (const Vector3 &unbiasedAcc, const Vector3 &unbiasedOmega, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedAcc={}, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedOmega={}, OptionalJacobian< 3, 3 > correctedOmega_H_unbiasedOmega={}) const
	Subtract estimate and correct for sensor pose Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) Ignore D_correctedOmega_measuredAcc as it is trivially zero.
virtual void	integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt)
	Version without derivatives.
void	integrateMeasurements (const Matrix &measuredAccs, const Matrix &measuredOmegas, const Matrix &dts)
	Add multiple measurements, in matrix columns.
NavState	predict (const NavState &state_i, const imuBias::ConstantBias &bias_i, const Vector3 &n_gravity, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 6 > H2={}, OptionalJacobian< 9, 3 > H3={}) const
	Predict state at time j, for a given gravity vector in the nav frame.
NavState	predict (const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 6 > H2={}) const
	Predict state at time j, using the gravity vector from params.
virtual void	print (const std::string &s="") const

Static Public Attributes
static constexpr bool	kLegacyUsesLogmap = true
	Select the SE_2(3) logarithm in Legacy factor-error mode.
Static Public Attributes inherited from gtsam::PreintegrationBase
static constexpr bool	kLegacyUsesLogmap = false
	Legacy factor-error choice for this preintegration backend.

Protected Member Functions
	LieGroupPreintegration ()=default
	Default constructor for serialization.
void	updateFactor (const Vector3 &bodyAcceleration, const Vector3 &bodyOmega, double dt, OptionalJacobian< 9, 9 > F={}, OptionalJacobian< 9, 3 > G1={}, OptionalJacobian< 9, 3 > G2={}) override
	Apply one unbiased IMU increment with NavState's group exponential.
void	updateBiasJacobians (const Rot3 &oldRotation, const Vector3 &bodyAcceleration, const Vector3 &bodyOmega, double dt, const Matrix9 &stateTransition, const Matrix93 &accelerationJacobian, const Matrix93 &omegaJacobian) override
	Update accumulated bias Jacobians after applying one IMU increment.
Protected Member Functions inherited from gtsam::ManifoldPreintegration
	ManifoldPreintegration ()
	Default constructor for serialization.
Protected Member Functions inherited from gtsam::PreintegrationBase
	PreintegrationBase ()
	Default constructor for serialization.
virtual	~PreintegrationBase ()
	Virtual destructor for serialization.

Additional Inherited Members
Public Types inherited from gtsam::PreintegrationBase
typedef imuBias::ConstantBias	Bias
typedef PreintegrationParams	Params
Protected Attributes inherited from gtsam::ManifoldPreintegration
NavState	deltaXij_
	Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into account velocity at time i, see deltap+, in [2] Note: velocity is now also in frame i, as opposed to deltaVij in [2].
Matrix3	delRdelBiasOmega_
	Jacobian of preintegrated rotation w.r.t. angular rate bias.
Matrix3	delPdelBiasAcc_
	Jacobian of preintegrated position w.r.t. acceleration bias.
Matrix3	delPdelBiasOmega_
	Jacobian of preintegrated position w.r.t. angular rate bias.
Matrix3	delVdelBiasAcc_
	Jacobian of preintegrated velocity w.r.t. acceleration bias.
Matrix3	delVdelBiasOmega_
	Jacobian of preintegrated velocity w.r.t. angular rate bias.
Protected Attributes inherited from gtsam::PreintegrationBase
std::shared_ptr< Params >	p_
Bias	biasHat_
	Acceleration and gyro bias used for preintegration.
double	deltaTij_
	Time interval from i to j.

Member Function Documentation

◆ biasCorrectedDelta()

Vector9 gtsam::LieGroupPreintegration::biasCorrectedDelta	(	const imuBias::ConstantBias &	bias,
		OptionalJacobian< 9, 6 >	H = {} ) const

overridevirtual

Return Brossard's right-corrected delta in PIM (R,p,v) ordering.

Implements gtsam::PreintegrationBase.

◆ updateBiasJacobians()

void gtsam::LieGroupPreintegration::updateBiasJacobians	(	const Rot3 &	oldRotation,
		const Vector3 &	bodyAcceleration,
		const Vector3 &	bodyOmega,
		double	dt,
		const Matrix9 &	stateTransition,
		const Matrix93 &	accelerationJacobian,
		const Matrix93 &	omegaJacobian )

overrideprotectedvirtual

Update accumulated bias Jacobians after applying one IMU increment.

Reimplemented from gtsam::ManifoldPreintegration.

◆ updateFactor()

void gtsam::LieGroupPreintegration::updateFactor	(	const Vector3 &	bodyAcceleration,
		const Vector3 &	bodyOmega,
		double	dt,
		OptionalJacobian< 9, 9 >	F = {},
		OptionalJacobian< 9, 3 >	G1 = {},
		OptionalJacobian< 9, 3 >	G2 = {} )

overrideprotectedvirtual

Apply one unbiased IMU increment with NavState's group exponential.

Reimplemented from gtsam::ManifoldPreintegration.

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/LieGroupPreintegration.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/LieGroupPreintegration.cpp

Detailed Description

Public Member Functions

Static Public Attributes

Protected Member Functions

Additional Inherited Members

Member Function Documentation

◆ biasCorrectedDelta()

◆ updateBiasJacobians()

◆ updateFactor()