class mola::state_estimation_smoother::FactorConstLocalVelocity

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 FactorConstLocalVelocity: public gtsam::ExpressionFactorN< gtsam::Point3, gtsam::Rot3, gtsam::Point3, gtsam::Rot3, gtsam::Point3 >
{
public:
    // construction

    FactorConstLocalVelocity();

    FactorConstLocalVelocity(
        gtsam::Key kRi,
        gtsam::Key kWi,
        gtsam::Key kRj,
        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

FactorConstLocalVelocity()

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