Loading...
Searching...
No Matches
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< TangentPreintegration > | clone () 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.
| ~TangentPreintegration () override
| 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.
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
Params & | p () const
| const reference to params
const imuBias::ConstantBias & | biasHat () 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.
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< Params > | p_
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

  p | Parameters, typically fixed in a single application
  bias | Current estimate of acceleration and rotation rate biases

## Member Function Documentation

## biasCorrectedDelta()

Vector9 gtsam::TangentPreintegration::biasCorrectedDelta | ( | const imuBias::ConstantBias & | bias_i,
|  | OptionalJacobian< 9, 6 > | H = boost::none ) const

override virtual

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.

## deltaPij()

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

inline override virtual

Implements gtsam::PreintegrationBase.

## deltaRij()

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

inline override virtual

Implements gtsam::PreintegrationBase.

## deltaVij()

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

inline override virtual

Implements gtsam::PreintegrationBase.

## deltaXij()

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

inline override virtual

Implements gtsam::PreintegrationBase.

## resetIntegration()

void gtsam::TangentPreintegration::resetIntegration | ( |  | ) |

override virtual

Implements gtsam::PreintegrationBase.

## update()

void gtsam::TangentPreintegration::update | ( | const Vector3 & | measuredAcc,
|  | const Vector3 & | measuredOmega,
|  | const double | dt,
|  | Matrix9 * | A,
|  | Matrix93 * | B,
|  | Matrix93 * | C )

override 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 NOTE(frank): implementation is different in two versions.

Implements gtsam::PreintegrationBase.

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

- /build/gtsam-gchP9J/gtsam-4.2.2+dfsg/gtsam/navigation/TangentPreintegration.h
- /build/gtsam-gchP9J/gtsam-4.2.2+dfsg/gtsam/navigation/TangentPreintegration.cpp