Added a "factors" function that returns indices to all factors connected to a variable.

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)
 {

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);

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);}
 /* ************************************************************************* */