Small performance improvement for matrix_augmented, with accompanying test in timeGaussianFactor

cpp/GaussianFactor.cpp

@@ -224,9 +224,9 @@ Matrix GaussianFactor::matrix_augmented(const Ordering& ordering, bool weight) c
 	}
 
 	// load b into a matrix
-	Matrix B_mat(numberOfRows(), 1);
-	for (int i=0; i<b_.size(); ++i)
-		B_mat(i,0) = b_(i);
+	size_t rows = b_.size();
+	Matrix B_mat(rows, 1);
+	memcpy(B_mat.data().begin(), b_.data().begin(), rows*sizeof(double));
 	matrices.push_back(&B_mat);
 
 	// divide in sigma so error is indeed 0.5*|Ax-b|

cpp/timeGaussianFactor.cpp

@@ -13,11 +13,28 @@
 using namespace std;
 
 #include <boost/tuple/tuple.hpp>
+#include <boost/assign/std/list.hpp> // for operator += in Ordering
+
 #include "Matrix.h"
 #include "GaussianFactor.h"
 #include "GaussianConditional.h"
+#include "Ordering.h"
 
 using namespace gtsam;
+using namespace boost::assign;
+
+/*
+ * Alex's Machine
+ * Results for Eliminate:
+ * Initial (1891): 17.91 sec, 55834.7 calls/sec
+ *
+ * Results for matrix_augmented:
+ * Initial (1891)       :  0.85 sec, 1.17647e+06 calls/sec
+ * int->size_t Version  :  8.45 sec (for n1 reps) with memcpy version of collect()
+ * w/ original collect():  8.73 sec (for n1 reps)
+ * b memcpy Version     :  8.64 sec (for n1 reps) with original version of collect()
+ * w/ memcpy collect()  :  8.40 sec (for n1 reps)
+ */
 
 int main()
 {
@@ -69,6 +86,7 @@ int main()
   b2(6) = 2;
   b2(7) = -1;
   
+  // time eliminate
   GaussianFactor combined("x2", Ax2,  "l1", Al1, "x1", Ax1, b2,1);
   long timeLog = clock();
   int n = 1000000;
@@ -80,7 +98,24 @@ int main()
 
   long timeLog2 = clock();
   double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+  cout << "Single Eliminate Timing:" << endl;
   cout << seconds << " seconds" << endl;
   cout << ((double)n/seconds) << " calls/second" << endl;
+
+  // time matrix_augmented
+  Ordering ordering;
+  ordering += "x2", "l1", "x1";
+  size_t n1 = 10000000;
+  timeLog = clock();
+
+  for(int i = 0; i < n1; i++)
+	  Matrix Ab = combined.matrix_augmented(ordering, true);
+
+  timeLog2 = clock();
+  seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+  cout << "matrix_augmented Timing:" << endl;
+  cout << seconds << " seconds" << endl;
+  cout << ((double)n/seconds) << " calls/second" << endl;
+
   return 0;
 }