12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

release/4.3a0
Frank Dellaert 2010-01-16 06:25:11 +00:00
parent fce2a668bb
commit 91a0fb23df
5 changed files with 96 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cpp/Matrix.cpp
View File

@ -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

95
cpp/Vector.cpp
View File

@ -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<m; ++i)
if (sigmas[i] < 1e-9 && fabs(a[i]) > 1e-9) {
pseudo=delta(m,i,1/a[i]);
return std::numeric_limits<double>::infinity();
}
size_t m = weights.size();
static const double inf = std::numeric_limits<double>::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<m; i++) {
double ai=a[i];
if (fabs(ai) < 1e-9) // also catches remaining sigma==0 rows
precisions[i] = 0.;
else {
double si=sigmas[i],pi = 1./(si*si);
precision += ai*ai*pi;
precisions[i] = pi;
}
}
// 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 f = 1.0/precision;
for(int i = 0; i<m; i++)
pseudo[i]=f*precisions[i]*a[i];
}
return precision;
}
// If there is a valid (a!=0) constraint (sigma==0) return the first one
for (int i = 0; i < m; ++i)
if (weights[i] == inf && fabs(a[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<Vector, double> weightedPseudoinverse(const Vector& a, const Vector& sigmas) {
size_t m = sigmas.size();
pair<Vector, double>
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

20
cpp/Vector.h
View File

@ -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
* @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<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

36
cpp/testVector.cpp
View File

@ -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); }
/* ************************************************************************* */

5
cpp/timeGaussianFactorGraph.cpp
View File

@ -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);