diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h
index 1c9bdf5fe..a555da1cd 100644
--- a/cpp/NonlinearOptimizer-inl.h
+++ b/cpp/NonlinearOptimizer-inl.h
@@ -186,6 +186,10 @@ namespace gtsam {
 			double relativeThreshold, double absoluteThreshold,
 			verbosityLevel verbosity, int maxIterations, double lambdaFactor) const {
 
+		// check if we're already close enough
+		if (error_ < absoluteThreshold)
+			return *this;
+
 		// do one iteration of LM
 		NonlinearOptimizer next = iterateLM(verbosity, lambdaFactor);
 
diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp
index 4976a34bc..799667cb6 100644
--- a/cpp/testNonlinearOptimizer.cpp
+++ b/cpp/testNonlinearOptimizer.cpp
@@ -32,7 +32,7 @@ using namespace boost;
 using namespace gtsam;
 using namespace example;
 
-const double tol = 1e-6;
+const double tol = 1e-5;
 
 typedef NonlinearOptimizer<Graph,Config> Optimizer;
 
diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp
index 4bb5b7ace..a50843662 100644
--- a/cpp/testSQP.cpp
+++ b/cpp/testSQP.cpp
@@ -627,14 +627,14 @@ TEST (SQP, stereo_truth_noisy ) {
 	VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT));
 
 	// verify
-	DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+	DOUBLES_EQUAL(0.0, optimizer.error(), 1e-5);
 
 	// check if correct
 	if (verbose) {
 		optimizer.config()->print("After iteration");
 		cout << "Final error: " << optimizer.error() << endl;
 	}
-	CHECK(assert_equal(*truthConfig,*(optimizer.config())));
+	CHECK(assert_equal(*truthConfig,*(optimizer.config()), 1e-5));
 }
 
 /* ********************************************************************* */