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

Automatic damping in LDL, disabled by default

release/4.3a0
Richard Roberts 2012-01-14 01:58:28 +00:00
parent a89d08ef30
commit 5d9a427f0a
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/base/cholesky.cpp
View File

@ -33,6 +33,7 @@ namespace gtsam {
static const double negativePivotThreshold = -1e-1; static const double negativePivotThreshold = -1e-1;
static const double zeroPivotThreshold = 1e-6; static const double zeroPivotThreshold = 1e-6;
static const double underconstrainedPrior = 1e-5; static const double underconstrainedPrior = 1e-5;
static const bool dampUnderconstrained = false;
/* ************************************************************************* */ /* ************************************************************************* */
static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) { static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) {
@ -173,16 +174,21 @@ Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal)
ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>()); ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>());
if (debug) ldlt.isNegative() ? cout << "Matrix is negative" << endl : cout << "Matrix is not negative" << endl; if (debug) ldlt.isNegative() ? cout << "Matrix is negative" << endl : cout << "Matrix is not negative" << endl;
if(ldlt.vectorD().unaryExpr(boost::bind(less<double>(), _1, 0.0)).any()) { Vector D = ldlt.vectorD();
if(dampUnderconstrained) {
D = D.array().max(Vector::Constant(D.rows(), D.cols(), underconstrainedPrior).array());
}
Vector sqrtD = D.cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL
if (debug) cout << "LDL Dsqrt:\n" << sqrtD << endl;
if(D.unaryExpr(boost::bind(less<double>(), _1, 0.0)).any()) {
if(ISDEBUG("detailed_exceptions")) if(ISDEBUG("detailed_exceptions"))
throw NegativeMatrixException(NegativeMatrixException::Detail(ABC, ldlt.matrixU(), ldlt.vectorD())); throw NegativeMatrixException(NegativeMatrixException::Detail(ABC, ldlt.matrixU(), ldlt.vectorD()));
else else
throw NegativeMatrixException(); throw NegativeMatrixException();
} }
Vector sqrtD = ldlt.vectorD().cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL
if (debug) cout << "LDL Dsqrt:\n" << sqrtD << endl;
// U = sqrtD * L^ // U = sqrtD * L^
Matrix U = ldlt.matrixU(); Matrix U = ldlt.matrixU();
if(debug) cout << "LDL U:\n" << U << endl; if(debug) cout << "LDL U:\n" << U << endl;