From ee21ef61a6cfbaf0c53dd059c73cba053b4748c4 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Thu, 6 Jun 2013 18:07:55 +0000
Subject: [PATCH] Added exists() to FactorGraph to allow for checking whether a
 factor exists at a given index.  Necessary for matlab interface.

---
 gtsam.h                       | 5 ++++-
 gtsam/inference/FactorGraph.h | 3 +++
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/gtsam.h b/gtsam.h
index e211068ad..8ff9056f9 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -895,6 +895,7 @@ class SymbolicFactorGraph {
   void print(string s) const;
   bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
   size_t size() const;
+  bool exists(size_t i) const;
 
   // Standard interface
   // FIXME: Must wrap FastSet<Index> for this to work
@@ -1282,6 +1283,7 @@ class GaussianFactorGraph {
   bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
   size_t size() const;
   gtsam::GaussianFactor* at(size_t idx) const;
+  bool exists(size_t idx) const;
 
   // Inference
   pair<gtsam::GaussianConditional*, gtsam::GaussianFactorGraph> eliminateFrontals(size_t nFrontals) const;
@@ -1508,7 +1510,8 @@ class NonlinearFactorGraph {
   bool empty() const;
   void remove(size_t i);
   size_t nrFactors() const;
-  gtsam::NonlinearFactor* at(size_t i) const;
+  gtsam::NonlinearFactor* at(size_t idx) const;
+  bool exists(size_t idx) const;
   void push_back(const gtsam::NonlinearFactorGraph& factors);
 
   // NonlinearFactorGraph
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 70954152f..0d3b10e31 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -165,6 +165,9 @@ class VariableIndex;
     const sharedFactor operator[](size_t i) const { return at(i); }
     sharedFactor& operator[](size_t i) { return at(i); }
 
+    /** Checks whether a valid factor exists at the given index */
+    inline bool exists(size_t i) const { return i<factors_.size() && factors_[i]; }
+
     /** STL begin, so we can use BOOST_FOREACH */
     const_iterator begin() const { return factors_.begin();}