diff --git a/geometry/Point2.cpp b/geometry/Point2.cpp index c9b225f16..288b019c5 100644 --- a/geometry/Point2.cpp +++ b/geometry/Point2.cpp @@ -40,7 +40,7 @@ namespace gtsam { /* ************************************************************************* */ Point2 Point2::transform_from(const Point2& point, boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(2); + if (H1) *H1 = -eye(2); if (H2) *H2 = eye(2); return point + *this; } diff --git a/geometry/tests/testPoint2.cpp b/geometry/tests/testPoint2.cpp index 13775e859..c169997bc 100644 --- a/geometry/tests/testPoint2.cpp +++ b/geometry/tests/testPoint2.cpp @@ -5,11 +5,14 @@ **/ #include +#include #include using namespace std; using namespace gtsam; +const double tol = 1e-5; + /* ************************************************************************* */ TEST( Point2, expmap) { @@ -41,15 +44,47 @@ TEST( Point2, norm) DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6); } +/* ************************************************************************* */ +Point2 transform_from_proxy(const Point2& pose, const Point2& point) { + return pose.transform_to(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 ) { - 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))))); } +/* ************************************************************************* */ +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); } /* ************************************************************************* */ +