class mola::factors::FactorTricycleKinematic

Overview

Factor implementing tricycle kinematic motion model with Euler integration.

This factor models the motion of a vehicle with Ackermann steering (like cars, motorcycles, or tricycles) based on linear and angular velocities. The motion follows circular arc trajectories determined by the turn radius.

Kinematic Model:

For angular velocity ω ≠ 0:

  • The vehicle follows a circular arc with radius R = v/ω

  • Position change: Δp = (v/ω) * [sin(ω·Δt), 1-cos(ω·Δt), 0]^T (in 2D)

  • Orientation change: Δθ = ω·Δt

For ω ≈ 0 (straight line motion):

  • Position change: Δp = v·Δt * [1, 0, 0]^T (forward direction)

  • Orientation change: Δθ ≈ 0

The factor uses Euler integration (forward integration at initial time):

\[T_j = T_i \oplus f(v_i, \omega_i, \Delta t)\]

where velocities are measured in the body frame at time i.

State Variables:

  • T_i: SE(3) pose at time i (position + orientation)

  • v_i: Linear velocity vector in body frame vx, vy, vz

  • ω_i: Angular velocity vector in body frame ωx, ωy, ωz

  • T_j: SE(3) pose at time j (predicted)

Use Cases:

  • Wheel odometry integration for cars, trucks, robots with Ackermann steering

  • Smoothing trajectories of wheeled vehicles

  • Fusing encoder measurements with other sensors

  • Ground vehicle state estimation

Example Usage:

using namespace mola::factors;

gtsam::NonlinearFactorGraph graph;

// State variables
gtsam::Key kT0 = gtsam::Symbol('x', 0);  // Initial pose
gtsam::Key kV0 = gtsam::Symbol('v', 0);  // Linear velocity
gtsam::Key kW0 = gtsam::Symbol('w', 0);  // Angular velocity
gtsam::Key kT1 = gtsam::Symbol('x', 1);  // Final pose

// Noise model (6D: 3D rotation + 3D translation error)
auto noise = gtsam::noiseModel::Diagonal::Sigmas(
    (gtsam::Vector(6) << 0.01, 0.01, 0.01,  // rotation std dev (rad)
                         0.05, 0.05, 0.02   // translation std dev (m)
    ).finished()
);

double dt = 0.1; // 100ms time step

// Add kinematic constraint
graph.add(FactorTricycleKinematic(kT0, kV0, kW0, kT1, dt, noise));

Notes:

  • For 2D motion (ground vehicles), vz and ωx, ωy should typically be zero

  • The threshold parameter controls when ω is considered “close to zero”

  • Analytical Jacobians are provided

See also:

FactorAngularVelocityIntegration for rotation-only integration

FactorTrapezoidalIntegrator for trapezoidal integration (uses both v_i and v_j)

#include <FactorTricycleKinematic.h>

class FactorTricycleKinematic: public gtsam::NoiseModelFactor4< gtsam::Pose3, gtsam::Point3, gtsam::Point3, gtsam::Pose3 >
{
public:
    // construction

    FactorTricycleKinematic();

    FactorTricycleKinematic(
        gtsam::Key kTi,
        gtsam::Key kVi,
        gtsam::Key kWi,
        gtsam::Key kTj,
        const double dt,
        const gtsam::SharedNoiseModel& model,
        const double w_threshold = 1e-4
        );

    // methods

    gtsam::NonlinearFactor::shared_ptr clone() const;

    gtsam::Vector evaluateError(
        const gtsam::Pose3& Ti,
        const gtsam::Point3& bVi,
        const gtsam::Point3& bWi,
        const gtsam::Pose3& Tj,
        gtsam::Matrix* H1 = nullptr,
        gtsam::Matrix* H2 = nullptr,
        gtsam::Matrix* H3 = nullptr,
        gtsam::Matrix* H4 = nullptr
        ) 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;

    static gtsam::Pose3 tricycleKinematicModel(
        const gtsam::Pose3& Ti,
        const gtsam::Point3& bVi,
        const gtsam::Point3& bWi,
        double dt,
        double w_threshold
        );
};

Construction

FactorTricycleKinematic()

Default constructor for serialization.

FactorTricycleKinematic(
    gtsam::Key kTi,
    gtsam::Key kVi,
    gtsam::Key kWi,
    gtsam::Key kTj,
    const double dt,
    const gtsam::SharedNoiseModel& model,
    const double w_threshold = 1e-4
    )

Construct a tricycle kinematic factor.

Parameters:

kTi

Key for initial pose T_i

kVi

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

kWi

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

kTj

Key for final pose T_j

dt

Time interval Δt (seconds)

model

Noise model (6D: rotation + translation error)

w_threshold

Threshold below which ω is treated as zero (default: 1e-4 rad/s) Adjust if numerical issues occur near zero angular velocity

Methods

gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

Deep copy of this factor

gtsam::Vector evaluateError(
    const gtsam::Pose3& Ti,
    const gtsam::Point3& bVi,
    const gtsam::Point3& bWi,
    const gtsam::Pose3& Tj,
    gtsam::Matrix* H1 = nullptr,
    gtsam::Matrix* H2 = nullptr,
    gtsam::Matrix* H3 = nullptr,
    gtsam::Matrix* H4 = nullptr
    ) const

Evaluate the error of this factor.

Computes the relative pose error between the predicted pose (from kinematics) and the actual T_j value.

Parameters:

Ti

Initial pose

bVi

Linear velocity

bWi

Angular velocity

Tj

Actual final pose

H1

Optional Jacobian w.r.t. Ti

H2

Optional Jacobian w.r.t. bVi

H3

Optional Jacobian w.r.t. bWi

H4

Optional Jacobian w.r.t. Tj

Returns:

6D error vector [rotation_error(3D), translation_error(3D)]

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

Print factor information

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

equals

static gtsam::Pose3 tricycleKinematicModel(
    const gtsam::Pose3& Ti,
    const gtsam::Point3& bVi,
    const gtsam::Point3& bWi,
    double dt,
    double w_threshold
    )

Compute predicted pose using tricycle kinematic model.

Static helper function that can be used independently for forward prediction.

Parameters:

Ti

Initial pose

bVi

Linear velocity in body frame

bWi

Angular velocity in body frame

dt

Time step

w_threshold

Threshold for straight-line motion

Returns:

Predicted pose T_j