From 0706caf17315286be2ff80e6a7bca68cceb00962 Mon Sep 17 00:00:00 2001
From: Paul Furgale <paul.furgale@gmail.com>
Date: Sun, 14 Dec 2014 20:28:25 +0100
Subject: [PATCH] Latest edits...started on Eigen types

---
 gtsam/base/LieMatrix.h                      |  12 +--
 gtsam/base/Matrix.h                         |   4 +-
 gtsam/base/concepts.h                       | 104 ++++++++++++++++++++
 gtsam/geometry/Point2.h                     |   2 +-
 gtsam/nonlinear/tests/testAdaptAutoDiff.cpp |   7 +-
 5 files changed, 114 insertions(+), 15 deletions(-)

diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h
index 4c250b3f2..e94d29817 100644
--- a/gtsam/base/LieMatrix.h
+++ b/gtsam/base/LieMatrix.h
@@ -36,6 +36,7 @@ struct LieMatrix : public Matrix {
 
   /// @name Constructors
   /// @{
+  enum { dimension = Eigen::Dynamic };
 
   /** default constructor - should be unnecessary */
   LieMatrix() {}
@@ -176,17 +177,8 @@ private:
 
 };
 
-// Define GTSAM traits
-namespace traits {
 
 template<>
-struct is_manifold<LieMatrix> : public boost::true_type {
-};
-
-template<>
-struct dimension<LieMatrix> : public Dynamic {
-};
-
-}
+struct traits_x<LieMatrix> : public internal::LieGroup<LieMatrix> {};
 
 } // \namespace gtsam
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 3ecfcf8fa..565960660 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <gtsam/base/concepts.h>
 #include <gtsam/base/Vector.h>
 #include <boost/format.hpp>
 #include <boost/tuple/tuple.hpp>
@@ -568,4 +569,5 @@ namespace boost {
   } // namespace serialization
 } // namespace boost
 
-BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix)
+BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix);
+
diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h
index 379cc1304..e07eea0ea 100644
--- a/gtsam/base/concepts.h
+++ b/gtsam/base/concepts.h
@@ -361,6 +361,110 @@ template<>
 struct traits_x<float> : public internal::ScalarTraits<float> {};
 
 
+template<int M, int N, int Options, int MaxRows, int MaxCols>
+struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
+  // Typedefs required by all manifold types.
+  typedef vector_space_tag structure_category;
+  enum { dimension = (M == Eigen::Dynamic ? Eigen::Dynamic : (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)) };
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+  typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> ManifoldType;
+
+  // For Testable
+  static void Print(const ManifoldType& m, const std::string& str = "") {
+    gtsam::print(m, str);
+  }
+  static bool Equals(const ManifoldType& m1,
+              const ManifoldType& m2,
+              double tol = 1e-8) {
+    return equal_with_abs_tol(m1, m2, 1e-9);
+  }
+
+//  static TangentVector Local(const ManifoldType& origin,
+//                             const ManifoldType& other) {
+//    TangentVector result(GetDimension(origin));
+//    Eigen::Map<Eigen::Matrix<double, M, N, Eigen::RowMajor> >(
+//      &result(0), origin.rows(), origin.cols()) = other - origin;
+//    return result;
+//  }
+//
+//  static ManifoldType Retract(const ManifoldType& origin,
+//                              const TangentVector& v) {
+//    return origin.retract(v);
+//  }
+//
+//  static TangentVector Local(const ManifoldType& origin,
+//                             const ManifoldType& other,
+//                             ChartJacobian Horigin,
+//                             ChartJacobian Hother) {
+//    return origin.localCoordinates(other, Horigin, Hother);
+//  }
+//
+//  static ManifoldType Retract(const ManifoldType& origin,
+//                              const TangentVector& v,
+//                              ChartJacobian Horigin,
+//                              ChartJacobian Hv) {
+//    return origin.retract(v, Horigin, Hv);
+//  }
+//
+//  static int GetDimension(const ManifoldType& m){ return m.dim(); }
+//
+//  // For Group. Only implemented for groups
+//  static ManifoldType Compose(const ManifoldType& m1,
+//                              const ManifoldType& m2) {
+//    return m1.compose(m2);
+//  }
+//
+//  static ManifoldType Between(const ManifoldType& m1,
+//                              const ManifoldType& m2) {
+//    return m1.between(m2);
+//  }
+//
+//  static ManifoldType Inverse(const ManifoldType& m) {
+//    return m.inverse();
+//  }
+//
+//  static ManifoldType Compose(const ManifoldType& m1,
+//                              const ManifoldType& m2,
+//                              ChartJacobian H1,
+//                              ChartJacobian H2) {
+//    return m1.compose(m2, H1, H2);
+//  }
+//
+//  static ManifoldType Between(const ManifoldType& m1,
+//                              const ManifoldType& m2,
+//                              ChartJacobian H1,
+//                              ChartJacobian H2) {
+//    return m1.between(m2, H1, H2);
+//  }
+//
+//  static ManifoldType Inverse(const ManifoldType& m,
+//                              ChartJacobian H) {
+//    return m.inverse(H);
+//  }
+//
+//  static ManifoldType Identity() {
+//    return ManifoldType::identity();
+//  }
+//
+//  static TangentVector Logmap(const ManifoldType& m) {
+//    return ManifoldType::Logmap(m);
+//  }
+//
+//  static ManifoldType Expmap(const TangentVector& v) {
+//    return ManifoldType::Expmap(v);
+//  }
+//
+//  static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm) {
+//    return ManifoldType::Logmap(m, Hm);
+//  }
+//
+//  static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) {
+//    return ManifoldType::Expmap(v, Hv);
+//  }
+
+};
+
 template<typename ManifoldType>
 struct Canonical {
   BOOST_STATIC_ASSERT_MSG(
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 618d2ed94..724306159 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -39,7 +39,7 @@ private:
   double x_, y_;
 
 public:
-  enum { dimension = 3 };
+  enum { dimension = 2 };
   /// @name Standard Constructors
   /// @{
 
diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
index 7d31616c5..9c9a15d0f 100644
--- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
+++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
@@ -183,11 +183,12 @@ TEST(Expression, AutoDiff3) {
   Matrix E2 = numericalDerivative22<Point2, Camera, Point3>(Adaptor(), P, X);
 
   // Get derivatives with AutoDiff, not gives RowMajor results!
-  Matrix29 H1;
-  Matrix23 H2;
+  OptionalJacobian<2,9> H1;
+  OptionalJacobian<2,3> H2;
   Point2 actual2 = snavely(P, X, H1, H2);
   EXPECT(assert_equal(expected,actual2,1e-9));
-  EXPECT(assert_equal(E1,H1,1e-8));
+  EXPECT(assert_equal(E1,*H1,1e-8));
+  EXPECT(assert_equal(E2,*H2,1e-8));
 }
 
 /* ************************************************************************* */