class mola::factors::FactorAngularVelocityIntegration

Overview

Factor for angular velocity integration using the SO(3) exponential map.

This factor implements the constraint:

\[R_j = R_i \oplus \exp(\omega_i \cdot \Delta t)\]

where:

  • \(R_i, R_j\) are 3D rotations (SO(3)) at times i and j

  • \(\omega_i\) is the angular velocity vector in the body frame at time i (rad/s)

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

  • \(\exp\) is the exponential map from so(3) to SO(3)

  • \(\oplus\) is rotation composition

The factor error is computed as:

\[e = \log((R_i \oplus \exp(\omega_i \cdot \Delta t))^{-1} \cdot R_j)\]

Use Cases:

  • Integrating gyroscope measurements from IMUs

  • Constraining orientation changes based on measured angular rates

  • Smoothing rotation trajectories with velocity priors

Example Usage:

using namespace mola::factors;

gtsam::NonlinearFactorGraph graph;

// Keys for two rotation states and angular velocity
gtsam::Key kR0 = gtsam::Symbol('R', 0);
gtsam::Key kW0 = gtsam::Symbol('W', 0);  // Angular velocity at time 0
gtsam::Key kR1 = gtsam::Symbol('R', 1);

// Noise model (uncertainty in rotation, typically from gyro noise)
auto noise = gtsam::noiseModel::Isotropic::Sigma(3, 0.01); // 0.01 rad std dev

// Time step
double dt = 0.1; // 100 milliseconds

// Add factor
graph.add(FactorAngularVelocityIntegration(kR0, kW0, kR1, dt, noise));

See also:

FactorAngularVelocityIntegrationPose for the Pose3 variant

#include <FactorAngularVelocityIntegration.h>

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

    FactorAngularVelocityIntegration();

    FactorAngularVelocityIntegration(
        gtsam::Key kRi,
        gtsam::Key kbWi,
        gtsam::Key kRj,
        const double dt,
        const gtsam::SharedNoiseModel& model
        );

    // methods

    gtsam::NonlinearFactor::shared_ptr clone() const;
    gtsam::Expression<gtsam::Rot3> 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

FactorAngularVelocityIntegration()

Default constructor for serialization.

FactorAngularVelocityIntegration(
    gtsam::Key kRi,
    gtsam::Key kbWi,
    gtsam::Key kRj,
    const double dt,
    const gtsam::SharedNoiseModel& model
    )

Constructor.

Parameters:

kRi

Key for initial rotation state R_i

kbWi

Key for angular velocity in body frame ω_i (rad/s)

kRj

Key for final rotation state R_j

dt

Time interval Δt between states (seconds)

model

Noise model (3D for rotation error in tangent space)

Methods

gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

a deep copy of this factor

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

Returns the measurement expression for this factor.

Implements: between(compose(Ri, Expmap(dt*bWi)), Rj)

Parameters:

keys

Array containing [kRi, kbWi, kRj]

Returns:

Expression computing the rotation error

void print(
    const std::string& s,
    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter
    ) const

implement functions needed for Testable print

bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const

equals