diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0bc07b753..d66ac8148 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -93,26 +93,6 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, return adjointMap(xi).transpose() * y; } -/* ************************************************************************* */ -Matrix6 Pose3::dExpInv_exp(const Vector6& xi) { - // Bernoulli numbers, from Wikipedia - static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, - 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); - static const int N = 5; // order of approximation - Matrix6 res; - res = I_6x6; - Matrix6 ad_i; - ad_i = I_6x6; - Matrix6 ad_xi = adjointMap(xi); - double fac = 1.0; - for (int i = 1; i < N; ++i) { - ad_i = ad_xi * ad_i; - fac = fac * i; - res = res + B(i) / fac * ad_i; - } - return res; -} - /* ************************************************************************* */ void Pose3::print(const string& s) const { cout << s; @@ -221,6 +201,64 @@ Vector6 Pose3::localCoordinates(const Pose3& T, } } +/* ************************************************************************* */ +Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::ExpmapDerivative(w); + + Vector3 v(sub(xi, 3, 6)); + Matrix3 V = skewSymmetric(v); + Matrix3 W = skewSymmetric(w); + +#ifdef NUMERICAL_EXPMAP_DERIV + Matrix3 Qj = Z_3x3; + double invFac = 1.0; + Matrix3 Q = Z_3x3; + Matrix3 Wj = I_3x3; + for (size_t j=1; j<10; ++j) { + Qj = Qj*W + Wj*V; + invFac = -invFac/(j+1); + Q = Q + invFac*Qj; + Wj = Wj*W; + } +#else + // The closed-form formula in Barfoot14tro eq. (102) + double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi, + phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Matrix3 Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W); +#endif + + Matrix6 J; + J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished(); + return J; +} + +/* ************************************************************************* */ +Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::LogmapDerivative(w); + + Vector3 v(sub(xi, 3, 6)); + Matrix3 V = skewSymmetric(v); + Matrix3 W = skewSymmetric(w); + + // The closed-form formula in Barfoot14tro eq. (102) + double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi, + phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Matrix3 Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W); + Matrix3 Q2 = -Jw*Q*Jw; + + Matrix6 J; + J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished(); + return J; +} + /* ************************************************************************* */ Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 805bc6012..15f71344b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -204,18 +204,11 @@ public: */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); - /** - * Compute the inverse right-trivialized tangent (derivative) map of the exponential map, - * as detailed in [Kobilarov09siggraph] eq. (15) - * The full formula is documented in [Celledoni99cmame] - * Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and - * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421-438, 2003. - * and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2 - * Ernst Hairer, et al., Geometric Numerical Integration, - * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. - * See also Iserles05an, pg. 33, formula 2.46 - */ - static Matrix6 dExpInv_exp(const Vector6 &xi); + /// Left-trivialized derivative of the exponential map + static Matrix6 ExpmapDerivative(const Vector6& v); + + /// Left-trivialized inverse derivative of the exponential map + static Matrix6 LogmapDerivative(const Vector6& v); /** * wedge for Pose3: diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 52f721f41..421dea879 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -672,21 +672,22 @@ TEST(Pose3, align_2) { } /* ************************************************************************* */ -/// exp(xi) exp(y) = exp(xi + x) -/// Hence, y = log (exp(-xi)*exp(xi+x)) -Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0).finished(); +/// exp(xi) exp(y) = exp(xi + dxi) +/// Hence, y = log (exp(-xi)*exp(xi+dxi)) +Vector6 xi = (Vector(6) << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0).finished(); -Vector testDerivExpmapInv(const Vector6& dxi) { +Vector6 testExpmapDerivative(const Vector6& xi, const Vector6& dxi) { return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); } -TEST( Pose3, dExpInv_TLN) { - Matrix res = Pose3::dExpInv_exp(xi); +TEST( Pose3, ExpmapDerivative) { + Matrix actualDexpL = Pose3::ExpmapDerivative(xi); + Matrix expectedDexpL = numericalDerivative11( + boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - Matrix numericalDerivExpmapInv = numericalDerivative11( - testDerivExpmapInv, Vector6::Zero(), 1e-5); - - EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); + Matrix actualDexpInvL = Pose3::LogmapDerivative(xi); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); } /* ************************************************************************* */