From f30e2501bef5f08c6b0d5f1a3d5a1f7d9772ea2c Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <dta-huynh@irobot.com>
Date: Fri, 15 May 2015 08:45:47 -0400
Subject: [PATCH] compute error for constrained linear factor graphs

---
 gtsam_unstable/linear/EqualityFactorGraph.h   | 13 +++++++++++++
 gtsam_unstable/linear/InequalityFactorGraph.h | 12 ++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/gtsam_unstable/linear/EqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h
index fa4e54fd9..194da5661 100644
--- a/gtsam_unstable/linear/EqualityFactorGraph.h
+++ b/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
diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h
index 178a0a4d5..5c1b17125 100644
--- a/gtsam_unstable/linear/InequalityFactorGraph.h
+++ b/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