added SubVector type and some related routines

cpp/Matrix.cpp

@@ -186,6 +186,19 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector
 #endif
 }
 
+/* ************************************************************************* */
+void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector& x) {
+	// ublas x += prod(trans(A),e) is terribly slow
+	// TODO: use BLAS
+  size_t m = A.size1(), n = A.size2();
+	for (int j = 0; j < n; j++) {
+		const double * ei = e.data().begin();
+		const double * aij = A.data().begin() + j;
+		for (int i = 0; i < m; i++, aij+=n, ei++)
+			x(j) += alpha * (*aij) * (*ei);
+	}
+}
+
 /* ************************************************************************* */
 Vector Vector_(const Matrix& A)
 {

cpp/Matrix.h

@@ -102,6 +102,11 @@ Vector operator^(const Matrix& A, const Vector & v);
  */
 void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x);
 
+/**
+ * BLAS Level-2 style x <- x + alpha*A'*e
+ */
+void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector& x);
+
 /**
  * overload * for vector*matrix multiplication (as BOOST does not)
  */

cpp/Vector.cpp

@@ -262,6 +262,14 @@ namespace gtsam {
   		y[i] += alpha * x[i];
   }
 
+  /* ************************************************************************* */
+  void axpy(double alpha, const Vector& x, SubVector& y) {
+  	size_t n = x.size();
+		assert (y.size()==n);
+  	for (size_t i = 0; i < n; i++)
+  		y[i] += alpha * x[i];
+  }
+
   /* ************************************************************************* */
   Vector operator/(double s, const Vector& v) {
     Vector result(v.size());

cpp/Vector.h

@@ -11,6 +11,7 @@
 
 #include <list>
 #include <boost/numeric/ublas/vector.hpp>
+#include <boost/numeric/ublas/vector_proxy.hpp>
 #include <boost/random/linear_congruential.hpp>
 
 // Vector is a *global* typedef
@@ -18,6 +19,7 @@
 #if ! defined (MEX_H)
 typedef boost::numeric::ublas::vector<double> Vector;
 #endif
+typedef boost::numeric::ublas::vector_range<Vector> SubVector;
 
 namespace gtsam {
 
@@ -218,6 +220,7 @@ void scal(double alpha, Vector& x);
  * BLAS Level 1 axpy: y <- alpha*x + y
  */
 void axpy(double alpha, const Vector& x, Vector& y);
+void axpy(double alpha, const Vector& x, SubVector& y);
 
 /**
  * Divide every element of a Vector into a scalar