diff --git a/geometry/Point2.cpp b/geometry/Point2.cpp index c9b225f16..4a5581750 100644 --- a/geometry/Point2.cpp +++ b/geometry/Point2.cpp @@ -29,20 +29,4 @@ namespace gtsam { return sqrt(x_*x_ + y_*y_); } - /* ************************************************************************* */ - 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 ef94e3ff8..1dfdd7a47 100644 --- a/geometry/Point2.h +++ b/geometry/Point2.h @@ -41,17 +41,6 @@ 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 4c8f5b11a..0f83f81d8 100644 --- a/geometry/tests/testPoint2.cpp +++ b/geometry/tests/testPoint2.cpp @@ -5,14 +5,11 @@ **/ #include -#include #include using namespace std; using namespace gtsam; -const double tol = 1e-5; - /* ************************************************************************* */ TEST( Point2, expmap) { @@ -44,46 +41,6 @@ TEST( Point2, norm) DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6); } -/* ************************************************************************* */ -Point2 transform_from_proxy(const Point2& pose, const Point2& point) { - return pose.transform_from(point); -} - -/* ************************************************************************* */ -Point2 transform_to_proxy(const Point2& pose, const Point2& point) { - return pose.transform_to(point); -} - -Point2 offset(3.0, 4.0), pt(-5.0, 6.0); - -/* ************************************************************************* */ -TEST( Point2, transforms ) { - 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))))); -} - -/* ************************************************************************* */ -TEST( Point2, transform_to_derivatives ) { - Matrix actH1, actH2; - offset.transform_to(pt, actH1, actH2); - Matrix numericalH1 = numericalDerivative21(transform_to_proxy, offset, pt, 1e-5); - Matrix numericalH2 = numericalDerivative22(transform_to_proxy, offset, pt, 1e-5); - EXPECT(assert_equal(numericalH1, actH1, tol)); - EXPECT(assert_equal(numericalH2, actH2, tol)); -} - -/* ************************************************************************* */ -TEST( Point2, transform_from_derivatives ) { - Matrix actH1, actH2; - offset.transform_from(pt, actH1, actH2); - Matrix numericalH1 = numericalDerivative21(transform_from_proxy, offset, pt, 1e-5); - Matrix numericalH2 = numericalDerivative22(transform_from_proxy, offset, pt, 1e-5); - EXPECT(assert_equal(numericalH1, actH1, tol)); - EXPECT(assert_equal(numericalH2, actH2, tol)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */