#include <CombinedImuFactor.h>
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... | |
Params & | p () const |
const reference to params, shadows definition in base class More... | |
Access instance variables | |
Return pre-integrated measurement covariance | |
Matrix | preintMeasCov () const |
Testable | |
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< ManifoldPreintegration > | clone () 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... | |
Params & | p () const |
const reference to params More... | |
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={}, 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< Params > | p_ |
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... | |
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.
Definition at line 69 of file CombinedImuFactor.h.
|
inline |
Default constructor only for serialization and wrappers.
Definition at line 87 of file CombinedImuFactor.h.
|
inline |
Default constructor, initializes the class with no measurements
p | Parameters, typically fixed in a single application |
biasHat | Current estimate of acceleration and rotation rate biases |
preintMeasCov | Covariance matrix used in noise model. |
Definition at line 95 of file CombinedImuFactor.h.
|
inline |
Construct preintegrated directly from members: base class and preintMeasCov
base | PreintegrationType instance |
preintMeasCov | Covariance matrix used in noise model. |
Definition at line 108 of file CombinedImuFactor.h.
|
inlineoverride |
Virtual destructor.
Definition at line 114 of file CombinedImuFactor.h.
bool gtsam::PreintegratedCombinedMeasurements::equals | ( | const PreintegratedCombinedMeasurements & | expected, |
double | tol = 1e-9 |
||
) | const |
equals
Definition at line 69 of file CombinedImuFactor.cpp.
|
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
.
measuredAcc | Measured acceleration (as given by the sensor) |
measuredOmega | Measured angular velocity (as given by the sensor) |
dt | Time interval between two consecutive IMU measurements |
Reimplemented from gtsam::PreintegrationBase.
Definition at line 106 of file CombinedImuFactor.cpp.
|
inline |
const reference to params, shadows definition in base class
Definition at line 133 of file CombinedImuFactor.h.
|
inline |
Definition at line 139 of file CombinedImuFactor.h.
|
overridevirtual |
Reimplemented from gtsam::PreintegrationBase.
Definition at line 63 of file CombinedImuFactor.cpp.
|
overridevirtual |
Re-initialize PreintegratedCombinedMeasurements.
Implements gtsam::PreintegrationBase.
Definition at line 76 of file CombinedImuFactor.cpp.
void gtsam::PreintegratedCombinedMeasurements::resetIntegration | ( | const gtsam::Matrix6 & | Q_init | ) |
Re-initialize PreintegratedCombinedMeasurements with initial bias covariance estimate.
Q_init | The initial bias covariance estimates as a 6x6 matrix. |
Definition at line 83 of file CombinedImuFactor.cpp.
|
friend |
Definition at line 80 of file CombinedImuFactor.h.
|
protected |
Definition at line 78 of file CombinedImuFactor.h.