Public Types | Protected Member Functions | Protected Attributes | List of all members
gtsam::PreintegrationBase Class Referenceabstract

#include <PreintegrationBase.h>

Inheritance diagram for gtsam::PreintegrationBase:
Inheritance graph
[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...
 
Paramsp () const
 const reference to params More...
 
Instance variables access
const imuBias::ConstantBiasbiasHat () 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< Paramsp_
 

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, Vector3correctMeasurementsBySensorPose (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

Definition at line 43 of file PreintegrationBase.h.

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
pParameters, typically fixed in a single application
biasCurrent 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
virtual Rot3 gtsam::PreintegrationBase::deltaRij ( ) const
pure virtual
double gtsam::PreintegrationBase::deltaTij ( ) const
inline

Definition at line 105 of file PreintegrationBase.h.

virtual Vector3 gtsam::PreintegrationBase::deltaVij ( ) const
pure virtual
virtual NavState gtsam::PreintegrationBase::deltaXij ( ) const
pure virtual
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
virtual void gtsam::PreintegrationBase::resetIntegration ( )
pure virtual
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:


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:25