From 4635d22d7f8d80425932202b3b63d31191cdb6c0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Dec 2014 16:22:21 +0100 Subject: [PATCH] Now argument of LogmapDerivative is correct --- gtsam/geometry/Pose2.cpp | 5 ++- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/tests/testPose2.cpp | 65 +++++++++++++----------------- 3 files changed, 32 insertions(+), 40 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 7b845c55d..b7d136382 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -76,7 +76,7 @@ Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { /* ************************************************************************* */ Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) { - if (H) CONCEPT_NOT_IMPLEMENTED; + if (H) *H = Pose2::LogmapDerivative(p); const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); @@ -164,7 +164,8 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { } /* ************************************************************************* */ -Matrix3 Pose2::LogmapDerivative(const Vector3& v) { +Matrix3 Pose2::LogmapDerivative(const Pose2& p) { + Vector3 v = Logmap(p); double alpha = v[2]; if (fabs(alpha) > 1e-5) { double alphaInv = 1/alpha; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 815043084..da2bb0dd6 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -157,7 +157,7 @@ public: static Matrix3 ExpmapDerivative(const Vector3& v); /// Left-trivialized derivative inverse of the exponential map - static Matrix3 LogmapDerivative(const Vector3& v); + static Matrix3 LogmapDerivative(const Pose2& v); // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP struct ChartAtOrigin { diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 1ba5d9af8..1a175168b 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -198,53 +198,44 @@ TEST(Pose2, logmap_full) { } /* ************************************************************************* */ -Vector3 w(0.1, 0.27, -0.3); -Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 - -// Left trivialization Derivative of exp(w) over w: -// How exp(w) changes when w changes? -// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector3 testDexpL(const Vector3 w, const Vector3& dw) { - Vector3 y = Pose2::Logmap(Pose2::Expmap(-w) * Pose2::Expmap(w + dw)); - return y; -} - -TEST( Pose2, ExpmapDerivative) { - Matrix actualDexpL = Pose2::ExpmapDerivative(w); - Matrix expectedDexpL = numericalDerivative11( - boost::bind(testDexpL, w, _1), zero(3), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - - Matrix actualDexpInvL = Pose2::LogmapDerivative(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); - - // test case where alpha = 0 - Matrix actualDexpL0 = Pose2::ExpmapDerivative(w0); - Matrix expectedDexpL0 = numericalDerivative11( - boost::bind(testDexpL, w0, _1), zero(3), 1e-2); - EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5)); - - Matrix actualDexpInvL0 = Pose2::LogmapDerivative(w0); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); +TEST( Pose2, ExpmapDerivative1) { + Matrix3 actualH; + Vector3 w(0.1, 0.27, -0.3); + Pose2::Expmap(w,actualH); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } /* ************************************************************************* */ TEST( Pose2, ExpmapDerivative2) { Matrix3 actualH; - Pose2::Expmap(w,actualH); - Matrix3 expectedH = numericalDerivative11( - boost::bind(Pose2::Expmap, _1, boost::none), w, 1e-2); + Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 + Pose2::Expmap(w0,actualH); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w0, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + +/* ************************************************************************* */ +TEST( Pose2, LogmapDerivative1) { + Matrix3 actualH; + Vector3 w(0.1, 0.27, -0.3); + Pose2 p = Pose2::Expmap(w); + EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5)); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } /* ************************************************************************* */ TEST( Pose2, LogmapDerivative2) { - Pose2 p = Pose2::Expmap(w); - EXPECT(assert_equal(w, Pose2::Logmap(p), 1e-5)); - Matrix3 actualH = Pose2::LogmapDerivative(w); - Matrix3 expectedH = numericalDerivative11( - boost::bind(Pose2::Logmap, _1, boost::none), p, 1e-2); + Matrix3 actualH; + Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 + Pose2 p = Pose2::Expmap(w0); + EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5)); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); }