From ef5bf03c81dcbad11118d9895fe2c25bdf76144c Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 13 Oct 2014 23:04:30 +0200
Subject: [PATCH] Clean up

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

diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index 53b38bba4..f0301ba4a 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/gtsam_unstable/nonlinear/Expression-inl.h
@@ -38,10 +38,6 @@ struct TestBinaryExpression;
 #include <boost/mpl/transform.hpp>
 #include <boost/mpl/at.hpp>
 
-// Boost Fusion includes allow us to create/access values from MPL vectors
-#include <boost/fusion/include/mpl.hpp>
-#include <boost/fusion/include/at_c.hpp>
-
 namespace MPL = boost::mpl::placeholders;
 
 class ExpressionFactorBinaryTest;
@@ -677,10 +673,13 @@ public:
   virtual Augmented<T> forward(const Values& values) const {
     using boost::none;
     Augmented<A1> a1 = this->template expression<A1, 1>()->forward(values);
-    typename Jacobian<T, A1>::type dTdA1;
-    T t = function_(a1.value(),
-        a1.constant() ? none : typename Optional<T,A1>::type(dTdA1));
-    return Augmented<T>(t, dTdA1, a1.jacobians());
+
+    // Declare Jacobians
+    using boost::mpl::at_c;
+    typename at_c<typename Base::Jacobians,0>::type H1;
+
+    T t = function_(a1.value(), H1);
+    return Augmented<T>(t, H1, a1.jacobians());
   }
 
   /// Construct an execution trace for reverse AD
@@ -741,12 +740,14 @@ public:
     using boost::none;
     Augmented<A1> a1 = this->template expression<A1, 1>()->forward(values);
     Augmented<A2> a2 = this->template expression<A2, 2>()->forward(values);
-    typename Jacobian<T, A1>::type dTdA1;
-    typename Jacobian<T, A2>::type dTdA2;
-    T t = function_(a1.value(), a2.value(),
-        a1.constant() ? none : typename Optional<T, A1>::type(dTdA1),
-        a2.constant() ? none : typename Optional<T, A2>::type(dTdA2));
-    return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians());
+
+    // Declare Jacobians
+    using boost::mpl::at_c;
+    typename at_c<typename Base::Jacobians,0>::type H1;
+    typename at_c<typename Base::Jacobians,1>::type H2;
+
+    T t = function_(a1.value(), a2.value(),H1,H2);
+    return Augmented<T>(t, H1, a1.jacobians(), H2, a2.jacobians());
   }
 
   /// Construct an execution trace for reverse AD
@@ -811,13 +812,12 @@ public:
     Augmented<A2> a2 = this->template expression<A2, 2>()->forward(values);
     Augmented<A3> a3 = this->template expression<A3, 3>()->forward(values);
 
-    typedef typename Base::Jacobians Jacobians;
+    // Declare Jacobians
+    using boost::mpl::at_c;
+    typename at_c<typename Base::Jacobians,0>::type H1;
+    typename at_c<typename Base::Jacobians,1>::type H2;
+    typename at_c<typename Base::Jacobians,2>::type H3;
 
-    using boost::fusion::at_c;
-    Jacobians H;
-    typename boost::mpl::at_c<Jacobians,0>::type H1;
-    typename boost::mpl::at_c<Jacobians,1>::type H2;
-    typename boost::mpl::at_c<Jacobians,2>::type H3;
     T t = function_(a1.value(), a2.value(), a3.value(),H1,H2,H3);
     return Augmented<T>(t, H1, a1.jacobians(), H2, a2.jacobians(),
         H3, a3.jacobians());
diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
index d7fe87c87..3a7ad5c72 100644
--- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
@@ -464,6 +464,11 @@ BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >));
 typedef mpl::at_c<Jacobians, 1>::type Jacobian23; // base zero !
 BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>));
 
+/* ************************************************************************* */
+// Boost Fusion includes allow us to create/access values from MPL vectors
+#include <boost/fusion/include/mpl.hpp>
+#include <boost/fusion/include/at_c.hpp>
+
 // Create a value and access it
 TEST(ExpressionFactor, JacobiansValue) {
   using boost::fusion::at_c;
@@ -478,7 +483,11 @@ TEST(ExpressionFactor, JacobiansValue) {
 }
 
 /* ************************************************************************* */
+// Test out polymorphic transform
+
 #include <boost/fusion/include/make_vector.hpp>
+#include <boost/fusion/include/transform.hpp>
+#include <boost/utility/result_of.hpp>
 
 struct triple {
   template<class> struct result; // says we will provide result
@@ -488,6 +497,16 @@ struct triple {
       typedef double type; // result for int argument
   };
 
+  template<class F>
+  struct result<F(const int&)> {
+      typedef double type; // result for int argument
+  };
+
+  template<class F>
+  struct result<F(const double &)> {
+      typedef double type; // result for double argument
+  };
+
   template<class F>
   struct result<F(double)> {
       typedef double type; // result for double argument
@@ -495,13 +514,11 @@ struct triple {
 
   // actual function
   template<typename T>
-  typename result<triple(T)>::type operator()(T& x) const {
+  typename result<triple(T)>::type operator()(const T& x) const {
     return (double) x;
   }
 };
 
-
-// Test out polymorphic transform
 TEST(ExpressionFactor, Triple) {
   typedef boost::fusion::vector<int, double> IntDouble;
   IntDouble H = boost::fusion::make_vector(1, 2.0);