Skip to content

Instantly share code, notes, and snippets.

@jlblancoc
Created January 20, 2020 09:30
Show Gist options
  • Save jlblancoc/0ead1201c2420b073fa426b00567f736 to your computer and use it in GitHub Desktop.
Save jlblancoc/0ead1201c2420b073fa426b00567f736 to your computer and use it in GitHub Desktop.
Minimal example: defining new factor and variable types for GTSAM
#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();
}
#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
#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;
}
#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
#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment