#include <CombinedImuFactor.h>

Inheritance diagram for gtsam::PreintegratedCombinedMeasurements:

Collaboration diagram for gtsam::PreintegratedCombinedMeasurements:

[legend]

Public Types
typedef PreintegrationCombinedParams	Params

typedef imuBias::ConstantBias	Bias

Public Member Functions
Constructors
	PreintegratedCombinedMeasurements ()
	Default constructor only for serialization and wrappers.

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

	PreintegratedCombinedMeasurements (const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)

	~PreintegratedCombinedMeasurements () override
	Virtual destructor.

Basic utilities
void	resetIntegration () override
	Re-initialize PreintegratedCombinedMeasurements.

Params &	p () const
	const reference to params, shadows definition in base class

Access instance variables
Return pre-integrated measurement covariance
Matrix	preintMeasCov () const

Testable
print
void	print (const std::string &s="Preintegrated Measurements:") const override

bool	equals (const PreintegratedCombinedMeasurements &expected, double tol=1e-9) const
	equals

Main functionality
void	integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) 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

Basic utilities
Re-initialize PreintegratedMeasurements and set new bias
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

Instance variables access
const imuBias::ConstantBias &	biasHat () const

double	deltaTij () const

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

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 Attributes
Eigen::Matrix< double, 15, 15 >	preintMeasCov_

NavState	deltaXij_

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

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

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

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

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

std::shared_ptr< Params >	p_

Bias	biasHat_
	Acceleration and gyro bias used for preintegration.

double	deltaTij_
	Time interval from i to j.

Friends
class	CombinedImuFactor

Detailed Description

PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. The measurements are then used to build the CombinedImuFactor. Integration is done incrementally (ideally, one integrates the measurement as soon as it is received from the IMU) so as to avoid costly integration at time of factor construction.

Constructor & Destructor Documentation

◆ PreintegratedCombinedMeasurements() [1/2]

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

inline

Default constructor, initializes the class with no measurements

Parameters

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

◆ PreintegratedCombinedMeasurements() [2/2]

gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements	(	const PreintegrationType &	base,
		const Eigen::Matrix< double, 15, 15 > &	preintMeasCov
	)

inline

Construct preintegrated directly from members: base class and preintMeasCov

Parameters

base	PreintegrationType instance
preintMeasCov	Covariance matrix used in noise model.

Member Function Documentation

◆ biasCorrectedDelta()

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

overridevirtualinherited

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.

◆ clone()

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

inlinevirtualinherited

Dummy clone for MATLAB

◆ 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

inherited

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

inherited

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

◆ integrateMeasurement()

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

overridevirtual

Add a single IMU measurement to the preintegration. Both accelerometer and gyroscope measurements are taken to be in the sensor frame and conversion to the body frame is handled by body_P_sensor in PreintegrationParams.

Parameters

measuredAcc	Measured acceleration (as given by the sensor)
measuredOmega	Measured angular velocity (as given by the sensor)
dt	Time interval between two consecutive IMU measurements

Reimplemented from gtsam::PreintegrationBase.

◆ update()

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

overridevirtualinherited

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.

Member Data Documentation

◆ deltaXij_

NavState gtsam::ManifoldPreintegration::deltaXij_

protectedinherited

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]

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/CombinedImuFactor.h

Public Types

Public Member Functions

Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ PreintegratedCombinedMeasurements() [1/2]

◆ PreintegratedCombinedMeasurements() [2/2]

Member Function Documentation

◆ biasCorrectedDelta()

◆ clone()

◆ computeErrorAndJacobians()

◆ correctMeasurementsBySensorPose()

◆ integrateMeasurement()

◆ update()

Member Data Documentation

◆ deltaXij_