Deal with traits changes

gtsam_unstable/nonlinear/Expression-inl.h

@@ -114,7 +114,7 @@ void handleLeafCase(
  */
 template<class T>
 class ExecutionTrace {
-  static const int Dim = dimension<T>::value;
+  static const int Dim = traits::dimension<T>::value;
   enum {
     Constant, Leaf, Function
   } kind;
@@ -196,7 +196,7 @@ public:
 /// Primary template calls the generic Matrix reverseAD pipeline
 template<size_t ROWS, class A>
 struct Select {
-  typedef Eigen::Matrix<double, ROWS, dimension<A>::value> Jacobian;
+  typedef Eigen::Matrix<double, ROWS, traits::dimension<A>::value> Jacobian;
   static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
       JacobianMap& jacobians) {
     trace.reverseAD(dTdA, jacobians);
@@ -206,7 +206,7 @@ struct Select {
 /// Partially specialized template calls the 2-dimensional output version
 template<class A>
 struct Select<2, A> {
-  typedef Eigen::Matrix<double, 2, dimension<A>::value> Jacobian;
+  typedef Eigen::Matrix<double, 2, traits::dimension<A>::value> Jacobian;
   static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
       JacobianMap& jacobians) {
     trace.reverseAD2(dTdA, jacobians);
@@ -317,7 +317,7 @@ public:
 
   /// Return dimensions for each argument
   virtual void dims(std::map<Key, size_t>& map) const {
-    map[key_] = dimension<T>::value;
+    map[key_] = traits::dimension<T>::value;
   }
 
   /// Return value
@@ -368,13 +368,15 @@ public:
 /// meta-function to generate fixed-size JacobianTA type
 template<class T, class A>
 struct Jacobian {
-  typedef Eigen::Matrix<double, dimension<T>::value, dimension<A>::value> type;
+  typedef Eigen::Matrix<double, traits::dimension<T>::value,
+      traits::dimension<A>::value> type;
 };
 
 /// meta-function to generate JacobianTA optional reference
 template<class T, class A>
 struct Optional {
-  typedef Eigen::Matrix<double, dimension<T>::value, dimension<A>::value> Jacobian;
+  typedef Eigen::Matrix<double, traits::dimension<T>::value,
+      traits::dimension<A>::value> Jacobian;
   typedef boost::optional<Jacobian&> type;
 };
 
@@ -385,7 +387,7 @@ template<class T>
 struct FunctionalBase: ExpressionNode<T> {
   static size_t const N = 0; // number of arguments
 
-  typedef CallRecord<dimension<T>::value> Record;
+  typedef CallRecord<traits::dimension<T>::value> Record;
 
   /// Construct an execution trace for reverse AD
   void trace(const Values& values, Record* record, char*& raw) const {
@@ -454,7 +456,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
     /// Start the reverse AD process
     virtual void startReverseAD(JacobianMap& jacobians) const {
       Base::Record::startReverseAD(jacobians);
-      Select<dimension<T>::value, A>::reverseAD(This::trace, This::dTdA,
+      Select<traits::dimension<T>::value, A>::reverseAD(This::trace, This::dTdA,
           jacobians);
     }
 
@@ -465,7 +467,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
     }
 
     /// Version specialized to 2-dimensional output
-    typedef Eigen::Matrix<double, 2, dimension<T>::value> Jacobian2T;
+    typedef Eigen::Matrix<double, 2, traits::dimension<T>::value> Jacobian2T;
     virtual void reverseAD2(const Jacobian2T& dFdT,
         JacobianMap& jacobians) const {
       Base::Record::reverseAD2(dFdT, jacobians);

gtsam_unstable/nonlinear/Expression.h

@@ -159,7 +159,7 @@ public:
 template<class T>
 struct apply_compose {
   typedef T result_type;
-  static const int Dim = dimension<T>::value;
+  static const int Dim = traits::dimension<T>::value;
   typedef Eigen::Matrix<double, Dim, Dim> Jacobian;
   T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
       boost::optional<Jacobian&> H2) const {

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -37,6 +37,8 @@ class ExpressionFactor: public NoiseModelFactor {
   std::vector<size_t> dimensions_; ///< dimensions of the Jacobian matrices
   size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
 
+  static const int Dim = traits::dimension<T>::value;
+
 public:
 
   /// Constructor
@@ -45,7 +47,7 @@ public:
       measurement_(measurement), expression_(expression) {
     if (!noiseModel)
       throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
-    if (noiseModel->dim() != dimension<T>::value)
+    if (noiseModel->dim() != Dim)
       throw std::invalid_argument(
           "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
     noiseModel_ = noiseModel;
@@ -68,7 +70,7 @@ public:
 #ifdef DEBUG_ExpressionFactor
     BOOST_FOREACH(size_t d, dimensions_)
     std::cout << d << " ";
-    std::cout << " -> " << dimension<T>::value << "x" << augmentedCols_ << std::endl;
+    std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl;
 #endif
   }
 
@@ -87,10 +89,9 @@ public:
       JacobianMap blocks;
       for (DenseIndex i = 0; i < size(); i++) {
         Matrix& Hi = H->at(i);
-        Hi.resize(dimension<T>::value, dimensions_[i]);
+        Hi.resize(Dim, dimensions_[i]);
         Hi.setZero(); // zero out
-        Eigen::Block<Matrix> block = Hi.block(0, 0, dimension<T>::value,
+        Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]);
-            dimensions_[i]);
         blocks.insert(std::make_pair(keys_[i], block));
       }
 
@@ -110,10 +111,9 @@ public:
     // to [expression_.value] below, which writes directly into Ab_.
 
     // Another malloc saved by creating a Matrix on the stack
-    static const int Dim = dimension<T>::value;
     double memory[Dim * augmentedCols_];
-    Eigen::Map<Eigen::Matrix<double, dimension<T>::value, Eigen::Dynamic> > //
+    Eigen::Map<Eigen::Matrix<double, Dim, Eigen::Dynamic> > //
-    matrix(memory, dimension<T>::value, augmentedCols_);
+    matrix(memory, Dim, augmentedCols_);
     matrix.setZero(); // zero out
 
     // Construct block matrix, is of right size but un-initialized

gtsam_unstable/nonlinear/tests/testExpression.cpp

@@ -327,6 +327,7 @@ struct SnavelyProjection {
 
 // is_manifold
 TEST(Expression, is_manifold) {
+  using namespace traits;
   EXPECT(!is_manifold<int>::value);
   EXPECT(is_manifold<Point2>::value);
   EXPECT(is_manifold<Matrix24>::value);
@@ -337,6 +338,7 @@ TEST(Expression, is_manifold) {
 
 // dimension
 TEST(Expression, dimension) {
+  using namespace traits;
   EXPECT_LONGS_EQUAL(2, dimension<Point2>::value);
   EXPECT_LONGS_EQUAL(8, dimension<Matrix24>::value);
   EXPECT_LONGS_EQUAL(1, dimension<double>::value);
@@ -374,6 +376,7 @@ TEST(Expression, Charts) {
 template<typename Y, typename X>
 Matrix numericalDerivative(boost::function<Y(const X&)> h, const X& x,
     double delta = 1e-5) {
+  using namespace traits;
 
   BOOST_STATIC_ASSERT(is_manifold<Y>::value);
   static const size_t M = dimension<Y>::value;
@@ -491,33 +494,14 @@ TEST(Expression, AutoDiff2) {
   EXPECT(assert_equal(E2,H2,1e-8));
 }
 
-/* ************************************************************************* */
-// zero<T>::value is intended to be the origin of a canonical coordinate system
-// with canonical(t) == DefaultChart<T>(zero<T>::value).apply(t)
-template<typename T> struct zero;
-template<typename T> class Canonical {
-  DefaultChart<T> chart;
-public:
-  typedef T type;
-  typedef typename DefaultChart<T>::vector vector;
-  Canonical() :
-      chart(zero<T>::value) {
-  }
-  vector vee(const T& t) {
-    return chart.apply(t);
-  }
-  T hat(const vector& v) {
-    return chart.retract(v);
-  }
-};
 /* ************************************************************************* */
 // Adapt ceres-style autodiff
 template<typename F, typename T, typename A1, typename A2>
 class AdaptAutoDiff {
 
-  static const int N = dimension<T>::value;
+  static const int N = traits::dimension<T>::value;
-  static const int M1 = dimension<A1>::value;
+  static const int M1 = traits::dimension<A1>::value;
-  static const int M2 = dimension<A2>::value;
+  static const int M2 = traits::dimension<A2>::value;
 
   typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
   typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;
@@ -547,8 +531,8 @@ public:
     using ceres::internal::AutoDiff;
 
     // Make arguments
-    Vector1 v1 = chart1.vee(a1);
+    Vector1 v1 = chart1.apply(a1);
-    Vector2 v2 = chart2.vee(a2);
+    Vector2 v2 = chart2.apply(a2);
 
     bool success;
     VectorT result;
@@ -574,7 +558,7 @@ public:
     if (!success)
       throw std::runtime_error(
           "AdaptAutoDiff: function call resulted in failure");
-    return chartT.hat(result);
+    return chartT.retract(result);
   }
 
 };
@@ -582,24 +566,6 @@ public:
 // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
 typedef PinholeCamera<Cal3Bundler> Camera;
 
-template<>
-struct zero<Camera> {
-  static const Camera value;
-};
-const Camera zero<Camera>::value(Camera(Pose3(), Cal3Bundler(0, 0, 0)));
-
-template<>
-struct zero<Point3> {
-  static const Point3 value;
-};
-const Point3 zero<Point3>::value(Point3(0, 0, 0));
-
-template<>
-struct zero<Point2> {
-  static const Point2 value;
-};
-const Point2 zero<Point2>::value(Point2(0, 0));
-
 /* ************************************************************************* */
 // Test AutoDiff wrapper Snavely
 TEST(Expression, AutoDiff3) {