Minimal example: defining new factor and variable types for GTSAM

common-types.cpp

#include <libfgcontrol/common-types.h>
#include <iostream>

using namespace fgcontrol;

void Params1stOrder::print(const std::string& prefix) const
{
    std::cout << prefix << "K=" << K() << " tau=" << tau() << " Td=" << Td()
              << "\n";
}
bool Params1stOrder::equals(const Params1stOrder& p, double tol) const
{
    throw std::runtime_error("to do");
    return false;
}

void state_t::print(const std::string& prefix) const
{
    std::cout << prefix << this->transpose();
}

common-types.h

#pragma once

#include <gtsam/base/Vector.h>
#include <gtsam/base/VectorSpace.h>

namespace fgcontrol
{
/** Type for system inputs u_{k} */
using action_t = gtsam::Vector1;

/** Type for system outputs y_{k} */
using output_t = gtsam::Vector1;

/** Parameters of a 1st order dynamic system: m=[K tau Td] */
class Params1stOrder : public gtsam::Vector3
{
   public:
    Params1stOrder() = default;
    Params1stOrder(double K, double tau, double Td) : gtsam::Vector3(K, tau, Td)
    {
    }
    Params1stOrder(const gtsam::Vector3& v) : gtsam::Vector3(v) {}

    double K() const { return (*this)[0]; }
    double tau() const { return (*this)[1]; }
    double Td() const { return (*this)[2]; }

    void K(double val) { (*this)[0] = val; }
    void tau(double val) { (*this)[1] = val; }
    void Td(double val) { (*this)[2] = val; }

    // --- Interface expected by gtsam::VectorSpace ---
    /** print with optional string */
    void print(const std::string& prefix = "") const;
    /** equals with an tolerance */
    bool equals(const Params1stOrder& p, double tol = 1e-9) const;

    enum
    {
        dimension = 3
    };
    /// identity for group operation
    inline static Params1stOrder identity()
    {
        return Params1stOrder(0.0, 0.0, 0.0);
    }
    /// return as Vector3
    const gtsam::Vector3& vector() const { return *this; }

    template <typename vector_like_t>
    Params1stOrder operator+(const vector_like_t& b) const
    {
        Params1stOrder a = *this;
        a += b;
        return a;
    }
    Params1stOrder operator-(const Params1stOrder& b) const
    {
        Params1stOrder a = *this;
        a -= b;
        return a;
    }
    // --- End of interface expected by gtsam::VectorSpace ---
};

/** Type for system internal states x_{k} */
class state_t : public gtsam::Vector
{
   public:
    state_t() = default;
    state_t(double val) : gtsam::Vector(1) { (*this)[0] = val; }
    state_t(const gtsam::Vector& v) : gtsam::Vector(v) {}

    // --- Interface expected by gtsam::VectorSpace ---
    /** print with optional string */
    void print(const std::string& prefix = "") const;
    /** equals with an tolerance */
    bool equals(const state_t& p, double tol = 1e-9) const
    {
        return gtsam::traits<gtsam::Vector>::Equals(*this, p, tol);
    }
    enum
    {
        dimension = -1
    };
    int dim() const { return dimension; }
    /// identity for group operation
    inline static state_t identity() { return state_t(0.0); }
    /// return as Vector
    const gtsam::Vector& vector() const { return *this; }

    template <typename vector_like_t>
    state_t operator+(const vector_like_t& b) const
    {
        state_t a = *this;
        a += b;
        return a;
    }
    state_t operator-(const state_t& b) const
    {
        state_t a = *this;
        a -= b;
        return a;
    }
    // --- End of interface expected by gtsam::VectorSpace ---
};

}  // namespace fgcontrol

namespace gtsam
{
// traits for: action_t & output_t
template <>
struct traits<gtsam::Vector1> : public internal::ScalarTraits<double>
{
};
template <>
struct traits<const gtsam::Vector1> : public internal::ScalarTraits<double>
{
};

// traits for: Params1stOrder
template <>
struct traits<fgcontrol::Params1stOrder>
    : public internal::VectorSpace<fgcontrol::Params1stOrder>
{
};
template <>
struct traits<const fgcontrol::Params1stOrder>
    : public internal::VectorSpace<fgcontrol::Params1stOrder>
{
};

// traits for: state_t
template <>
struct traits<fgcontrol::state_t>
    : public internal::VectorSpace<fgcontrol::state_t>
{
};
template <>
struct traits<const fgcontrol::state_t>
    : public internal::VectorSpace<fgcontrol::state_t>
{
};

}  // namespace gtsam

Factor1stOrderOutput.cpp

#include <libfgcontrol/Factor1stOrderOutput.h>
#include <mrpt/core/exceptions.h>

using namespace fgcontrol;

Factor1stOrderOutput::~Factor1stOrderOutput() = default;

gtsam::NonlinearFactor::shared_ptr Factor1stOrderOutput::clone() const
{
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

void Factor1stOrderOutput::print(
    const std::string& s, const gtsam::KeyFormatter& keyFormatter) const
{
    std::cout << s << "Factor1stOrderOutput(" << keyFormatter(this->key1())
              << "," << keyFormatter(this->key2()) << ")\n";
    gtsam::traits<gtsam::Vector1>::Print(u_[0], "  u: ");
    model_.print("  modelParams: ");
    this->noiseModel_->print("  noise model: ");
}

bool Factor1stOrderOutput::equals(
    const gtsam::NonlinearFactor& expected, double tol) const
{
    const This* e = dynamic_cast<const This*>(&expected);
    return e != nullptr && Base::equals(*e, tol) &&
           gtsam::traits<gtsam::Vector1>::Equals(this->u_[0], e->u_[0], tol) &&
           gtsam::traits<gtsam::Vector3>::Equals(this->model_, e->model_, tol);
}

gtsam::Vector Factor1stOrderOutput::evaluateError(
    const state_t& x_k, const output_t& y_k, boost::optional<gtsam::Matrix&> H1,
    boost::optional<gtsam::Matrix&> H2) const
{
    ASSERT_ABOVE_(x_k.size(), 0);
    const auto n = x_k.size();

    gtsam::Vector err(1);

    if (H1)
    {
        auto& H1v = H1.value();
        // H1v.setZero(6, 6);
        // H1v.block<3, 3>(0, 3) = gtsam::I_3x3;
    }

    return err;
}

Factor1stOrderOutput.h

#pragma once

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <libfgcontrol/common-types.h>

namespace fgcontrol
{
/** First-order system: output factor with unknowns: x_{k},y_{k}, fixed
 * data: u_k, m_k */
class Factor1stOrderOutput : public gtsam::NoiseModelFactor2<state_t, output_t>
{
   private:
    using This = Factor1stOrderOutput;
    using Base = gtsam::NoiseModelFactor2<state_t, output_t>;

    Params1stOrder model_;
    action_t       u_;  //!< action

   public:
    // shorthand for a smart pointer to a factor
    using shared_ptr = boost::shared_ptr<This>;

    /** default constructor - only use for serialization */
    Factor1stOrderOutput() = default;

    /** Constructor */
    Factor1stOrderOutput(
        const Params1stOrder& modelParams, const action_t u_action,
        const gtsam::SharedNoiseModel& noiseModel, gtsam::Key key_x_k,
        gtsam::Key key_y_k)
        : Base(noiseModel, key_x_k, key_y_k), model_(modelParams), u_(u_action)
    {
    }

    virtual ~Factor1stOrderOutput() override;

    /// @return a deep copy of this factor
    virtual gtsam::NonlinearFactor::shared_ptr clone() const override;

    /** implement functions needed for Testable */

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

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

    /** implement functions needed to derive from Factor */

    /** vector of errors */
    gtsam::Vector evaluateError(
        const state_t& x_k, const output_t& y_k,
        boost::optional<gtsam::Matrix&> H1 = boost::none,
        boost::optional<gtsam::Matrix&> H2 = boost::none) const override;

    /** number of variables attached to this factor */
    std::size_t size() const { return 2; }

   private:
    /** Serialization function */
    friend class boost::serialization::access;
    template <class ARCHIVE>
    void serialize(ARCHIVE& ar, const unsigned int /*version*/)
    {
        ar& boost::serialization::make_nvp(
            "Factor1stOrderOutput",
            boost::serialization::base_object<Base>(*this));
        ar& BOOST_SERIALIZATION_NVP(u_);
    }
};

}  // namespace fgcontrol

simulate-1storder-dynamics.cpp

#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <libfgcontrol/Factor1stOrderOutput.h>
#include <libfgcontrol/Factor1stOrderTran.h>
#include <mrpt/core/exceptions.h>
#include <iostream>

void example()
{
    using gtsam::symbol_shorthand::X;
    using gtsam::symbol_shorthand::Y;

    // Parameters for a first-order system with delay:
    fgcontrol::Params1stOrder systemModel;
    systemModel.K(2.0);
    systemModel.tau(1.0);
    systemModel.Td(0.0);

    // Create the factor graph:
    gtsam::NonlinearFactorGraph graph;
    gtsam::Values               initValues;

    // Add factors:
    auto transitionNoise = gtsam::noiseModel::Unit::Create(1);

    const int k_step = 4;

    fgcontrol::state_t x_zero;
    x_zero.setZero(1);

    for (int k = 0; k < 10; k++)
    {
        // Input: Step function
        double u = (k >= k_step ? 1.0 : 0.0);

        graph.emplace_shared<fgcontrol::Factor1stOrderTran>(
            systemModel, u, transitionNoise, X(k), X(k + 1));

        initValues.insert(X(k + 1), x_zero);
        if (k == 0) initValues.insert(X(k), x_zero);
    }

    // Run optimizer:
    graph.print("Factor graph: ");
    initValues.print("initValues: ");
    gtsam::LevenbergMarquardtOptimizer optimizer(graph, initValues);

    const auto& optimValues = optimizer.optimize();

    // Process results:
    optimValues.print("optimValues");
}

int main(int argc, char** argv)
{
    try
    {
        example();
    }
    catch (const std::exception& e)
    {
        std::cerr << mrpt::exception_to_str(e);
    }
    return 0;
}