From 041c4518bf6258caad9013d4db7bcd5b71c8135c Mon Sep 17 00:00:00 2001
From: Erik Nelson <eanelson@eecs.berkeley.edu>
Date: Fri, 22 Nov 2019 13:04:48 -0800
Subject: [PATCH] Fix an infinite loop in GTSAM's nonlinear optimizer

---
 gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp
index ad3b9c4cc..0c5d99c0f 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.cpp
+++ b/gtsam/nonlinear/NonlinearOptimizer.cpp
@@ -101,7 +101,7 @@ void NonlinearOptimizer::defaultOptimize() {
       cout << "newError: " << error() << endl;
   } while (iterations() < params.maxIterations &&
            !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol,
-                             currentError, error(), params.verbosity));
+                             currentError, error(), params.verbosity) && std::isfinite(currentError));
 
   // Printing if verbose
   if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {