From 91a0fb23df0a93d29b583396ba7dcf5be83684ee Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 16 Jan 2010 06:25:11 +0000 Subject: [PATCH] Weighted pseudo-inverse now takes weights (1/sigma^2). Does not make a lot of performance difference. --- cpp/Matrix.cpp | 7 ++- cpp/Vector.cpp | 95 ++++++++++++++++++--------------- cpp/Vector.h | 20 ++++--- cpp/testVector.cpp | 36 ++++++++----- cpp/timeGaussianFactorGraph.cpp | 5 +- 5 files changed, 96 insertions(+), 67 deletions(-) diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index 494a466f0..96a386e94 100644 --- a/cpp/Matrix.cpp +++ b/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 - // TODO return variance - double precision = weightedPseudoinverse(a, sigmas, pseudo); + double precision = weightedPseudoinverse(a, weights, 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 diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp index d5bd9a38f..843d00d45 100644 --- a/cpp/Vector.cpp +++ b/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())); @@ -227,53 +236,57 @@ 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) { - size_t m = sigmas.size(); + // Fast version *no error checking* ! + // Pass in initialized vector of size m or will crash ! + double weightedPseudoinverse(const Vector& a, const Vector& weights, + Vector& pseudo) { - // If there is a valid (a!=0) constraint (sigma==0) return the first one - for(int i=0; i 1e-9) { - pseudo=delta(m,i,1/a[i]); - return std::numeric_limits::infinity(); - } + size_t m = weights.size(); + static const double inf = std::numeric_limits::infinity(); - // 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 - Vector& precisions = pseudo; - for(int i = 0; i 1e-9) { + pseudo = delta(m, i, 1 / a[i]); + return inf; + } + + // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) + // For diagonal Sigma, inv(Sigma) = diag(precisions) + double precision = 0; + for (int i = 0; i < m; i++) { + 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 + pseudo[i] = 0; + else + pseudo[i] = variance * weights[i] * ai; + } + } + return precision; // sum of weights + } /* ************************************************************************* */ // Slow version with error checking - pair weightedPseudoinverse(const Vector& a, const Vector& sigmas) { - size_t m = sigmas.size(); + pair + 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 diff --git a/cpp/Vector.h b/cpp/Vector.h index 721906572..01a0cb353 100644 --- a/cpp/Vector.h +++ b/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 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 - * @return a pair of the pseudoinverse of v and the precision + * @param weights is a vector of weights/precisions where w=1/(s*s) + * @return a pair of the pseudoinverse of v and the associated precision/weight */ -std::pair weightedPseudoinverse(const Vector& v, const Vector& sigmas); +std::pair +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 diff --git a/cpp/testVector.cpp b/cpp/testVector.cpp index fb19c2966..67e5b7642 100644 --- a/cpp/testVector.cpp +++ b/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; - boost::tie(act, precision) = weightedPseudoinverse(x, sigmas); + Vector actual; double precision; + boost::tie(actual, precision) = weightedPseudoinverse(x, weights); // construct expected - Vector exp(2); - exp(0) = 0.5; exp(1) = 0.25; + Vector expected(2); + 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; - boost::tie(act, precision) = weightedPseudoinverse(x, sigmas); + Vector actual; double precision; + boost::tie(actual, precision) = weightedPseudoinverse(x, weights); // construct expected - Vector exp(2); - exp(0) = 1.0; exp(1) = 0.0; + Vector expected(2); + 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.); - CHECK(assert_equal(pseudo, exp)); + Vector expected = Vector_(4, 1., 0., 0.,0.); + 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); } /* ************************************************************************* */ diff --git a/cpp/timeGaussianFactorGraph.cpp b/cpp/timeGaussianFactorGraph.cpp index a783492ee..2c65d5508 100644 --- a/cpp/timeGaussianFactorGraph.cpp +++ b/cpp/timeGaussianFactorGraph.cpp @@ -51,8 +51,9 @@ TEST(timeGaussianFactorGraph, linearTime) /* ************************************************************************* */ TEST(timeGaussianFactorGraph, planar) { - // 1741: 8.12, 8.12, 8.12, 8.16, 8.14 - // 1742: 5.99, 5.97, 5.97, 6.02, 5.97 + // 1741: 8.12, 8.12, 8.12, 8.14, 8.16 + // 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);