class mola::factors::FactorGyroPreintegration
Overview
Gyroscope-only preintegration factor between the rotations of two keyframe poses: the relative rotation integrated from the raw gyro readings (with the gyro bias correction to first order), as gtsam::AHRSFactor, but on Pose3 keyframes and on the gyroscope part of an imuBias::ConstantBias variable, the same variables full IMU preintegration uses. Translations are not involved.
pim must hold the readings already rotated into the vehicle frame (integrate_gyro_readings() with R_body_sensor), with no body_P_sensor in its parameters and its bias linearization point in the vehicle frame: the bias variable is in the sensor frame, as for gtsam::ImuFactor, and rotated here (gtsam::AHRSFactor corrects the bias in the vehicle frame).
Keys: T_i (Pose3), T_j (Pose3), B_i (imuBias::ConstantBias).
#include <FactorGyroPreintegration.h> class FactorGyroPreintegration: public gtsam::NoiseModelFactorN< gtsam::Pose3, gtsam::Pose3, gtsam::imuBias::ConstantBias > { public: // construction FactorGyroPreintegration( gtsam::Key kTi, gtsam::Key kTj, gtsam::Key kBi, const gtsam::PreintegratedAhrsMeasurements& pim, const gtsam::Rot3& R_body_sensor, const gtsam::Matrix3& covFloor = gtsam::Matrix3::Zero() ); // methods gtsam::NonlinearFactor::shared_ptr clone() const; gtsam::Vector evaluateError( const gtsam::Pose3& Ti, const gtsam::Pose3& Tj, const gtsam::imuBias::ConstantBias& bi, detail::OptionalMatrix H1, detail::OptionalMatrix H2, detail::OptionalMatrix H3 ) const; const gtsam::AHRSFactor& ahrsFactor() const; };
Construction
FactorGyroPreintegration( gtsam::Key kTi, gtsam::Key kTj, gtsam::Key kBi, const gtsam::PreintegratedAhrsMeasurements& pim, const gtsam::Rot3& R_body_sensor, const gtsam::Matrix3& covFloor = gtsam::Matrix3::Zero() )
covFloor [rad^2] is added to the preintegrated covariance, which for intervals of a few milliseconds is small enough to make the whole system numerically singular next to the other factors.