From 7bf0d8ebf1620d19cb9e9944235f3aa05b52cd3d Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 27 Jan 2014 22:56:21 -0500 Subject: [PATCH] commented functionality - not working yet --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index ed91112f7..04159df04 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -163,13 +163,14 @@ void LevenbergMarquardtOptimizer::iterate() { // linearizedCostChange = oldCost - newLinearizedCost = f'f/2 - 1/2 [ f'f + 2f'J * step + step' * J' * J * step] // = -f'J * step - step' * J' * J * step / 2 = -(f' + modelResidual') * (modelResidual) // (with modelResidual = J * step) - Errors modelResidualList = (*linear) * delta; // modelResidual = A * delta + /* Errors modelResidualList = (*linear) * delta; // modelResidual = A * delta Vector modelResidual = concatVectors(modelResidualList); // TODO: is this an ordered list? Vector residuals = - linear->jacobian().second; // TODO: optimize this computation, TODO: is there a minus sign? double linearizedCostChange = dot(- modelResidual, (residuals + modelResidual / 2.0) ); // Measure of mismatch between original (usually nonlinear) system and its linearized version modelMismatch = costChange / linearizedCostChange; + */ if (error <= state_.error) { state_.values.swap(newValues);