Merge remote-tracking branch 'origin/develop' into feature/tighteningTraits

Conflicts:
	gtsam/geometry/Pose2.h
	gtsam/geometry/Pose3.cpp
	gtsam/geometry/tests/testPose3.cpp

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) <<
+    J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha,
-        sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha,
+         c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha,
-        c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha,
+         0, 0, 1;
-        0, 0, 1).finished();
   }
   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) <<
+    J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2,
-        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.5*alpha, alpha*halfCotHalfAlpha,  v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha,
+         0, 0, 1;
-        0, 0, 1).finished();
   }
   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;
 }
 
 /* ************************************************************************* */

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

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 {

gtsam/geometry/Pose3.h

@@ -218,18 +218,13 @@ public:
      */
     static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none);
 
-    /**
+public:
-     * Compute the inverse right-trivialized tangent (derivative) map of the exponential map,
+
-     * as detailed in [Kobilarov09siggraph] eq. (15)
+    /// Derivative of Expmap
-     * The full formula is documented in [Celledoni99cmame]
+    static Matrix6 ExpmapDerivative(const Vector6& xi);
-     *    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.
+    /// Derivative of Logmap
-     * and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2
+    static Matrix6 LogmapDerivative(const Vector6& xi);
-     *    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);
 
     /**
      * wedge for Pose3:

gtsam/geometry/tests/testPose3.cpp

@@ -672,21 +672,22 @@ TEST(Pose3, align_2) {
 }
 
 /* ************************************************************************* */
-/// exp(xi) exp(y) = exp(xi + x)
+/// exp(xi) exp(y) = exp(xi + dxi)
-/// Hence, y = log (exp(-xi)*exp(xi+x))
+/// Hence, y = log (exp(-xi)*exp(xi+dxi))
-Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0).finished();
+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) {
+TEST( Pose3, ExpmapDerivative) {
-  Matrix res = Pose3::dExpInv_exp(xi);
+  Matrix actualDexpL = Pose3::ExpmapDerivative(xi);
+  Matrix expectedDexpL = numericalDerivative11<Vector6, Vector6>(
+      boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2);
+  EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
 
-  Matrix numericalDerivExpmapInv = numericalDerivative11<Vector6, Vector6>(
+  Matrix actualDexpInvL = Pose3::LogmapDerivative(xi);
-      testDerivExpmapInv, Vector6::Zero(), 1e-5);
+  EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
-
-  EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1));
 }
 
 /* ************************************************************************* */