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);
}