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

Added a debug printing switch in Cholesky

release/4.3a0
Richard Roberts 2010-11-19 16:27:08 +00:00
parent 1193d2f9be
commit 0fdc384089
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/base/cholesky.cpp
View File

@ -53,6 +53,8 @@ void cholesky_inplace(MatrixColMajor& I) {
/* ************************************************************************* */
size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab) {
bool debug = false;
size_t m = Ab.size1();
size_t n = Ab.size2();
@ -60,7 +62,7 @@ size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab) {
// factorization of A'A. If m < n, A'A is rank-deficient this function
// instead computes the upper-trapazoidal factor [ R S ], as described in the
// header file comment.
size_t rank = std::min(m,n);
size_t rank = std::min(m,n-1);
if(rank > 0) {
@ -68,6 +70,11 @@ size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab) {
ublas::matrix_range<MatrixColMajor> F(ublas::project(Ab, ublas::range(0,m), ublas::range(0,rank)));
ublas::matrix_range<MatrixColMajor> G(ublas::project(Ab, ublas::range(0,m), ublas::range(rank,n)));
if(debug) {
print(F, "F: ");
print(G, "G: ");
}
ublas::matrix_range<MatrixColMajor> R(ublas::project(Ab, ublas::range(0,rank), ublas::range(0,rank)));
ublas::matrix_range<MatrixColMajor> S(ublas::project(Ab, ublas::range(0,rank), ublas::range(rank,n)));
@ -86,13 +93,18 @@ size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab) {
"Bad input to choleskyFactorUnderdetermined, dpotrf returned %d.\n")%info));
else
throw std::domain_error(boost::str(boost::format(
"The matrix passed into choleskyFactorUnderdetermined is numerically rank-deficient, dpotrf returned rank=%d, expected rank was %d.\n")%info%rank));
"The matrix passed into choleskyFactorUnderdetermined is numerically rank-deficient, dpotrf returned rank=%d, expected rank was %d.\n")%(info-1)%rank));
}
// Compute S = inv(R') * F' * G, i.e. solve S when R'S = F'G
if(S.size2() > 0)
cblas_dtrsm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, S.size1(), S.size2(), 1.0, &R(0,0), m, &S(0,0), m);
if(debug) {
print(R, "R: ");
print(S, "S: ");
}
return rank;
} else
return 0;