gtsam  4.0.0
gtsam
gtsam::TangentPreintegration Class Reference

Detailed Description

Integrate on the 9D tangent space of the NavState manifold.

See extensive discussion in ImuFactor.lyx

+ Inheritance diagram for gtsam::TangentPreintegration:

Main functionality

void update (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override
 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.
 
Vector9 biasCorrectedDelta (const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H=boost::none) const override
 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.
 
void mergeWith (const TangentPreintegration &pim, Matrix9 *H1, Matrix9 *H2)
 Merge in a different set of measurements and update bias derivatives accordingly The derivatives apply to the preintegrated Vector9.
 
static Vector9 UpdatePreintegrated (const Vector3 &a_body, const Vector3 &w_body, const double dt, const Vector9 &preintegrated, OptionalJacobian< 9, 9 > A=boost::none, OptionalJacobian< 9, 3 > B=boost::none, OptionalJacobian< 9, 3 > C=boost::none)
 
static Vector9 Compose (const Vector9 &zeta01, const Vector9 &zeta12, double deltaT12, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 9 > H2=boost::none)
 

Public Member Functions

virtual boost::shared_ptr< TangentPreintegrationclone () const
 Dummy clone for MATLAB.
 
Constructors/destructors
 TangentPreintegration (const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
 Constructor, initializes the variables in the base class. More...
 
virtual ~TangentPreintegration ()
 Virtual destructor.
 
Basic utilities

Re-initialize PreintegratedMeasurements

void resetIntegration () override
 
Instance variables access
Vector3 deltaPij () const override
 
Vector3 deltaVij () const override
 
Rot3 deltaRij () const override
 
NavState deltaXij () const override
 
const Vector9 & preintegrated () const
 
Vector3 theta () const
 
const Matrix93 & preintegrated_H_biasAcc () const
 
const Matrix93 & preintegrated_H_biasOmega () const
 
Testable
bool equals (const TangentPreintegration &other, double tol) const
 
- Public Member Functions inherited from gtsam::PreintegrationBase
 PreintegrationBase (const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
 Constructor, initializes the variables in the base class. More...
 
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
 
const imuBias::ConstantBiasbiasHat () const
 
double deltaTij () const
 
Vector6 biasHatVector () const
 
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 integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt)
 Version without derivatives.
 
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...
 
virtual void print (const std::string &s) const
 

Protected Member Functions

 TangentPreintegration ()
 Default constructor for serialization.
 
- Protected Member Functions inherited from gtsam::PreintegrationBase
 PreintegrationBase ()
 Default constructor for serialization.
 
virtual ~PreintegrationBase ()
 Virtual destructor for serialization.
 

Protected Attributes

Vector9 preintegrated_
 Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta, position, velocity.
 
Matrix93 preintegrated_H_biasAcc_
 Jacobian of preintegrated_ w.r.t. acceleration bias.
 
Matrix93 preintegrated_H_biasOmega_
 Jacobian of preintegrated_ w.r.t. angular rate bias.
 
- Protected Attributes inherited from gtsam::PreintegrationBase
boost::shared_ptr< Paramsp_
 Parameters. More...
 
Bias biasHat_
 Acceleration and gyro bias used for preintegration.
 
double deltaTij_
 Time interval from i to j.
 

Friends

class boost::serialization::access
 Serialization function.
 

Additional Inherited Members

- Public Types inherited from gtsam::PreintegrationBase
typedef imuBias::ConstantBias Bias
 
typedef PreintegrationParams Params
 

Constructor & Destructor Documentation

◆ TangentPreintegration()

gtsam::TangentPreintegration::TangentPreintegration ( 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

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