From 9c97b1d8a0720a9a45e85ee532cdb3fedd28be32 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 18 Oct 2014 16:45:04 +0200
Subject: [PATCH] Some more refactoring

---
 .../nonlinear/tests/testExpression.cpp        | 81 +++++++++++++------
 1 file changed, 58 insertions(+), 23 deletions(-)

diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp
index 28d160799..b0abf6b6f 100644
--- a/gtsam_unstable/nonlinear/tests/testExpression.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp
@@ -323,23 +323,39 @@ struct SnavelyReprojectionError {
 };
 
 /* ************************************************************************* */
-// manifold_traits prototype
-// Same style as Boost.TypeTraits
+
+/**
+ * A manifold defines a space in which there is a notion of a linear tangent space
+ * that can be centered around a given point on the manifold.  These nonlinear
+ * spaces may have such properties as wrapping around (as is the case with rotations),
+ * which might make linear operations on parameters not return a viable element of
+ * the manifold.
+ *
+ * We perform optimization by computing a linear delta in the tangent space of the
+ * current estimate, and then apply this change using a retraction operation, which
+ * maps the change in tangent space back to the manifold itself.
+ *
+ * There may be multiple possible retractions for a given manifold, which can be chosen
+ * between depending on the computational complexity.  The important criteria for
+ * the creation for the retract and localCoordinates functions is that they be
+ * inverse operations.
+ *
+ */
+
+// Traits, same style as Boost.TypeTraits
 // All meta-functions below ever only declare a single type
 // or a type/value/value_type
-#include <boost/static_assert.hpp>
-#include <type_traits>
-
-// is manifold
+// is manifold, by default this is false
 template<typename T>
-struct is_manifold: public false_type {
+struct is_manifold: public std::false_type {
 };
 
-// dimension
+// dimension, can return Eigen::Dynamic (-1) if not known at compile time
 template<typename T>
-struct dimension;
+struct dimension: public std::integral_constant<int, T::dimension> {
+};
 
-// TangentVector is eigen::Matrix type in tangent space
+// TangentVector is Eigen::Matrix type in tangent space, can be Dynamic...
 template<typename T>
 struct TangentVector {
   BOOST_STATIC_ASSERT(is_manifold<T>::value);
@@ -348,7 +364,7 @@ struct TangentVector {
 
 // default localCoordinates
 template<typename T>
-struct localCoordinates {
+struct LocalCoordinates {
   typename TangentVector<T>::type operator()(const T& t1, const T& t2) {
     return t1.localCoordinates(t2);
   }
@@ -356,7 +372,7 @@ struct localCoordinates {
 
 // default retract
 template<typename T>
-struct retract {
+struct Retract {
   T operator()(const T& t, const typename TangentVector<T>::type& d) {
     return t.retract(d);
   }
@@ -375,7 +391,7 @@ struct dimension<Eigen::Matrix<double, M, N, Options> > : public integral_consta
 };
 
 template<int M, int N, int Options>
-struct localCoordinates<Eigen::Matrix<double, M, N, Options> > {
+struct LocalCoordinates<Eigen::Matrix<double, M, N, Options> > {
   typedef Eigen::Matrix<double, M, N, Options> T;
   typedef typename TangentVector<T>::type result_type;
   result_type operator()(const T& t1, const T& t2) {
@@ -385,7 +401,7 @@ struct localCoordinates<Eigen::Matrix<double, M, N, Options> > {
 };
 
 template<int M, int N, int Options>
-struct retract<Eigen::Matrix<double, M, N, Options> > {
+struct Retract<Eigen::Matrix<double, M, N, Options> > {
   typedef Eigen::Matrix<double, M, N, Options> T;
   T operator()(const T& t, const typename TangentVector<T>::type& d) {
     return t + Eigen::Map<const T>(d.data());
@@ -417,8 +433,14 @@ TEST(Expression, dimension) {
 
 // localCoordinates
 TEST(Expression, localCoordinates) {
-  EXPECT(localCoordinates<Point2>()(Point2(0,0),Point2(1,0))==Vector2(1,0));
-  EXPECT(localCoordinates<Vector2>()(Vector2(0,0),Vector2(1,0))==Vector2(1,0));
+  EXPECT(LocalCoordinates<Point2>()(Point2(0,0),Point2(1,0))==Vector2(1,0));
+  EXPECT(LocalCoordinates<Vector2>()(Vector2(0,0),Vector2(1,0))==Vector2(1,0));
+}
+
+// retract
+TEST(Expression, retract) {
+  EXPECT(Retract<Point2>()(Point2(0,0),Vector2(1,0))==Point2(1,0));
+  EXPECT(Retract<Vector2>()(Vector2(0,0),Vector2(1,0))==Vector2(1,0));
 }
 
 /* ************************************************************************* */
@@ -426,21 +448,34 @@ TEST(Expression, localCoordinates) {
 template<typename Y, typename X>
 Matrix numericalDerivative(boost::function<Y(const X&)> h, const X& x,
     double delta = 1e-5) {
+
   BOOST_STATIC_ASSERT(is_manifold<Y>::value);
-  BOOST_STATIC_ASSERT(is_manifold<X>::value);
-  Y hx = h(x);
-  double factor = 1.0 / (2.0 * delta);
   static const size_t M = dimension<Y>::value;
+  typedef typename TangentVector<Y>::type TangentY;
+  LocalCoordinates<Y> localCoordinates;
+
+  BOOST_STATIC_ASSERT(is_manifold<X>::value);
   static const size_t N = dimension<X>::value;
-  Eigen::Matrix<double, N, 1> d;
+  typedef typename TangentVector<X>::type TangentX;
+  Retract<X> retract;
+
+  // get value at x
+  Y hx = h(x);
+
+  // Prepare a tangent vector to perturb x with
+  TangentX d;
+  d.setZero();
+
+  // Fill in Jacobian H
   Matrix H = zeros(M, N);
+  double factor = 1.0 / (2.0 * delta);
   for (size_t j = 0; j < N; j++) {
-    d.setZero();
     d(j) = delta;
-    Vector hxplus = localCoordinates<Y>()(hx, h(retract<X>()(x, d)));
+    TangentY hxplus = localCoordinates(hx, h(retract(x, d)));
     d(j) = -delta;
-    Vector hxmin = localCoordinates<Y>()(hx, h(retract<X>()(x, d)));
+    TangentY hxmin = localCoordinates(hx, h(retract(x, d)));
     H.block<M, 1>(0, j) << (hxplus - hxmin) * factor;
+    d(j) = 0;
   }
   return H;
 }