class mola::ConstVelocityFactorSE3

Overview

Factor for constant velocity model in SE(3) between pairs Pose3+Velocity3 More…

#include <ConstVelocityFactorSE3.h>

class ConstVelocityFactorSE3: public gtsam::NoiseModelFactor4< gtsam::Pose3, gtsam::Velocity3, gtsam::Pose3, gtsam::Velocity3 >
{
public:
    // typedefs

    typedef boost::shared_ptr<ConstVelocityFactorSE3> shared_ptr;

    // construction

    ConstVelocityFactorSE3();

    ConstVelocityFactorSE3(
        gtsam::Key pose1,
        gtsam::Key vel1,
        gtsam::Key pose2,
        gtsam::Key vel2,
        const double deltaTime,
        const gtsam::SharedNoiseModel& model
        );

    // methods

    virtual gtsam::NonlinearFactor::shared_ptr clone() const;

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

    virtual bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const;

    gtsam::Vector evaluateError(
        const gtsam::Pose3& p1,
        const gtsam::Velocity3& v1,
        const gtsam::Pose3& p2,
        const gtsam::Velocity3& v2,
        boost::optional<gtsam::Matrix&> H1 = boost::none,
        boost::optional<gtsam::Matrix&> H2 = boost::none,
        boost::optional<gtsam::Matrix&> H3 = boost::none,
        boost::optional<gtsam::Matrix&> H4 = boost::none
        ) const;

    std::size_t size() const;
};

Detailed Documentation

Factor for constant velocity model in SE(3) between pairs Pose3+Velocity3

Construction

ConstVelocityFactorSE3()

default constructor - only use for serialization

ConstVelocityFactorSE3(
    gtsam::Key pose1,
    gtsam::Key vel1,
    gtsam::Key pose2,
    gtsam::Key vel2,
    const double deltaTime,
    const gtsam::SharedNoiseModel& model
    )

Constructor.

Methods

virtual gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

a deep copy of this factor

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

implement functions needed for Testable print

virtual bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const

equals

gtsam::Vector evaluateError(
    const gtsam::Pose3& p1,
    const gtsam::Velocity3& v1,
    const gtsam::Pose3& p2,
    const gtsam::Velocity3& v2,
    boost::optional<gtsam::Matrix&> H1 = boost::none,
    boost::optional<gtsam::Matrix&> H2 = boost::none,
    boost::optional<gtsam::Matrix&> H3 = boost::none,
    boost::optional<gtsam::Matrix&> H4 = boost::none
    ) const

implement functions needed to derive from Factor vector of errors

std::size_t size() const

number of variables attached to this factor