class mola::factors::FactorGnssEnu

Overview

Factor for GNSS position measurements in East-North-Up (ENU) coordinates.

This factor constrains a vehicle pose based on GNSS position observations. The GNSS antenna position is computed by transforming the sensor offset from the vehicle frame to the global ENU frame.

Coordinate Systems:

ENU (East-North-Up) : Local tangent plane coordinate system
- Origin: arbitrary reference point (e.g., first GNSS fix)
- X-axis: East direction
- Y-axis: North direction
- Z-axis: Up (perpendicular to Earth ellipsoid)

Mathematical Model:

\[p_{antenna}^{ENU} = T_i \oplus p_{sensor}^{vehicle}\]

where:

\(T_i\) is the vehicle pose in ENU frame
\(p_{sensor}^{vehicle}\) is the antenna offset in vehicle coordinates
\(p_{antenna}^{ENU}\) is the measured GNSS position

Use Cases:

GPS/GNSS integration in SLAM systems
Georeferencing local maps
Drift correction for odometry-based navigation
Multi-sensor fusion with IMU, wheel encoders, etc.

Example Usage:

using namespace mola::factors;

// GNSS measurement in ENU coordinates (meters)
gtsam::Point3 gnss_enu(1523.4, 2341.7, 15.3);  // East, North, Up

// Antenna offset from vehicle center (meters, in vehicle frame)
gtsam::Point3 antenna_offset(0.5, 0.0, 1.8);  // 0.5m forward, 1.8m up

// Measurement uncertainty (typically 2-10m horizontal, 5-20m vertical for civilian GPS)
auto noise = gtsam::noiseModel::Diagonal::Sigmas(
    gtsam::Vector3(3.0, 3.0, 5.0)  // σ_east, σ_north, σ_up in meters
);

// Add GNSS factor
gtsam::Key vehicle_pose_key = gtsam::Symbol('x', 10);
graph.add(FactorGnssEnu(
    vehicle_pose_key,
    antenna_offset,
    gnss_enu,
    noise
));

Notes:

GNSS measurements should be converted from Lat/Lon/Alt to ENU before use
Noise model should reflect actual GNSS accuracy (varies with satellite visibility, multipath, atmospheric conditions, receiver quality, etc.)
For RTK-GPS, use much smaller noise values (cm-level accuracy)
Consider using FactorGnssMapEnu if optimizing map-to-ENU alignment

See also:

FactorGnssMapEnu for georeferencing with explicit map transform

#include <FactorGnssEnu.h>

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

    FactorGnssEnu();

    FactorGnssEnu(
        gtsam::Key kPi,
        const gtsam::Point3& sensorOnVehicle,
        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

FactorGnssEnu()

Default constructor for serialization.

FactorGnssEnu(
    gtsam::Key kPi,
    const gtsam::Point3& sensorOnVehicle,
    const gtsam::Point3& observedENU,
    const gtsam::SharedNoiseModel& model
    )

Construct a GNSS ENU factor.

Parameters:

kPi	Key for vehicle pose in ENU frame
sensorOnVehicle	GNSS antenna position in vehicle coordinates (meters) For example: (0.5, 0, 1.8) means 0.5m forward, 1.8m above vehicle origin
observedENU	Measured GNSS position in ENU coordinates (meters)
model	Noise model (3D position uncertainty)

Methods

gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

Deep copy of this factor

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

Measurement expression.

Computes: transformFrom(Pi, sensorOnVehicle)

Parameters:

keys	Array containing vehicle pose key

Returns:

Expression for antenna position in ENU

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