Protected Member Functions | Protected Attributes | List of all members
gtsam::ManifoldPreintegration Class Reference

#include <ManifoldPreintegration.h>

Inheritance diagram for gtsam::ManifoldPreintegration:
Inheritance graph
[legend]

Public Member Functions

Constructors
 ManifoldPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
 
Basic utilities

Re-initialize PreintegratedMeasurements

void resetIntegration () 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< ManifoldPreintegrationclone () const
 
- Public Member Functions inherited from gtsam::PreintegrationBase
 PreintegrationBase (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
 
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 std::shared_ptr< Params > & params () const
 shared pointer to params More...
 
Paramsp () const
 const reference to params More...
 
const imuBias::ConstantBiasbiasHat () const
 
double deltaTij () const
 
Vector6 biasHatVector () const
 
std::pair< Vector3, Vector3correctMeasurementsBySensorPose (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
 
virtual void integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt)
 Version without derivatives. More...
 
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. 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={}, OptionalJacobian< 9, 3 > H2={}, OptionalJacobian< 9, 6 > H3={}, OptionalJacobian< 9, 3 > H4={}, OptionalJacobian< 9, 6 > H5={}) const
 
virtual void print (const std::string &s="") const
 

Protected Member Functions

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

Protected Attributes

Matrix3 delPdelBiasAcc_
 Jacobian of preintegrated position w.r.t. acceleration bias. More...
 
Matrix3 delPdelBiasOmega_
 Jacobian of preintegrated position w.r.t. angular rate bias. More...
 
Matrix3 delRdelBiasOmega_
 Jacobian of preintegrated rotation w.r.t. angular rate bias. More...
 
NavState deltaXij_
 
Matrix3 delVdelBiasAcc_
 Jacobian of preintegrated velocity w.r.t. acceleration bias. More...
 
Matrix3 delVdelBiasOmega_
 Jacobian of preintegrated velocity w.r.t. angular rate bias. More...
 
- Protected Attributes inherited from gtsam::PreintegrationBase
Bias biasHat_
 Acceleration and gyro bias used for preintegration. More...
 
double deltaTij_
 Time interval from i to j. More...
 
std::shared_ptr< Paramsp_
 

Additional Inherited Members

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

Detailed Description

IMU pre-integration on NavState manifold. This corresponds to the original RSS paper (with one difference: V is rotated)

Definition at line 33 of file ManifoldPreintegration.h.

Constructor & Destructor Documentation

◆ ManifoldPreintegration() [1/2]

gtsam::ManifoldPreintegration::ManifoldPreintegration ( )
inlineprotected

Default constructor for serialization.

Definition at line 49 of file ManifoldPreintegration.h.

◆ ManifoldPreintegration() [2/2]

gtsam::ManifoldPreintegration::ManifoldPreintegration ( const std::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 29 of file ManifoldPreintegration.cpp.

Member Function Documentation

◆ biasCorrectedDelta()

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

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.

Definition at line 112 of file ManifoldPreintegration.cpp.

◆ clone()

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

Dummy clone for MATLAB

Definition at line 109 of file ManifoldPreintegration.h.

◆ delPdelBiasAcc()

Matrix3 gtsam::ManifoldPreintegration::delPdelBiasAcc ( ) const
inline

Definition at line 82 of file ManifoldPreintegration.h.

◆ delPdelBiasOmega()

Matrix3 gtsam::ManifoldPreintegration::delPdelBiasOmega ( ) const
inline

Definition at line 83 of file ManifoldPreintegration.h.

◆ delRdelBiasOmega()

Matrix3 gtsam::ManifoldPreintegration::delRdelBiasOmega ( ) const
inline

Definition at line 81 of file ManifoldPreintegration.h.

◆ deltaPij()

Vector3 gtsam::ManifoldPreintegration::deltaPij ( ) const
inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 78 of file ManifoldPreintegration.h.

◆ deltaRij()

Rot3 gtsam::ManifoldPreintegration::deltaRij ( ) const
inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 77 of file ManifoldPreintegration.h.

◆ deltaVij()

Vector3 gtsam::ManifoldPreintegration::deltaVij ( ) const
inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 79 of file ManifoldPreintegration.h.

◆ deltaXij()

NavState gtsam::ManifoldPreintegration::deltaXij ( ) const
inlineoverridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 76 of file ManifoldPreintegration.h.

◆ delVdelBiasAcc()

Matrix3 gtsam::ManifoldPreintegration::delVdelBiasAcc ( ) const
inline

Definition at line 84 of file ManifoldPreintegration.h.

◆ delVdelBiasOmega()

Matrix3 gtsam::ManifoldPreintegration::delVdelBiasOmega ( ) const
inline

Definition at line 85 of file ManifoldPreintegration.h.

◆ equals()

bool gtsam::ManifoldPreintegration::equals ( const ManifoldPreintegration other,
double  tol 
) const

Definition at line 47 of file ManifoldPreintegration.cpp.

◆ resetIntegration()

void gtsam::ManifoldPreintegration::resetIntegration ( )
overridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 36 of file ManifoldPreintegration.cpp.

◆ update()

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

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.

Definition at line 60 of file ManifoldPreintegration.cpp.

Member Data Documentation

◆ delPdelBiasAcc_

Matrix3 gtsam::ManifoldPreintegration::delPdelBiasAcc_
protected

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

Definition at line 43 of file ManifoldPreintegration.h.

◆ delPdelBiasOmega_

Matrix3 gtsam::ManifoldPreintegration::delPdelBiasOmega_
protected

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

Definition at line 44 of file ManifoldPreintegration.h.

◆ delRdelBiasOmega_

Matrix3 gtsam::ManifoldPreintegration::delRdelBiasOmega_
protected

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

Definition at line 42 of file ManifoldPreintegration.h.

◆ deltaXij_

NavState gtsam::ManifoldPreintegration::deltaXij_
protected

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]

Definition at line 41 of file ManifoldPreintegration.h.

◆ delVdelBiasAcc_

Matrix3 gtsam::ManifoldPreintegration::delVdelBiasAcc_
protected

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

Definition at line 45 of file ManifoldPreintegration.h.

◆ delVdelBiasOmega_

Matrix3 gtsam::ManifoldPreintegration::delVdelBiasOmega_
protected

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

Definition at line 46 of file ManifoldPreintegration.h.


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


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:15:38