order 1 factors by using map

cpp/FactorGraph-inl.h

@@ -16,12 +16,35 @@
 #include "Ordering.h"
 #include "FactorGraph.h"
 
+
 #include <colamd/colamd.h>
 
+using namespace std;
+
 namespace gtsam {
 
 /* ************************************************************************* */
+template<class Factor, class Config>
+void FactorGraph<Factor,Config>::push_back(shared_factor factor) {
+	factors_.push_back(factor);  // add the actual factor
+	int i = factors_.size() - 1; // index of factor
+	list<string> keys = factor->keys(); // get keys for factor
+	BOOST_FOREACH(string key, keys){    // for each key push i onto list
+		Indices::iterator it = indices_.find(key); // old list for that key (if exists)
+		if (it==indices_.end()){  // there's no list yet, so make one
+			list<int> indices(1, i);
+			indices_.insert(pair<string, list<int> >(key, indices));  // insert new indices into factorMap
+		}
+		else{
+			list<int> *indices_ptr;
+			indices_ptr = &(it->second);
+			indices_ptr->push_back(i);
+		}
+	}
+}
 
+
+/* ************************************************************************* */
 /**
  * Call colamd given a column-major symbolic matrix A
  * @param n_col colamd arg 1: number of rows in A

cpp/FactorGraph.h

@@ -37,6 +37,10 @@ namespace gtsam {
 		/** Collection of factors */
 		std::vector<shared_factor> factors_;
 
+		/** For each variable a list of factor indices connected to it  */
+		typedef std::map<std::string, std::list<int> > Indices;
+		Indices indices_;
+
 	public:
 
 		/** STL like, return the iterator pointing to the first factor */
@@ -65,9 +69,7 @@ namespace gtsam {
 		}
 
 		/** Add a factor */
-		void push_back(shared_factor ptr_f) {
-			factors_.push_back(ptr_f);
-		}
+		void push_back(shared_factor factor);
 
 		/** unnormalized error */
 		double error(const Config& c) const {

cpp/LinearFactorGraph.cpp

@@ -48,18 +48,14 @@ 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(1)                                                                     */
+/* ************************************************************************* */
+list<int> LinearFactorGraph::factors(const string& key) const {
+	Indices::const_iterator it = indices_.find(key);
+	return it->second;
 }
 
+
 /* ************************************************************************* */
 /** O(n)                                                                     */
 /* ************************************************************************* */

cpp/LinearFactorGraph.h

@@ -56,7 +56,7 @@ namespace gtsam {
      * 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);
+    std::list<int> factors(const std::string& key) const;
 
     /**
      * find all the factors that involve the given node and remove them

cpp/smallExample.cpp

@@ -23,6 +23,7 @@ using namespace std;
 #include "Simulated2DOdometry.h"
 #include "Simulated2DMeasurement.h"
 #include "simulated2D.h"
+#include "FactorGraph-inl.h"
 
 namespace gtsam {
 

cpp/testLinearFactorGraph.cpp

@@ -459,15 +459,15 @@ TEST( LinearFactorGraph, factor_lookup)
 	LinearFactorGraph fg = createLinearFactorGraph();
 
 	// ask for all factor indices connected to x1
-	vector<int> x1_factors = fg.factors("x1");
+	list<int> x1_factors = fg.factors("x1");
 	int x1_indices[] = { 0, 1, 2 };
-	vector<int> x1_expected(x1_indices, x1_indices + 3);
+	list<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");
+	list<int> x2_factors = fg.factors("x2");
 	int x2_indices[] = { 1, 3 };
-	vector<int> x2_expected(x2_indices, x2_indices + 2);
+	list<int> x2_expected(x2_indices, x2_indices + 2);
 	CHECK(x2_factors==x2_expected);
 }