diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index b7d136382..9416ada8b 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -137,6 +137,7 @@ Matrix3 Pose2::adjointMap(const Vector& v) { /* ************************************************************************* */ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { double alpha = v[2]; + Matrix3 J; if (fabs(alpha) > 1e-5) { // Chirikjian11book2, pg. 36 /* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26 @@ -149,39 +150,41 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { */ double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; - return (Matrix(3,3) << - sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, - c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, - 0, 0, 1).finished(); + J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, + c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, + 0, 0, 1; } else { // Thanks to Krunal: Apply L'Hospital rule to several times to // compute the limits when alpha -> 0 - return (Matrix(3,3) << 1,0,-0.5*v[1], + J << 1,0,-0.5*v[1], 0,1, 0.5*v[0], - 0,0, 1).finished(); + 0,0, 1; } + + return J; } /* ************************************************************************* */ Matrix3 Pose2::LogmapDerivative(const Pose2& p) { Vector3 v = Logmap(p); double alpha = v[2]; + Matrix3 J; if (fabs(alpha) > 1e-5) { double alphaInv = 1/alpha; double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); double v1 = v[0], v2 = v[1]; - return (Matrix(3,3) << - alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, - 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, - 0, 0, 1).finished(); + J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, + 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, + 0, 0, 1; } else { - return (Matrix(3,3) << 1,0, 0.5*v[1], + J << 1,0, 0.5*v[1], 0,1, -0.5*v[0], - 0,0, 1).finished(); + 0,0, 1; } + return J; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index b556ef5e3..147986a06 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -155,10 +155,10 @@ public: return m; } - /// Left-trivialized derivative of the exponential map + /// Derivative of Expmap static Matrix3 ExpmapDerivative(const Vector3& v); - /// Left-trivialized derivative inverse of the exponential map + /// Derivative of Logmap static Matrix3 LogmapDerivative(const Pose2& v); // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 3892ac5ce..1587a5614 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -91,26 +91,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; @@ -126,7 +106,7 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { - if (H) CONCEPT_NOT_IMPLEMENTED; + if (H) *H = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -222,6 +202,66 @@ Vector6 Pose3::localCoordinates(const Pose3& T, } } +/* ************************************************************************* */ +/** + * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix + * J(xi) = [J_(w) Z_3x3; + * Q J_(w)] + * where J_(w) is the SO3 Expmap derivative. + * (see Chirikjian11book2, pg 44, eq 10.95. + * The closed-form formula is similar to formula 102 in Barfoot14tro) + */ +static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Vector3 v(sub(xi, 3, 6)); + Matrix3 V = skewSymmetric(v); + Matrix3 W = skewSymmetric(w); + + Matrix3 Q; + +#ifdef NUMERICAL_EXPMAP_DERIV + Matrix3 Qj = Z_3x3; + double invFac = 1.0; + 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 + 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 + + return Q; +} + +/* ************************************************************************* */ +Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::ExpmapDerivative(w); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix6 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); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix3 Q2 = -Jw*Q*Jw; + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished(); + return J; +} + /* ************************************************************************* */ Pose3 Pose3::retract(const Vector& d, OptionalJacobian<6, 6> Hthis, OptionalJacobian<6, 6> Hd, Pose3::CoordinatesMode mode) const { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index b92d743b8..b707b1cab 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -218,18 +218,13 @@ 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); +public: + + /// Derivative of Expmap + static Matrix6 ExpmapDerivative(const Vector6& xi); + + /// Derivative of Logmap + static Matrix6 LogmapDerivative(const Vector6& xi); /** * wedge for Pose3: diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 561e912db..ff4b830ad 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)); } /* ************************************************************************* */