From 85122bed95db5d34a34a9d0a8e3673a25cdd2168 Mon Sep 17 00:00:00 2001
From: Christian Forster <ch.forster@gmail.com>
Date: Mon, 1 Dec 2014 14:44:22 -0500
Subject: [PATCH] using traits and Chart in BetweenFactor

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

diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 1b630374c..2aa0b50d2 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -74,14 +74,14 @@ namespace gtsam {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";
-      measured_.print("  measured: ");
+      traits::print<T>()(measured_, "  measured: ");
       this->noiseModel_->print("  noise model: ");
     }
 
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
+      return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(this->measured_, e->measured_, tol);
     }
 
     /** implement functions needed to derive from Factor */
@@ -92,7 +92,8 @@ namespace gtsam {
             boost::none) const {
       T hx = p1.between(p2, H1, H2); // h(x)
       // manifold equivalent of h(x)-z -> log(z,h(x))
-      return measured_.localCoordinates(hx);
+      DefaultChart<T> chart;
+      return chart.local(measured_, hx);
     }
 
     /** return the measured */
@@ -129,7 +130,9 @@ namespace gtsam {
 
     /** Syntactic sugar for constrained version */
     BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
-      BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
+      BetweenFactor<VALUE>(key1, key2, measured,
+                           noiseModel::Constrained::All(traits::dimension<VALUE>(), fabs(mu)))
+    {}
 
   private: