gtsam
Loading...
Searching...
No Matches
gtsam::GalileanPreintegration Class Reference

Detailed Description

IMU preintegration on Gal(3) using GTSAM's left-invariant local error.

The internal tangent ordering is (rotation, velocity, position, time). The public factor contract projects this to NavState ordering (rotation, position, velocity) and removes deterministic time. Biases use GTSAM's (accelerometer, gyroscope) ordering and are corrected on the right.

Inheritance diagram for gtsam::GalileanPreintegration:

Public Member Functions

 GalileanPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat={})
 Construct and reset a Galilean preintegrated measurement.
Rot3 deltaRij () const override
Vector3 deltaPij () const override
Vector3 deltaVij () const override
NavState deltaXij () const override
Vector9 preintegrated () const
 Return the Galilean delta in NavState tangent ordering.
Vector9 biasCorrectedDelta (const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override
 Apply the first-order physical bias correction on the right.
void update (const Vector3 &measuredAcc, const Vector3 &measuredOmega, double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override
 Update the mean and bias Jacobian, returning NavState-chart Jacobians.
void resetIntegration () override
 Reset the mean, elapsed time, and bias Jacobian.
void print (const std::string &s="GalileanPreintegration") const override
bool equals (const GalileanPreintegration &other, double tol=1e-9) const
Public Member Functions inherited from gtsam::PreintegrationBase
 PreintegrationBase (const std::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 std::shared_ptr< Params > & params () const
 shared pointer to params
Params & p () const
 const reference to params
const imuBias::ConstantBias & biasHat () const
double deltaTij () const
virtual Vector3 so3TangentAt (double t) const
 Return the SO(3) tangent vector at local time t in [0, deltaTij].
Matrix deskewPoints (ConstMatrixView points, const Vector3 &velocity_i=Vector3::Zero()) const
 Deskew endpoint-exclusive ordered point batches over the integration interval.
Matrix deskewPointsAtTimes (ConstMatrixView points, const Vector &times, const Vector3 &velocity_i=Vector3::Zero()) const
 Deskew stacked point batches at explicit times and initial velocity.
Vector6 biasHatVector () const
std::pair< Vector3, Vector3 > correctMeasurementsBySensorPose (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
 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.
void integrateMeasurements (const Matrix &measuredAccs, const Matrix &measuredOmegas, const Matrix &dts)
 Add multiple measurements, in matrix columns.
NavState predict (const NavState &state_i, const imuBias::ConstantBias &bias_i, const Vector3 &n_gravity, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 6 > H2={}, OptionalJacobian< 9, 3 > H3={}) const
 Predict state at time j, for a given gravity vector in the nav frame.
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, using the gravity vector from params.

Static Public Attributes

static constexpr bool kLegacyUsesLogmap = true
 Select the SE_2(3) logarithm in Legacy factor-error mode.
Static Public Attributes inherited from gtsam::PreintegrationBase
static constexpr bool kLegacyUsesLogmap = false
 Legacy factor-error choice for this preintegration backend.

Public Types

using Base = PreintegrationBase
using Params = PreintegrationBase::Params
using Matrix10 = Eigen::Matrix<double, 10, 10>
using Matrix106 = Eigen::Matrix<double, 10, 6>
using Matrix910 = Eigen::Matrix<double, 9, 10>
Public Types inherited from gtsam::PreintegrationBase
typedef imuBias::ConstantBias Bias
typedef PreintegrationParams Params

Protected Member Functions

void updateGal3 (const Vector3 &measuredAcc, const Vector3 &measuredOmega, double dt, Matrix10 *A, Matrix106 *B)
 Update the Gal(3) mean and return its transition and input Jacobian.
 GalileanPreintegration ()
 Default constructor for serialization.
Protected Member Functions inherited from gtsam::PreintegrationBase
 PreintegrationBase ()
 Default constructor for serialization.
virtual ~PreintegrationBase ()
 Virtual destructor for serialization.

Static Protected Member Functions

static Vector9 NavStateTangent (const Gal3 &galilean, OptionalJacobian< 9, 10 > H={})
 Extract the factor tangent and its Jacobian from a Gal(3) element.

Protected Attributes

Gal3 preintMatrix_
 Mean increment in Gal(3).
Matrix106 biasJacobian_
 Right correction wrt (accel, gyro) bias.
Protected Attributes inherited from gtsam::PreintegrationBase
std::shared_ptr< Params > p_
Bias biasHat_
 Acceleration and gyro bias used for preintegration.
double deltaTij_
 Time interval from i to j.

Member Function Documentation

◆ biasCorrectedDelta()

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

Apply the first-order physical bias correction on the right.

Implements gtsam::PreintegrationBase.

◆ deltaPij()

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

Implements gtsam::PreintegrationBase.

◆ deltaRij()

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

Implements gtsam::PreintegrationBase.

◆ deltaVij()

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

Implements gtsam::PreintegrationBase.

◆ deltaXij()

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

Implements gtsam::PreintegrationBase.

◆ print()

void gtsam::GalileanPreintegration::print ( const std::string & s = "GalileanPreintegration") const
overridevirtual

Reimplemented from gtsam::PreintegrationBase.

Reimplemented in gtsam::PreintegratedImuMeasurementsT< GalileanPreintegration >.

◆ resetIntegration()

void gtsam::GalileanPreintegration::resetIntegration ( )
overridevirtual

Reset the mean, elapsed time, and bias Jacobian.

Implements gtsam::PreintegrationBase.

Reimplemented in gtsam::PreintegratedCombinedMeasurementsT< GalileanPreintegration >, and gtsam::PreintegratedImuMeasurementsT< GalileanPreintegration >.

◆ update()

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

Update the mean and bias Jacobian, returning NavState-chart Jacobians.

Covariance propagation is performed by PreintegratedImuMeasurementsT.

Implements gtsam::PreintegrationBase.

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