changes to append_factor which give more than 2x speedup on my data. Matrices in As are no longer copied/edited/replaced each time they need to be changed, but created once and then updated in place.

cpp/GaussianFactor.cpp

@@ -252,27 +252,24 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, const size_t m,
 	// iterate over all matrices from the factor f
 	GaussianFactor::const_iterator it = f->begin();
 	for (; it != f->end(); it++) {
-		string j = it->first;
+		string key = it->first;
-		Matrix A = it->second;
 
 		// find rows and columns
-		const size_t mrhs = A.size1(), n = A.size2();
+		const size_t mrhs = it->second.size1(), n = it->second.size2();
 
 		// find the corresponding matrix among As
-		const_iterator mine = As_.find(j);
+		iterator mine = As_.find(key);
 		const bool exists = mine != As_.end();
 
 		// create the matrix or use existing
-		Matrix Anew = exists ? mine->second : zeros(m, n);
+		if(exists) {
-
+		  copy(it->second.data().begin(), it->second.data().end(), (mine->second).data().begin()+pos*n);
-		// copy the values in the existing matrix
+		}
-		for (size_t i = 0; i < mrhs; i++)
+		else {
-			for (size_t j = 0; j < n; j++)
+			Matrix Z = zeros(m, n);
-				Anew(pos + i, j) = A(i, j);
+			copy(it->second.data().begin(), it->second.data().end(), Z.data().begin()+pos*n);
-
+			insert(key, Z);
-		// insert the matrix into the factor
+		}
-		if (exists) As_.erase(j);
-		insert(j, Anew);
 	}
 	if (verbose) cout << "GaussianFactor::append_factor done" << endl;
 

cpp/GaussianFactor.h

@@ -241,7 +241,7 @@ public:
 	 * @param m final number of rows of f, needs to be known in advance
 	 * @param pos where to insert in the m-sized matrices
 	 */
-	void append_factor(GaussianFactor::shared_ptr f, const size_t m,
+	inline void append_factor(GaussianFactor::shared_ptr f, const size_t m,
 			const size_t pos);
 
 }; // GaussianFactor