From 732ff54b83c72549fc92e4f43e006af9352a2f78 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 10 Oct 2014 11:41:01 +0200
Subject: [PATCH] More experiments

---
 gtsam_unstable/nonlinear/Expression-inl.h     | 40 +++++++---
 .../nonlinear/tests/testExpressionFactor.cpp  | 74 +++++++++++++++++++
 2 files changed, 102 insertions(+), 12 deletions(-)

diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index b7038fc4c..53f531149 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/gtsam_unstable/nonlinear/Expression-inl.h
@@ -24,6 +24,13 @@
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 
+// template meta-programming headers
+#include <boost/mpl/vector.hpp>
+#include <boost/mpl/plus.hpp>
+#include <boost/mpl/front.hpp>
+#include <boost/mpl/pop_front.hpp>
+#include <boost/mpl/fold.hpp>
+
 namespace gtsam {
 
 template<typename T>
@@ -380,31 +387,40 @@ public:
 
 //-----------------------------------------------------------------------------
 
-#include <boost/mpl/list.hpp>
+// Abrahams, David; Gurtovoy, Aleksey (2004-12-10).
+// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
+// and Beyond (Kindle Location 1244). Pearson Education.
 
 /// Recursive Trace Class
 template<class T, class LIST>
 struct MakeTrace: public JacobianTrace<T> {
-  typedef boost::mpl::front<LIST> A1;
-  static const size_t dimA = A1::dimension;
-  typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA;
-  typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
 
-  typename JacobianTrace<A1>::Pointer trace1;
-  JacobianTA dTdA1;
+  typedef typename boost::mpl::front<LIST>::type A;
+
+  // define dimensions
+  static const size_t m = T::dimension;
+  static const size_t n = A::dimension;
+
+  // define fixed size Jacobian matrix types
+  typedef Eigen::Matrix<double, m, n> JacobianTA;
+  typedef Eigen::Matrix<double, 2, m> Jacobian2T;
+
+  // declare trace that produces value A, and corresponding Jacobian
+  typename JacobianTrace<A>::Pointer trace;
+  JacobianTA dTdA;
 
   /// Start the reverse AD process
   virtual void startReverseAD(JacobianMap& jacobians) const {
-    Select<T::dimension, A1>::reverseAD(trace1, dTdA1, jacobians);
+    Select<T::dimension, A>::reverseAD(trace, dTdA, jacobians);
   }
   /// Given df/dT, multiply in dT/dA and continue reverse AD process
   virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
-    trace1.reverseAD(dFdT * dTdA1, jacobians);
+    trace.reverseAD(dFdT * dTdA, jacobians);
   }
   /// Version specialized to 2-dimensional output
   virtual void reverseAD2(const Jacobian2T& dFdT,
       JacobianMap& jacobians) const {
-    trace1.reverseAD2(dFdT * dTdA1, jacobians);
+    trace.reverseAD2(dFdT * dTdA, jacobians);
   }
 };
 
@@ -460,8 +476,8 @@ public:
       typename JacobianTrace<T>::Pointer& p) const {
     Trace* trace = new Trace();
     p.setFunction(trace);
-    A1 a = this->expressionA1_->traceExecution(values, trace->trace1);
-    return function_(a, trace->dTdA1);
+    A1 a = this->expressionA1_->traceExecution(values, trace->trace);
+    return function_(a, trace->dTdA);
   }
 };
 
diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
index 87da6fde9..163139543 100644
--- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
@@ -318,6 +318,80 @@ TEST(ExpressionFactor, composeTernary) {
   EXPECT( assert_equal(expected, *jf,1e-9));
 }
 
+/* ************************************************************************* */
+
+namespace mpl = boost::mpl;
+
+template<class TYPES>
+struct ProtoTrace: public ProtoTrace<typename mpl::pop_front<TYPES>::type> {
+
+  typedef typename mpl::front<TYPES>::type A;
+
+};
+
+/// Recursive Trace Class, Base case
+template<>
+struct ProtoTrace<mpl::vector0<> > {
+};
+
+template<>
+struct ProtoTrace<int> {
+};
+
+/// Recursive Trace Class, Primary Template
+template<class A, class More>
+struct store: More {
+  // define dimensions
+  static const size_t m = 3;
+  static const size_t n = A::dimension;
+
+  // define fixed size Jacobian matrix types
+  typedef Eigen::Matrix<double, m, n> JacobianTA;
+  typedef Eigen::Matrix<double, 2, m> Jacobian2T;
+
+  // declare trace that produces value A, and corresponding Jacobian
+  typename JacobianTrace<A>::Pointer trace;
+  JacobianTA dTdA;
+
+};
+typedef mpl::vector<Pose3, Point3, Cal3_S2> MyTypes;
+
+template<class T> struct Incomplete;
+
+#include<boost/mpl/empty_base.hpp>
+#include<boost/mpl/placeholders.hpp>
+#include<boost/mpl/reverse_fold.hpp>
+namespace MPL = mpl::placeholders;
+typedef mpl::reverse_fold<MyTypes, mpl::empty_base, store<MPL::_2, MPL::_1> >::type Generated;
+Generated generated;
+//Incomplete<Generated> incomplete;
+#include <boost/mpl/assert.hpp>
+
+typedef mpl::vector1<Point3> OneType;
+typedef mpl::pop_front<OneType>::type Empty;
+typedef mpl::pop_front<Empty>::type Bad;
+//typedef ProtoTrace<OneType> UnaryTrace;
+//BOOST_MPL_ASSERT((boost::is_same< UnaryTrace::A, Point3 >));
+
+#include <boost/static_assert.hpp>
+#include <boost/mpl/plus.hpp>
+#include <boost/mpl/int.hpp>
+#include <boost/mpl/assert.hpp>
+//#include <boost/mpl/print.hpp>
+
+BOOST_STATIC_ASSERT((mpl::plus<mpl::int_<2>,mpl::int_<3> >::type::value==5));
+
+typedef mpl::vector0<> List0;
+typedef ProtoTrace<int> Proto0;
+//typedef ProtoTrace<mpl::vec/tor0<> > Proto0;
+//typedef mpl::print<Proto0>::type Dbg;
+//incomplete<ProtoTrace<List0> > proto0;
+
+typedef struct {
+} Expected0;
+BOOST_MPL_ASSERT((boost::is_same< Expected0, Expected0 >));
+//BOOST_MPL_ASSERT((boost::is_same< ProtoTrace<int>, ProtoTrace<List0> >));
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;