class mola::factors::FactorTrapezoidalIntegratorPose

Overview

Variant of FactorTrapezoidalIntegrator using Pose3 states.

This factor is functionally identical to FactorTrapezoidalIntegrator but operates on full 6-DOF poses (gtsam::Pose3) instead of separate position and rotation variables. Position and orientation are automatically extracted from the pose states.

Use when:

• Your state graph uses Pose3 instead of separate Point3 + Rot3

• You want fewer state variables (one Pose3 vs two variables)

Example Usage:

using namespace mola::factors;

// Using Pose3 for state (position + orientation combined)
gtsam::Key kTi = gtsam::Symbol('x', 0);  // Pose at time i
gtsam::Key kVi = gtsam::Symbol('v', 0);  // Velocity at time i
gtsam::Key kTj = gtsam::Symbol('x', 1);  // Pose at time j
gtsam::Key kVj = gtsam::Symbol('v', 1);  // Velocity at time j

auto noise = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);
double dt = 0.1;

graph.add(FactorTrapezoidalIntegratorPose(kTi, kVi, kTj, kVj, dt, noise));

See also:

FactorTrapezoidalIntegrator for detailed mathematical description

#include <FactorTrapezoidalIntegrator.h>

class FactorTrapezoidalIntegratorPose: public gtsam::ExpressionFactorN< gtsam::Point3, gtsam::Pose3, gtsam::Point3, gtsam::Pose3, gtsam::Point3 >
{
public:
    // construction

    FactorTrapezoidalIntegratorPose();

    FactorTrapezoidalIntegratorPose(
        gtsam::Key kTi,
        gtsam::Key kVi,
        gtsam::Key kTj,
        gtsam::Key kVj,
        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

FactorTrapezoidalIntegratorPose()

Default constructor for serialization.

FactorTrapezoidalIntegratorPose(
    gtsam::Key kTi,
    gtsam::Key kVi,
    gtsam::Key kTj,
    gtsam::Key kVj,
    const double dt,
    const gtsam::SharedNoiseModel& model
    )

Construct a trapezoidal integrator factor using Pose3.

Parameters:

kTi	Key for initial pose (contains position and orientation)
kVi	Key for initial velocity (body frame)
kTj	Key for final pose
kVj	Key for final velocity (body frame)
dt	Time interval
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 extracting position/rotation from poses.

Parameters:

keys	Array containing [kTi, kVi, kTj, kVj]

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