Fixed AdaptAutoDiff

gtsam/nonlinear/AdaptAutoDiff.h

@@ -18,8 +18,7 @@
 
 #pragma once
 
-#include <gtsam/base/Group.h>
-#include <gtsam/base/concepts.h>
+#include <gtsam/base/VectorSpace.h>
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/3rdparty/ceres/autodiff.h>
 
@@ -28,33 +27,41 @@
 
 namespace gtsam {
 
-template <typename T> struct traits_x;
+namespace detail {
 
+// By default, we assume an Identity element
+template<typename T, typename structure_category>
+struct Origin { T operator()() { return traits_x<T>::Identity();} };
+
+// but dimple manifolds don't have one, so we just use the default constructor
+template<typename T>
+struct Origin<T, manifold_tag> { T operator()() { return T();} };
+
+} // \ detail
+
+/**
+ * Canonical is a template that creates canonical coordinates for a given type.
+ * A simple manifold type (i.e., not a Lie Group) has no concept of identity,
+ * hence in that case we use the value given by the default constructor T() as
+ * the origin of a "canonical coordinate" parameterization.
+ */
 template<typename T>
 struct Canonical {
+
+  GTSAM_CONCEPT_MANIFOLD_TYPE(T)
+
   typedef traits_x<T> Traits;
-  BOOST_STATIC_ASSERT_MSG(
-      (boost::is_base_of<group_tag, typename Traits::structure_category>::value),
-      "This type's trait does not assert it is a manifold (or derived)");
-  typedef typename Traits::TangentVector TangentVector;
   enum { dimension = Traits::dimension };
-  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+  typedef typename Traits::TangentVector TangentVector;
+  typedef typename Traits::structure_category Category;
+  typedef detail::Origin<T, Category> Origin;
 
   static TangentVector Local(const T& other) {
-    return Traits::Local(Traits::Identity(), other);
+    return Traits::Local(Origin()(), other);
   }
 
   static T Retract(const TangentVector& v) {
-    return Traits::Retract(Traits::Identity(), v);
-  }
-
-  static TangentVector Local(const T& other, ChartJacobian Hother) {
-    return Traits::Local(Traits::Identity(), other, boost::none, Hother);
-  }
-
-  static T Retract(const T& origin, const TangentVector& v,
-      ChartJacobian Horigin, ChartJacobian Hv) {
-    return Traits::Retract(Traits::Identity(), v, boost::none, Hv);
+    return Traits::Retract(Origin()(), v);
   }
 };
 

gtsam/nonlinear/tests/testAdaptAutoDiff.cpp

@@ -33,15 +33,35 @@
 using boost::assign::list_of;
 using boost::assign::map_list_of;
 
+namespace gtsam {
+
+// Special version of Cal3Bundler so that default constructor = 0,0,0
+struct Cal: public Cal3Bundler {
+  Cal(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) :
+      Cal3Bundler(f, k1, k2, u0, v0) {
+  }
+  Cal retract(const Vector& d) const {
+    return Cal(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
+  }
+  Vector3 localCoordinates(const Cal& T2) const {
+    return T2.vector() - vector();
+  }
+};
+
+template<>
+struct traits_x<Cal> : public internal::Manifold<Cal> {};
+
+// With that, camera below behaves like Snavely's 9-dim vector
+typedef PinholeCamera<Cal> Camera;
+
+}
+
 using namespace std;
 using namespace gtsam;
 
-// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
-typedef PinholeCamera<Cal3Bundler> Camera;
-
 /* ************************************************************************* */
 // charts
-TEST(Manifold, Canonical) {
+TEST(AdaptAutoDiff, Canonical) {
 
   Canonical<Point2> chart1;
   EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0));
@@ -73,21 +93,17 @@ TEST(Manifold, Canonical) {
   EXPECT(assert_equal(v6, chart5.Local(pose)));
   EXPECT(assert_equal(chart5.Retract(v6), pose));
 
-#if 0 // TODO: Canonical should not require group?
-  Canonical<Camera> chart6;
-  Cal3Bundler cal0(0, 0, 0);
+  Canonical<Cal> chart6;
+  Cal cal0;
+  Vector z3 = Vector3::Zero();
+  EXPECT(assert_equal(z3, chart6.Local(cal0)));
+  EXPECT(assert_equal(chart6.Retract(z3), cal0));
+
+  Canonical<Camera> chart7;
   Camera camera(Pose3(), cal0);
   Vector z9 = Vector9::Zero();
-  EXPECT(assert_equal(z9, chart6.Local(camera)));
-  EXPECT(assert_equal(chart6.Retract(z9), camera));
-
-  Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value()
-  Camera camera2(pose, cal);
-  Vector v9(9);
-  v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0;
-  EXPECT(assert_equal(v9, chart6.Local(camera2)));
-  EXPECT(assert_equal(chart6.Retract(v9), camera2));
-#endif
+  EXPECT(assert_equal(z9, chart7.Local(camera)));
+  EXPECT(assert_equal(chart7.Retract(z9), camera));
 }
 
 /* ************************************************************************* */
@@ -137,7 +153,7 @@ struct Projective {
 
 /* ************************************************************************* */
 // Test Ceres AutoDiff
-TEST(Expression, AutoDiff) {
+TEST(AdaptAutoDiff, AutoDiff) {
   using ceres::internal::AutoDiff;
 
   // Instantiate function
@@ -181,7 +197,7 @@ Vector2 adapted(const Vector9& P, const Vector3& X) {
     throw std::runtime_error("Snavely fail");
 }
 
-TEST(Expression, AutoDiff2) {
+TEST(AdaptAutoDiff, AutoDiff2) {
   using ceres::internal::AutoDiff;
 
   // Instantiate function
@@ -212,12 +228,12 @@ TEST(Expression, AutoDiff2) {
   EXPECT(assert_equal(E2,H2,1e-8));
 }
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 // Test AutoDiff wrapper Snavely
-TEST(Expression, AutoDiff3) {
+TEST(AdaptAutoDiff, AutoDiff3) {
 
   // Make arguments
-  Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0));
+  Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal(1, 0, 0));
   Point3 X(10, 0, -5); // negative Z-axis convention of Snavely!
 
   typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
@@ -233,17 +249,17 @@ TEST(Expression, AutoDiff3) {
   Matrix E2 = numericalDerivative22<Point2, Camera, Point3>(Adaptor(), P, X);
 
   // Get derivatives with AutoDiff, not gives RowMajor results!
-  OptionalJacobian<2,9> H1;
-  OptionalJacobian<2,3> H2;
+  Matrix29 H1;
+  Matrix23 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(E2,*H2,1e-8));
+  EXPECT(assert_equal(E1,H1,1e-8));
+  EXPECT(assert_equal(E2,H2,1e-8));
 }
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 // Test AutoDiff wrapper in an expression
-TEST(Expression, Snavely) {
+TEST(AdaptAutoDiff, Snavely) {
   Expression<Camera> P(1);
   Expression<Point3> X(2);
   typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;