diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 752caec73..5e2e0ff42 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -148,6 +148,11 @@ namespace gtsam { return eye(3); } + /// Left-trivialized derivative inverse of the exponential map + static Matrix dexpInvL(const Vector& v) { + return eye(3); + } + /// @} /// @name Vector Space /// @{ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fc632f8d6..9c99cd7aa 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -271,6 +271,9 @@ namespace gtsam { /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector3& v); + /// Left-trivialized derivative inverse of the exponential map + static Matrix3 dexpInvL(const Vector3& v); + /// @} /// @name Group Action on Point3 /// @{ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 9f8ef1960..2663f1bf8 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -316,6 +316,19 @@ Matrix3 Rot3::dexpL(const Vector3& v) { return res; } +/* ************************************************************************* */ +/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpInvL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + Matrix res = eye(3) + 0.5*x - s2*x2; + return res; +} + + /* ************************************************************************* */ Matrix3 Rot3::matrix() const { return rot_; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 56e1a7cce..5874af9a0 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -419,6 +419,9 @@ TEST( Rot3, dexpL) { boost::function( boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); + + Matrix actualDexpInvL = Rot3::dexpInvL(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); } /* ************************************************************************* */