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

Fixed bug in revealing rank, even simpler than before.

release/4.3a0
Frank Dellaert 2009-11-13 06:16:56 +00:00
parent 7c045a0802
commit f5fc14c0f4
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
cpp/Matrix.cpp
View File

@ -301,45 +301,46 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
list<boost::tuple<Vector, double, double> > list<boost::tuple<Vector, double, double> >
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
bool verbose = false; bool verbose = false;
// get sizes size_t m = A.size1(), n = A.size2(); // get size(A)
size_t m = A.size1();
size_t n = A.size2();
size_t maxRank = min(m,n); size_t maxRank = min(m,n);
// create list // create list
list<boost::tuple<Vector, double, double> > results; list<boost::tuple<Vector, double, double> > results;
// loop over the columns // We loop over all columns, because the columns that can be eliminated
for (int j=0; j<maxRank; ++j) { // are not necessarily contiguous. For each one, estimate the corresponding
// scalar variable x as d-rS, with S the separator (remaining columns).
// Then update A and b by substituting x with d-rS, zero-ing out x's column.
for (int j=0; j<n; ++j) {
// extract the first column of A // extract the first column of A
Vector a = column(A, j); Vector a = column(A, j);
if (verbose) print(a,"a"); if (verbose) print(a,"a = ");
// find weighted pseudoinverse // Calculate weighted pseudo-inverse and corresponding precision
Vector pseudo; double precision; Vector pseudo; double precision;
if (verbose) print(sigmas, "sigmas");
boost::tie(pseudo, precision) = weightedPseudoinverse(a, sigmas); boost::tie(pseudo, precision) = weightedPseudoinverse(a, sigmas);
if (verbose) cout << "precision = " << precision << endl;
// if precision is zero, rank(A) was less than maxRank, so return // if precision is zero, no information on this column
if (precision < 1e-8) { if (precision < 1e-8) continue;
maxRank = j; if (verbose) print(pseudo, "pseudo = ");
return results;
}
if (verbose) print(pseudo, "pseudo");
// create solution and copy into r // create solution and copy into r
Vector r = basis(n, j); Vector r = basis(n, j);
for (int j2=j+1; j2<n; ++j2) for (int j2=j+1; j2<n; ++j2)
r(j2) = inner_prod(pseudo, column(A, j2)); r(j2) = inner_prod(pseudo, column(A, j2));
if (verbose) print(r, "r = ");
// create the rhs // create the rhs
double d = inner_prod(pseudo, b); double d = inner_prod(pseudo, b);
if (verbose) print(r, "updatedR");
if (verbose) cout << "d = " << d << endl; if (verbose) cout << "d = " << d << endl;
// construct solution (r, d, sigma) // construct solution (r, d, sigma)
results.push_back(boost::make_tuple(r, d, 1./sqrt(precision))); results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
// exit after rank exhausted
if (results.size()>=maxRank) break;
// update A, b // update A, b
for (int i=0;i<m;++i) { // update all rows for (int i=0;i<m;++i) { // update all rows
double ai = a(i); double ai = a(i);
@ -347,9 +348,8 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
for (int j2=j+1;j2<n;++j2) // limit to only columns in separator for (int j2=j+1;j2<n;++j2) // limit to only columns in separator
A(i,j2) -= ai*r(j2); A(i,j2) -= ai*r(j2);
} }
if (verbose) print(A, "updatedA"); if (verbose) print(sub(A,0,m,j+1,n), "updated A");
//if (verbose) print(sub(A, 0,m, j+1,n), "updatedA"); // bad index if (verbose) print(b, "updated b ");
} }
return results; return results;