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

Levenberg-Marquardt increases lambda when the Hessian is indefinite or negative by catching an exception

release/4.3a0
Richard Roberts 2011-08-24 20:51:54 +00:00
parent 9be9251d1b
commit b57210efcc
4 changed files with 107 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/base/cholesky.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <boost/format.hpp>
#include <boost/bind.hpp>
using namespace std;
@ -125,6 +126,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
tic(1, "lld");
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>() =
ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>().llt().matrixU();
assert(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().toDenseMatrix().unaryExpr(&isfinite<double>).all());
toc(1, "lld");
if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>()) << endl;
@ -135,6 +137,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(
ABC.topRightCorner(nFrontal, n-nFrontal));
}
assert(ABC.topRightCorner(nFrontal, n-nFrontal).unaryExpr(&isfinite<double>).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<Eigen::Upper>().rankUpdate(
ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
assert(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().toDenseMatrix().unaryExpr(&isfinite<double>).all());
if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
toc(3, "compute L");
}
@ -163,6 +167,12 @@ Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal)
Eigen::LDLT<Matrix> ldlt;
ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>());
if(ldlt.vectorD().unaryExpr(boost::bind(less<double>(), _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;

6
gtsam/base/cholesky.h
View File

@ -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<size_t,bool> 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].

85
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/GaussianFactorGraph.h>
@ -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) {

59
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -22,6 +22,7 @@
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/cholesky.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#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