mola_gtsam_factors

C++ library providing reusable GTSAM (Georgia Tech Smoothing and Mapping) factors for georeferencing and state estimation in robotics and autonomous systems.

Overview

This library is part of the MOLA (Modular Optimization framework for Localization and mApping) project and provides custom factor graph nodes that can be used with GTSAM for:

• Sensor fusion: Combine IMU, GNSS, wheel odometry, and other sensors
• State estimation: Track robot pose, velocities, and orientation over time
• Georeferencing: Align local maps with global coordinate systems
• Motion modeling: Apply kinematic and dynamic constraints

Features

Motion Integration Factors

• FactorAngularVelocityIntegration: Integrates angular velocity measurements to constrain orientation changes
• FactorTrapezoidalIntegrator: Trapezoidal rule integration for velocity-to-position constraints
• FactorTricycleKinematic: Kinematic model for tricycle-like vehicles (cars, Ackermann steering)

Velocity Constraints

• FactorConstLocalVelocity: Enforces constant velocity assumption in the body frame (useful for smoothing)

GNSS/GPS Factors

• FactorGnssEnu: GNSS observations in East-North-Up (ENU) coordinates
• FactorGnssMapEnu: GNSS with explicit ENU-to-map transform optimization

IMU/Orientation Factors

• MeasuredGravityFactor: Gravity-aligned orientation from accelerometer measurements
• Pose3RotationFactor: Constraints on rotation only (decoupled from translation)

Installation

ROS 2 (Recommended)

# Using apt (if available in your ROS distro)
sudo apt install ros-${ROS_DISTRO}-mola-gtsam-factors

# Or build from source in your workspace
cd ~/ros2_ws/src
git clone https://github.com/MOLAorg/mola_state_estimation.git
cd ~/ros2_ws
colcon build --packages-select mola_gtsam_factors

Standalone CMake

# Install dependencies
sudo apt install libgtsam-dev libmrpt-poses-dev

# Clone and build
git clone https://github.com/MOLAorg/mola_state_estimation.git
cd mola_state_estimation/mola_gtsam_factors
mkdir build && cd build
cmake ..
make
sudo make install

Dependencies

• GTSAM (>= 4.0): Factor graph optimization library
• MRPT (mrpt-poses): Mobile Robot Programming Toolkit for pose representations
• mola_common: Common MOLA utilities and CMake scripts

Quick Start

Example: Using Angular Velocity Integration

#include <mola_gtsam_factors/FactorAngularVelocityIntegration.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>

using namespace mola::factors;

// Create factor graph
gtsam::NonlinearFactorGraph graph;

// Define keys for rotations and angular velocity
gtsam::Key kR0 = gtsam::Symbol('R', 0);
gtsam::Key kW0 = gtsam::Symbol('W', 0);
gtsam::Key kR1 = gtsam::Symbol('R', 1);

// Create noise model (3D rotation uncertainty)
auto noise = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);

// Add factor: R1 should be R0 rotated by angular velocity W0 over time dt
double dt = 0.1; // 100ms
graph.add(FactorAngularVelocityIntegration(kR0, kW0, kR1, dt, noise));

// Initialize values and optimize...

Example: GNSS Positioning

```cpp #include <mola_gtsam_factors/FactorGnssEnu.h>

File truncated at 100 lines see the full file

Changelog for package mola_gtsam_factors

2.0.1 (2026-01-15)

• Fix potential bug in sm_georeferencing
• Merge pull request #11 from MOLAorg/feat/improve-docs Improve docs for mola_gtsam_factors
• Improve docs for mola_gtsam_factors
• Contributors: Jose Luis Blanco-Claraco

2.0.0 (2026-01-14)

• Merge pull request #7 from MOLAorg/feature/tricycle-kinematic Add tricycle kinematics
• fix jacobians
• remove now unused ctor param
• Fix build for gtsam>=4.3
• Copyright year bump
• Add FactorTricycleKinematic
• Merge pull request #9 from MOLAorg/fix/build-deps Fix wrong dep name
• Fix wrong dep name
• Merge pull request #5 from MOLAorg/feature/fuse-gnss-imu-odom Refactor: new packages mola_georeferencing, mola_gtsam_factors, functional smoother state estimator
• fixed imu acc gravity alignment
• add IMU+GPS based azimuth estimation
• Refactor: symbolic factor classes moved as internal smoother classes
• Make mola_gtsam_factors non headers-only and rename GNSS2ENU as GnssEnu for consistency
• implement correct auto geo-referenciation
• refactor to use Pose3 instead of P+R
• One further fix for cmake
• integrate code coverage in cmake
• package.xml: add FILE to license tags
• Fix build with gtsam>2.3
• Refactor to expose all gtsam factors into a new library 'mola_gtsam_factors'
• Contributors: Jose Luis Blanco-Claraco