From 1b199a4d3b9d201f4cb445d073a2ca8874f361fc Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 22 Oct 2009 05:02:31 +0000
Subject: [PATCH] Added a "factors" function that returns indices to all
 factors connected to a variable.

---
 cpp/LinearFactorGraph.cpp     | 13 +++++++++++++
 cpp/LinearFactorGraph.h       |  8 ++++++--
 cpp/testLinearFactorGraph.cpp | 28 ++++++++++++++++++++++++++++
 3 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp
index 94f42b20a..d884a1624 100644
--- a/cpp/LinearFactorGraph.cpp
+++ b/cpp/LinearFactorGraph.cpp
@@ -50,6 +50,19 @@ set<string> LinearFactorGraph::find_separator(const string& key) const
 /* ************************************************************************* */
 /** O(n)                                                                     */
 /* ************************************************************************* */ 
+std::vector<int> LinearFactorGraph::factors(const std::string& key) {
+  vector<int> found;
+
+  for(int i=0;i<factors_.size();i++)
+		if (factors_[i]->involves(key))
+			found.push_back(i);
+
+	return found;
+}
+
+/* ************************************************************************* */
+/** O(n)                                                                     */
+/* ************************************************************************* */
 LinearFactorSet
 LinearFactorGraph::find_factors_and_remove(const string& key)
 {
diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h
index cf3e35ee5..7aa244f06 100644
--- a/cpp/LinearFactorGraph.h
+++ b/cpp/LinearFactorGraph.h
@@ -52,11 +52,15 @@ namespace gtsam {
      */
     std::set<std::string> find_separator(const std::string& key) const;
 
+    /**
+     * Return indices for all factors that involve the given node
+     * @param key the key for the given node
+     */
+    std::vector<int> factors(const std::string& key);
+
     /**
      * find all the factors that involve the given node and remove them
      * from the factor graph
-     * NOTE: the ordering of the LinearFactor::shared_ptrs will change
-     * between systems, so do not rely on this ordering
      * @param key the key for the given node
      */
     LinearFactorSet find_factors_and_remove(const std::string& key);
diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp
index 9c9134194..4a571fbf6 100644
--- a/cpp/testLinearFactorGraph.cpp
+++ b/cpp/testLinearFactorGraph.cpp
@@ -537,6 +537,34 @@ TEST( LinearFactorGraph, COMBINE_GRAPHS)
 		
 		CHECK(size1+size2 == fg3.size()); 
 }
+
+/* ************************************************************************* */
+// print a vector of ints if needed for debugging
+void print(vector<int> v) {
+	for (int k = 0; k < v.size(); k++)
+		cout << v[k] << " ";
+	cout << endl;
+}
+
+/* ************************************************************************* */
+TEST( LinearFactorGraph, factor_lookup)
+{
+	// create a test graph
+	LinearFactorGraph fg = createLinearFactorGraph();
+
+	// ask for all factor indices connected to x1
+	vector<int> x1_factors = fg.factors("x1");
+	int x1_indices[] = { 0, 1, 2 };
+	vector<int> x1_expected(x1_indices, x1_indices + 3);
+	CHECK(x1_factors==x1_expected);
+
+	// ask for all factor indices connected to x2
+	vector<int> x2_factors = fg.factors("x2");
+	int x2_indices[] = { 1, 3 };
+	vector<int> x2_expected(x2_indices, x2_indices + 2);
+	CHECK(x2_factors==x2_expected);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */