class mola::factors::FactorImuPreintegrationBodyVel

Overview

IMU preintegration factor between two keyframes whose linear velocity is expressed in the BODY frame, as in mola_state_estimation_smoother.

It wraps gtsam::ImuFactor (rotation, velocity and position increments preintegrated from raw readings, with the bias correction to first order): the world-frame velocities that factor expects are R_i * v_i and R_j * v_j, and the Jacobians are chained through that rotation. The bias random walk is a separate factor between the two bias variables.

Keys: T_i (Pose3), V_i (Point3, body), T_j, V_j, B_i (imuBias::ConstantBias).

#include <FactorImuPreintegrationBodyVel.h>

class FactorImuPreintegrationBodyVel: public gtsam::NoiseModelFactorN< gtsam::Pose3, gtsam::Point3, gtsam::Pose3, gtsam::Point3, gtsam::imuBias::ConstantBias >
{
public:
    // construction

    FactorImuPreintegrationBodyVel();

    FactorImuPreintegrationBodyVel(
        gtsam::Key kTi,
        gtsam::Key kVi,
        gtsam::Key kTj,
        gtsam::Key kVj,
        gtsam::Key kBi,
        const gtsam::PreintegratedImuMeasurements& pim,
        const gtsam::Matrix9& covFloor = gtsam::Matrix9::Zero()
        );

    // methods

    gtsam::NonlinearFactor::shared_ptr clone() const;

    gtsam::Vector evaluateError(
        const gtsam::Pose3& Ti,
        const gtsam::Point3& vbi,
        const gtsam::Pose3& Tj,
        const gtsam::Point3& vbj,
        const gtsam::imuBias::ConstantBias& bi,
        gtsam::OptionalMatrixType H1,
        gtsam::OptionalMatrixType H2,
        gtsam::OptionalMatrixType H3,
        gtsam::OptionalMatrixType H4,
        gtsam::OptionalMatrixType H5
        ) const;

    const gtsam::ImuFactor& imuFactor() const;
};

Construction

FactorImuPreintegrationBodyVel(
    gtsam::Key kTi,
    gtsam::Key kVi,
    gtsam::Key kTj,
    gtsam::Key kVj,
    gtsam::Key kBi,
    const gtsam::PreintegratedImuMeasurements& pim,
    const gtsam::Matrix9& covFloor = gtsam::Matrix9::Zero()
    )

covFloor (9x9, in the order of the preintegrated state: rotation, position, velocity) 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.