From 7213f0b2eb9350a44ca7b6c75b7e625f9a6366c8 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 25 Dec 2014 17:42:44 +0100
Subject: [PATCH] Moved Canonical to AdaptAutoDiff.h for now

---
 gtsam/base/concepts.h                       |  52 +--
 gtsam/nonlinear/AdaptAutoDiff.h             |  36 +-
 gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 388 +++++++++++---------
 tests/testManifold.cpp                      |  51 ---
 4 files changed, 261 insertions(+), 266 deletions(-)

diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h
index da01be89c..210eee3aa 100644
--- a/gtsam/base/concepts.h
+++ b/gtsam/base/concepts.h
@@ -8,63 +8,25 @@
 
 #pragma once
 
-#include <gtsam/base/Group.h>
-#include <gtsam/base/Matrix.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/OptionalJacobian.h>
-
-#include <boost/concept_check.hpp>
-#include <boost/concept/requires.hpp>
-#include <boost/static_assert.hpp>
-#include <boost/type_traits/is_base_of.hpp>
-
 // This is a helper to ease the transition to the new traits defined in this file.
 // Uncomment this if you want all methods that are tagged as not implemented
 // to cause compilation errors.
-// #define COMPILE_ERROR_NOT_IMPLEMENTED
-
 #ifdef COMPILE_ERROR_NOT_IMPLEMENTED
+
+#include <boost/static_assert.hpp>
 #define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \
 "This method is required by the new concepts framework but has not been implemented yet.");
+
 #else
+
+#include <exception>
 #define CONCEPT_NOT_IMPLEMENTED \
   throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet.");
+
 #endif
 
 namespace gtsam {
 
 template <typename T> struct traits_x;
 
-template<typename ManifoldType>
-struct Canonical {
-  BOOST_STATIC_ASSERT_MSG(
-      (boost::is_base_of<group_tag, typename traits_x<ManifoldType>::structure_category>::value),
-      "This type's trait does not assert it is a manifold (or derived)");
-  typedef traits_x<ManifoldType> Traits;
-  typedef typename Traits::TangentVector TangentVector;
-  enum { dimension = Traits::dimension };
-  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
-
-  static TangentVector Local(const ManifoldType& other) {
-    return Traits::Local(Traits::Identity(), other);
-  }
-
-  static ManifoldType Retract(const TangentVector& v) {
-    return Traits::Retract(Traits::Identity(), v);
-  }
-
-  static TangentVector Local(const ManifoldType& other,
-                             ChartJacobian Hother) {
-    return Traits::Local(Traits::Identity(), other, boost::none, Hother);
-  }
-
-  static ManifoldType Retract(const ManifoldType& origin,
-                              const TangentVector& v,
-                              ChartJacobian Horigin,
-                              ChartJacobian Hv) {
-    return Traits::Retract(Traits::Identity(), v, boost::none, Hv);
-  }
-};
-
-} // namespace gtsam
-
+}
diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h
index 78aed81eb..f907dd879 100644
--- a/gtsam/nonlinear/AdaptAutoDiff.h
+++ b/gtsam/nonlinear/AdaptAutoDiff.h
@@ -18,12 +18,46 @@
 
 #pragma once
 
-#include <gtsam/3rdparty/ceres/autodiff.h>
+#include <gtsam/base/Group.h>
 #include <gtsam/base/concepts.h>
 #include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/3rdparty/ceres/autodiff.h>
+
+#include <boost/static_assert.hpp>
+#include <boost/type_traits/is_base_of.hpp>
 
 namespace gtsam {
 
+template <typename T> struct traits_x;
+
+template<typename T>
+struct Canonical {
+  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;
+
+  static TangentVector Local(const T& other) {
+    return Traits::Local(Traits::Identity(), 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);
+  }
+};
+
 /// Adapt ceres-style autodiff
 template<typename F, typename T, typename A1, typename A2>
 class AdaptAutoDiff {
diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
index edf3305a9..93aaa94b0 100644
--- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
+++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
@@ -26,7 +26,6 @@
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/LieScalar.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -37,175 +36,226 @@ using boost::assign::map_list_of;
 using namespace std;
 using namespace gtsam;
 
-//// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
-//typedef PinholeCamera<Cal3Bundler> Camera;
-//
-///* ************************************************************************* */
-//// Some Ceres Snippets copied for testing
-//// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
-//template<typename T> inline T &RowMajorAccess(T *base, int rows, int cols,
-//    int i, int j) {
-//  return base[cols * i + j];
-//}
-//
-//inline double RandDouble() {
-//  double r = static_cast<double>(rand());
-//  return r / RAND_MAX;
-//}
-//
-//// A structure for projecting a 3x4 camera matrix and a
-//// homogeneous 3D point, to a 2D inhomogeneous point.
-//struct Projective {
-//  // Function that takes P and X as separate vectors:
-//  //   P, X -> x
-//  template<typename A>
-//  bool operator()(A const P[12], A const X[4], A x[2]) const {
-//    A PX[3];
-//    for (int i = 0; i < 3; ++i) {
-//      PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0]
-//          + RowMajorAccess(P, 3, 4, i, 1) * X[1]
-//          + RowMajorAccess(P, 3, 4, i, 2) * X[2]
-//          + RowMajorAccess(P, 3, 4, i, 3) * X[3];
-//    }
-//    if (PX[2] != 0.0) {
-//      x[0] = PX[0] / PX[2];
-//      x[1] = PX[1] / PX[2];
-//      return true;
-//    }
-//    return false;
-//  }
-//
-//  // Adapt to eigen types
-//  Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const {
-//    Vector2 x;
-//    if (operator()(P.data(), X.data(), x.data()))
-//      return x;
-//    else
-//      throw std::runtime_error("Projective fail");
-//  }
-//};
-//
-///* ************************************************************************* */
-//// Test Ceres AutoDiff
-//TEST(Expression, AutoDiff) {
-//  using ceres::internal::AutoDiff;
-//
-//  // Instantiate function
-//  Projective projective;
-//
-//  // Make arguments
-//  typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> M;
-//  M P;
-//  P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0;
-//  Vector4 X(10, 0, 5, 1);
-//
-//  // Apply the mapping, to get image point b_x.
-//  Vector expected = Vector2(2, 1);
-//  Vector2 actual = projective(P, X);
-//  EXPECT(assert_equal(expected,actual,1e-9));
-//
+// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
+typedef PinholeCamera<Cal3Bundler> Camera;
+
+/* ************************************************************************* */
+// charts
+TEST(Manifold, Canonical) {
+
+  Canonical<Point2> chart1;
+  EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0));
+  EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0));
+
+  Vector v2(2);
+  v2 << 1, 0;
+  Canonical<Vector2> chart2;
+  EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0))));
+  EXPECT(chart2.Retract(v2)==Vector2(1, 0));
+
+  Canonical<double> chart3;
+  Eigen::Matrix<double, 1, 1> v1;
+  v1 << 1;
+  EXPECT(chart3.Local(1)==v1);
+  EXPECT_DOUBLES_EQUAL(chart3.Retract(v1), 1, 1e-9);
+
+  Canonical<Point3> chart4;
+  Point3 point(1, 2, 3);
+  Vector v3(3);
+  v3 << 1, 2, 3;
+  EXPECT(assert_equal(v3, chart4.Local(point)));
+  EXPECT(assert_equal(chart4.Retract(v3), point));
+
+  Canonical<Pose3> chart5;
+  Pose3 pose(Rot3::identity(), point);
+  Vector v6(6);
+  v6 << 0, 0, 0, 1, 2, 3;
+  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);
+  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
+}
+
+/* ************************************************************************* */
+// Some Ceres Snippets copied for testing
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+template<typename T> inline T &RowMajorAccess(T *base, int rows, int cols,
+    int i, int j) {
+  return base[cols * i + j];
+}
+
+inline double RandDouble() {
+  double r = static_cast<double>(rand());
+  return r / RAND_MAX;
+}
+
+// A structure for projecting a 3x4 camera matrix and a
+// homogeneous 3D point, to a 2D inhomogeneous point.
+struct Projective {
+  // Function that takes P and X as separate vectors:
+  //   P, X -> x
+  template<typename A>
+  bool operator()(A const P[12], A const X[4], A x[2]) const {
+    A PX[3];
+    for (int i = 0; i < 3; ++i) {
+      PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0]
+          + RowMajorAccess(P, 3, 4, i, 1) * X[1]
+          + RowMajorAccess(P, 3, 4, i, 2) * X[2]
+          + RowMajorAccess(P, 3, 4, i, 3) * X[3];
+    }
+    if (PX[2] != 0.0) {
+      x[0] = PX[0] / PX[2];
+      x[1] = PX[1] / PX[2];
+      return true;
+    }
+    return false;
+  }
+
+  // Adapt to eigen types
+  Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const {
+    Vector2 x;
+    if (operator()(P.data(), X.data(), x.data()))
+      return x;
+    else
+      throw std::runtime_error("Projective fail");
+  }
+};
+
+/* ************************************************************************* */
+// Test Ceres AutoDiff
+TEST(Expression, AutoDiff) {
+  using ceres::internal::AutoDiff;
+
+  // Instantiate function
+  Projective projective;
+
+  // Make arguments
+  typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> M;
+  M P;
+  P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0;
+  Vector4 X(10, 0, 5, 1);
+
+  // Apply the mapping, to get image point b_x.
+  Vector expected = Vector2(2, 1);
+  Vector2 actual = projective(P, X);
+  EXPECT(assert_equal(expected,actual,1e-9));
+
+  // Get expected derivatives
+  Matrix E1 = numericalDerivative21<Vector2, M, Vector4>(Projective(), P, X);
+  Matrix E2 = numericalDerivative22<Vector2, M, Vector4>(Projective(), P, X);
+
+  // Get derivatives with AutoDiff
+  Vector2 actual2;
+  MatrixRowMajor H1(2, 12), H2(2, 4);
+  double *parameters[] = { P.data(), X.data() };
+  double *jacobians[] = { H1.data(), H2.data() };
+  CHECK(
+      (AutoDiff<Projective, double, 12, 4>::Differentiate( projective, parameters, 2, actual2.data(), jacobians)));
+  EXPECT(assert_equal(E1,H1,1e-8));
+  EXPECT(assert_equal(E2,H2,1e-8));
+}
+
+/* ************************************************************************* */
+// Test Ceres AutoDiff on Snavely, defined in ceres_example.h
+// Adapt to GTSAM types
+Vector2 adapted(const Vector9& P, const Vector3& X) {
+  SnavelyProjection snavely;
+  Vector2 x;
+  if (snavely(P.data(), X.data(), x.data()))
+    return x;
+  else
+    throw std::runtime_error("Snavely fail");
+}
+
+TEST(Expression, AutoDiff2) {
+  using ceres::internal::AutoDiff;
+
+  // Instantiate function
+  SnavelyProjection snavely;
+
+  // Make arguments
+  Vector9 P; // zero rotation, (0,5,0) translation, focal length 1
+  P << 0, 0, 0, 0, 5, 0, 1, 0, 0;
+  Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely!
+
+  // Apply the mapping, to get image point b_x.
+  Vector expected = Vector2(2, 1);
+  Vector2 actual = adapted(P, X);
+  EXPECT(assert_equal(expected,actual,1e-9));
+
+  // Get expected derivatives
+  Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
+  Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
+
+  // Get derivatives with AutoDiff
+  Vector2 actual2;
+  MatrixRowMajor H1(2, 9), H2(2, 3);
+  double *parameters[] = { P.data(), X.data() };
+  double *jacobians[] = { H1.data(), H2.data() };
+  CHECK(
+      (AutoDiff<SnavelyProjection, double, 9, 3>::Differentiate( snavely, parameters, 2, actual2.data(), jacobians)));
+  EXPECT(assert_equal(E1,H1,1e-8));
+  EXPECT(assert_equal(E2,H2,1e-8));
+}
+
+/* ************************************************************************* *
+// Test AutoDiff wrapper Snavely
+TEST(Expression, AutoDiff3) {
+
+  // Make arguments
+  Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0));
+  Point3 X(10, 0, -5); // negative Z-axis convention of Snavely!
+
+  typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
+  Adaptor snavely;
+
+  // Apply the mapping, to get image point b_x.
+  Point2 expected(2, 1);
+  Point2 actual = snavely(P, X);
+  EXPECT(assert_equal(expected,actual,1e-9));
+
 //  // Get expected derivatives
-//  Matrix E1 = numericalDerivative21<Vector2, M, Vector4>(Projective(), P, X);
-//  Matrix E2 = numericalDerivative22<Vector2, M, Vector4>(Projective(), P, X);
-//
-//  // Get derivatives with AutoDiff
-//  Vector2 actual2;
-//  MatrixRowMajor H1(2, 12), H2(2, 4);
-//  double *parameters[] = { P.data(), X.data() };
-//  double *jacobians[] = { H1.data(), H2.data() };
-//  CHECK(
-//      (AutoDiff<Projective, double, 12, 4>::Differentiate( projective, parameters, 2, actual2.data(), jacobians)));
-//  EXPECT(assert_equal(E1,H1,1e-8));
-//  EXPECT(assert_equal(E2,H2,1e-8));
-//}
-//
-///* ************************************************************************* */
-//// Test Ceres AutoDiff on Snavely, defined in ceres_example.h
-//// Adapt to GTSAM types
-//Vector2 adapted(const Vector9& P, const Vector3& X) {
-//  SnavelyProjection snavely;
-//  Vector2 x;
-//  if (snavely(P.data(), X.data(), x.data()))
-//    return x;
-//  else
-//    throw std::runtime_error("Snavely fail");
-//}
-//
-//TEST(Expression, AutoDiff2) {
-//  using ceres::internal::AutoDiff;
-//
-//  // Instantiate function
-//  SnavelyProjection snavely;
-//
-//  // Make arguments
-//  Vector9 P; // zero rotation, (0,5,0) translation, focal length 1
-//  P << 0, 0, 0, 0, 5, 0, 1, 0, 0;
-//  Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely!
-//
-//  // Apply the mapping, to get image point b_x.
-//  Vector expected = Vector2(2, 1);
-//  Vector2 actual = adapted(P, X);
-//  EXPECT(assert_equal(expected,actual,1e-9));
-//
-//  // Get expected derivatives
-//  Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
-//  Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
-//
-//  // Get derivatives with AutoDiff
-//  Vector2 actual2;
-//  MatrixRowMajor H1(2, 9), H2(2, 3);
-//  double *parameters[] = { P.data(), X.data() };
-//  double *jacobians[] = { H1.data(), H2.data() };
-//  CHECK(
-//      (AutoDiff<SnavelyProjection, double, 9, 3>::Differentiate( snavely, parameters, 2, actual2.data(), jacobians)));
-//  EXPECT(assert_equal(E1,H1,1e-8));
-//  EXPECT(assert_equal(E2,H2,1e-8));
-//}
-//
-///* ************************************************************************* */
-//// Test AutoDiff wrapper Snavely
-//TEST(Expression, AutoDiff3) {
-//
-//  // Make arguments
-//  Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0));
-//  Point3 X(10, 0, -5); // negative Z-axis convention of Snavely!
-//
-//  typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
-//  Adaptor snavely;
-//
-//  // Apply the mapping, to get image point b_x.
-//  Point2 expected(2, 1);
-//  Point2 actual = snavely(P, X);
-//  EXPECT(assert_equal(expected,actual,1e-9));
-//
-////  // Get expected derivatives
-//  Matrix E1 = numericalDerivative21<Point2, Camera, Point3>(Adaptor(), P, X);
-//  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;
-//  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));
-//}
-//
-///* ************************************************************************* */
-//// Test AutoDiff wrapper in an expression
-//TEST(Expression, Snavely) {
-//  Expression<Camera> P(1);
-//  Expression<Point3> X(2);
-//  typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
-//  Expression<Point2> expression(Adaptor(), P, X);
-//#ifdef GTSAM_USE_QUATERNIONS
-//  EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero
-//#else
-//  EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero
-//#endif
-//  set<Key> expected = list_of(1)(2);
-//  EXPECT(expected == expression.keys());
-//}
+  Matrix E1 = numericalDerivative21<Point2, Camera, Point3>(Adaptor(), P, X);
+  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;
+  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));
+}
+
+/* ************************************************************************* *
+// Test AutoDiff wrapper in an expression
+TEST(Expression, Snavely) {
+  Expression<Camera> P(1);
+  Expression<Point3> X(2);
+  typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
+  Expression<Point2> expression(Adaptor(), P, X);
+#ifdef GTSAM_USE_QUATERNIONS
+  EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero
+#else
+  EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero
+#endif
+  set<Key> expected = list_of(1)(2);
+  EXPECT(expected == expression.keys());
+}
 
 /* ************************************************************************* */
 int main() {
diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp
index 291ed7b06..70845b24f 100644
--- a/tests/testManifold.cpp
+++ b/tests/testManifold.cpp
@@ -159,57 +159,6 @@ TEST(Manifold, _identity) {
   EXPECT_DOUBLES_EQUAL(0.0, traits_x<double>::Identity(), 0.0);
 }
 
-/* ************************************************************************* */
-// charts
-TEST(Manifold, Canonical) {
-
-  Canonical<Point2> chart1;
-  EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0));
-  EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0));
-
-  Vector v2(2);
-  v2 << 1, 0;
-  Canonical<Vector2> chart2;
-  EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0))));
-  EXPECT(chart2.Retract(v2)==Vector2(1, 0));
-
-  Canonical<double> chart3;
-  Eigen::Matrix<double, 1, 1> v1;
-  v1 << 1;
-  EXPECT(chart3.Local(1)==v1);
-  EXPECT_DOUBLES_EQUAL(chart3.Retract(v1), 1, 1e-9);
-
-  Canonical<Point3> chart4;
-  Point3 point(1, 2, 3);
-  Vector v3(3);
-  v3 << 1, 2, 3;
-  EXPECT(assert_equal(v3, chart4.Local(point)));
-  EXPECT(assert_equal(chart4.Retract(v3), point));
-
-  Canonical<Pose3> chart5;
-  Pose3 pose(Rot3::identity(), point);
-  Vector v6(6);
-  v6 << 0, 0, 0, 1, 2, 3;
-  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);
-  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
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;