From 149bfe1729929c3393ad14263a3e88f7360a22d8 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 23 Nov 2014 23:15:10 +0100
Subject: [PATCH] Chery-picked Paul's changes

---
 gtsam/base/Manifold.h                       |  2 +-
 gtsam_unstable/nonlinear/ExpressionFactor.h | 13 ++++++++-----
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index a5a3d5f34..1d7eda12f 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -235,7 +235,7 @@ struct DefaultChart<double> {
   static double retract(double origin, const vector& d) {
     return origin + d[0];
   }
-  static const int getDimension(double) {
+  static int getDimension(double) {
     return 1;
   }
 };
diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index 37a89af6b..f609071cb 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -81,6 +81,8 @@ public:
    */
   virtual Vector unwhitenedError(const Values& x,
       boost::optional<std::vector<Matrix>&> H = boost::none) const {
+    // TODO(PTF) Is this a place for custom charts?
+    DefaultChart<T> chart;
     if (H) {
       // H should be pre-allocated
       assert(H->size()==size());
@@ -95,18 +97,19 @@ public:
       T value = expression_.value(x, map); // <<< Reverse AD happens here !
 
       // Copy blocks into the vector of jacobians passed in
-      for (DenseIndex i = 0; i < size(); i++)
+      for (DenseIndex i = 0; i < static_cast<DenseIndex>(size()); i++)
         H->at(i) = Ab(i);
 
-      return measurement_.localCoordinates(value);
+      return chart.local(measurement_, value);
     } else {
       const T& value = expression_.value(x);
-      return measurement_.localCoordinates(value);
+      return chart.local(measurement_, value);
     }
   }
 
   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
-
+    // TODO(PTF) Is this a place for custom charts?
+    DefaultChart<T> chart;
     // Only linearize if the factor is active
     if (!active(x))
       return boost::shared_ptr<JacobianFactor>();
@@ -128,7 +131,7 @@ public:
 
     // Evaluate error to get Jacobians and RHS vector b
     T value = expression_.value(x, map); // <<< Reverse AD happens here !
-    Ab(size()).col(0) = -measurement_.localCoordinates(value);
+    Ab(size()).col(0) = -chart.local(measurement_, value);
 
     // Whiten the corresponding system, Ab already contains RHS
     Vector dummy(Dim);