Detailed Description

PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor).

It includes the definitions of the preintegrated variables and the methods to access, print, and compare them.

Inheritance diagram for gtsam::PreintegrationBase:

Testable
virtual void	print (const std::string &s="") const
GTSAM_EXPORT friend std::ostream &	operator<< (std::ostream &os, const PreintegrationBase &pim)

Public Member Functions
Constructors
	PreintegrationBase (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
	Constructor, initializes the variables in the base class.
Basic utilities
Re-initialize PreintegratedMeasurements and set new bias
virtual void	resetIntegration ()=0
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
Instance variables access
const imuBias::ConstantBias &	biasHat () const
double	deltaTij () const
virtual Vector3	deltaPij () const =0
virtual Vector3	deltaVij () const =0
virtual Rot3	deltaRij () const =0
virtual NavState	deltaXij () const =0
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
Main functionality
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	update (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 A, Matrix93 B, Matrix93 *C)=0
	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.
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.
virtual Vector9	biasCorrectedDelta (const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const =0
	Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU measurements so far.
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.

Static Public Attributes
static constexpr bool	kLegacyUsesLogmap = false
	Legacy factor-error choice for this preintegration backend.

Public Types
typedef imuBias::ConstantBias	Bias
typedef PreintegrationParams	Params

Protected Member Functions
	PreintegrationBase ()
	Default constructor for serialization.
virtual	~PreintegrationBase ()
	Virtual destructor for serialization.

Protected Attributes
std::shared_ptr< Params >	p_
Bias	biasHat_
	Acceleration and gyro bias used for preintegration.
double	deltaTij_
	Time interval from i to j.

Constructor & Destructor Documentation

◆ PreintegrationBase()

gtsam::PreintegrationBase::PreintegrationBase	(	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

Member Function Documentation

◆ biasCorrectedDelta()

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

pure virtual

Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU measurements so far.

Implemented in gtsam::GalileanPreintegration, gtsam::LieGroupPreintegration, gtsam::ManifoldPreintegration, and gtsam::TangentPreintegration.

◆ deskewPoints()

Matrix gtsam::PreintegrationBase::deskewPoints	(	ConstMatrixView	points,
		const Vector3 &	velocity_i = Vector3::Zero() ) const

Deskew endpoint-exclusive ordered point batches over the integration interval.

Each column is a batch containing one or more stacked 3-vectors, so points must have shape 3m x n.

◆ integrateMeasurement()

void gtsam::PreintegrationBase::integrateMeasurement	(	const Vector3 &	measuredAcc,
		const Vector3 &	measuredOmega,
		const double	dt )

virtual

Version without derivatives.

Reimplemented in gtsam::PreintegratedCombinedMeasurementsT< DefaultPreintegrationType >, gtsam::PreintegratedCombinedMeasurementsT< GalileanPreintegration >, gtsam::PreintegratedImuMeasurementsT< DefaultPreintegrationType >, and gtsam::PreintegratedImuMeasurementsT< GalileanPreintegration >.

◆ predict()

NavState gtsam::PreintegrationBase::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.

This overload allows gravity to differ from params (eg. when gravity is an optimized variable, see ImuFactorWithGravityDirection and ImuFactorWithGravityVector); H3 is the Jacobian wrt that vector.

◆ print()

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

virtual

Reimplemented in gtsam::PreintegratedImuMeasurementsT< DefaultPreintegrationType >, and gtsam::PreintegratedImuMeasurementsT< GalileanPreintegration >.

◆ resetIntegration()

virtual void gtsam::PreintegrationBase::resetIntegration ( )

pure virtual

Implemented in gtsam::GalileanPreintegration, gtsam::PreintegratedCombinedMeasurementsT< DefaultPreintegrationType >, gtsam::PreintegratedCombinedMeasurementsT< GalileanPreintegration >, gtsam::PreintegratedImuMeasurementsT< DefaultPreintegrationType >, and gtsam::PreintegratedImuMeasurementsT< GalileanPreintegration >.

◆ so3TangentAt()

Vector3 gtsam::PreintegrationBase::so3TangentAt ( double t ) const

virtual

Return the SO(3) tangent vector at local time t in [0, deltaTij].

Reimplemented in gtsam::TangentPreintegration.

◆ update()

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

pure virtual

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.

Implemented in gtsam::GalileanPreintegration, gtsam::ManifoldPreintegration, and gtsam::TangentPreintegration.

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

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

Detailed Description

Testable

Public Member Functions

Static Public Attributes

Public Types

Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

◆ PreintegrationBase()

Member Function Documentation

◆ biasCorrectedDelta()

◆ deskewPoints()

◆ integrateMeasurement()

◆ predict()

◆ print()

◆ resetIntegration()

◆ so3TangentAt()

◆ update()