class mola::factors::FactorTricycleKinematic

Overview

Factor for tricycle kinematic model using Euler integration.

Models motion as a circular arc based on linear velocity v and angular velocity w at the initial timestep (Euler integration).

The kinematic model:

For w ≈ 0: straight line motion
For w ≠ 0: circular arc with radius R = v/w

State variables:

Ti: SE(3) pose at time i
bVi: linear velocity vector in body frame [vx, vy, vz]
bWi: angular velocity vector in body frame [wx, wy, wz]
Tj: SE(3) pose at time j

The factor constrains Tj to be the predicted pose after following the tricycle kinematic model from Ti for duration dt using velocities at time i.

#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
    )

Constructor

Parameters:

kTi	Key for initial pose
kVi	Key for initial linear velocity (body frame)
kWi	Key for initial angular velocity (body frame)
kTj	Key for final pose
dt	Time interval
model	Noise model (should be 6D for Pose3)
w_threshold	Threshold below which angular velocity is considered zero

Methods

gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

a 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

Error function Computes the error as the relative pose between predicted and actual Tj Returns a 6D vector: [rotation_error (3D), translation_error (3D)]

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

print

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 with Euler integration