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

gtsam/base/cholesky.cpp

@@ -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;
 

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<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].

gtsam/linear/GaussianFactorGraph.cpp

@@ -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 {
+#ifdef NDEBUG
-			tic(2, "EliminateCholesky");
+		static const bool diag = false;
-			ret = EliminateCholesky(factors, nrFrontals);
+#else
-			toc(2, "EliminateCholesky");
+		static const bool diag = !ISDEBUG("NoCholeskyDiagnostics");
-		} catch (const exception& e) {
+#endif
-			cout << "Exception in EliminateCholesky: " << e.what() << endl;
+		if(!diag) {
-			SETDEBUG("EliminateCholesky", true);
+      tic(2, "EliminateCholesky");
-			SETDEBUG("updateATA", true);
+      ret = EliminateCholesky(factors, nrFrontals);
-			SETDEBUG("JacobianFactor::eliminate", true);
+      toc(2, "EliminateCholesky");
-			SETDEBUG("JacobianFactor::Combine", true);
+		} else {
-			SETDEBUG("choleskyPartial", true);
+		  try {
-			factors.print("Combining factors: ");
+		    tic(2, "EliminateCholesky");
-			EliminateCholesky(factors, nrFrontals);
+		    ret = EliminateCholesky(factors, nrFrontals);
-			throw;
+		    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 {
+#ifdef NDEBUG
-			tic(2, "EliminateLDL");
+    static const bool diag = false;
-			ret = EliminateLDL(factors, nrFrontals);
+#else
-			toc(2, "EliminateLDL");
+    static const bool diag = !ISDEBUG("NoLDLDiagnostics");
-		} catch (const exception& e) {
+#endif
-			cout << "Exception in EliminateLDL: " << e.what() << endl;
+    if(!diag) {
-			SETDEBUG("EliminateLDL", true);
+      tic(2, "EliminateLDL");
-			SETDEBUG("updateATA", true);
+      ret = EliminateLDL(factors, nrFrontals);
-			SETDEBUG("JacobianFactor::eliminate", true);
+      toc(2, "EliminateLDL");
-			SETDEBUG("JacobianFactor::Combine", true);
+    } else {
-			SETDEBUG("ldlPartial", true);
+      try {
-			SETDEBUG("findScatterAndDims", true);
+        tic(2, "EliminateLDL");
-			factors.print("Combining factors: ");
+        ret = EliminateLDL(factors, nrFrontals);
-			EliminateLDL(factors, nrFrontals);
+        toc(2, "EliminateLDL");
-			throw;
+      } 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) {

gtsam/nonlinear/NonlinearOptimizer-inl.h

@@ -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();
+		  try {
-		  if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
+		    VectorValues delta = *solver->optimize();
+		    if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
 
-		  // update values
+		    // update values
-		  shared_values newValues(new C(values_->expmap(delta, *ordering_)));
+		    shared_values newValues(new C(values_->expmap(delta, *ordering_)));
 
-		  // create new optimization state with more adventurous lambda
+		    // create new optimization state with more adventurous lambda
-		  double error = graph_->error(*newValues);
+		    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_ ) {
+		    if( error <= error_ ) {
-		  	next_values = newValues;
+		      next_values = newValues;
-		  	next_error = error;
+		      next_error = error;
-			  lambda /= factor;
+		      lambda /= factor;
-		  	break;
+		      break;
-		  }
+		    }
-		  else {
+		    else {
-		  	// Either we're not cautious, or the same lambda was worse than the current error.
+		      // 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
+		      // The more adventurous lambda was worse too, so make lambda more conservative
-		  	// and keep the same values.
+		      // and keep the same values.
-		  	if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) {
+		      if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) {
-		  		break;
+		        break;
-		  	} else {
+		      } else {
-		  		lambda *= factor;
+		        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