From 9a22f3a825321a13814d76b9cec5b3f6ca84963e Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 22 Dec 2014 11:25:21 +0100
Subject: [PATCH] Fixed bug in BetweenFactor

---
 .cproject                  |  8 ++++++++
 gtsam/slam/BetweenFactor.h | 16 +++++++++++-----
 2 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/.cproject b/.cproject
index 4dc49b994..b9fdfae51 100644
--- a/.cproject
+++ b/.cproject
@@ -2999,6 +2999,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testBetweenFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 5889f6d03..ccb981de7 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -93,12 +93,18 @@ namespace gtsam {
             boost::none) const {
       T hx = traits_x<T>::Between(p1, p2, H1, H2); // h(x)
       // manifold equivalent of h(x)-z -> log(z,h(x))
-      typename traits_x<T>::ChartJacobian Hlocal;
-      //OptionalJacobian<traits_x<T>::dimension, traits_x<T>::dimension> Hlocal;
-      Vector rval = traits_x<T>::Local(measured_, hx, boost::none, Hlocal);
-      (*H1) = ((*Hlocal) * (*H1)).eval();
-      (*H2) = ((*Hlocal) * (*H2)).eval();
+#ifdef BETWEENFACTOR_ASSUME_SMALL
+      return traits_x<T>::Local(measured_, hx);
+#else
+      return traits_x<T>::Local(measured_, hx);
+      static const int N = traits_x<T>::dimension;
+      Eigen::Matrix<double,N,N> Hlocal;
+    Vector rval = traits_x<T>::Local(measured_, hx, boost::none,
+        (H1 || H2) ? &Hlocal : 0);
+      if (H1) *H1 = Hlocal * (*H1);
+      if (H1) *H2 = Hlocal * (*H2);
       return rval;
+#endif
     }
 
     /** return the measured */