Public Types | Protected Attributes | Friends | List of all members
gtsam::PreintegratedCombinedMeasurements Class Reference

#include <CombinedImuFactor.h>

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

Public Types

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

Public Member Functions

Constructors
 PreintegratedCombinedMeasurements ()
 Default constructor only for serialization and wrappers. More...
 
 PreintegratedCombinedMeasurements (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias(), const Eigen::Matrix< double, 15, 15 > &preintMeasCov=Eigen::Matrix< double, 15, 15 >::Zero())
 
 PreintegratedCombinedMeasurements (const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
 
 ~PreintegratedCombinedMeasurements () override
 Virtual destructor. More...
 
Basic utilities
void resetIntegration () override
 Re-initialize PreintegratedCombinedMeasurements. More...
 
void resetIntegration (const gtsam::Matrix6 &Q_init)
 Re-initialize PreintegratedCombinedMeasurements with initial bias covariance estimate. More...
 
Paramsp () const
 const reference to params, shadows definition in base class More...
 
Access instance variables

Return pre-integrated measurement covariance

Matrix preintMeasCov () const
 
Testable

print

void print (const std::string &s="Preintegrated Measurements:") const override
 
bool equals (const PreintegratedCombinedMeasurements &expected, double tol=1e-9) const
 equals More...
 
Main functionality
void integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
 
- Public Member Functions inherited from gtsam::ManifoldPreintegration
 ManifoldPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
 
void resetIntegration () override
 
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
 
bool equals (const ManifoldPreintegration &other, double tol) const
 
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
 
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
 

Protected Attributes

Eigen::Matrix< double, 15, 15 > preintMeasCov_
 
- Protected Attributes inherited from gtsam::ManifoldPreintegration
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_
 

Friends

class CombinedImuFactor
 

Additional Inherited Members

- Protected Member Functions inherited from gtsam::ManifoldPreintegration
 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...
 

Detailed Description

PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. The measurements are then used to build the CombinedImuFactor. Integration is done incrementally (ideally, one integrates the measurement as soon as it is received from the IMU) so as to avoid costly integration at time of factor construction.

Definition at line 66 of file CombinedImuFactor.h.

Member Typedef Documentation

◆ Params

Definition at line 69 of file CombinedImuFactor.h.

Constructor & Destructor Documentation

◆ PreintegratedCombinedMeasurements() [1/3]

gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements ( )
inline

Default constructor only for serialization and wrappers.

Definition at line 87 of file CombinedImuFactor.h.

◆ PreintegratedCombinedMeasurements() [2/3]

gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements ( const std::shared_ptr< Params > &  p,
const imuBias::ConstantBias biasHat = imuBias::ConstantBias(),
const Eigen::Matrix< double, 15, 15 > &  preintMeasCov = Eigen::Matrix<double, 15, 15>::Zero() 
)
inline

Default constructor, initializes the class with no measurements

Parameters
pParameters, typically fixed in a single application
biasHatCurrent estimate of acceleration and rotation rate biases
preintMeasCovCovariance matrix used in noise model.

Definition at line 95 of file CombinedImuFactor.h.

◆ PreintegratedCombinedMeasurements() [3/3]

gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements ( const PreintegrationType base,
const Eigen::Matrix< double, 15, 15 > &  preintMeasCov 
)
inline

Construct preintegrated directly from members: base class and preintMeasCov

Parameters
basePreintegrationType instance
preintMeasCovCovariance matrix used in noise model.

Definition at line 108 of file CombinedImuFactor.h.

◆ ~PreintegratedCombinedMeasurements()

gtsam::PreintegratedCombinedMeasurements::~PreintegratedCombinedMeasurements ( )
inlineoverride

Virtual destructor.

Definition at line 114 of file CombinedImuFactor.h.

Member Function Documentation

◆ equals()

bool gtsam::PreintegratedCombinedMeasurements::equals ( const PreintegratedCombinedMeasurements expected,
double  tol = 1e-9 
) const

equals

Definition at line 69 of file CombinedImuFactor.cpp.

◆ integrateMeasurement()

void gtsam::PreintegratedCombinedMeasurements::integrateMeasurement ( const Vector3 measuredAcc,
const Vector3 measuredOmega,
const double  dt 
)
overridevirtual

Add a single IMU measurement to the preintegration. Both accelerometer and gyroscope measurements are taken to be in the sensor frame and conversion to the body frame is handled by body_P_sensor in PreintegrationParams.

Parameters
measuredAccMeasured acceleration (as given by the sensor)
measuredOmegaMeasured angular velocity (as given by the sensor)
dtTime interval between two consecutive IMU measurements

Reimplemented from gtsam::PreintegrationBase.

Definition at line 106 of file CombinedImuFactor.cpp.

◆ p()

Params& gtsam::PreintegratedCombinedMeasurements::p ( ) const
inline

const reference to params, shadows definition in base class

Definition at line 133 of file CombinedImuFactor.h.

◆ preintMeasCov()

Matrix gtsam::PreintegratedCombinedMeasurements::preintMeasCov ( ) const
inline

Definition at line 139 of file CombinedImuFactor.h.

◆ print()

void gtsam::PreintegratedCombinedMeasurements::print ( const std::string &  s = "Preintegrated Measurements:") const
overridevirtual

Reimplemented from gtsam::PreintegrationBase.

Definition at line 63 of file CombinedImuFactor.cpp.

◆ resetIntegration() [1/2]

void gtsam::PreintegratedCombinedMeasurements::resetIntegration ( )
overridevirtual

Re-initialize PreintegratedCombinedMeasurements.

Implements gtsam::PreintegrationBase.

Definition at line 76 of file CombinedImuFactor.cpp.

◆ resetIntegration() [2/2]

void gtsam::PreintegratedCombinedMeasurements::resetIntegration ( const gtsam::Matrix6 &  Q_init)

Re-initialize PreintegratedCombinedMeasurements with initial bias covariance estimate.

Parameters
Q_initThe initial bias covariance estimates as a 6x6 matrix.

Definition at line 83 of file CombinedImuFactor.cpp.

Friends And Related Function Documentation

◆ CombinedImuFactor

friend class CombinedImuFactor
friend

Definition at line 80 of file CombinedImuFactor.h.

Member Data Documentation

◆ preintMeasCov_

Eigen::Matrix<double, 15, 15> gtsam::PreintegratedCombinedMeasurements::preintMeasCov_
protected

Definition at line 78 of file CombinedImuFactor.h.


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


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