#include <PreintegrationBase.h>

Inheritance diagram for gtsam::PreintegrationBase:

[legend]

Public Types
typedef imuBias::ConstantBias	Bias

typedef PreintegrationParams	Params

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

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

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

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

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

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

virtual void	update (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 A, Matrix93 B, Matrix93 *C)=0

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

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

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

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

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

Protected Attributes
Bias	biasHat_
	Acceleration and gyro bias used for preintegration. More...

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

std::shared_ptr< Params >	p_

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

GTSAM_EXPORT friend std::ostream &	operator<< (std::ostream &os, const PreintegrationBase &pim)

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.

Definition at line 41 of file PreintegrationBase.h.

Member Typedef Documentation

◆ Bias

typedef imuBias::ConstantBias gtsam::PreintegrationBase::Bias

Definition at line 43 of file PreintegrationBase.h.

◆ Params

typedef PreintegrationParams gtsam::PreintegrationBase::Params

Definition at line 44 of file PreintegrationBase.h.

Constructor & Destructor Documentation

◆ PreintegrationBase() [1/2]

gtsam::PreintegrationBase::PreintegrationBase ( )

inlineprotected

Default constructor for serialization.

Definition at line 56 of file PreintegrationBase.h.

◆ ~PreintegrationBase()

virtual gtsam::PreintegrationBase::~PreintegrationBase ( )

inlineprotectedvirtual

Virtual destructor for serialization.

Definition at line 59 of file PreintegrationBase.h.

◆ PreintegrationBase() [2/2]

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

Definition at line 33 of file PreintegrationBase.cpp.

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::TangentPreintegration, and gtsam::ManifoldPreintegration.

◆ biasHat()

const imuBias::ConstantBias& gtsam::PreintegrationBase::biasHat ( ) const

inline

Definition at line 104 of file PreintegrationBase.h.

◆ biasHatVector()

Vector6 gtsam::PreintegrationBase::biasHatVector ( ) const

inline

Definition at line 113 of file PreintegrationBase.h.

◆ computeError()

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

Definition at line 143 of file PreintegrationBase.cpp.

◆ computeErrorAndJacobians()

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

Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j

Definition at line 169 of file PreintegrationBase.cpp.

◆ correctMeasurementsBySensorPose()

pair< Vector3, Vector3 > gtsam::PreintegrationBase::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

Definition at line 61 of file PreintegrationBase.cpp.

◆ deltaPij()

virtual Vector3 gtsam::PreintegrationBase::deltaPij ( ) const

pure virtual

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

◆ deltaRij()

virtual Rot3 gtsam::PreintegrationBase::deltaRij ( ) const

pure virtual

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

◆ deltaTij()

double gtsam::PreintegrationBase::deltaTij ( ) const

inline

Definition at line 105 of file PreintegrationBase.h.

◆ deltaVij()

virtual Vector3 gtsam::PreintegrationBase::deltaVij ( ) const

pure virtual

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

◆ deltaXij()

virtual NavState gtsam::PreintegrationBase::deltaXij ( ) const

pure virtual

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

◆ integrateMeasurement()

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

virtual

Version without derivatives.

Reimplemented in gtsam::PreintegratedCombinedMeasurements, and gtsam::PreintegratedImuMeasurements.

Definition at line 107 of file PreintegrationBase.cpp.

◆ matchesParamsWith()

bool gtsam::PreintegrationBase::matchesParamsWith ( const PreintegrationBase & other ) const

inline

check parameters equality: checks whether shared pointer points to same Params object.

Definition at line 86 of file PreintegrationBase.h.

◆ p()

Params& gtsam::PreintegrationBase::p ( ) const

inline

const reference to params

Definition at line 96 of file PreintegrationBase.h.

◆ params()

const std::shared_ptr<Params>& gtsam::PreintegrationBase::params ( ) const

inline

shared pointer to params

Definition at line 91 of file PreintegrationBase.h.

◆ predict()

NavState gtsam::PreintegrationBase::predict	(	const NavState &	state_i,
		const imuBias::ConstantBias &	bias_i,
		OptionalJacobian< 9, 9 >	H1 = `{}`,
		OptionalJacobian< 9, 6 >	H2 = `{}`
	)		const

Predict state at time j.

Definition at line 117 of file PreintegrationBase.cpp.

◆ print()

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

virtual

Reimplemented in gtsam::PreintegratedCombinedMeasurements, and gtsam::PreintegratedImuMeasurements.

Definition at line 50 of file PreintegrationBase.cpp.

◆ resetIntegration()

virtual void gtsam::PreintegrationBase::resetIntegration ( )

pure virtual

Implemented in gtsam::PreintegratedCombinedMeasurements, gtsam::PreintegratedImuMeasurements, gtsam::ManifoldPreintegration, and gtsam::TangentPreintegration.

◆ resetIntegrationAndSetBias()

void gtsam::PreintegrationBase::resetIntegrationAndSetBias ( const Bias & biasHat )

Definition at line 55 of file PreintegrationBase.cpp.

◆ 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::TangentPreintegration, and gtsam::ManifoldPreintegration.

Friends And Related Function Documentation

◆ operator<<

GTSAM_EXPORT friend std::ostream& operator<<	(	std::ostream &	os,
		const PreintegrationBase &	pim
	)

friend

Member Data Documentation

◆ biasHat_

Bias gtsam::PreintegrationBase::biasHat_

protected

Acceleration and gyro bias used for preintegration.

Definition at line 50 of file PreintegrationBase.h.

◆ deltaTij_

double gtsam::PreintegrationBase::deltaTij_

protected

Time interval from i to j.

Definition at line 53 of file PreintegrationBase.h.

◆ p_

std::shared_ptr<Params> gtsam::PreintegrationBase::p_

protected

Definition at line 47 of file PreintegrationBase.h.

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Testable

Detailed Description

Member Typedef Documentation

◆ Bias

◆ Params

Constructor & Destructor Documentation

◆ PreintegrationBase() [1/2]

◆ ~PreintegrationBase()

◆ PreintegrationBase() [2/2]

Member Function Documentation

◆ biasCorrectedDelta()

◆ biasHat()

◆ biasHatVector()

◆ computeError()

◆ computeErrorAndJacobians()

◆ correctMeasurementsBySensorPose()

◆ deltaPij()

◆ deltaRij()

◆ deltaTij()

◆ deltaVij()

◆ deltaXij()

◆ integrateMeasurement()

◆ matchesParamsWith()

◆ p()

◆ params()

◆ predict()

◆ print()

◆ resetIntegration()

◆ resetIntegrationAndSetBias()

◆ update()

Friends And Related Function Documentation

◆ operator<<

Member Data Documentation

◆ biasHat_

◆ deltaTij_

◆ p_