Cleaned up and tested exmpap

gtsam_unstable/geometry/Similarity3.cpp

@@ -38,6 +38,10 @@ Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
     R_(R), t_(t), s_(s) {
 }
 
+Similarity3::Similarity3(const Matrix4& T) :
+    R_(T.topLeftCorner<3, 3>()), t_(T.topRightCorner<3, 1>()), s_(1.0 / T(3, 3)) {
+}
+
 bool Similarity3::equals(const Similarity3& other, double tol) const {
   return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
       && s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
@@ -100,7 +104,7 @@ Matrix7 Similarity3::AdjointMap() const {
   // http://www.ethaneade.org/latex2html/lie/node30.html
   const Matrix3 R = R_.matrix();
   const Vector3 t = t_.vector();
-  Matrix3 A = s_ * skewSymmetric(t) * R;
+  const Matrix3 A = s_ * skewSymmetric(t) * R;
   Matrix7 adj;
   adj <<
       R, Z_3x3, Matrix31::Zero(), // 3*7
@@ -110,91 +114,86 @@ Matrix7 Similarity3::AdjointMap() const {
 }
 
 Matrix3 Similarity3::GetV(Vector3 w, double lambda) {
-  Matrix3 wx = skewSymmetric(w[0], w[1], w[2]);
+  // http://www.ethaneade.org/latex2html/lie/node29.html
-  double lambdasquared = lambda * lambda;
+  double lambda2 = lambda * lambda;
-  double thetasquared = w.transpose() * w;
+  double theta2 = w.transpose() * w;
-  double theta = sqrt(thetasquared);
+  double theta = sqrt(theta2);
-  double X, Y, Z, W, alpha, beta, gama, mu, upsilon, A, B, C;
+  double A, B, C;
-  if (thetasquared > 1e-9 && lambdasquared > 1e-9) {
+  // TODO(frank): eliminate copy/paste
-    X = sin(theta) / theta;
+  if (theta2 > 1e-9 && lambda2 > 1e-9) {
-    Y = (1 - cos(theta)) / thetasquared;
+    const double X = sin(theta) / theta;
-    Z = (1 - X) / thetasquared;
+    const double Y = (1 - cos(theta)) / theta2;
-    W = (0.5 - Y) / thetasquared;
+    const double Z = (1 - X) / theta2;
-    alpha = lambdasquared / (lambdasquared + thetasquared);
+    const double W = (0.5 - Y) / theta2;
-    beta = (exp(-lambda) - 1 + lambda) / lambdasquared;
+    const double alpha = lambda2 / (lambda2 + theta2);
-    gama = Y - (lambda * Z);
+    const double beta = (exp(-lambda) - 1 + lambda) / lambda2;
-    mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda))
+    const double gamma = Y - (lambda * Z);
-        / (lambdasquared * lambda);
+    const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda))
-    upsilon = Z - (lambda * W);
+        / (lambda2 * lambda);
+    const double upsilon = Z - (lambda * W);
     A = (1 - exp(-lambda)) / lambda;
-    B = alpha * (beta - gama) + gama;
+    B = alpha * (beta - gamma) + gamma;
     C = alpha * (mu - upsilon) + upsilon;
-  } else if (thetasquared <= 1e-9 && lambdasquared > 1e-9) {
+  } else if (theta2 <= 1e-9 && lambda2 > 1e-9) {
     //Taylor series expansions
-    X = 1;
+    const double Y = 0.5 - theta2 / 24.0;
-    Y = 0.5 - thetasquared / 24.0;
+    const double Z = 1.0 / 6.0 - theta2 / 120.0;
-    Z = 1.0 / 6.0 - thetasquared / 120.0;
+    const double W = 1.0 / 24.0 - theta2 / 720.0;
-    W = 1.0 / 24.0 - thetasquared / 720.0;
+    const double alpha = lambda2 / (lambda2 + theta2);
-    alpha = lambdasquared / (lambdasquared + thetasquared);
+    const double beta = (exp(-lambda) - 1 + lambda) / lambda2;
-    beta = (exp(-lambda) - 1 + lambda) / lambdasquared;
+    const double gamma = Y - (lambda * Z);
-    gama = Y - (lambda * Z);
+    const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda))
-    mu = (1 - lambda + (0.5 * lambdasquared) - exp(-lambda))
+        / (lambda2 * lambda);
-        / (lambdasquared * lambda);
+    const double upsilon = Z - (lambda * W);
-    upsilon = Z - (lambda * W);
     A = (1 - exp(-lambda)) / lambda;
-    B = alpha * (beta - gama) + gama;
+    B = alpha * (beta - gamma) + gamma;
     C = alpha * (mu - upsilon) + upsilon;
-  } else if (thetasquared > 1e-9 && lambdasquared <= 1e-9) {
+  } else if (theta2 > 1e-9 && lambda2 <= 1e-9) {
-    X = sin(theta) / theta;
+    const double X = sin(theta) / theta;
-    Y = (1 - cos(theta)) / thetasquared;
+    const double Y = (1 - cos(theta)) / theta2;
-    Z = (1 - X) / thetasquared;
+    const double Z = (1 - X) / theta2;
-    W = (0.5 - Y) / thetasquared;
+    const double W = (0.5 - Y) / theta2;
-    alpha = lambdasquared / (lambdasquared + thetasquared);
+    const double alpha = lambda2 / (lambda2 + theta2);
-    beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0
+    const double beta = 0.5 - lambda / 6.0 + lambda2 / 24.0
-        - (lambda * lambdasquared) / 120;
+        - (lambda * lambda2) / 120;
-    gama = Y - (lambda * Z);
+    const double gamma = Y - (lambda * Z);
-    mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120
+    const double mu = 1.0 / 6.0 - lambda / 24 + lambda2 / 120
-        - (lambda * lambdasquared) / 720;
+        - (lambda * lambda2) / 720;
-    upsilon = Z - (lambda * W);
+    const double upsilon = Z - (lambda * W);
     if (lambda < 1e-9) {
-      A = 1 - lambda / 2.0 + lambdasquared / 6.0;
+      A = 1 - lambda / 2.0 + lambda2 / 6.0;
     } else {
       A = (1 - exp(-lambda)) / lambda;
     }
-    B = alpha * (beta - gama) + gama;
+    B = alpha * (beta - gamma) + gamma;
     C = alpha * (mu - upsilon) + upsilon;
   } else {
-    X = 1;
+    const double Y = 0.5 - theta2 / 24.0;
-    Y = 0.5 - thetasquared / 24.0;
+    const double Z = 1.0 / 6.0 - theta2 / 120.0;
-    Z = 1.0 / 6.0 - thetasquared / 120.0;
+    const double W = 1.0 / 24.0 - theta2 / 720.0;
-    W = 1.0 / 24.0 - thetasquared / 720.0;
+    const double gamma = Y - (lambda * Z);
-    alpha = lambdasquared / (lambdasquared + thetasquared);
+    const double upsilon = Z - (lambda * W);
-    beta = 0.5 - lambda / 6.0 + lambdasquared / 24.0
-        - (lambda * lambdasquared) / 120;
-    gama = Y - (lambda * Z);
-    mu = 1.0 / 6.0 - lambda / 24 + lambdasquared / 120
-        - (lambda * lambdasquared) / 720;
-    upsilon = Z - (lambda * W);
     if (lambda < 1e-9) {
-      A = 1 - lambda / 2.0 + lambdasquared / 6.0;
+      A = 1 - lambda / 2.0 + lambda2 / 6.0;
     } else {
       A = (1 - exp(-lambda)) / lambda;
     }
-    B = gama;
+    B = gamma;
     C = upsilon;
   }
-  return A * I_3x3 + B * wx + C * wx * wx;
+  const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]);
+  return A * I_3x3 + B * Wx + C * Wx * Wx;
 }
 
-Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
+Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) {
-  // To get the logmap, calculate w and lambda, then solve for u as show at ethaneade.org
+  // To get the logmap, calculate w and lambda, then solve for u as shown by Ethan at
   // www.ethaneade.org/latex2html/lie/node29.html
-  Vector3 w = Rot3::Logmap(s.R_);
+  const Vector3 w = Rot3::Logmap(T.R_);
-  double lambda = log(s.s_);
+  const double lambda = log(T.s_);
   Vector7 result;
-  result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda;
+  result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda;
   if (Hm) {
-    // incomplete
+    throw std::runtime_error("Similarity3::Logmap: derivative not implemented");
   }
   return result;
 }
@@ -202,10 +201,9 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
 Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
   const auto w = v.head<3>();
   const auto u = v.segment<3>(3);
-  double lambda = v[6];
+  const double lambda = v[6];
   if (Hm) {
-    // Matrix6 J_pose = Pose3::ExpmapDerivative(v.head<6>());
+    throw std::runtime_error("Similarity3::Expmap: derivative not implemented");
-    // incomplete
   }
   const Matrix3 V = GetV(w, lambda);
   return Similarity3(Rot3::Expmap(w), Point3(V * u), exp(lambda));

gtsam_unstable/geometry/Similarity3.h

@@ -60,6 +60,9 @@ public:
   /// Construct from Eigen types
   Similarity3(const Matrix3& R, const Vector3& t, double s);
 
+  /// Construct from matrix [R t; 0 s^-1]
+  Similarity3(const Matrix4& T);
+
   /// @}
   /// @name Testable
   /// @{
@@ -118,10 +121,10 @@ public:
   /// Chart at the origin
   struct ChartAtOrigin {
     static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
-      return Similarity3::Expmap(v);
+      return Similarity3::Expmap(v, H);
     }
     static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
-      return Similarity3::Logmap(other);
+      return Similarity3::Logmap(other, H);
     }
   };
 
@@ -183,15 +186,17 @@ private:
   static Matrix3 GetV(Vector3 w, double lambda);
 
   /// @}
-
 };
 
 template<>
-struct traits<Similarity3> : public internal::LieGroup<Similarity3> {
+inline Matrix wedge<Similarity3>(const Vector& xi) {
-};
+  return Similarity3::wedge(xi);
-
-template<>
-struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {
-};
-
 }
+
+template<>
+struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
+
+template<>
+struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
+
+} // namespace gtsam

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -26,6 +26,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/testLie.h>
 #include <gtsam/base/Testable.h>
 
 #include <CppUnitLite/TestHarness.h>
@@ -39,15 +40,16 @@ using symbol_shorthand::X;
 
 GTSAM_CONCEPT_TESTABLE_INST(Similarity3)
 
-static Point3 P(0.2, 0.7, -2);
+static const Point3 P(0.2, 0.7, -2);
-static Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
+static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
-static double s = 4;
+static const double s = 4;
-static Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1);
+static const Similarity3 id;
-static Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1);
+static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1);
-static Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
+static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1);
-static Similarity3 T4(R, P, s);
+static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
-static Similarity3 T5(R, P, 10);
+static const Similarity3 T4(R, P, s);
-static Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
+static const Similarity3 T5(R, P, 10);
+static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
 
 //******************************************************************************
 TEST(Similarity3, Concepts) {
@@ -197,10 +199,10 @@ TEST(Similarity3, Matrix) {
 //*****************************************************************************
 // Exponential and log maps
 TEST(Similarity3, ExpLogMap) {
-  Vector7 expected;
+  Vector7 delta;
-  expected << 0.1,0.2,0.3,0.4,0.5,0.6,0.7;
+  delta << 0.1,0.2,0.3,0.4,0.5,0.6,0.7;
-  Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(expected));
+  Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta));
-  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal(delta, actual));
 
   Vector7 zeros;
   zeros << 0,0,0,0,0,0,0;
@@ -211,14 +213,8 @@ TEST(Similarity3, ExpLogMap) {
   Similarity3 ident = Similarity3::identity();
   EXPECT(assert_equal(expZero, ident));
 
-  // Compare to matrix exponential calculated using matlab expm
+  // Compare to matrix exponential, using expm in Lie.h
-  Rot3 Rexpm(0.9358,   -0.2832,    0.2102,
+  EXPECT(assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3));
-             0.3029,    0.9506,   -0.0680,
-            -0.1805,    0.1273,    0.9753);
-  Point3 texpm(0.2724, 0.3825, 0.4213);
-  Similarity3 Sexpm(Rexpm, texpm, 2.0138);
-  Similarity3 Scalc = Similarity3::Expmap(expected);
-  EXPECT(assert_equal(Sexpm, Scalc, 1e-3));
 }
 
 //******************************************************************************
@@ -387,6 +383,31 @@ TEST(Similarity3, AlignScaledPointClouds) {
 //  graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3|
 }
 
+//******************************************************************************
+TEST(Similarity3 , Invariants) {
+  Similarity3 id;
+
+  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(id,T3));
+  EXPECT(check_group_invariants(T2,id));
+  EXPECT(check_group_invariants(T2,T3));
+
+  EXPECT(check_manifold_invariants(id,id));
+  EXPECT(check_manifold_invariants(id,T3));
+  EXPECT(check_manifold_invariants(T2,id));
+  EXPECT(check_manifold_invariants(T2,T3));
+}
+
+//******************************************************************************
+TEST(Similarity3 , LieGroupDerivatives) {
+  Similarity3 id;
+
+  CHECK_LIE_GROUP_DERIVATIVES(id,id);
+  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;