From b57210efcc5b7508005851bbaadc2c32f70a00f3 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 24 Aug 2011 20:51:54 +0000 Subject: [PATCH] Levenberg-Marquardt increases lambda when the Hessian is indefinite or negative by catching an exception --- gtsam/base/cholesky.cpp | 10 +++ gtsam/base/cholesky.h | 6 +- gtsam/linear/GaussianFactorGraph.cpp | 85 +++++++++++++++--------- gtsam/nonlinear/NonlinearOptimizer-inl.h | 59 ++++++++++------ 4 files changed, 107 insertions(+), 53 deletions(-) diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index bc590b564..670f0b9c2 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -24,6 +24,7 @@ #include #include +#include using namespace std; @@ -125,6 +126,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) { tic(1, "lld"); ABC.block(0,0,nFrontal,nFrontal).triangularView() = ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt().matrixU(); + assert(ABC.topLeftCorner(nFrontal,nFrontal).triangularView().toDenseMatrix().unaryExpr(&isfinite).all()); toc(1, "lld"); if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView()) << endl; @@ -135,6 +137,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) { ABC.topLeftCorner(nFrontal,nFrontal).triangularView().transpose().solveInPlace( ABC.topRightCorner(nFrontal, n-nFrontal)); } + assert(ABC.topRightCorner(nFrontal, n-nFrontal).unaryExpr(&isfinite).all()); if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; toc(2, "compute S"); @@ -144,6 +147,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) { if(n - nFrontal > 0) ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().rankUpdate( ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); + assert(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().toDenseMatrix().unaryExpr(&isfinite).all()); if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; toc(3, "compute L"); } @@ -163,6 +167,12 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) Eigen::LDLT ldlt; ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView()); + if(ldlt.vectorD().unaryExpr(boost::bind(less(), _1, 0.0)).any()) { + gtsam::print(Matrix(ldlt.matrixU()), "U: "); + gtsam::print(Vector(ldlt.vectorD()), "D: "); + throw NegativeMatrixException(); + } + Vector sqrtD = ldlt.vectorD().cwiseSqrt(); if (debug) cout << "Dsqrt: " << sqrtD << endl; diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 6bb105356..a71d72da1 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -21,6 +21,11 @@ namespace gtsam { +/** + * An exception indicating an attempt to factor a negative or indefinite matrix. + */ +class NegativeMatrixException : public std::exception { }; + /** * "Careful" Cholesky computes the positive square-root of a positive symmetric * semi-definite matrix (i.e. that may be rank-deficient). Unlike standard @@ -50,7 +55,6 @@ std::pair choleskyCareful(Matrix& ATA, int order = -1); */ void choleskyPartial(Matrix& ABC, size_t nFrontal); - /** * Partial LDL computes a factor [R S such that [P'R' 0 [RP S = [A B * 0 F] S' I] 0 F] B' C]. diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 587b3a471..2677c1b7c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -424,20 +425,31 @@ namespace gtsam { if (useQR) return EliminateQR(factors, nrFrontals); GaussianFactorGraph::EliminationResult ret; - try { - tic(2, "EliminateCholesky"); - ret = EliminateCholesky(factors, nrFrontals); - toc(2, "EliminateCholesky"); - } catch (const exception& e) { - cout << "Exception in EliminateCholesky: " << e.what() << endl; - SETDEBUG("EliminateCholesky", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - SETDEBUG("choleskyPartial", true); - factors.print("Combining factors: "); - EliminateCholesky(factors, nrFrontals); - throw; +#ifdef NDEBUG + static const bool diag = false; +#else + static const bool diag = !ISDEBUG("NoCholeskyDiagnostics"); +#endif + if(!diag) { + tic(2, "EliminateCholesky"); + ret = EliminateCholesky(factors, nrFrontals); + toc(2, "EliminateCholesky"); + } else { + try { + tic(2, "EliminateCholesky"); + ret = EliminateCholesky(factors, nrFrontals); + toc(2, "EliminateCholesky"); + } catch (const exception& e) { + cout << "Exception in EliminateCholesky: " << e.what() << endl; + SETDEBUG("EliminateCholesky", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + SETDEBUG("choleskyPartial", true); + factors.print("Combining factors: "); + EliminateCholesky(factors, nrFrontals); + throw; + } } const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky"); @@ -563,22 +575,35 @@ namespace gtsam { if (useQR) return EliminateQR(factors, nrFrontals); GaussianFactorGraph::EliminationResult ret; - try { - tic(2, "EliminateLDL"); - ret = EliminateLDL(factors, nrFrontals); - toc(2, "EliminateLDL"); - } catch (const exception& e) { - cout << "Exception in EliminateLDL: " << e.what() << endl; - SETDEBUG("EliminateLDL", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - SETDEBUG("ldlPartial", true); - SETDEBUG("findScatterAndDims", true); - factors.print("Combining factors: "); - EliminateLDL(factors, nrFrontals); - throw; - } +#ifdef NDEBUG + static const bool diag = false; +#else + static const bool diag = !ISDEBUG("NoLDLDiagnostics"); +#endif + if(!diag) { + tic(2, "EliminateLDL"); + ret = EliminateLDL(factors, nrFrontals); + toc(2, "EliminateLDL"); + } else { + try { + tic(2, "EliminateLDL"); + ret = EliminateLDL(factors, nrFrontals); + toc(2, "EliminateLDL"); + } catch (const NegativeMatrixException& e) { + throw; + } catch (const exception& e) { + cout << "Exception in EliminateLDL: " << e.what() << endl; + SETDEBUG("EliminateLDL", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + SETDEBUG("ldlPartial", true); + SETDEBUG("findScatterAndDims", true); + factors.print("Combining factors: "); + EliminateLDL(factors, nrFrontals); + throw; + } + } const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL"); if (checkLDL) { diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index 5409efd43..1a828d468 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -22,6 +22,7 @@ #include #include +#include #include #define INSTANTIATE_NONLINEAR_OPTIMIZER(G,C) \ @@ -170,32 +171,46 @@ namespace gtsam { if (spcg_solver_) spcg_solver_->replaceFactors(damped); shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver( new S(damped, structure_, parameters_->useQR_)); - VectorValues delta = *solver->optimize(); - if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); + try { + VectorValues delta = *solver->optimize(); + if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); - // update values - shared_values newValues(new C(values_->expmap(delta, *ordering_))); + // update values + shared_values newValues(new C(values_->expmap(delta, *ordering_))); - // create new optimization state with more adventurous lambda - double error = graph_->error(*newValues); + // create new optimization state with more adventurous lambda + double error = graph_->error(*newValues); - if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << error << endl; + if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << error << endl; - if( error <= error_ ) { - next_values = newValues; - next_error = error; - lambda /= factor; - break; - } - else { - // Either we're not cautious, or the same lambda was worse than the current error. - // The more adventurous lambda was worse too, so make lambda more conservative - // and keep the same values. - if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { - break; - } else { - lambda *= factor; - } + if( error <= error_ ) { + next_values = newValues; + next_error = error; + lambda /= factor; + break; + } + else { + // Either we're not cautious, or the same lambda was worse than the current error. + // The more adventurous lambda was worse too, so make lambda more conservative + // and keep the same values. + if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { + break; + } else { + lambda *= factor; + } + } + } catch(const NegativeMatrixException& e) { + cout << "Negative matrix, increasing lambda" << endl; + // Either we're not cautious, or the same lambda was worse than the current error. + // The more adventurous lambda was worse too, so make lambda more conservative + // and keep the same values. + if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { + break; + } else { + lambda *= factor; + } + } catch(...) { + throw; } } // end while