#include <ManifoldPreintegration.h>
Public Member Functions | |
Constructors | |
ManifoldPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) | |
Basic utilities | |
Re-initialize PreintegratedMeasurements | |
void | resetIntegration () override |
Instance variables access | |
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 |
Testable | |
bool | equals (const ManifoldPreintegration &other, double tol) const |
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 |
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 |
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 | |
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... | |
Protected Attributes | |
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_ |
Additional Inherited Members | |
Public Types inherited from gtsam::PreintegrationBase | |
typedef imuBias::ConstantBias | Bias |
typedef PreintegrationParams | Params |
IMU pre-integration on NavState manifold. This corresponds to the original RSS paper (with one difference: V is rotated)
Definition at line 33 of file ManifoldPreintegration.h.
|
inlineprotected |
Default constructor for serialization.
Definition at line 49 of file ManifoldPreintegration.h.
gtsam::ManifoldPreintegration::ManifoldPreintegration | ( | const std::shared_ptr< Params > & | p, |
const imuBias::ConstantBias & | biasHat = imuBias::ConstantBias() |
||
) |
Constructor, initializes the variables in the base class
p | Parameters, typically fixed in a single application |
bias | Current estimate of acceleration and rotation rate biases |
Definition at line 29 of file ManifoldPreintegration.cpp.
|
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 112 of file ManifoldPreintegration.cpp.
|
inlinevirtual |
Dummy clone for MATLAB
Definition at line 109 of file ManifoldPreintegration.h.
|
inline |
Definition at line 82 of file ManifoldPreintegration.h.
|
inline |
Definition at line 83 of file ManifoldPreintegration.h.
|
inline |
Definition at line 81 of file ManifoldPreintegration.h.
|
inlineoverridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 78 of file ManifoldPreintegration.h.
|
inlineoverridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 77 of file ManifoldPreintegration.h.
|
inlineoverridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 79 of file ManifoldPreintegration.h.
|
inlineoverridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 76 of file ManifoldPreintegration.h.
|
inline |
Definition at line 84 of file ManifoldPreintegration.h.
|
inline |
Definition at line 85 of file ManifoldPreintegration.h.
bool gtsam::ManifoldPreintegration::equals | ( | const ManifoldPreintegration & | other, |
double | tol | ||
) | const |
Definition at line 47 of file ManifoldPreintegration.cpp.
|
overridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 36 of file ManifoldPreintegration.cpp.
|
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 60 of file ManifoldPreintegration.cpp.
|
protected |
Jacobian of preintegrated position w.r.t. acceleration bias.
Definition at line 43 of file ManifoldPreintegration.h.
|
protected |
Jacobian of preintegrated position w.r.t. angular rate bias.
Definition at line 44 of file ManifoldPreintegration.h.
|
protected |
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition at line 42 of file ManifoldPreintegration.h.
|
protected |
Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into account velocity at time i, see deltap+, in [2] Note: velocity is now also in frame i, as opposed to deltaVij in [2]
Definition at line 41 of file ManifoldPreintegration.h.
|
protected |
Jacobian of preintegrated velocity w.r.t. acceleration bias.
Definition at line 45 of file ManifoldPreintegration.h.
|
protected |
Jacobian of preintegrated velocity w.r.t. angular rate bias.
Definition at line 46 of file ManifoldPreintegration.h.