#include <PreintegrationBase.h>

Inheritance diagram for gtsam::PreintegrationBase:

[legend]

Collaboration diagram for gtsam::PreintegrationBase:

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.

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

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.

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.

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.

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.

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.

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.

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

◆ 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

◆ correctMeasurementsBySensorPose()

std::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

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

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

/home/docs/checkouts/readthedocs.org/user_builds/gtsam-jlblanco-docs/checkouts/latest/gtsam/navigation/PreintegrationBase.h

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Testable