class mola::factors::FactorGnssMapEnuConstVel

Overview

GNSS position factor for a fix stamped dt seconds after (or before, if negative) the keyframe it is attached to.

Like FactorGnssMapEnu, but the antenna is first moved to the fix time with the keyframe’s own body-frame twist (constant over dt, to first order):

p_body(dt) = sensorOnVehicle + dt * (v + w x sensorOnVehicle) ENU = T_enu2map * T_map2i * p_body(dt)

so a vehicle moving at speed |v| is not pulled by |v| * dt towards a fix that was taken elsewhere. With dt = 0 it is FactorGnssMapEnu.

Keys: T_enu2map (Pose3), T_map2i (Pose3), v_i (Point3, body frame), w_i (Point3, body frame).

#include <FactorGnssMapEnuConstVel.h>

class FactorGnssMapEnuConstVel: public gtsam::ExpressionFactorN< gtsam::Point3, gtsam::Pose3, gtsam::Pose3, gtsam::Point3, gtsam::Point3 >
{
public:
    // construction

    FactorGnssMapEnuConstVel();

    FactorGnssMapEnuConstVel(
        gtsam::Key kT_enu2map,
        gtsam::Key kT_map2i,
        gtsam::Key kV,
        gtsam::Key kW,
        const gtsam::Point3& sensorOnVehicle,
        double dt,
        const gtsam::Point3& observedENU,
        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

FactorGnssMapEnuConstVel()

Default constructor for serialization.

FactorGnssMapEnuConstVel(
    gtsam::Key kT_enu2map,
    gtsam::Key kT_map2i,
    gtsam::Key kV,
    gtsam::Key kW,
    const gtsam::Point3& sensorOnVehicle,
    double dt,
    const gtsam::Point3& observedENU,
    const gtsam::SharedNoiseModel& model
    )

Parameters:

kT_enu2map	Key for the ENU-to-map transform
kT_map2i	Key for the keyframe pose in map coordinates
kV	Key for the keyframe linear velocity (body frame)
kW	Key for the keyframe angular velocity (body frame)
sensorOnVehicle	GNSS antenna position in the vehicle frame
dt	Fix time minus keyframe time [s]
observedENU	Measured GNSS position in ENU
model	Noise model (3D position uncertainty)

Methods

gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

Deep copy of this factor

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

Print factor details

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

equals