inline bool check_pos_definite(const char* function, const char* name, const Eigen::LLT<Derived>& cholesky) { if (cholesky.info() != Eigen::Success || !(cholesky.matrixLLT().diagonal().array() > 0.0).all()) domain_error(function, "Cholesky decomposition of", " failed", name); return true; }
/* ************************************************************************* */ bool choleskyPartial(Matrix& ABC, size_t nFrontal) { const bool debug = ISDEBUG("choleskyPartial"); assert(ABC.rows() == ABC.cols()); 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"); Eigen::LLT<Matrix, Eigen::Upper> llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>().llt(); ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>() = llt.matrixU(); toc(1, "lld"); if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>()) << endl; // Compute S = inv(R') * B tic(2, "compute S"); if(n - nFrontal > 0) { ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace( ABC.topRightCorner(nFrontal, n-nFrontal)); } if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; toc(2, "compute S"); // Compute L = C - S' * S tic(3, "compute L"); if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl; if(n - nFrontal > 0) ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate( ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl; toc(3, "compute L"); // Check last diagonal element - Eigen does not check it bool ok; if(llt.info() == Eigen::Success) { if(nFrontal >= 2) { int exp2, exp1; (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2); (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1); ok = (exp2 - exp1 < underconstrainedExponentDifference); } else if(nFrontal == 1) { int exp1; (void)frexp(ABC(0,0), &exp1); ok = (exp1 > -underconstrainedExponentDifference); } else { ok = true; } } else { ok = false; } return ok; }
inline void check_pos_definite(const char* function, const char* name, const Eigen::LLT<Derived>& cholesky) { if (cholesky.info() != Eigen::Success || !(cholesky.matrixLLT().diagonal().array() > 0.0).all()) domain_error(function, "Matrix", " is not positive definite", name); }