From fac426c5acad260a750d2bb6a0bce8f079eb8805 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 17 Aug 2012 03:45:32 +0000
Subject: [PATCH] Added "SEARCH_REDUCE_ONLY" mode to Dogleg

---
 gtsam/nonlinear/DoglegOptimizerImpl.h | 21 +++++++++++++++------
 1 file changed, 15 insertions(+), 6 deletions(-)

diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h
index 27a8d47ec..fa2179afa 100644
--- a/gtsam/nonlinear/DoglegOptimizerImpl.h
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.h
@@ -61,6 +61,7 @@ struct DoglegOptimizerImpl {
    */
   enum TrustRegionAdaptationMode {
     SEARCH_EACH_ITERATION,
+		SEARCH_REDUCE_ONLY,
     ONE_STEP_PER_ITERATION
   };
 
@@ -210,7 +211,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
       const double dx_d_norm = result.dx_d.vector().norm();
       const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta
 
-      if(mode == ONE_STEP_PER_ITERATION)
+      if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)
         stay = false;   // If not searching, just return with the new Delta
       else if(mode == SEARCH_EACH_ITERATION) {
         if(newDelta == Delta || lastAction == DECREASED_DELTA)
@@ -231,13 +232,17 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
     } else if(0.25 > rho && rho >= 0.0) {
       // M does not agree well with f, decrease Delta until it does
       double newDelta;
-      if(Delta > 1e-5)
+			bool hitMinimumDelta;
+      if(Delta > 1e-5) {
         newDelta = 0.5 * Delta;
-      else
+				hitMinimumDelta = false;
+			} else {
         newDelta = Delta;
-      if(mode == ONE_STEP_PER_ITERATION || lastAction == INCREASED_DELTA || newDelta == Delta)
+				hitMinimumDelta = true;
+			}
+      if(mode == ONE_STEP_PER_ITERATION || /* mode == SEARCH_EACH_ITERATION && */ lastAction == INCREASED_DELTA || hitMinimumDelta)
         stay = false;   // If not searching, just return with the new smaller delta
-      else if(mode == SEARCH_EACH_ITERATION) {
+      else if(mode == SEARCH_EACH_ITERATION || mode == SEARCH_REDUCE_ONLY) {
         stay = true;
         lastAction = DECREASED_DELTA;
       } else {
@@ -246,7 +251,9 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
       Delta = newDelta; // Update Delta from new Delta
 
     } else {
-      // f actually increased, so keep decreasing Delta until f does not decrease
+			// f actually increased, so keep decreasing Delta until f does not decrease.
+			// NOTE:  NaN and Inf solutions also will fall into this case, so that we
+			// decrease Delta if the solution becomes undetermined.
       assert(0.0 > rho);
       if(Delta > 1e-5) {
         Delta *= 0.5;
@@ -254,6 +261,8 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
         lastAction = DECREASED_DELTA;
       } else {
         if(verbose) std::cout << "Warning:  Dog leg stopping because cannot decrease error with minimum Delta" << std::endl;
+				result.dx_d.setZero(); // Set delta to zero - don't allow error to increase
+				result.f_error = f_error;
         stay = false;
       }
     }