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

#include <TangentPreintegration.h>

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

Public Member Functions

virtual std::shared_ptr< TangentPreintegrationclone () const
 
Constructors/destructors
 TangentPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
 
 ~TangentPreintegration () override
 Virtual destructor. More...
 
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 Matrix93preintegrated_H_biasAcc () const
 
const Matrix93preintegrated_H_biasOmega () const
 
Testable
bool equals (const TangentPreintegration &other, double tol) 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

 TangentPreintegration ()
 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

Vector9 preintegrated_
 
Matrix93 preintegrated_H_biasAcc_
 Jacobian of preintegrated_ w.r.t. acceleration bias. More...
 
Matrix93 preintegrated_H_biasOmega_
 Jacobian of preintegrated_ 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_
 

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
 
void mergeWith (const TangentPreintegration &pim, Matrix9 *H1, Matrix9 *H2)
 
static Vector9 UpdatePreintegrated (const Vector3 &a_body, const Vector3 &w_body, const double dt, const Vector9 &preintegrated, OptionalJacobian< 9, 9 > A={}, OptionalJacobian< 9, 3 > B={}, OptionalJacobian< 9, 3 > C={})
 
static Vector9 Compose (const Vector9 &zeta01, const Vector9 &zeta12, double deltaT12, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={})
 

Additional Inherited Members

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

Detailed Description

Integrate on the 9D tangent space of the NavState manifold. See extensive discussion in ImuFactor.lyx

Definition at line 28 of file TangentPreintegration.h.

Constructor & Destructor Documentation

◆ TangentPreintegration() [1/2]

gtsam::TangentPreintegration::TangentPreintegration ( )
inlineprotected

Default constructor for serialization.

Definition at line 40 of file TangentPreintegration.h.

◆ TangentPreintegration() [2/2]

gtsam::TangentPreintegration::TangentPreintegration ( 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 26 of file TangentPreintegration.cpp.

◆ ~TangentPreintegration()

gtsam::TangentPreintegration::~TangentPreintegration ( )
inlineoverride

Virtual destructor.

Definition at line 57 of file TangentPreintegration.h.

Member Function Documentation

◆ biasCorrectedDelta()

Vector9 gtsam::TangentPreintegration::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 144 of file TangentPreintegration.cpp.

◆ clone()

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

Dummy clone for MATLAB

Definition at line 123 of file TangentPreintegration.h.

◆ Compose()

Vector9 gtsam::TangentPreintegration::Compose ( const Vector9 &  zeta01,
const Vector9 &  zeta12,
double  deltaT12,
OptionalJacobian< 9, 9 >  H1 = {},
OptionalJacobian< 9, 9 >  H2 = {} 
)
static

Definition at line 172 of file TangentPreintegration.cpp.

◆ deltaPij()

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

Implements gtsam::PreintegrationBase.

Definition at line 71 of file TangentPreintegration.h.

◆ deltaRij()

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

Implements gtsam::PreintegrationBase.

Definition at line 73 of file TangentPreintegration.h.

◆ deltaVij()

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

Implements gtsam::PreintegrationBase.

Definition at line 72 of file TangentPreintegration.h.

◆ deltaXij()

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

Implements gtsam::PreintegrationBase.

Definition at line 74 of file TangentPreintegration.h.

◆ equals()

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

Definition at line 41 of file TangentPreintegration.cpp.

◆ mergeWith()

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

Definition at line 216 of file TangentPreintegration.cpp.

◆ preintegrated()

const Vector9& gtsam::TangentPreintegration::preintegrated ( ) const
inline

Definition at line 76 of file TangentPreintegration.h.

◆ preintegrated_H_biasAcc()

const Matrix93& gtsam::TangentPreintegration::preintegrated_H_biasAcc ( ) const
inline

Definition at line 78 of file TangentPreintegration.h.

◆ preintegrated_H_biasOmega()

const Matrix93& gtsam::TangentPreintegration::preintegrated_H_biasOmega ( ) const
inline

Definition at line 79 of file TangentPreintegration.h.

◆ resetIntegration()

void gtsam::TangentPreintegration::resetIntegration ( )
overridevirtual

Implements gtsam::PreintegrationBase.

Definition at line 33 of file TangentPreintegration.cpp.

◆ theta()

Vector3 gtsam::TangentPreintegration::theta ( ) const
inline

Definition at line 77 of file TangentPreintegration.h.

◆ update()

void gtsam::TangentPreintegration::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 105 of file TangentPreintegration.cpp.

◆ UpdatePreintegrated()

Vector9 gtsam::TangentPreintegration::UpdatePreintegrated ( const Vector3 a_body,
const Vector3 w_body,
const double  dt,
const Vector9 &  preintegrated,
OptionalJacobian< 9, 9 >  A = {},
OptionalJacobian< 9, 3 >  B = {},
OptionalJacobian< 9, 3 >  C = {} 
)
static

Definition at line 54 of file TangentPreintegration.cpp.

Member Data Documentation

◆ preintegrated_

Vector9 gtsam::TangentPreintegration::preintegrated_
protected

Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta, position, velocity

Definition at line 35 of file TangentPreintegration.h.

◆ preintegrated_H_biasAcc_

Matrix93 gtsam::TangentPreintegration::preintegrated_H_biasAcc_
protected

Jacobian of preintegrated_ w.r.t. acceleration bias.

Definition at line 36 of file TangentPreintegration.h.

◆ preintegrated_H_biasOmega_

Matrix93 gtsam::TangentPreintegration::preintegrated_H_biasOmega_
protected

Jacobian of preintegrated_ w.r.t. angular rate bias.

Definition at line 37 of file TangentPreintegration.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:13