diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 8a8f8670d..71b9d2817 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -31,6 +31,10 @@ using namespace std; namespace gtsam { + static const double negativePivotThreshold = -1e-1; + static const double zeroPivotThreshold = 1e-3; + static const double underconstrainedPrior = 0.001; + /* ************************************************************************* */ void cholesky_inplace(MatrixColMajor& I) { @@ -116,40 +120,51 @@ size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab, size_t nFrontal) { } /* ************************************************************************* */ -static inline bool choleskyStep(MatrixColMajor& ATA, size_t k) { - - // Tolerance for being equal to zero -// static const double zeroTol = numeric_limits::epsilon(); - static const double zeroTol = 1.e-15; +static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) { const size_t n = ATA.size1(); - const double alpha = ATA(k,k); + double alpha = ATA(k,k); + if(alpha < negativePivotThreshold) { + cout << "pivot = " << alpha << endl; + print(ATA, "Partially-factorized matrix: "); + throw(invalid_argument("The matrix was found to be non-positive-semidefinite when factoring with careful Cholesky.")); + } else if(alpha < 0.0) + alpha = 0.0; + const double beta = sqrt(alpha); - if(beta > zeroTol) { + if(beta > zeroPivotThreshold) { const double betainv = 1.0 / beta; // Update k,k ATA(k,k) = beta; - if(k < (n-1)) { + if(k < (order-1)) { + ublas::matrix_row Vf(ublas::row(ATA, k)); + ublas::vector_range V(ublas::subrange(Vf, k+1,order)); + // Update A(k,k+1:end) <- A(k,k+1:end) / beta - cblas_dscal(n-k-1, betainv, &(ATA(k,k+1)), n); + V *= betainv; // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha - cblas_dsyr(CblasColMajor, CblasUpper, n-k-1, -1.0, &(ATA(k,k+1)), n, &(ATA(k+1,k+1)), n); + ublas::matrix_range L(ublas::subrange(ATA, k+1,order, k+1,order)); + L -= ublas::outer_prod(V, V); } return true; - } else + } else { + ATA(k,k) = underconstrainedPrior; + for(size_t j=k+1; j choleskyCareful(MatrixColMajor& ATA, int order) { - static const bool debug = false; + const bool debug = ISDEBUG("choleskyCareful"); // // Tolerance for being equal to zero //// static const double zeroTol = numeric_limits::epsilon(); @@ -161,78 +176,34 @@ size_t choleskyCareful(MatrixColMajor& ATA) { // Number of rows/columns const size_t n = ATA.size1(); + if(order < 0) + order = n; + + assert(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 < n; ++k) { + for(size_t k = 0; k < order; ++k) { - if(choleskyStep(ATA, k)) { + if(choleskyStep(ATA, k, order)) { if(debug) cout << "choleskyCareful: Factored through " << k << endl; if(debug) print(ATA, "ATA: "); maxrank = k+1; } else { + fullRank = false; if(debug) cout << "choleskyCareful: Skipping " << k << endl; } - -// // If the diagonal element is not zero, run Cholesky as far as possible - -// // Cholesky will stop on the first zero diagonal element. Because ATA is -// // symmetric positive semi-definite, a zero diagonal element implies -// // a corresponding row and column of zeros, thus we need only check the -// // diagonal. -// if(ATA(k,k) > zeroTol || ATA(k,k) < -zeroTol) { -// -// // Try to do Cholesky on the remaining lower-right square submatrix. -// int info = lapack_dpotf2('U', n-k, &(ATA(k,k)), n); -// -// if(info > 0) { -// // The submatrix is rank-deficient, but Cholesky factored the first -// // subRank rows/columns, leaving a positive semi-definite matrix -// // starting at subRank. (we're speaking in zero-based indexices). -// size_t subRank = info - 1; -// -// // The row/column after the last nonzero one. -// maxrank = k + subRank; -// -// // We know that the row/column just after the factored part is zero, so -// // skip it. Note that after this statement k will be the next -// // row/column to process. -// k += subRank + 1; -// -// if(debug) cout << "choleskyCareful: Factored until " << maxrank << ", skipping next." << endl; -// -// } else if(info == 0) { -// // Cholesky successfully factored the rest of the matrix. Note that -// // after this statement k will be the last processed row/column, and -// // will be incremented by 1 by the 'for' loop. -// k += n - k; -// -// // The last row/column is nonzero. -// maxrank = n; -// -// if(debug) cout << "choleskyCareful: Factored the remainder" << endl; -// -// } else { -// throw std::domain_error(boost::str(boost::format( -// "Bad input to choleskyFactorUnderdetermined, dpotrf returned %d.\n")%info)); -// } -// } else { -// -// if(debug) cout << "choleskyCareful: Skipping " << k << endl; -// -// // The diagonal element is numerically zero, so skip this row/column. -// ++ k; -// } -// -// if(debug) print(ATA, "ATA: "); } - return maxrank; + return make_pair(maxrank, fullRank); } /* ************************************************************************* */ void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal) { - static const bool debug = false; + const bool debug = ISDEBUG("choleskyPartial"); assert(ABC.size1() == ABC.size2()); assert(nFrontal <= ABC.size1()); diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 90396b122..e0eb75394 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -58,7 +58,7 @@ size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab, size_t nFrontal); * (always?) the case during elimination of a fully-constrained least-squares * problem. */ -size_t choleskyCareful(MatrixColMajor& ATA); +std::pair choleskyCareful(MatrixColMajor& ATA, int order = -1); /** * Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 342716083..dd26d2004 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -96,7 +96,33 @@ TEST(cholesky, choleskyPartial) { Matrix actual(R1 * R2); EXPECT(assert_equal(ublas::symmetric_adaptor(ABC), actual, 1e-9)); +} +/* ************************************************************************* */ +TEST(cholesky, choleskyCareful) { + Matrix A = Matrix_(7,7, + 5184.9, -4381.3, 1137.3, -4448.6, 5127.2, 3264.2, 2.4894, + -4381.3, 1.1345e+05, -28036., -13136., -1.1277e+05, -76360., 19.118, + 1137.3, -28036., 1.3193e+05, 3192.3, 27877., -1.0613e+05, -38.623, + -4448.6, -13136., 3192.3, 6417.7, 12294., 8530., -5.4028, + 5127.2, -1.1277e+05, 27877., 12294., 1.1222e+05, 75952., -18.507, + 3264.2, -76360., -1.0613e+05, 8530., 75952., 1.7642e+05, 21.269, + 2.4894, 19.118, -38.623, -5.4028, -18.507, 21.269, 0.014511); + + Matrix expected = Matrix_(7,7, + 72.006, -60.846, 15.795, -61.781, 71.206, 45.332, 0.034572, + 0.0, 331.28, -81.728, -50.999, -327.33, -222.17, 0.064059, + 0.0, 0.0, 353.55, 3.6466e-06, 0.00014052, -353.55, -0.09598, + 0.0, 0.0, 0.0, 0.024427, -2.3538, 0.33228, -0.0004039, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + MatrixColMajor actual = A; + choleskyCareful(actual); + actual = ublas::triangular_adaptor(actual); + + EXPECT(assert_equal(expected, Matrix(actual), 1e-3)); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 568566249..9db82a4d3 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -273,7 +273,7 @@ SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const { // Use Cholesky to factor Ab tic("Cholesky: 3 careful"); - size_t maxrank = choleskyCareful(Ab); + size_t maxrank = choleskyCareful(Ab).first; toc("Cholesky: 3 careful"); // Due to numerical error the rank could appear to be more than the number