gtsam  4.0.0
gtsam::PreintegrationBase Class Referenceabstract

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.

+ Inheritance diagram for gtsam::PreintegrationBase:


virtual void print (const std::string &s) const
GTSAM_EXPORT friend std::ostream & operator<< (std::ostream &os, const PreintegrationBase &pim)

Public Member Functions

 PreintegrationBase (const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
 Constructor, initializes the variables in the base class. More...
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 boost::shared_ptr< Params > & params () const
 shared pointer to params
const Paramsp () const
 const reference to params
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
Main functionality
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
 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.
virtual void update (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C)=0
 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.
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=boost::none) const =0
 Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU measurements so far.
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.
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=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. More...

Public Types

typedef imuBias::ConstantBias Bias
typedef PreintegrationParams Params

Protected Member Functions

 PreintegrationBase ()
 Default constructor for serialization.
virtual ~PreintegrationBase ()
 Virtual destructor for serialization.

Protected Attributes

boost::shared_ptr< Paramsp_
 Parameters. More...
Bias biasHat_
 Acceleration and gyro bias used for preintegration.
double deltaTij_
 Time interval from i to j.

Constructor & Destructor Documentation

◆ PreintegrationBase()

gtsam::PreintegrationBase::PreintegrationBase ( const boost::shared_ptr< Params > &  p,
const imuBias::ConstantBias biasHat = imuBias::ConstantBias() 

Constructor, initializes the variables in the base class.

pParameters, typically fixed in a single application
biasCurrent estimate of acceleration and rotation rate biases

Member Function Documentation

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

Member Data Documentation

◆ p_

boost::shared_ptr<Params> gtsam::PreintegrationBase::p_


Declared mutable only for deprecated predict method. TODO(frank): make const once deprecated method is removed

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