class mola::factors::MapGravityFactor

Overview

Unary GTSAM factor for IMU observations of gravity-aligned orientation, imposing leveling directly in the (assumed gravity-aligned, z-up) map frame.

Unlike MeasuredGravityFactor, this factor does NOT depend on the enu-to-map geo-reference node: it asserts that the measured up-vector, carried from the body frame into the MAP frame through Pi, points along the map +Z axis. This requires the map frame to be defined gravity-aligned (z-up). It is the right tool when vertical (gravity) and horizontal (azimuth/geo-reference) are decoupled, so gravity-leveling works even with geo-referencing/GNSS disabled.

#include <MapGravityFactor.h>

class MapGravityFactor: public gtsam::ExpressionFactorN< gtsam::Vector3, gtsam::Pose3 >
{
public:
    // construction

    MapGravityFactor();

    MapGravityFactor(
        gtsam::Key kPi,
        const gtsam::Pose3& sensorPoseOnVehicle,
        const gtsam::Vector3& observedNormalizedAcc_b,
        const gtsam::SharedNoiseModel& model
        );

    // methods

    gtsam::NonlinearFactor::shared_ptr clone() const;
    gtsam::Expression<gtsam::Point3> expression(const std::array<gtsam::Key, NARY_EXPRESSION_SIZE>& keys) const;

    void print(
        const std::string& s,
        const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter
        ) const;

    bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const;
};

Construction

MapGravityFactor()

default constructor

Methods

gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

a deep copy of this factor

void print(
    const std::string& s,
    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter
    ) const

implement functions needed for Testable print

bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const

equals