class mola::factors::FactorImuPreintegrationBodyVelEnu

Overview

The same factor for keyframe poses in a frame {map} that need not be gravity-aligned: gravity is along -z of {enu}, and T_enu_to_map (the pose of {map} in {enu}) is one more variable. Only its roll and pitch change the residual, not its azimuth or translation.

Keys: T_enu_to_map (Pose3), T_i (Pose3, in {map}), V_i (Point3, body), T_j, V_j, B_i (imuBias::ConstantBias).

#include <FactorImuPreintegrationBodyVel.h>

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

    FactorImuPreintegrationBodyVelEnu();

    FactorImuPreintegrationBodyVelEnu(
        gtsam::Key kEnuToMap,
        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& T_enu_to_map,
        const gtsam::Pose3& Ti,
        const gtsam::Point3& vbi,
        const gtsam::Pose3& Tj,
        const gtsam::Point3& vbj,
        const gtsam::imuBias::ConstantBias& bi,
        detail::OptionalMatrix H1,
        detail::OptionalMatrix H2,
        detail::OptionalMatrix H3,
        detail::OptionalMatrix H4,
        detail::OptionalMatrix H5,
        detail::OptionalMatrix H6
        ) const;

    const FactorImuPreintegrationBodyVel& inEnuFactor() const;
};

Construction

FactorImuPreintegrationBodyVelEnu(
    gtsam::Key kEnuToMap,
    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 as in FactorImuPreintegrationBodyVel.