class mola::factors::FactorTrapezoidalIntegrator

Overview

Factor implementing trapezoidal rule integration for velocity.

This factor constrains position states based on velocity measurements using the trapezoidal integration rule, which provides second-order accuracy.

Mathematical Model:

\[p_j = p_i + \frac{\Delta t}{2} \left( R_i \cdot v_i + R_j \cdot v_j \right)\]

where:

  • \(p_i, p_j\) are 3D positions at times i and j

  • \(v_i, v_j\) are velocity vectors in the body frame

  • \(R_i, R_j\) are orientation matrices (rotate body frame to global frame)

  • \(\Delta t\) is the time interval

The factor error is:

\[e = p_i + \frac{\Delta t}{2}(R_i v_i + R_j v_j) - p_j\]

Advantages over Euler Integration:

  • More accurate for varying velocities (2nd order vs 1st order)

  • Better numerical stability for larger time steps

  • Accounts for velocity changes during the interval

When to Use:

  • Smoothing trajectories with velocity estimates at each timestep

  • Fusing IMU linear acceleration (integrated to velocity)

  • Visual-inertial odometry

  • Any scenario where velocities are available at both endpoints

Comparison:

  • Euler integration : Uses only v_i → simpler, less accurate

  • Trapezoidal integration : Uses both v_i and v_j → more accurate, needs both velocities

  • RK4 : Even more accurate but computationally expensive

Example Usage:

using namespace mola::factors;

gtsam::NonlinearFactorGraph graph;

// State variables at time i
gtsam::Key kPi = gtsam::Symbol('p', 0);  // Position
gtsam::Key kVi = gtsam::Symbol('v', 0);  // Velocity (body frame)
gtsam::Key kRi = gtsam::Symbol('r', 0);  // Orientation

// State variables at time j
gtsam::Key kPj = gtsam::Symbol('p', 1);
gtsam::Key kVj = gtsam::Symbol('v', 1);
gtsam::Key kRj = gtsam::Symbol('r', 1);

// Noise model (3D position error)
auto noise = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);  // 10cm std dev

double dt = 0.1; // 100ms

// Add trapezoidal integration constraint
graph.add(FactorTrapezoidalIntegrator(
    kPi, kVi, kRi,
    kPj, kVj, kRj,
    dt, noise
));

Important Notes:

  • Velocities must be in the body frame (not global frame)

  • Orientations R_i and R_j rotate from body to global frame

  • For 2D motion, set vz = 0 in velocities

  • Time step dt should be reasonably small (typically < 1 second)

See also:

FactorTrapezoidalIntegratorPose for the Pose3 variant

FactorTricycleKinematic for kinematic models (Euler integration)

#include <FactorTrapezoidalIntegrator.h>

class FactorTrapezoidalIntegrator: public gtsam::ExpressionFactorN< gtsam::Point3, gtsam::Point3, gtsam::Point3, gtsam::Rot3, gtsam::Point3, gtsam::Point3, gtsam::Rot3 >
{
public:
    // construction

    FactorTrapezoidalIntegrator();

    FactorTrapezoidalIntegrator(
        gtsam::Key kPi,
        gtsam::Key kVi,
        gtsam::Key kRi,
        gtsam::Key kPj,
        gtsam::Key kVj,
        gtsam::Key kRj,
        const double dt,
        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

FactorTrapezoidalIntegrator()

Default constructor for serialization.

FactorTrapezoidalIntegrator(
    gtsam::Key kPi,
    gtsam::Key kVi,
    gtsam::Key kRi,
    gtsam::Key kPj,
    gtsam::Key kVj,
    gtsam::Key kRj,
    const double dt,
    const gtsam::SharedNoiseModel& model
    )

Construct a trapezoidal integrator factor.

Parameters:

kPi

Key for initial position p_i

kVi

Key for initial velocity v_i (body frame, m/s)

kRi

Key for initial orientation R_i

kPj

Key for final position p_j

kVj

Key for final velocity v_j (body frame, m/s)

kRj

Key for final orientation R_j

dt

Time interval Δt (seconds)

model

Noise model (3D position error)

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.

Implements: Pi + 0.5*dt*(rotate(Ri, bVi) + rotate(Rj, bVj)) - Pj

Parameters:

keys

Array containing [kPi, kVi, kRi, kPj, kVj, kRj]

Returns:

Expression computing position integration error

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