class mola::FactorTrapezoidalIntegrator

Overview

Factor for constant angular velocity model, equivalent to expression:

Pi + 0.5 * dt * (gtsam::rotate(Ri, bVi) + gtsam::rotate(Rj, bVj)) - Pj = errZero

#include <FactorTrapezoidalIntegrator.h>

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

    FactorTrapezoidalIntegrator();

    FactorTrapezoidalIntegrator(
        gtsam::Key kPi,
        gtsam::Key kVi,
        gtsam::Key kRi,
        gtsam::Key kPj,
        gtsam::Key kVj,
        gtsam::Key kRj,
        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

FactorTrapezoidalIntegrator()

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