/** * @file simulated2D.h * @brief measurement functions and derivatives for simulated 2D robot * @author Frank Dellaert */ // \callgraph #pragma once #include "Point2.h" #include "TupleConfig.h" #include "NonlinearFactor.h" // \namespace namespace gtsam { namespace simulated2D { // Simulated2D robots have no orientation, just a position typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; typedef PairConfig Config; /** * Prior on a single pose, and optional derivative version */ inline Point2 prior(const Point2& x) { return x; } Point2 prior(const Point2& x, boost::optional H = boost::none); /** * odometry between two poses, and optional derivative version */ inline Point2 odo(const Point2& x1, const Point2& x2) { return x2 - x1; } Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none); /** * measurement between landmark and pose, and optional derivative version */ inline Point2 mea(const Point2& x, const Point2& l) { return l - x; } Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = boost::none, boost::optional H2 = boost::none); /** * Unary factor encoding a soft prior on a vector */ struct Prior: public NonlinearFactor1 { Point2 z_; Prior(const Point2& z, const SharedGaussian& model, const PoseKey& key) : NonlinearFactor1 (model, key), z_(z) { } Vector evaluateError(const Point2& x, boost::optional H = boost::none) const { return (prior(x, H) - z_).vector(); } }; /** * Binary factor simulating "odometry" between two Vectors */ struct Odometry: public NonlinearFactor2 { Point2 z_; Odometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1, const PoseKey& j2) : z_(z), NonlinearFactor2 ( model, j1, j2) { } Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { return (odo(x1, x2, H1, H2) - z_).vector(); } }; /** * Binary factor simulating "measurement" between two Vectors */ struct Measurement: public NonlinearFactor2 { Point2 z_; Measurement(const Point2& z, const SharedGaussian& model, const PoseKey& j1, const PointKey& j2) : z_(z), NonlinearFactor2 ( model, j1, j2) { } Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { return (mea(x1, x2, H1, H2) - z_).vector(); } }; } // namespace simulated2D } // namespace gtsam