class mola::factors::FactorConstLocalVelocityPose

Overview

Factor for constant velocity model in local coordinates, equivalent to expression:

gtsam::rotate(Ri, bWi) - gtsam::rotate(Rj, bWj) = errZero

This works for both, linear and angular velocities.

Note that angular and linear velocities are stored in Values in the body “b” frame, hence the “b” prefix, and the need for the orientations “R”.

#include <FactorConstLocalVelocity.h>

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

    FactorConstLocalVelocityPose();

    FactorConstLocalVelocityPose(
        gtsam::Key kTi,
        gtsam::Key kWi,
        gtsam::Key kTj,
        gtsam::Key kWj,
        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

FactorConstLocalVelocityPose()

default constructor

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