#include <PreintegratedRotation.h>
Public Types | |
typedef PreintegratedRotationParams | Params |
Public Member Functions | |
Constructors | |
PreintegratedRotation (const std::shared_ptr< Params > &p) | |
Default constructor, resets integration to zero. More... | |
PreintegratedRotation (const std::shared_ptr< Params > &p, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega) | |
Explicit initialization of all class members. More... | |
Basic utilities | |
void | resetIntegration () |
Re-initialize PreintegratedMeasurements. More... | |
bool | matchesParamsWith (const PreintegratedRotation &other) const |
check parameters equality: checks whether shared pointer points to same Params object. More... | |
Access instance variables | |
const std::shared_ptr< Params > & | params () const |
const double & | deltaTij () const |
const Rot3 & | deltaRij () const |
const Matrix3 & | delRdelBiasOmega () const |
Testable | |
void | print (const std::string &s) const |
bool | equals (const PreintegratedRotation &other, double tol) const |
Main functionality | |
void | integrateGyroMeasurement (const Vector3 &measuredOmega, const Vector3 &biasHat, double deltaT, OptionalJacobian< 3, 3 > F={}) |
Calculate an incremental rotation given the gyro measurement and a time interval, and update both deltaTij_ and deltaRij_. More... | |
Rot3 | biascorrectedDeltaRij (const Vector3 &biasOmegaIncr, OptionalJacobian< 3, 3 > H={}) const |
Return a bias corrected version of the integrated rotation. More... | |
Vector3 | integrateCoriolis (const Rot3 &rot_i) const |
Integrate coriolis correction in body frame rot_i. More... | |
Protected Member Functions | |
PreintegratedRotation () | |
Default constructor for serialization. More... | |
Protected Attributes | |
Matrix3 | delRdelBiasOmega_ |
Jacobian of preintegrated rotation w.r.t. angular rate bias. More... | |
Rot3 | deltaRij_ |
Preintegrated relative orientation (in frame i) More... | |
double | deltaTij_ |
Time interval from i to j. More... | |
std::shared_ptr< Params > | p_ |
Parameters. More... | |
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). It includes the definitions of the preintegrated rotation.
Definition at line 117 of file PreintegratedRotation.h.
Definition at line 119 of file PreintegratedRotation.h.
|
inlineprotected |
Default constructor for serialization.
Definition at line 130 of file PreintegratedRotation.h.
|
inlineexplicit |
Default constructor, resets integration to zero.
Definition at line 137 of file PreintegratedRotation.h.
|
inline |
Explicit initialization of all class members.
Definition at line 142 of file PreintegratedRotation.h.
Rot3 gtsam::PreintegratedRotation::biascorrectedDeltaRij | ( | const Vector3 & | biasOmegaIncr, |
OptionalJacobian< 3, 3 > | H = {} |
||
) | const |
Return a bias corrected version of the integrated rotation.
biasOmegaIncr | An increment with respect to biasHat used above. |
H | optional Jacobian of the correction w.r.t. the bias increment. |
Definition at line 130 of file PreintegratedRotation.cpp.
|
inline |
Definition at line 166 of file PreintegratedRotation.h.
|
inline |
Definition at line 165 of file PreintegratedRotation.h.
|
inline |
Definition at line 164 of file PreintegratedRotation.h.
bool gtsam::PreintegratedRotation::equals | ( | const PreintegratedRotation & | other, |
double | tol | ||
) | const |
Definition at line 63 of file PreintegratedRotation.cpp.
Integrate coriolis correction in body frame rot_i.
Definition at line 139 of file PreintegratedRotation.cpp.
void gtsam::PreintegratedRotation::integrateGyroMeasurement | ( | const Vector3 & | measuredOmega, |
const Vector3 & | biasHat, | ||
double | deltaT, | ||
OptionalJacobian< 3, 3 > | F = {} |
||
) |
Calculate an incremental rotation given the gyro measurement and a time interval, and update both deltaTij_ and deltaRij_.
measuredOmega | The measured angular velocity (as given by the sensor) |
bias | The biasHat estimate |
deltaT | The time interval |
F | optional Jacobian of internal compose, used in AhrsFactor. |
Definition at line 97 of file PreintegratedRotation.cpp.
|
inline |
check parameters equality: checks whether shared pointer points to same Params object.
Definition at line 156 of file PreintegratedRotation.h.
|
inline |
Definition at line 163 of file PreintegratedRotation.h.
void gtsam::PreintegratedRotation::print | ( | const std::string & | s | ) | const |
Definition at line 57 of file PreintegratedRotation.cpp.
void gtsam::PreintegratedRotation::resetIntegration | ( | ) |
Re-initialize PreintegratedMeasurements.
Definition at line 51 of file PreintegratedRotation.cpp.
|
protected |
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition at line 127 of file PreintegratedRotation.h.
|
protected |
Preintegrated relative orientation (in frame i)
Definition at line 126 of file PreintegratedRotation.h.
|
protected |
Time interval from i to j.
Definition at line 125 of file PreintegratedRotation.h.
|
protected |
Parameters.
Definition at line 123 of file PreintegratedRotation.h.