diff --git a/tests/simulated2D.h b/tests/simulated2D.h index f7a9a9284..22f3ee776 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -83,42 +83,43 @@ namespace simulated2D { } }; + namespace { + /// Prior on a single pose + inline Point2 prior(const Point2& x) { + return x; + } - /// Prior on a single pose - inline Point2 prior(const Point2& x) { - return x; - } + /// Prior on a single pose, optionally returns derivative + Point2 prior(const Point2& x, boost::optional H = boost::none) { + if (H) *H = gtsam::eye(2); + return x; + } - /// Prior on a single pose, optionally returns derivative - Point2 prior(const Point2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(2); - return x; - } + /// odometry between two poses + inline Point2 odo(const Point2& x1, const Point2& x2) { + return x2 - x1; + } - /// odometry between two poses - inline Point2 odo(const Point2& x1, const Point2& x2) { - return x2 - x1; - } - - /// odometry between two poses, optionally returns derivative - Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = + /// odometry between two poses, optionally returns derivative + Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); - return x2 - x1; - } + if (H1) *H1 = -gtsam::eye(2); + if (H2) *H2 = gtsam::eye(2); + return x2 - x1; + } - /// measurement between landmark and pose - inline Point2 mea(const Point2& x, const Point2& l) { - return l - x; - } + /// measurement between landmark and pose + inline Point2 mea(const Point2& x, const Point2& l) { + return l - x; + } - /// measurement between landmark and pose, optionally returns derivative - Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = + /// measurement between landmark and pose, optionally returns derivative + Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); - return l - x; + if (H1) *H1 = -gtsam::eye(2); + if (H2) *H2 = gtsam::eye(2); + return l - x; + } } /** diff --git a/tests/smallExample.h b/tests/smallExample.h index ea9f30b54..eb87e84e2 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -28,6 +28,7 @@ namespace gtsam { namespace example { + namespace { typedef NonlinearFactorGraph Graph; @@ -150,13 +151,9 @@ Ordering planarOrdering(size_t N); std::pair splitOffPlanarTree( size_t N, const GaussianFactorGraph& original); -} // example -} // gtsam // Implementations -namespace gtsam { -namespace example { // using namespace gtsam::noiseModel; @@ -649,5 +646,6 @@ std::pair splitOffPlanarTree(size_t N /* ************************************************************************* */ +} // anonymous namespace } // \namespace example } // \namespace gtsam