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

Option to use careful Cholesky instead of lapack Cholesky, more debug checks in Cholesky, and addional use of ublas instead of regular blas.

release/4.3a0
Richard Roberts 2011-02-04 01:05:52 +00:00
parent 24fbe9b02b
commit f89262bd13
1 changed files with 65 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
gtsam/base/cholesky.cpp
View File

@ -212,31 +212,64 @@ void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal) {
const size_t n = ABC.size1(); const size_t n = ABC.size1();
// Compute Cholesky factorization of A, overwrites A. // Compute Cholesky factorization of A, overwrites A.
tic(1, "dpotrf"); if(true) {
int info = lapack_dpotrf('U', nFrontal, &ABC(0,0), n); tic(1, "dpotrf");
if(info != 0) { int info = lapack_dpotrf('U', nFrontal, &ABC(0,0), n);
if(info < 0) if(info != 0) {
throw std::domain_error(boost::str(boost::format( if(info < 0)
"Bad input to choleskyFactorUnderdetermined, dpotrf returned %d.\n")%info)); throw std::domain_error(boost::str(boost::format(
else "Bad input to choleskyPartial, dpotrf returned %d.\n")%info));
throw std::domain_error(boost::str(boost::format( else
"The matrix passed into choleskyFactorUnderdetermined is numerically rank-deficient, dpotrf returned rank=%d, expected rank was %d.\n")%(info-1)%nFrontal)); throw std::domain_error(boost::str(boost::format(
"The matrix passed into choleskyPartial is numerically rank-deficient, dpotrf returned rank=%d, expected rank was %d.\n")%(info-1)%nFrontal));
}
toc(1, "dpotrf");
} else {
bool fullRank = choleskyCareful(ABC, nFrontal).second;
if(!fullRank)
throw invalid_argument("Rank-deficient");
} }
toc(1, "dpotrf"); toc(1, "dpotrf");
#ifndef NDEBUG
// Check for non-finite values
for(size_t i=0; i<ABC.size1(); ++i)
for(size_t j=0; j<ABC.size2(); ++j)
if(!isfinite(ABC(i,j))) {
cout << nFrontal << " frontal variables" << endl;
print(ABC, "Partially-factored matrix: ");
throw invalid_argument("After dpotrf, matrix contains non-finite matrix entries.");
}
#endif
// Views of R, S, and L submatrices. // Views of R, S, and L submatrices.
ublas::matrix_range<MatrixColMajor> R(ublas::project(ABC, ublas::range(0,nFrontal), ublas::range(0,nFrontal))); ublas::matrix_range<MatrixColMajor> Rf(ublas::project(ABC, ublas::range(0,nFrontal), ublas::range(0,nFrontal)));
ublas::triangular_adaptor<ublas::matrix_range<MatrixColMajor>, ublas::upper> R(Rf);
ublas::matrix_range<MatrixColMajor> S(ublas::project(ABC, ublas::range(0,nFrontal), ublas::range(nFrontal,n))); ublas::matrix_range<MatrixColMajor> S(ublas::project(ABC, ublas::range(0,nFrontal), ublas::range(nFrontal,n)));
ublas::matrix_range<MatrixColMajor> Lf(ublas::project(ABC, ublas::range(nFrontal,n), ublas::range(nFrontal,n))); ublas::matrix_range<MatrixColMajor> Lf(ublas::project(ABC, ublas::range(nFrontal,n), ublas::range(nFrontal,n)));
ublas::symmetric_adaptor<typeof(Lf), ublas::upper> L(Lf); ublas::symmetric_adaptor<typeof(Lf), ublas::upper> L(Lf);
// Compute S = inv(R') * B // Compute S = inv(R') * B
tic(2, "compute S"); tic(2, "compute S");
if(S.size2() > 0) if(S.size2() > 0) {
cblas_dtrsm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, S.size1(), S.size2(), 1.0, &R(0,0), n, &S(0,0), n); typeof(ublas::trans(R)) RT(ublas::trans(R));
ublas::inplace_solve(RT, S, ublas::lower_tag());
//cblas_dtrsm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, S.size1(), S.size2(), 1.0, &R(0,0), n, &S(0,0), n);
}
if(debug) gtsam::print(S, "S: "); if(debug) gtsam::print(S, "S: ");
toc(2, "compute S"); toc(2, "compute S");
#ifndef NDEBUG
// Check for non-finite values
for(size_t i=0; i<ABC.size1(); ++i)
for(size_t j=0; j<ABC.size2(); ++j)
if(!isfinite(ABC(i,j))) {
cout << nFrontal << " frontal variables" << endl;
print(ABC, "Partially-factored matrix: ");
throw invalid_argument("After computing S, matrix contains non-finite matrix entries.");
}
#endif
// Compute L = C - S' * S // Compute L = C - S' * S
tic(3, "compute L"); tic(3, "compute L");
if(debug) gtsam::print(L, "C: "); if(debug) gtsam::print(L, "C: ");
@ -244,6 +277,26 @@ void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal) {
L -= ublas::prod(ublas::trans(S), S); L -= ublas::prod(ublas::trans(S), S);
if(debug) gtsam::print(L, "L: "); if(debug) gtsam::print(L, "L: ");
toc(3, "compute L"); toc(3, "compute L");
#ifndef NDEBUG
// Check for positive semi-definiteness of L
try {
MatrixColMajor Lf(L);
choleskyCareful(Lf);
} catch(const invalid_argument& e) {
cout << "Remaining Hessian L is not positive semi-definite: " << e.what() << endl;
throw runtime_error("Remaining Hessian L is not positive semi-definite");
}
// Check for non-finite values
for(size_t i=0; i<ABC.size1(); ++i)
for(size_t j=0; j<ABC.size2(); ++j)
if(!isfinite(ABC(i,j))) {
cout << nFrontal << " frontal variables" << endl;
print(ABC, "Partially-factored matrix: ");
throw invalid_argument("After Cholesky, matrix contains non-finite matrix entries.");
}
#endif
} }
} }