From 896d006ddf6fdcafcc459788e96c9af34b4326b6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 16 Sep 2012 04:36:35 +0000
Subject: [PATCH] return all keys (slow)

---
 gtsam/inference/FactorGraph-inl.h | 28 ++++++++++++++++++++--------
 gtsam/inference/FactorGraph.h     |  6 +++++-
 2 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h
index 04490dbc6..47e330d7b 100644
--- a/gtsam/inference/FactorGraph-inl.h
+++ b/gtsam/inference/FactorGraph-inl.h
@@ -77,14 +77,26 @@ namespace gtsam {
 		return true;
 	}
 
-	/* ************************************************************************* */
-	template<class FACTOR>
-	size_t FactorGraph<FACTOR>::nrFactors() const {
-		size_t size_ = 0;
-		for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
-			if (*factor != NULL) size_++;
-		return size_;
-	}
+  /* ************************************************************************* */
+  template<class FACTOR>
+  size_t FactorGraph<FACTOR>::nrFactors() const {
+    size_t size_ = 0;
+    BOOST_FOREACH(const sharedFactor& factor, factors_)
+      if (factor) size_++;
+    return size_;
+  }
+
+  /* ************************************************************************* */
+  template<class FACTOR>
+  std::set<typename FACTOR::KeyType> FactorGraph<FACTOR>::keys() const {
+    std::set<KeyType> allKeys;
+    BOOST_FOREACH(const sharedFactor& factor, factors_)
+      if (factor) {
+        BOOST_FOREACH(Index j, factor->keys())
+          allKeys.insert(j);
+      }
+    return allKeys;
+  }
 
 	/* ************************************************************************* */
 	template<class FACTOR>
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index ed3e7d952..ae6c03439 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -27,6 +27,7 @@
 
 #include <boost/serialization/nvp.hpp>
 #include <boost/function.hpp>
+#include <set>
 
 namespace gtsam {
 
@@ -44,7 +45,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 	public:
 
 		typedef FACTOR FactorType;  ///< factor type
-		typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index factors with
+		typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index variables with
 		typedef boost::shared_ptr<FACTOR> sharedFactor;  ///< Shared pointer to a factor
 		typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional;  ///< Shared pointer to a conditional
 
@@ -208,6 +209,9 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 		/** return the number valid factors */
 		size_t nrFactors() const;
 
+    /** Potentially very slow function to return all keys involved */
+    std::set<KeyType> keys() const;
+
 	private:
 
 		/** Serialization function */