class mola::factors::FactorGnssMapEnuConstVelBias

Overview

GNSS position factor with a receiver bias state, for a fix stamped dt seconds after (or before, if negative) its keyframe.

Measurement model (antenna position in ENU, from the fix):

z = T_enu2map * T_map2i * l(dt) + b

where b is the slowly varying position error of the receiver, in ENU axes, as a variable of the graph, and l(dt) the antenna (lever arm l in the vehicle frame) moved to the fix time with the keyframe’s body-frame twist, to first order (as in FactorGnssMapEnuConstVel):

l(dt) = l + dt * (v + w x l)

With dt = 0 this is FactorGnssMapEnuBias, with zero Jacobians for v and w : use that one instead.

#include <FactorGnssMapEnuConstVelBias.h>

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

    FactorGnssMapEnuConstVelBias();

    FactorGnssMapEnuConstVelBias(
        gtsam::Key kT_enu2map,
        gtsam::Key kT_map2i,
        gtsam::Key kV,
        gtsam::Key kW,
        gtsam::Key kBias,
        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

FactorGnssMapEnuConstVelBias()

Default constructor for serialization.

FactorGnssMapEnuConstVelBias(
    gtsam::Key kT_enu2map,
    gtsam::Key kT_map2i,
    gtsam::Key kV,
    gtsam::Key kW,
    gtsam::Key kBias,
    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)

kBias

Key for the receiver bias (Point3, ENU axes) [m]

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, white part only)

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