Added weighted Householder transforms that use precisions perform QDR factorization. Functions create a weighted vector pseudoinverse, and then use the pseudoinverse to substitute a solution into system.

cpp/Matrix.cpp

@@ -269,6 +269,32 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
   }
 }
 
+/* ************************************************************************* */
+void whouse_subs(Matrix& A, unsigned int row, const Vector& pseudo, const Vector& x) {
+	// get sizes
+	size_t m = A.size1(); size_t n = A.size2();
+
+	// calculate Hw
+	Matrix Hw = eye(m,m);
+	for (int i=0; i<m; ++i)
+		for (int j=0; j<m; ++j)
+			Hw(i,j) -= x(i)*pseudo(j);
+
+	// zero the selected column below the diagonal
+	for (int i=row+1; i<m; ++i)
+		A(i,row) = 0.0;
+
+	// update As
+	//FIXME: this uselessly updates the top rows as well, and should be fixed
+	Matrix As = sub(A,0,m,row+1,n);
+	Matrix As_new = Hw*As;
+
+	// copy in updated As
+	for (int i=1; i<m; ++i) //index through rows of A
+		for (int j=0; j<As_new.size2(); ++j) //index through columns of As_new
+			A(i,j+row+1) = As_new(i,j);
+}
+
 /* ************************************************************************* */
 /** Imperative version of Householder QR factorization, Golub & Van Loan p 224
  * version with Householder vectors below diagonal, as in GVL

cpp/Matrix.h

@@ -137,6 +137,16 @@ std::pair<Matrix,Matrix> qr(const Matrix& A);
  */
 void householder_update(Matrix &A, int j, double beta, const Vector& vjm);
 
+/*
+ * Imperative version of weighted Householder substitution
+ * @param A is the matrix to reduce
+ * @param row is the row to start on (rows above aren't affected)
+ * @param pseudo is the pseudoinverse of the first column of A
+ * @param x is the first column of A
+ * A is updated into non-normalied R of A that has been updated
+ */
+void whouse_subs(Matrix& A, unsigned int row, const Vector& pseudo, const Vector& x);
+
 /**
  * Householder tranformation, Householder vectors below diagonal
  * @param k number of columns to zero out below diagonal

cpp/Vector.cpp

@@ -91,6 +91,12 @@ namespace gtsam {
     return v;
   }
   
+  /* ************************************************************************* */
+  Vector ones(size_t n) {
+    Vector v(n); fill_n(v.begin(),n,1.0);
+    return v;
+  }
+
   /* ************************************************************************* */
   void print(const Vector& v, const string& s) {
     size_t n = v.size();
@@ -178,6 +184,24 @@ namespace gtsam {
     return make_pair(beta, v);
   }
   
+  /* ************************************************************************* */
+  Vector whouse_solve(const Vector& v, const Vector& precisions) {
+	  if (v.size() != precisions.size())
+		  throw invalid_argument("V and precisions have different sizes!");
+	  // get the square root of the precisions
+	  //FIXME: this probably means that storing precisions is a bad idea
+	  Vector D(precisions.size());
+	  for(int i=0;i<D.size();i++)
+		  D(i)=sqrt(precisions[i]);
+	  double normV = 0;
+	  for(int i = 0; i<v.size(); i++)
+		  normV += v[i]*v[i]*D[i];
+	  Vector sol(v.size());
+	  for(int i = 0; i<v.size(); i++)
+		  sol[i] = D[i]*v[i];
+	  return sol/normV;
+  }
+
   /* ************************************************************************* */
   Vector concatVectors(size_t nrVectors, ...)
   {

cpp/Vector.h

@@ -40,6 +40,12 @@ Vector Vector_(size_t m, ...);
  */
 Vector zero(size_t n);
 
+/**
+ * Create vector initialized to ones
+ * @ param size
+ */
+Vector ones(size_t n);
+	
 /**
  * check if all zero
  */
@@ -97,6 +103,15 @@ Vector sub(const Vector &v, size_t i1, size_t i2);
  */
 std::pair<double,Vector> house(Vector &x);
 
+/**
+ * Weighted Householder solution vector,
+ * a.k.a., the pseudoinverse of the column
+ * @param v is the first column of the matrix to solve
+ * @param precisions is a vector of precisions ( sigma^(-2) )
+ * @return the pseudoinverse of v
+ */
+Vector whouse_solve(const Vector& v, const Vector& precisions);
+
 /**
  * concatenate Vectors
  */

cpp/testMatrix.cpp

@@ -438,7 +438,6 @@ TEST( matrix, row_major_access )
 }
 
 /* ************************************************************************* */
-
 TEST( matrix, svd )
 { 
   double data[] = {2,1,0};
@@ -452,6 +451,38 @@ TEST( matrix, svd )
   EQUALITY(S,S1);
 }
 
+/* ************************************************************************* */
+TEST( matrix, whouse_subs )
+{
+	// create the system
+	Matrix A(2,2);
+	A(0,0) = 1; A(0,1) = 3;
+	A(1,0) = 2; A(1,1) = 4;
+
+	// Vector to eliminate
+	Vector x(2);
+	x(0) = 1.0; x(1) = 2.0;
+
+	Vector tau(2); //correspond to sigmas = [0.1 0.2]
+	tau(0) = 100; tau(1) = 25;
+
+	// find the pseudoinverse
+	Vector pseudo = whouse_solve(x, tau);
+
+	// substitute
+	int row = 0; // eliminating the first column
+	whouse_subs(A, row, pseudo, x);
+
+	// create expected value
+	Matrix exp(2,2);
+	exp(0,0) = 1.0; exp(0,1) = 3.0;
+	exp(1,0) = 0.0; exp(1,1) = -2.0/3.0;
+
+	// verify
+	CHECK(assert_equal(A, exp));
+}
+
+
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

cpp/testVector.cpp

@@ -126,6 +126,28 @@ TEST( TestVector, concatVectors)
   CHECK(AB == C);
 }
 
+/* ************************************************************************* */
+TEST( TestVector, whouse_solve )
+{
+	// column from a matrix
+	Vector x(2);
+	x(0) = 1.0; x(1) = 2.0;
+
+	// create precisions - correspond to sigmas = [0.1 0.2]
+	Vector tau(2);
+	tau(0) = 100; tau(1) = 25;
+
+	// perform solve
+	Vector act = whouse_solve(x, tau);
+
+	// construct expected
+	Vector exp(2);
+	exp(0) = 1.0/3.0; exp(1) = 1.0/3.0;
+
+	// verify
+	CHECK(assert_equal(act, exp)); 
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */