From dcd9415ddf4dcd3cb23146477f0818bff5ad777b Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Tue, 28 Jul 2020 21:37:02 +0200
Subject: [PATCH] fix evaluateError() methods

---
 gtsam/sam/RangeFactor.h | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h
index 0150505b2..d9890d2ef 100644
--- a/gtsam/sam/RangeFactor.h
+++ b/gtsam/sam/RangeFactor.h
@@ -65,7 +65,7 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
   {
     std::vector<Matrix> Hs(2);
     const auto &keys = Factor::keys();
-    const Vector error = unwhitenedError(
+    const Vector error = Base::unwhitenedError(
       {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
       Hs);
     if (H1) *H1 = Hs[0];
@@ -136,6 +136,20 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
     return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
   }
 
+  Vector evaluateError(const A1& a1, const A2& a2,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const
+  {
+    std::vector<Matrix> Hs(2);
+    const auto &keys = Factor::keys();
+    const Vector error = Base::unwhitenedError(
+      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+      Hs);
+    if (H1) *H1 = Hs[0];
+    if (H2) *H2 = Hs[1];
+    return error;
+  }
+
   /** print contents */
   void print(const std::string& s = "",
              const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {