diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index 660b0da3d..1e1cccaba 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -52,12 +52,13 @@ namespace gtsam { /** * Unary factor encoding a soft prior on a vector */ - struct Prior: public NonlinearFactor1 { + template + struct GenericPrior: public NonlinearFactor1 { Point2 z_; - Prior(const Point2& z, const SharedGaussian& model, const PoseKey& key) : - NonlinearFactor1 (model, key), z_(z) { + GenericPrior(const Point2& z, const SharedGaussian& model, const PoseKey& key) : + NonlinearFactor1 (model, key), z_(z) { } Vector evaluateError(const Point2& x, boost::optional H = @@ -70,13 +71,14 @@ namespace gtsam { /** * Binary factor simulating "odometry" between two Vectors */ - struct Odometry: public NonlinearFactor2 + struct GenericOdometry: public NonlinearFactor2 { Point2 z_; - Odometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1, + GenericOdometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1, const PoseKey& j2) : - z_(z), NonlinearFactor2 ( + z_(z), NonlinearFactor2 ( model, j1, j2) { } @@ -90,23 +92,30 @@ namespace gtsam { /** * Binary factor simulating "measurement" between two Vectors */ - struct Measurement: public NonlinearFactor2 + class GenericMeasurement: public NonlinearFactor2 { + public: - Point2 z_; + Point2 z_; - Measurement(const Point2& z, const SharedGaussian& model, - const PoseKey& j1, const PointKey& j2) : - z_(z), NonlinearFactor2 ( - model, j1, j2) { - } + GenericMeasurement(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(); - } + 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(); + } }; + /** Typedefs for regular use */ + typedef GenericPrior Prior; + typedef GenericOdometry Odometry; + typedef GenericMeasurement Measurement; + } // namespace simulated2D } // namespace gtsam