class mola::factors::FactorAngularVelocityIntegrationPose

Overview

Variant of FactorAngularVelocityIntegration using Pose3 states.

This factor is identical to FactorAngularVelocityIntegration but operates on full 6-DOF poses (gtsam::Pose3) instead of pure rotations. Only the rotation component is used; translation is ignored.

Use when: Your state graph uses Pose3 instead of separate Rot3 variables

See also:

FactorAngularVelocityIntegration for detailed mathematical description

#include <FactorAngularVelocityIntegration.h>

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

    FactorAngularVelocityIntegrationPose();

    FactorAngularVelocityIntegrationPose(
        gtsam::Key kTi,
        gtsam::Key kbWi,
        gtsam::Key kTj,
        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

FactorAngularVelocityIntegrationPose()

Default constructor for serialization.

FactorAngularVelocityIntegrationPose(
    gtsam::Key kTi,
    gtsam::Key kbWi,
    gtsam::Key kTj,
    const double dt,
    const gtsam::SharedNoiseModel& model
    )

Constructor.

Parameters:

kTi	Key for initial pose (only rotation used)
kbWi	Key for angular velocity in body frame
kTj	Key for final pose (only rotation used)
dt	Time interval between states
model	Noise model (3D for rotation error)

Methods

gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

a deep copy of this factor

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