Weighted pseudo-inverse now takes weights (1/sigma^2). Does not make a lot of performance difference.

cpp/Matrix.cpp

@@ -328,7 +328,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
 	Vector pseudo(m); // allocate storage for pseudo-inverse
 
 	// TODO: calculate weights once
-	// Vector weights =
+	Vector weights = reciprocal(emul(sigmas,sigmas));
 
 	// We loop over all columns, because the columns that can be eliminated
 	// are not necessarily contiguous. For each one, estimate the corresponding
@@ -341,9 +341,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
 		Vector a(column(A, j));
 
 		// Calculate weighted pseudo-inverse and corresponding precision
-		// TODO: pass in weights which are calculated once
+		double precision = weightedPseudoinverse(a, weights, pseudo);
-		// TODO return variance
-		double precision = weightedPseudoinverse(a, sigmas, pseudo);
 
 		// if precision is zero, no information on this column
 		if (precision < 1e-8) continue;
@@ -357,6 +355,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
 		double d = inner_prod(pseudo, b);
 
 		// construct solution (r, d, sigma)
+		// TODO: avoid sqrt, store precision or at least variance
 		results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
 
 		// exit after rank exhausted

cpp/Vector.cpp

@@ -195,6 +195,15 @@ namespace gtsam {
 		return result;
 		}
 
+  /* ************************************************************************* */
+  Vector reciprocal(const Vector &a) {
+  	size_t n = a.size();
+  	Vector b(n);
+  	for( size_t i = 0; i < n; i++ )
+  		b(i) = 1.0/a(i);
+  	return b;
+  	}
+
   /* ************************************************************************* */
   double max(const Vector &a) {
   	return *(std::max_element(a.begin(), a.end()));
@@ -229,51 +238,55 @@ namespace gtsam {
   /* ************************************************************************* */
 	// Fast version *no error checking* !
 	// Pass in initialized vector of size m or will crash !
-  double weightedPseudoinverse(const Vector& a, const Vector& sigmas, Vector& pseudo) {
+	double weightedPseudoinverse(const Vector& a, const Vector& weights,
-	  size_t m = sigmas.size();
+			Vector& pseudo) {
+
+		size_t m = weights.size();
+		static const double inf = std::numeric_limits<double>::infinity();
 
 		// If there is a valid (a!=0) constraint (sigma==0) return the first one
 		for (int i = 0; i < m; ++i)
-		  if (sigmas[i] < 1e-9 && fabs(a[i]) > 1e-9) {
+			if (weights[i] == inf && fabs(a[i]) > 1e-9) {
 				pseudo = delta(m, i, 1 / a[i]);
-			  return std::numeric_limits<double>::infinity();
+				return inf;
 			}
 
 		// Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
 		// For diagonal Sigma, inv(Sigma) = diag(precisions)
 		double precision = 0;
-	  // pseudo will be used to store both precisions (an intermediate) and result
+		for (int i = 0; i < m; i++) {
-	  Vector& precisions = pseudo;
+			double ai = a[i];
+			if (fabs(ai) > 1e-9) // also catches remaining sigma==0 rows
+			precision += weights[i] * (ai * ai);
+		}
+
+		// precision = a'inv(Sigma)a
+		if (precision < 1e-9)
+			for (int i = 0; i < m; i++)
+				pseudo[i] = 0;
+		else {
+			// emul(precisions,a)/precision
+			double variance = 1.0 / precision;
 			for (int i = 0; i < m; i++) {
 				double ai = a[i];
 				if (fabs(ai) < 1e-9) // also catches remaining sigma==0 rows
-			  precisions[i] = 0.;
+					pseudo[i] = 0;
-		  else {
+				else
-		  	double si=sigmas[i],pi = 1./(si*si);
+					pseudo[i] = variance * weights[i] * ai;
-			  precision += ai*ai*pi;
-			  precisions[i] = pi;
 			}
 		}
-	  // precision = a'inv(Sigma)a
+		return precision; // sum of weights
-	  if (precision<1e-9)
-	  	for(int i = 0; i<m; i++) pseudo[i]=0;
-	  else {
-	  	// emul(precisions,a)/precision
-	  	double f = 1.0/precision;
-	  	for(int i = 0; i<m; i++)
-	  		pseudo[i]=f*precisions[i]*a[i];
-	  }
-	  return precision;
 	}
 
   /* ************************************************************************* */
   // Slow version with error checking
-  pair<Vector, double> weightedPseudoinverse(const Vector& a, const Vector& sigmas) {
+  pair<Vector, double>
-	  size_t m = sigmas.size();
+  weightedPseudoinverse(const Vector& a, const Vector& weights) {
+	  size_t m = weights.size();
 	  if (a.size() != m)
-		  throw invalid_argument("V and precisions have different sizes!");
+		  throw invalid_argument("a and weights have different sizes!");
 	  Vector pseudo(m);
-  	double precision = weightedPseudoinverse(a, sigmas, pseudo);
+  	double precision = weightedPseudoinverse(a, weights, pseudo);
 	  return make_pair(pseudo, precision);
   }
 
@@ -325,6 +338,4 @@ namespace gtsam {
   
   /* ************************************************************************* */
   
-  
-  
 } // namespace gtsam

cpp/Vector.h

@@ -170,6 +170,13 @@ Vector ediv_(const Vector &a, const Vector &b);
  */
 double sum(const Vector &a);
 
+/**
+ * elementwise reciprocal of vector elements
+ * @param a vector
+ * @return [1/a(i)]
+ */
+Vector reciprocal(const Vector &a);
+
 /**
  * return the max element of a vector
  * @param a vector
@@ -192,19 +199,20 @@ std::pair<double,Vector> house(Vector &x);
  * a.k.a., the pseudoinverse of the column
  * NOTE: if any sigmas are zero (indicating a constraint)
  * the pseudoinverse will be a selection vector, and the
- * precision will be infinite
+ * variance will be zero
  * @param v is the first column of the matrix to solve
- * @param simgas is a vector of standard deviations
+ * @param weights is a vector of weights/precisions where w=1/(s*s)
- * @return a pair of the pseudoinverse of v and the precision
+ * @return a pair of the pseudoinverse of v and the associated precision/weight
  */
-std::pair<Vector, double> weightedPseudoinverse(const Vector& v, const Vector& sigmas);
+std::pair<Vector, double>
+weightedPseudoinverse(const Vector& v, const Vector& weights);
 
 /*
  * Fast version *no error checking* !
- * Pass in initialized vector pseudo of size(sigma) or will crash !
+ * Pass in initialized vector pseudo of size(weights) or will crash !
  * @return the precision, pseudoinverse in third argument
  */
-double weightedPseudoinverse(const Vector& a, const Vector& sigmas, Vector& pseudo);
+double weightedPseudoinverse(const Vector& a, const Vector& weights, Vector& pseudo);
 
 /**
  * concatenate Vectors

cpp/testVector.cpp

@@ -141,18 +141,19 @@ TEST( TestVector, weightedPseudoinverse )
 	// create sigmas
 	Vector sigmas(2);
 	sigmas(0) = 0.1; sigmas(1) = 0.2;
+	Vector weights = reciprocal(emul(sigmas,sigmas));
 
 	// perform solve
-	Vector act; double precision;
+	Vector actual; double precision;
-	boost::tie(act, precision) = weightedPseudoinverse(x, sigmas);
+	boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
 
 	// construct expected
-	Vector exp(2);
+	Vector expected(2);
-	exp(0) = 0.5; exp(1) = 0.25;
+	expected(0) = 0.5; expected(1) = 0.25;
 	double expPrecision = 200.0;
 
 	// verify
-	CHECK(assert_equal(act, exp));
+	CHECK(assert_equal(expected,actual));
 	CHECK(fabs(expPrecision-precision) < 1e-5);
 }
 
@@ -166,17 +167,18 @@ TEST( TestVector, weightedPseudoinverse_constraint )
 	// create sigmas
 	Vector sigmas(2);
 	sigmas(0) = 0.0; sigmas(1) = 0.2;
+	Vector weights = reciprocal(emul(sigmas,sigmas));
 
 	// perform solve
-	Vector act; double precision;
+	Vector actual; double precision;
-	boost::tie(act, precision) = weightedPseudoinverse(x, sigmas);
+	boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
 
 	// construct expected
-	Vector exp(2);
+	Vector expected(2);
-	exp(0) = 1.0; exp(1) = 0.0;
+	expected(0) = 1.0; expected(1) = 0.0;
 
 	// verify
-	CHECK(assert_equal(act, exp));
+	CHECK(assert_equal(expected,actual));
 	CHECK(isinf(precision));
 }
 
@@ -185,11 +187,12 @@ TEST( TestVector, weightedPseudoinverse_nan )
 {
 	Vector a = Vector_(4, 1., 0., 0., 0.);
 	Vector sigmas = Vector_(4, 0.1, 0.1, 0., 0.);
+	Vector weights = reciprocal(emul(sigmas,sigmas));
 	Vector pseudo; double precision;
-	boost::tie(pseudo, precision) = weightedPseudoinverse(a, sigmas);
+	boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
 
-	Vector exp = Vector_(4, 1., 0., 0.,0.);
+	Vector expected = Vector_(4, 1., 0., 0.,0.);
-	CHECK(assert_equal(pseudo, exp));
+	CHECK(assert_equal(expected, pseudo));
 	DOUBLES_EQUAL(100, precision, 1e-5);
 }
 
@@ -223,6 +226,13 @@ TEST( TestVector, greater_than )
 	CHECK(greaterThanOrEqual(v1, v2)); // test equals
 }
 
+/* ************************************************************************* */
+TEST( TestVector, reciprocal )
+{
+	Vector v = Vector_(3, 1.0, 2.0, 4.0);
+  CHECK(assert_equal(Vector_(3, 1.0, 0.5, 0.25),reciprocal(v)));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

cpp/timeGaussianFactorGraph.cpp

@@ -51,8 +51,9 @@ TEST(timeGaussianFactorGraph, linearTime)
 /* ************************************************************************* */
 TEST(timeGaussianFactorGraph, planar)
 {
-	// 1741: 8.12, 8.12, 8.12, 8.16, 8.14
+	// 1741: 8.12, 8.12, 8.12, 8.14, 8.16
-	// 1742: 5.99, 5.97, 5.97, 6.02, 5.97
+	// 1742: 5.97, 5.97, 5.97, 5.99, 6.02
+	// 1746: 5.96, 5.96, 5.97, 6.00, 6.04
 	int N = 30;
 	double time = timePlanarSmoother(N); cout << time << endl;
 	DOUBLES_EQUAL(5.97,time,0.1);