From f1fb2c09a4f835700130b04f0b2d57645a720a3b Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 2 May 2014 15:01:42 -0400 Subject: [PATCH] bug fix in LM --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 187abece6..f3c145ffa 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -268,14 +268,17 @@ void LevenbergMarquardtOptimizer::iterate() { // cost change in the original, nonlinear system (old - new) double costChange = state_.error - newError; - if (linearizedCostChange > 1e-15) { // the error has to decrease to satisfy this condition + if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition // fidelity of linearized model VS original system between modelFidelity = costChange / linearizedCostChange; // if we decrease the error in the nonlinear system and modelFidelity is above threshold step_is_successful = modelFidelity > params_.minModelFidelity; - } else { - step_is_successful = true; // linearizedCostChange close to zero + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "modelFidelity: " << modelFidelity << endl; } + //else { + // step_is_successful = true; // linearizedCostChange close to zero + //} double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; // if the change is small we terminate