From 4aec076568c08e8a4566d2f4757f4cc8cc6e1c64 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Sun, 16 Feb 2014 16:33:43 -0500
Subject: [PATCH] do not cross upper bound for lambda

---
 gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 650db75d7..6c4ff987b 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -184,10 +184,12 @@ void LevenbergMarquardtOptimizer::iterate() {
       if (error < state_.error) {
         state_.values.swap(newValues);
         state_.error = error;
-        if(modelFidelity > params_.minModelFidelity)
+        if(modelFidelity > params_.minModelFidelity){
           decreaseLambda(modelFidelity);
-        else
-          increaseLambda(modelFidelity);
+        }else{
+          if(state_.lambda < params_.lambdaUpperBound)
+            increaseLambda(modelFidelity);
+        }
         break;
       } else {
         // Either we're not cautious, or the same lambda was worse than the current error.