Recover the convenient evaluateError() method

gtsam/sam/BearingFactor.h

@@ -61,6 +61,21 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
     Base::print(s, kf);
   }
   
+  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 = unwhitenedError(
+      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+      Hs);
+    if (H1) *H1 = Hs[0];
+    if (H2) *H2 = Hs[1];
+    return error;
+  }
+
+
  private:
   friend class boost::serialization::access;
   template <class ARCHIVE>

gtsam/sam/BearingRangeFactor.h

@@ -66,6 +66,20 @@ class BearingRangeFactor
                          Expression<A2>(keys[1]));
   }
 
+  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 = unwhitenedError(
+      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+      Hs);
+    if (H1) *H1 = Hs[0];
+    if (H2) *H2 = Hs[1];
+    return error;
+  }
+
   /// print
   void print(const std::string& s = "",
                      const KeyFormatter& kf = DefaultKeyFormatter) const override {

gtsam/sam/RangeFactor.h

@@ -59,6 +59,20 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
     return Expression<T>(Range<A1, A2>(), a1_, 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 = unwhitenedError(
+      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+      Hs);
+    if (H1) *H1 = Hs[0];
+    if (H2) *H2 = Hs[1];
+    return error;
+  }
+
   /// print
   void print(const std::string& s = "",
              const KeyFormatter& kf = DefaultKeyFormatter) const override {

gtsam/sam/tests/testRangeFactor.cpp

@@ -47,29 +47,25 @@ double measurement(10.0);
 /* ************************************************************************* */
 Vector factorError2D(const Pose2& pose, const Point2& point,
     const RangeFactor2D& factor) {
-  const auto &keys = factor.keys();
+  return factor.evaluateError(pose, point);
-  return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
 }
 
 /* ************************************************************************* */
 Vector factorError3D(const Pose3& pose, const Point3& point,
     const RangeFactor3D& factor) {
-  const auto &keys = factor.keys();
+  return factor.evaluateError(pose, point);
-  return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
 }
 
 /* ************************************************************************* */
 Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
     const RangeFactorWithTransform2D& factor) {
-  const auto &keys = factor.keys();
+  return factor.evaluateError(pose, point);
-  return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
 }
 
 /* ************************************************************************* */
 Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
     const RangeFactorWithTransform3D& factor) {
-  const auto &keys = factor.keys();
+  return factor.evaluateError(pose, point);
-  return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}});
 }
 
 /* ************************************************************************* */
@@ -262,10 +258,8 @@ TEST( RangeFactor, Jacobian2D ) {
   Point2 point(-4.0, 11.0);
 
   // Use the factor to calculate the Jacobians
-  std::vector<Matrix> actualHs(2);
+  Matrix H1Actual, H2Actual;
-  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs); 
+  factor.evaluateError(pose, point, H1Actual, H2Actual);
-  const Matrix& H1Actual = actualHs.at(0);
-  const Matrix& H2Actual = actualHs.at(1);
 
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;