compute error for constrained linear factor graphs

gtsam_unstable/linear/EqualityFactorGraph.h

@@ -26,6 +26,19 @@ namespace gtsam {
 class EqualityFactorGraph : public FactorGraph<LinearEquality> {
 public:
   typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
+
+  /** compute error of a guess.
+   * TODO: This code is duplicated in GaussianFactorGraph and NonlinearFactorGraph!!
+   * Remove it!
+   */
+  double error(const VectorValues& x) const {
+    double total_error = 0.;
+    BOOST_FOREACH(const sharedFactor& factor, *this){
+      if(factor)
+        total_error += factor->error(x);
+    }
+    return total_error;
+  }
 };
 
 /// traits

gtsam_unstable/linear/InequalityFactorGraph.h

@@ -41,6 +41,18 @@ public:
       double tol = 1e-9) const {
     return Base::equals(other, tol);
   }
+
+  /**
+   * Compute error of a guess.
+   * Infinity error if it violates an inequality; zero otherwise. */
+  double error(const VectorValues& x) const {
+    BOOST_FOREACH(const sharedFactor& factor, *this){
+      if(factor)
+        if (factor->error(x) > 0)
+          return std::numeric_limits<double>::infinity();
+    }
+    return 0.0;
+  }
 };
 
 /// traits