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 { public: typedef boost::shared_ptr 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::infinity(); + } + return 0.0; + } }; /// traits