From 613cb0bb129750584f3a41707a9e3ef6ca64f5e3 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 7 Oct 2014 01:01:46 +0200
Subject: [PATCH] Binary functions now take fixed Jacobians

---
 gtsam_unstable/nonlinear/Expression-inl.h       | 14 ++++++++------
 gtsam_unstable/nonlinear/Expression.h           | 17 +++++++++++------
 .../nonlinear/tests/testExpression.cpp          |  4 ++--
 gtsam_unstable/timing/timeCameraExpression.cpp  |  4 ++--
 4 files changed, 23 insertions(+), 16 deletions(-)

diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index fb0af0d54..5a16895d0 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/gtsam_unstable/nonlinear/Expression-inl.h
@@ -386,8 +386,8 @@ public:
   typedef Eigen::Matrix<double,T::dimension,A1::dimension> JacobianTA1;
   typedef Eigen::Matrix<double,T::dimension,A2::dimension> JacobianTA2;
   typedef boost::function<
-      T(const A1&, const A2&, boost::optional<Matrix&>,
-          boost::optional<Matrix&>)> Function;
+      T(const A1&, const A2&, boost::optional<JacobianTA1&>,
+          boost::optional<JacobianTA2&>)> Function;
 
 private:
 
@@ -429,10 +429,11 @@ public:
     using boost::none;
     Augmented<A1> a1 = this->expressionA1_->forward(values);
     Augmented<A2> a2 = this->expressionA2_->forward(values);
-    Matrix dTdA1, dTdA2;
+    JacobianTA1 dTdA1;
+    JacobianTA2 dTdA2;
     T t = function_(a1.value(), a2.value(),
-        a1.constant() ? none : boost::optional<Matrix&>(dTdA1),
-        a2.constant() ? none : boost::optional<Matrix&>(dTdA2));
+        a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
+        a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2));
     return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians());
   }
 
@@ -440,7 +441,8 @@ public:
   struct Trace: public JacobianTrace<T> {
     boost::shared_ptr<JacobianTrace<A1> > trace1;
     boost::shared_ptr<JacobianTrace<A2> > trace2;
-    Matrix dTdA1, dTdA2;
+    JacobianTA1 dTdA1;
+    JacobianTA2 dTdA2;
     /// Start the reverse AD process
     virtual void reverseAD(JacobianMap& jacobians) const {
       trace1->reverseAD(dTdA1, jacobians);
diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h
index 18e6c35e1..1c16fab25 100644
--- a/gtsam_unstable/nonlinear/Expression.h
+++ b/gtsam_unstable/nonlinear/Expression.h
@@ -76,8 +76,10 @@ public:
   /// Construct a unary method expression
   template<typename A1, typename A2>
   Expression(const Expression<A1>& expression1,
-      T (A1::*method)(const A2&, boost::optional<Matrix&>,
-          boost::optional<Matrix&>) const, const Expression<A2>& expression2) {
+      T (A1::*method)(const A2&,
+          boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA1&>,
+          boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA2&>) const,
+      const Expression<A2>& expression2) {
     root_.reset(
         new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
             expression1, expression2));
@@ -94,9 +96,11 @@ public:
   /// Construct a ternary function expression
   template<typename A1, typename A2, typename A3>
   Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
-      const Expression<A1>& expression1, const Expression<A2>& expression2,  const Expression<A3>& expression3) {
+      const Expression<A1>& expression1, const Expression<A2>& expression2,
+      const Expression<A3>& expression3) {
     root_.reset(
-        new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2, expression3));
+        new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2,
+            expression3));
   }
 
   /// Return keys that play in this expression
@@ -132,8 +136,9 @@ public:
 template<class T>
 struct apply_compose {
   typedef T result_type;
-  T operator()(const T& x, const T& y, boost::optional<Matrix&> H1,
-      boost::optional<Matrix&> H2) const {
+  typedef Eigen::Matrix<double, T::dimension, T::dimension> Jacobian;
+  T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
+      boost::optional<Jacobian&> H2) const {
     return x.compose(y, H1, H2);
   }
 };
diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp
index 51a7ecbc7..5532b0da3 100644
--- a/gtsam_unstable/nonlinear/tests/testExpression.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp
@@ -32,8 +32,8 @@ using namespace gtsam;
 /* ************************************************************************* */
 
 template<class CAL>
-Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal,
-    boost::optional<Matrix&> Dp) {
+Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix25&> Dcal,
+    boost::optional<Matrix2&> Dp) {
   return K.uncalibrate(p, Dcal, Dp);
 }
 
diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp
index baa515029..4e4e31d18 100644
--- a/gtsam_unstable/timing/timeCameraExpression.cpp
+++ b/gtsam_unstable/timing/timeCameraExpression.cpp
@@ -47,9 +47,9 @@ void time(const NonlinearFactor& f, const Values& values) {
 boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());
 
 Point2 myProject(const Pose3& pose, const Point3& point,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+    boost::optional<Matrix26&> H1, boost::optional<Matrix23&> H2) {
   PinholeCamera<Cal3_S2> camera(pose, *fixedK);
-  return camera.project(point, H1, H2);
+  return camera.project(point, H1, H2, boost::none);
 }
 
 int main() {