class mola::factors::FactorGnssMapEnu

Overview

Factor for GNSS measurements with explicit ENU-to-map transform optimization.

This factor extends FactorGnssEnu by adding a transformation between the local map coordinate system and the ENU coordinate system. This is useful when:

• Georeferencing a SLAM map to global coordinates

• The initial map origin is arbitrary (e.g., starting pose)

• You want to optimize the map’s global position and orientation

Mathematical Model:

\[p_{antenna}^{ENU} = T_{ENU \leftarrow map} \cdot T_{map \leftarrow i} \oplus p_{sensor}^{vehicle}\]

where:

• \(T_{map \leftarrow i}\) is the vehicle pose in the local map frame

• \(T_{ENU \leftarrow map}\) is the transform from map to ENU coordinates

• This transform is part of the optimization variables

Use Cases:

• SLAM systems that build maps in local coordinates but need global alignment

• Loop closure with global constraints

• Multi-session SLAM with consistent global frame

• Incremental georeferencing as more GNSS data arrives

Example Usage:

using namespace mola::factors;

// Keys
gtsam::Key kEnuToMap = gtsam::Symbol('T', 0);   // ENU-to-map transform (to optimize)
gtsam::Key kVehicle  = gtsam::Symbol('x', 10);  // Vehicle pose in map frame

// GNSS measurement
gtsam::Point3 gnss_enu(1523.4, 2341.7, 15.3);
gtsam::Point3 antenna_offset(0.5, 0.0, 1.8);
auto noise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(3.0, 3.0, 5.0));

// Add factor
graph.add(FactorGnssMapEnu(
    kEnuToMap,
    kVehicle,
    antenna_offset,
    gnss_enu,
    noise
));

// Initialize map-to-ENU transform (e.g., identity as prior)
values.insert(kEnuToMap, gtsam::Pose3::Identity());

See also:

FactorGnssEnu for direct pose constraints without transform optimization

#include <FactorGnssMapEnu.h>

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

    FactorGnssMapEnu();

    FactorGnssMapEnu(
        gtsam::Key kT_enu2map,
        gtsam::Key kT_map2i,
        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

FactorGnssMapEnu()

Default constructor for serialization.

FactorGnssMapEnu(
    gtsam::Key kT_enu2map,
    gtsam::Key kT_map2i,
    const gtsam::Point3& sensorOnVehicle,
    const gtsam::Point3& observedENU,
    const gtsam::SharedNoiseModel& model
    )

Construct a GNSS factor with map transform.

Parameters:

kT_enu2map	Key for ENU-to-map coordinate transform
kT_map2i	Key for vehicle pose in map coordinates
sensorOnVehicle	GNSS antenna position in vehicle frame
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

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

Measurement expression.

Computes: transformFrom(T_enu2map * T_map2i, sensorOnVehicle)

Parameters:

keys	Array containing [kT_enu2map, kT_map2i]

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