diff --git a/geometry/Point2.cpp b/geometry/Point2.cpp index 87caea645..c9b225f16 100644 --- a/geometry/Point2.cpp +++ b/geometry/Point2.cpp @@ -30,5 +30,19 @@ namespace gtsam { } /* ************************************************************************* */ + Point2 Point2::transform_to(const Point2& point, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -eye(2); + if (H2) *H2 = eye(2); + return point - *this; + } + + /* ************************************************************************* */ + Point2 Point2::transform_from(const Point2& point, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = eye(2); + if (H2) *H2 = eye(2); + return point + *this; + } } // namespace gtsam diff --git a/geometry/Point2.h b/geometry/Point2.h index 1dfdd7a47..ef94e3ff8 100644 --- a/geometry/Point2.h +++ b/geometry/Point2.h @@ -41,6 +41,17 @@ namespace gtsam { /** equals with an tolerance, prints out message if unequal*/ bool equals(const Point2& q, double tol = 1e-9) const; + /** simple transform_[to|from] to allow for generic algorithms */ + /** Return point coordinates in pose coordinate frame */ + Point2 transform_to(const Point2& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /** Return point coordinates in global frame */ + Point2 transform_from(const Point2& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + /** Lie requirements */ /** Size of the tangent space of the Lie type */ diff --git a/geometry/tests/testPoint2.cpp b/geometry/tests/testPoint2.cpp index cc90cefef..13775e859 100644 --- a/geometry/tests/testPoint2.cpp +++ b/geometry/tests/testPoint2.cpp @@ -42,8 +42,14 @@ TEST( Point2, norm) } /* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); +TEST( Point2, transforms ) { + Point2 offset(3.0, 4.0); + EXPECT(assert_equal(Point2(5.0, 6.0), offset.transform_from(Point2(2.0, 2.0)))); + EXPECT(assert_equal(Point2(-1.0, -2.0), offset.transform_to(Point2(2.0, 2.0)))); + EXPECT(assert_equal(Point2(1.0, 2.0), offset.transform_to( + offset.transform_from(Point2(1.0, 2.0))))); } + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */