diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index dc9a1dac8..1e342d715 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -174,6 +174,16 @@ public: /// Log map around identity - just return the Point2 as a vector static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); } + /// Left-trivialized derivative of the exponential map + static Matrix dexpL(const Vector2& v) { + return eye(2); + } + + /// Left-trivialized derivative inverse of the exponential map + static Matrix dexpInvL(const Vector2& v) { + return eye(2); + } + /// @} /// @name Vector Space /// @{ diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index d121beb12..38c5704b7 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -173,6 +173,16 @@ namespace gtsam { return (Vector(1) << r.theta()); } + /// Left-trivialized derivative of the exponential map + static Matrix dexpL(const Vector& v) { + return ones(1); + } + + /// Left-trivialized derivative inverse of the exponential map + static Matrix dexpInvL(const Vector& v) { + return ones(1); + } + /// @} /// @name Group Action on Point2 /// @{ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index c67031a13..91b6ec4d7 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -153,6 +153,28 @@ TEST( Rot2, relativeBearing ) CHECK(assert_equal(expectedH,actualH)); } +/* ************************************************************************* */ +Vector w = (Vector(1) << 0.27); + +// 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)) +Vector testDexpL(const LieVector& dw) { + Vector y = Rot2::Logmap(Rot2::Expmap(-w) * Rot2::Expmap(w + dw)); + return y; +} + +TEST( Rot2, dexpL) { + Matrix actualDexpL = Rot2::dexpL(w); + Matrix expectedDexpL = numericalDerivative11( + boost::function( + boost::bind(testDexpL, _1)), LieVector(zero(1)), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); + + Matrix actualDexpInvL = Rot2::dexpInvL(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr;