#include <ManifoldPreintegration.h>

Inheritance diagram for gtsam::ManifoldPreintegration:

Public Member Functions
Constructors
	ManifoldPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())

Basic utilities
Re-initialize PreintegratedMeasurements
void	resetIntegration () override

Instance variables access
NavState	deltaXij () const override

Rot3	deltaRij () const override

Vector3	deltaPij () const override

Vector3	deltaVij () const override

Matrix3	delRdelBiasOmega () const

Matrix3	delPdelBiasAcc () const

Matrix3	delPdelBiasOmega () const

Matrix3	delVdelBiasAcc () const

Matrix3	delVdelBiasOmega () const

Testable
bool	equals (const ManifoldPreintegration &other, double tol) const

Main functionality
void	update (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 A, Matrix93 B, Matrix93 *C) override

Vector9	biasCorrectedDelta (const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override

virtual std::shared_ptr< ManifoldPreintegration >	clone () const

Public Member Functions inherited from gtsam::PreintegrationBase
	PreintegrationBase (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())

void	resetIntegrationAndSetBias (const Bias &biasHat)

bool	matchesParamsWith (const PreintegrationBase &other) const
	check parameters equality: checks whether shared pointer points to same Params object. More...

const std::shared_ptr< Params > &	params () const
	shared pointer to params More...

Params &	p () const
	const reference to params More...

const imuBias::ConstantBias &	biasHat () const

double	deltaTij () const

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

virtual void	integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt)
	Version without derivatives. More...

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

Vector9	computeError (const NavState &state_i, const NavState &state_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1, OptionalJacobian< 9, 9 > H2, OptionalJacobian< 9, 6 > H3) const
	Calculate error given navStates. More...

Vector9	computeErrorAndJacobians (const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H1={}, OptionalJacobian< 9, 3 > H2={}, OptionalJacobian< 9, 6 > H3={}, OptionalJacobian< 9, 3 > H4={}, OptionalJacobian< 9, 6 > H5={}) const

virtual void	print (const std::string &s="") const

Protected Member Functions
	ManifoldPreintegration ()
	Default constructor for serialization. More...

Protected Member Functions inherited from gtsam::PreintegrationBase
	PreintegrationBase ()
	Default constructor for serialization. More...

virtual	~PreintegrationBase ()
	Virtual destructor for serialization. More...

Protected Attributes
Matrix3	delPdelBiasAcc_
	Jacobian of preintegrated position w.r.t. acceleration bias. More...

Matrix3	delPdelBiasOmega_
	Jacobian of preintegrated position w.r.t. angular rate bias. More...

Matrix3	delRdelBiasOmega_
	Jacobian of preintegrated rotation w.r.t. angular rate bias. More...

NavState	deltaXij_

Matrix3	delVdelBiasAcc_
	Jacobian of preintegrated velocity w.r.t. acceleration bias. More...

Matrix3	delVdelBiasOmega_
	Jacobian of preintegrated velocity w.r.t. angular rate bias. More...

Protected Attributes inherited from gtsam::PreintegrationBase
Bias	biasHat_
	Acceleration and gyro bias used for preintegration. More...

double	deltaTij_
	Time interval from i to j. More...

std::shared_ptr< Params >	p_

Additional Inherited Members
Public Types inherited from gtsam::PreintegrationBase
typedef imuBias::ConstantBias	Bias

typedef PreintegrationParams	Params

Detailed Description

IMU pre-integration on NavState manifold. This corresponds to the original RSS paper (with one difference: V is rotated)

Definition at line 33 of file ManifoldPreintegration.h.

Constructor & Destructor Documentation

◆ ManifoldPreintegration() [1/2]

gtsam::ManifoldPreintegration::ManifoldPreintegration ( )

inlineprotected

Default constructor for serialization.

Definition at line 49 of file ManifoldPreintegration.h.

◆ ManifoldPreintegration() [2/2]

gtsam::ManifoldPreintegration::ManifoldPreintegration	(	const std::shared_ptr< Params > &	p,
		const imuBias::ConstantBias &	biasHat = `imuBias::ConstantBias()`
	)

Constructor, initializes the variables in the base class

Parameters

p	Parameters, typically fixed in a single application
bias	Current estimate of acceleration and rotation rate biases

Definition at line 29 of file ManifoldPreintegration.cpp.

Member Function Documentation

◆ biasCorrectedDelta()

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

overridevirtual

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

Implements gtsam::PreintegrationBase.

Definition at line 112 of file ManifoldPreintegration.cpp.

◆ clone()

virtual std::shared_ptr<ManifoldPreintegration> gtsam::ManifoldPreintegration::clone ( ) const

inlinevirtual

Dummy clone for MATLAB

Definition at line 109 of file ManifoldPreintegration.h.

◆ delPdelBiasAcc()

Matrix3 gtsam::ManifoldPreintegration::delPdelBiasAcc ( ) const

inline

Definition at line 82 of file ManifoldPreintegration.h.

◆ delPdelBiasOmega()

Matrix3 gtsam::ManifoldPreintegration::delPdelBiasOmega ( ) const

inline

Definition at line 83 of file ManifoldPreintegration.h.

◆ delRdelBiasOmega()

Matrix3 gtsam::ManifoldPreintegration::delRdelBiasOmega ( ) const

inline

Definition at line 81 of file ManifoldPreintegration.h.

◆ deltaPij()

Vector3 gtsam::ManifoldPreintegration::deltaPij ( ) const

inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 78 of file ManifoldPreintegration.h.

◆ deltaRij()

Rot3 gtsam::ManifoldPreintegration::deltaRij ( ) const

inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 77 of file ManifoldPreintegration.h.

◆ deltaVij()

Vector3 gtsam::ManifoldPreintegration::deltaVij ( ) const

inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 79 of file ManifoldPreintegration.h.

◆ deltaXij()

NavState gtsam::ManifoldPreintegration::deltaXij ( ) const

inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 76 of file ManifoldPreintegration.h.

◆ delVdelBiasAcc()

Matrix3 gtsam::ManifoldPreintegration::delVdelBiasAcc ( ) const

inline

Definition at line 84 of file ManifoldPreintegration.h.

◆ delVdelBiasOmega()

Matrix3 gtsam::ManifoldPreintegration::delVdelBiasOmega ( ) const

inline

Definition at line 85 of file ManifoldPreintegration.h.

◆ equals()

bool gtsam::ManifoldPreintegration::equals	(	const ManifoldPreintegration &	other,
		double	tol
	)		const

Definition at line 47 of file ManifoldPreintegration.cpp.

◆ resetIntegration()

void gtsam::ManifoldPreintegration::resetIntegration ( )

overridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 36 of file ManifoldPreintegration.cpp.

◆ update()

void gtsam::ManifoldPreintegration::update	(	const Vector3 &	measuredAcc,
		const Vector3 &	measuredOmega,
		const double	dt,
		Matrix9 *	A,
		Matrix93 *	B,
		Matrix93 *	C
	)

overridevirtual

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

Implements gtsam::PreintegrationBase.

Definition at line 60 of file ManifoldPreintegration.cpp.

Member Data Documentation

◆ delPdelBiasAcc_

Matrix3 gtsam::ManifoldPreintegration::delPdelBiasAcc_

protected

Jacobian of preintegrated position w.r.t. acceleration bias.

Definition at line 43 of file ManifoldPreintegration.h.

◆ delPdelBiasOmega_

Matrix3 gtsam::ManifoldPreintegration::delPdelBiasOmega_

protected

Jacobian of preintegrated position w.r.t. angular rate bias.

Definition at line 44 of file ManifoldPreintegration.h.

◆ delRdelBiasOmega_

Matrix3 gtsam::ManifoldPreintegration::delRdelBiasOmega_

protected

Jacobian of preintegrated rotation w.r.t. angular rate bias.

Definition at line 42 of file ManifoldPreintegration.h.

◆ deltaXij_

NavState gtsam::ManifoldPreintegration::deltaXij_

protected

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]

Definition at line 41 of file ManifoldPreintegration.h.

◆ delVdelBiasAcc_

Matrix3 gtsam::ManifoldPreintegration::delVdelBiasAcc_

protected

Jacobian of preintegrated velocity w.r.t. acceleration bias.

Definition at line 45 of file ManifoldPreintegration.h.

◆ delVdelBiasOmega_

Matrix3 gtsam::ManifoldPreintegration::delVdelBiasOmega_

protected

Jacobian of preintegrated velocity w.r.t. angular rate bias.

Definition at line 46 of file ManifoldPreintegration.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ ManifoldPreintegration() [1/2]

◆ ManifoldPreintegration() [2/2]

Member Function Documentation

◆ biasCorrectedDelta()

◆ clone()

◆ delPdelBiasAcc()

◆ delPdelBiasOmega()

◆ delRdelBiasOmega()

◆ deltaPij()

◆ deltaRij()

◆ deltaVij()

◆ deltaXij()

◆ delVdelBiasAcc()

◆ delVdelBiasOmega()

◆ equals()

◆ resetIntegration()

◆ update()

Member Data Documentation

◆ delPdelBiasAcc_

◆ delPdelBiasOmega_

◆ delRdelBiasOmega_

◆ deltaXij_

◆ delVdelBiasAcc_

◆ delVdelBiasOmega_