From 1324cfd283d6c183d299c33300e2cd8db33718b1 Mon Sep 17 00:00:00 2001 From: Michael Bosse Date: Thu, 23 Jan 2020 19:13:41 -0800 Subject: [PATCH] Only compute old error when solved successfully When the LM system has not been solved successfully, an exception may be thrown when computing the old error, so this change makes sure the old error is computed after checking. --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index e889f260e..9b894db25 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -152,18 +152,18 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, systemSolvedSuccessfully = false; } - // Compute the old linearized error as it is not the same - // as the nonlinear error when robust noise models are used. - double oldLinearizedError = linear.error(VectorValues::Zero(delta)); if (systemSolvedSuccessfully) { if (verbose) cout << "linear delta norm = " << delta.norm() << endl; if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); - // cost change in the linearized system (old - new) + // Compute the old linearized error as it is not the same + // as the nonlinear error when robust noise models are used. + double oldLinearizedError = linear.error(VectorValues::Zero(delta)); double newlinearizedError = linear.error(delta); + // cost change in the linearized system (old - new) double linearizedCostChange = oldLinearizedError - newlinearizedError; if (verbose) cout << "newlinearizedError = " << newlinearizedError