From 09f25edcbb1ba6a733e27a5b06bd9188b8e1a6d7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 15 Feb 2011 15:21:09 +0000 Subject: [PATCH] Fixed warnings, comments, and removed redundant debug code in Cholesky --- gtsam/base/cholesky.cpp | 103 ++++++---------------------------------- 1 file changed, 14 insertions(+), 89 deletions(-) diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 8c73924b0..d19ceb599 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -126,9 +126,10 @@ size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab, size_t nFrontal) { /* ************************************************************************* */ static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) { - const size_t n = ATA.size1(); - + // Get pivot value double alpha = ATA(k,k); + + // Correct negative pivots from round-off error if(alpha < negativePivotThreshold) { cout << "pivot = " << alpha << endl; print(ATA, "Partially-factorized matrix: "); @@ -158,6 +159,7 @@ static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) { return true; } else { + // For zero pivots, add the underconstrained variable prior ATA(k,k) = underconstrainedPrior; for(size_t j=k+1; j choleskyCareful(MatrixColMajor& ATA, int order) { const bool debug = ISDEBUG("choleskyCareful"); -// // Tolerance for being equal to zero -//// static const double zeroTol = numeric_limits::epsilon(); -// static const double zeroTol = 1.e-15; - // Check that the matrix is square (we do not check for symmetry) assert(ATA.size1() == ATA.size2()); // Number of rows/columns const size_t n = ATA.size1(); + // Negative order means factor the entire matrix if(order < 0) order = n; - assert(order <= n); + assert(size_t(order) <= n); // The index of the row after the last non-zero row of the square-root factor size_t maxrank = 0; bool fullRank = true; - for(size_t k = 0; k < order; ++k) { - - if(choleskyStep(ATA, k, order)) { + // Factor row-by-row + for(size_t k = 0; k < size_t(order); ++k) { + if(choleskyStep(ATA, k, size_t(order))) { if(debug) cout << "choleskyCareful: Factored through " << k << endl; if(debug) print(ATA, "ATA: "); maxrank = k+1; @@ -212,54 +211,15 @@ void choleskyPartial(MatrixColMajor& ABCublas, size_t nFrontal) { Eigen::Map ABC(&ABCublas(0,0), ABCublas.size1(), ABCublas.size2()); assert(ABC.rows() == ABC.cols()); - assert(nFrontal <= ABC.rows()); + assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows())); const size_t n = ABC.rows(); // Compute Cholesky factorization of A, overwrites A. - tic(1, "lld"); - ABC.block(0,0,nFrontal,nFrontal).triangularView() = - ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt().matrixU(); - toc(1, "lld"); -#if 0 - if(true) { - tic(1, "dpotrf"); - int info = lapack_dpotrf('U', nFrontal, &ABC(0,0), n); - if(info != 0) { - if(info < 0) - throw std::domain_error(boost::str(boost::format( - "Bad input to choleskyPartial, dpotrf returned %d.\n")%info)); - else - 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 { - tic(1, "choleskyCareful"); - bool fullRank = choleskyCareful(ABC, nFrontal).second; - if(!fullRank) - throw invalid_argument("Rank-deficient"); - toc(1, "choleskyCareful"); - } -#endif - -#ifndef NDEBUG - // Check for non-finite values - for(size_t i=0; i Rf(ublas::project(ABC, ublas::range(0,nFrontal), ublas::range(0,nFrontal))); -// ublas::triangular_adaptor, ublas::upper> R(Rf); -// ublas::matrix_range S(ublas::project(ABC, ublas::range(0,nFrontal), ublas::range(nFrontal,n))); -// ublas::matrix_range Lf(ublas::project(ABC, ublas::range(nFrontal,n), ublas::range(nFrontal,n))); -// ublas::symmetric_adaptor L(Lf); + tic(1, "lld"); + ABC.block(0,0,nFrontal,nFrontal).triangularView() = + ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt().matrixU(); + toc(1, "lld"); if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView()) << endl; @@ -268,53 +228,18 @@ void choleskyPartial(MatrixColMajor& ABCublas, size_t nFrontal) { if(n - nFrontal > 0) { ABC.topLeftCorner(nFrontal,nFrontal).triangularView().transpose().solveInPlace( ABC.topRightCorner(nFrontal, n-nFrontal)); -// typeof(ublas::trans(R)) RT(ublas::trans(R)); -// ublas::inplace_solve(RT, S, ublas::lower_tag()); } if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; toc(2, "compute S"); -#ifndef NDEBUG - // Check for non-finite values - for(size_t i=0; i()) << endl; if(n - nFrontal > 0) ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().rankUpdate( ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); -// L -= ublas::prod(ublas::trans(S), S); if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; 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 - // Check for non-finite values - for(size_t i=0; i