From c9ead8bbd33488398c3682b774d4b876d7ebeda4 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 7 Feb 2014 18:27:37 -0500 Subject: [PATCH] computation of modelMismatch in LM --- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 04159df04..e88e0761f 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -163,14 +163,20 @@ 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 - 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) ); + // Errors modelResidualList = (*linear) * delta; // modelResidual = A * delta + // modelResidualList.print(""); + // Vector modelResidual = concatVectors(modelResidualList); // TODO: is this an ordered list? + //cout << "modelResidual: " << modelResidual << endl; + // cout << "linear->jacobian().second: " << linear->jacobian().second << endl; + // cout << "linear->augmentedJacobian().second: " << linear->augmentedJacobian() << endl; + // cout << "linear->augmentedHessian().second: " << linear->augmentedHessian() << endl; + + // 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; - // Measure of mismatch between original (usually nonlinear) system and its linearized version - modelMismatch = costChange / linearizedCostChange; - */ if (error <= state_.error) { state_.values.swap(newValues);