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.