#include <PreintegrationBase.h>

Inheritance diagram for gtsam::PreintegrationBase:

[legend]

Public Types
typedef imuBias::ConstantBias	Bias

typedef PreintegrationParams	Params

Public Member Functions
Constructors
	PreintegrationBase (const boost::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 boost::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

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

boost::shared_ptr< Params >	p_

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

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

Main functionality
class	boost::serialization::access

std::pair< Vector3, Vector3 >	correctMeasurementsBySensorPose (const Vector3 &unbiasedAcc, const Vector3 &unbiasedOmega, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedAcc=boost::none, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedOmega=boost::none, OptionalJacobian< 3, 3 > correctedOmega_H_unbiasedOmega=boost::none) 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=boost::none) const =0

NavState	predict (const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 6 > H2=boost::none) 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=boost::none, OptionalJacobian< 9, 3 > H2=boost::none, OptionalJacobian< 9, 6 > H3=boost::none, OptionalJacobian< 9, 3 > H4=boost::none, OptionalJacobian< 9, 6 > H5=boost::none) const

template<class ARCHIVE >
void	serialize (ARCHIVE &ar, const unsigned int)

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

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

Definition at line 43 of file PreintegrationBase.h.

typedef PreintegrationParams gtsam::PreintegrationBase::Params

Definition at line 44 of file PreintegrationBase.h.

Constructor & Destructor Documentation

gtsam::PreintegrationBase::PreintegrationBase ( )

inlineprotected

Default constructor for serialization.

Definition at line 56 of file PreintegrationBase.h.

virtual gtsam::PreintegrationBase::~PreintegrationBase ( )

inlineprotectedvirtual

Virtual destructor for serialization.

Definition at line 59 of file PreintegrationBase.h.

gtsam::PreintegrationBase::PreintegrationBase	(	const boost::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 32 of file PreintegrationBase.cpp.

Member Function Documentation

virtual Vector9 gtsam::PreintegrationBase::biasCorrectedDelta	(	const imuBias::ConstantBias &	bias_i,
		OptionalJacobian< 9, 6 >	H = `boost::none`
	)		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.

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

inline

Definition at line 104 of file PreintegrationBase.h.

Vector6 gtsam::PreintegrationBase::biasHatVector ( ) const

inline

Definition at line 113 of file PreintegrationBase.h.

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 142 of file PreintegrationBase.cpp.

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 = `boost::none`,
		OptionalJacobian< 9, 3 >	H2 = `boost::none`,
		OptionalJacobian< 9, 6 >	H3 = `boost::none`,
		OptionalJacobian< 9, 3 >	H4 = `boost::none`,
		OptionalJacobian< 9, 6 >	H5 = `boost::none`
	)		const

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

Definition at line 168 of file PreintegrationBase.cpp.

pair< Vector3, Vector3 > gtsam::PreintegrationBase::correctMeasurementsBySensorPose	(	const Vector3 &	unbiasedAcc,
		const Vector3 &	unbiasedOmega,
		OptionalJacobian< 3, 3 >	correctedAcc_H_unbiasedAcc = `boost::none`,
		OptionalJacobian< 3, 3 >	correctedAcc_H_unbiasedOmega = `boost::none`,
		OptionalJacobian< 3, 3 >	correctedOmega_H_unbiasedOmega = `boost::none`
	)		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 60 of file PreintegrationBase.cpp.

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

pure virtual

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

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

pure virtual

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

double gtsam::PreintegrationBase::deltaTij ( ) const

inline

Definition at line 105 of file PreintegrationBase.h.

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

pure virtual

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

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

pure virtual

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

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 106 of file PreintegrationBase.cpp.

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.

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

inline

const reference to params

Definition at line 96 of file PreintegrationBase.h.

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

inline

shared pointer to params

Definition at line 91 of file PreintegrationBase.h.

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

Predict state at time j.

Definition at line 116 of file PreintegrationBase.cpp.

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

virtual

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

Definition at line 49 of file PreintegrationBase.cpp.

virtual void gtsam::PreintegrationBase::resetIntegration ( )

pure virtual

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

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

Definition at line 54 of file PreintegrationBase.cpp.

template<class ARCHIVE >

void gtsam::PreintegrationBase::serialize	(	ARCHIVE &	ar,
		const unsigned	int
	)

inlineprivate

Definition at line 179 of file PreintegrationBase.h.

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

friend class boost::serialization::access

friend

Serialization function

Definition at line 177 of file PreintegrationBase.h.

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

friend

Member Data Documentation

Bias gtsam::PreintegrationBase::biasHat_

protected

Acceleration and gyro bias used for preintegration.

Definition at line 50 of file PreintegrationBase.h.

double gtsam::PreintegrationBase::deltaTij_

protected

Time interval from i to j.

Definition at line 53 of file PreintegrationBase.h.

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

Main functionality

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation