#include <TangentPreintegration.h>

Inheritance diagram for gtsam::TangentPreintegration:

Public Member Functions
virtual std::shared_ptr< TangentPreintegration >	clone () const

Constructors/destructors
	TangentPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())

	~TangentPreintegration () override
	Virtual destructor. More...

Basic utilities
Re-initialize PreintegratedMeasurements
void	resetIntegration () override

Instance variables access
Vector3	deltaPij () const override

Vector3	deltaVij () const override

Rot3	deltaRij () const override

NavState	deltaXij () const override

const Vector9 &	preintegrated () const

Vector3	theta () const

const Matrix93 &	preintegrated_H_biasAcc () const

const Matrix93 &	preintegrated_H_biasOmega () const

Testable
bool	equals (const TangentPreintegration &other, double tol) 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
	TangentPreintegration ()
	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
Vector9	preintegrated_

Matrix93	preintegrated_H_biasAcc_
	Jacobian of preintegrated_ w.r.t. acceleration bias. More...

Matrix93	preintegrated_H_biasOmega_
	Jacobian of preintegrated_ 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_

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

void	mergeWith (const TangentPreintegration &pim, Matrix9 H1, Matrix9 H2)

static Vector9	UpdatePreintegrated (const Vector3 &a_body, const Vector3 &w_body, const double dt, const Vector9 &preintegrated, OptionalJacobian< 9, 9 > A={}, OptionalJacobian< 9, 3 > B={}, OptionalJacobian< 9, 3 > C={})

static Vector9	Compose (const Vector9 &zeta01, const Vector9 &zeta12, double deltaT12, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={})

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

typedef PreintegrationParams	Params

Detailed Description

Integrate on the 9D tangent space of the NavState manifold. See extensive discussion in ImuFactor.lyx

Definition at line 28 of file TangentPreintegration.h.

Constructor & Destructor Documentation

◆ TangentPreintegration() [1/2]

gtsam::TangentPreintegration::TangentPreintegration ( )

inlineprotected

Default constructor for serialization.

Definition at line 40 of file TangentPreintegration.h.

◆ TangentPreintegration() [2/2]

gtsam::TangentPreintegration::TangentPreintegration	(	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 26 of file TangentPreintegration.cpp.

◆ ~TangentPreintegration()

gtsam::TangentPreintegration::~TangentPreintegration ( )

inlineoverride

Virtual destructor.

Definition at line 57 of file TangentPreintegration.h.

Member Function Documentation

◆ biasCorrectedDelta()

Vector9 gtsam::TangentPreintegration::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 144 of file TangentPreintegration.cpp.

◆ clone()

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

inlinevirtual

Dummy clone for MATLAB

Definition at line 123 of file TangentPreintegration.h.

◆ Compose()

Vector9 gtsam::TangentPreintegration::Compose	(	const Vector9 &	zeta01,
		const Vector9 &	zeta12,
		double	deltaT12,
		OptionalJacobian< 9, 9 >	H1 = `{}`,
		OptionalJacobian< 9, 9 >	H2 = `{}`
	)

static

Definition at line 172 of file TangentPreintegration.cpp.

◆ deltaPij()

Vector3 gtsam::TangentPreintegration::deltaPij ( ) const

inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 71 of file TangentPreintegration.h.

◆ deltaRij()

Rot3 gtsam::TangentPreintegration::deltaRij ( ) const

inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 73 of file TangentPreintegration.h.

◆ deltaVij()

Vector3 gtsam::TangentPreintegration::deltaVij ( ) const

inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 72 of file TangentPreintegration.h.

◆ deltaXij()

NavState gtsam::TangentPreintegration::deltaXij ( ) const

inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 74 of file TangentPreintegration.h.

◆ equals()

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

Definition at line 41 of file TangentPreintegration.cpp.

◆ mergeWith()

void gtsam::TangentPreintegration::mergeWith	(	const TangentPreintegration &	pim,
		Matrix9 *	H1,
		Matrix9 *	H2
	)

Merge in a different set of measurements and update bias derivatives accordingly The derivatives apply to the preintegrated Vector9

Definition at line 216 of file TangentPreintegration.cpp.

◆ preintegrated()

const Vector9& gtsam::TangentPreintegration::preintegrated ( ) const

inline

Definition at line 76 of file TangentPreintegration.h.

◆ preintegrated_H_biasAcc()

const Matrix93& gtsam::TangentPreintegration::preintegrated_H_biasAcc ( ) const

inline

Definition at line 78 of file TangentPreintegration.h.

◆ preintegrated_H_biasOmega()

const Matrix93& gtsam::TangentPreintegration::preintegrated_H_biasOmega ( ) const

inline

Definition at line 79 of file TangentPreintegration.h.

◆ resetIntegration()

void gtsam::TangentPreintegration::resetIntegration ( )

overridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 33 of file TangentPreintegration.cpp.

◆ theta()

Vector3 gtsam::TangentPreintegration::theta ( ) const

inline

Definition at line 77 of file TangentPreintegration.h.

◆ update()

void gtsam::TangentPreintegration::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 105 of file TangentPreintegration.cpp.

◆ UpdatePreintegrated()

Vector9 gtsam::TangentPreintegration::UpdatePreintegrated	(	const Vector3 &	a_body,
		const Vector3 &	w_body,
		const double	dt,
		const Vector9 &	preintegrated,
		OptionalJacobian< 9, 9 >	A = `{}`,
		OptionalJacobian< 9, 3 >	B = `{}`,
		OptionalJacobian< 9, 3 >	C = `{}`
	)

static

Definition at line 54 of file TangentPreintegration.cpp.

Member Data Documentation

◆ preintegrated_

Vector9 gtsam::TangentPreintegration::preintegrated_

protected

Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta, position, velocity

Definition at line 35 of file TangentPreintegration.h.

◆ preintegrated_H_biasAcc_

Matrix93 gtsam::TangentPreintegration::preintegrated_H_biasAcc_

protected

Jacobian of preintegrated_ w.r.t. acceleration bias.

Definition at line 36 of file TangentPreintegration.h.

◆ preintegrated_H_biasOmega_

Matrix93 gtsam::TangentPreintegration::preintegrated_H_biasOmega_

protected

Jacobian of preintegrated_ w.r.t. angular rate bias.

Definition at line 37 of file TangentPreintegration.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Main functionality

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ TangentPreintegration() [1/2]

◆ TangentPreintegration() [2/2]

◆ ~TangentPreintegration()

Member Function Documentation

◆ biasCorrectedDelta()

◆ clone()

◆ Compose()

◆ deltaPij()

◆ deltaRij()

◆ deltaVij()

◆ deltaXij()

◆ equals()

◆ mergeWith()

◆ preintegrated()

◆ preintegrated_H_biasAcc()

◆ preintegrated_H_biasOmega()

◆ resetIntegration()

◆ theta()

◆ update()

◆ UpdatePreintegrated()

Member Data Documentation

◆ preintegrated_

◆ preintegrated_H_biasAcc_

◆ preintegrated_H_biasOmega_