Merge pull request #2051 from borglab/fix/sim2

Fix Sim(2) Expmap and Logmap

gtsam/geometry/Similarity2.cpp

@@ -194,13 +194,40 @@ Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
   return internal::AlignGivenR(abPointPairs, aRb_estimate);
 }
 
+Matrix2 Similarity2::GetV(double theta, double lambda) {
+  // Derivation from https://ethaneade.com/lie_groups.pdf page 6
+  const double lambda2 = lambda * lambda, theta2 = theta * theta;
+  double A, B, C;
+  if (theta2 > 1e-9) {
+    A = sin(theta) / theta;
+    B = (1 - cos(theta)) / theta2;
+    C = (1 - A) / theta2;
+  } else {
+    // Taylor series expansion for theta=0
+    A = 1.0;
+    B = 0.5 - theta2 / 24.0;
+    C = 1.0 / 6.0 - theta2 / 120.0;
+  }
+  double alpha = 1.0 / (1.0 + theta2 / lambda2);
+  const double s = exp(lambda);
+
+  double s_inv = 1.0 / s;
+  double X = alpha * (1 - s_inv) / lambda + (1 - alpha) * (A - lambda * B);
+  double Y =
+      alpha * (s_inv - 1 + lambda) / lambda2 + (1 - alpha) * (B - lambda * C);
+
+  Matrix2 V;
+  V << X, -theta * Y, theta * Y, X;
+  return V;
+}
+
 Vector4 Similarity2::Logmap(const Similarity2& S,  //
                             OptionalJacobian<4, 4> Hm) {
-  const Vector2 u = S.t_;
   const Vector1 w = Rot2::Logmap(S.R_);
-  const double s = log(S.s_);
+  const double lambda = log(S.s_);
+  // In Expmap, t = V * u -> in Logmap, u = V^{-1} * t
   Vector4 result;
-  result << u, w, s;
+  result << GetV(w[0], lambda).inverse() * S.t_, w, lambda;
   if (Hm) {
     throw std::runtime_error("Similarity2::Logmap: derivative not implemented");
   }
@@ -209,13 +236,14 @@ Vector4 Similarity2::Logmap(const Similarity2& S,  //
 
 Similarity2 Similarity2::Expmap(const Vector4& v,  //
                                 OptionalJacobian<4, 4> Hm) {
-  const Vector2 t = v.head<2>();
+  const Vector2 u = v.head<2>();
-  const Rot2 R = Rot2::Expmap(v.segment<1>(2));
+  const double theta = v[2];
-  const double s = v[3];
+  const double lambda = v[3];
   if (Hm) {
     throw std::runtime_error("Similarity2::Expmap: derivative not implemented");
   }
-  return Similarity2(R, t, s);
+  const Matrix2 V = GetV(theta, lambda);
+  return Similarity2(Rot2::Expmap(v.segment<1>(2)), V * u, exp(lambda));
 }
 
 Matrix4 Similarity2::AdjointMap() const {

gtsam/geometry/Similarity2.h

@@ -141,6 +141,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
 
   using LieAlgebra = Matrix3;
 
+  /// Calculate expmap and logmap coefficients.
+  static Matrix2 GetV(double theta, double lambda);
+
   /**
    * Log map at the identity
    * \f$ [t_x, t_y, \delta, \lambda] \f$

gtsam/geometry/tests/testSimilarity2.cpp

@@ -79,7 +79,7 @@ TEST(Similarity2, HatAndVee) {
 
 /* ************************************************************************* */
 // Checks correct exponential map (Expmap) with brute force matrix exponential
-TEST_DISABLED(Similarity2, BruteForceExpmap) {
+TEST(Similarity2, BruteForceExpmap) {
   const Vector4 xi(0.1, 0.2, 0.3, 0.4);
   EXPECT(assert_equal(Similarity2::Expmap(xi), expm<Similarity2>(xi), 1e-4));
 }