#include <TangentPreintegration.h>
Public Member Functions | |
virtual std::shared_ptr< TangentPreintegration > | clone () const |
Constructors/destructors | |
TangentPreintegration (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) | |
~TangentPreintegration () override | |
Virtual destructor. More... | |
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 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 | |
TangentPreintegration () | |
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 | |
Vector9 | preintegrated_ |
Matrix93 | preintegrated_H_biasAcc_ |
Jacobian of preintegrated_ w.r.t. acceleration bias. More... | |
Matrix93 | preintegrated_H_biasOmega_ |
Jacobian of preintegrated_ 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_ |
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 |
void | mergeWith (const TangentPreintegration &pim, Matrix9 *H1, Matrix9 *H2) |
static Vector9 | UpdatePreintegrated (const Vector3 &a_body, const Vector3 &w_body, const double dt, const Vector9 &preintegrated, OptionalJacobian< 9, 9 > A={}, OptionalJacobian< 9, 3 > B={}, OptionalJacobian< 9, 3 > C={}) |
static Vector9 | Compose (const Vector9 &zeta01, const Vector9 &zeta12, double deltaT12, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) |
Additional Inherited Members | |
Public Types inherited from gtsam::PreintegrationBase | |
typedef imuBias::ConstantBias | Bias |
typedef PreintegrationParams | Params |
Integrate on the 9D tangent space of the NavState manifold. See extensive discussion in ImuFactor.lyx
Definition at line 28 of file TangentPreintegration.h.
|
inlineprotected |
Default constructor for serialization.
Definition at line 40 of file TangentPreintegration.h.
gtsam::TangentPreintegration::TangentPreintegration | ( | 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 26 of file TangentPreintegration.cpp.
|
inlineoverride |
Virtual destructor.
Definition at line 57 of file TangentPreintegration.h.
|
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 144 of file TangentPreintegration.cpp.
|
inlinevirtual |
Dummy clone for MATLAB
Definition at line 123 of file TangentPreintegration.h.
|
static |
Definition at line 172 of file TangentPreintegration.cpp.
|
inlineoverridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 71 of file TangentPreintegration.h.
|
inlineoverridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 73 of file TangentPreintegration.h.
|
inlineoverridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 72 of file TangentPreintegration.h.
|
inlineoverridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 74 of file TangentPreintegration.h.
bool gtsam::TangentPreintegration::equals | ( | const TangentPreintegration & | other, |
double | tol | ||
) | const |
Definition at line 41 of file TangentPreintegration.cpp.
void gtsam::TangentPreintegration::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
Definition at line 216 of file TangentPreintegration.cpp.
|
inline |
Definition at line 76 of file TangentPreintegration.h.
|
inline |
Definition at line 78 of file TangentPreintegration.h.
|
inline |
Definition at line 79 of file TangentPreintegration.h.
|
overridevirtual |
Implements gtsam::PreintegrationBase.
Definition at line 33 of file TangentPreintegration.cpp.
|
inline |
Definition at line 77 of file TangentPreintegration.h.
|
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 105 of file TangentPreintegration.cpp.
|
static |
Definition at line 54 of file TangentPreintegration.cpp.
|
protected |
Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta, position, velocity
Definition at line 35 of file TangentPreintegration.h.
|
protected |
Jacobian of preintegrated_ w.r.t. acceleration bias.
Definition at line 36 of file TangentPreintegration.h.
|
protected |
Jacobian of preintegrated_ w.r.t. angular rate bias.
Definition at line 37 of file TangentPreintegration.h.