From 903e5a3a86894309ed6f4737a892b5058bd1f828 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 22 Dec 2014 12:45:35 +0100
Subject: [PATCH] Restored old path, added SLOW_BUT_CORRECT_BETWEENFACTOR ifdef

---
 gtsam/slam/BetweenFactor.h | 18 +++++++-----------
 1 file changed, 7 insertions(+), 11 deletions(-)

diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index ccb981de7..0b20c1286 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -88,22 +88,18 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-    Vector evaluateError(const T& p1, const T& p2,
-        boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 =
-            boost::none) const {
+  Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
+      boost::none, boost::optional<Matrix&> H2 = 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))
-#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);
+#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR
+      typename traits_x<T>::ChartJacobian::Fixed 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;
+#else
+      return traits_x<T>::Local(measured_, hx);
 #endif
     }