From 63ef77156e2c724f9d0b82b47fd96141ef2d2eb8 Mon Sep 17 00:00:00 2001 From: Michael Bosse Date: Tue, 10 Dec 2019 13:05:18 -0800 Subject: [PATCH] Fix LM linearizedCostChange for robust noise models When robust noise models are used, the linearized error is not the same as the nonlinear error even at zero delta. In the L2 loss case, the rho(error) = 0.5 * error^2 which is the same as 0.5 *(sqrt(weight)*error)^2 since weight is always 1.0; However this is not the case with other loss functions since in general, rho(error) != 0.5 * weight * error^2 This PR fixes the LevenbergMarquardtOptimizer to explicityly compute the linear cost change. --- 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 c85891af2..e889f260e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -152,6 +152,9 @@ 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; @@ -161,7 +164,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, // cost change in the linearized system (old - new) double newlinearizedError = linear.error(delta); - double linearizedCostChange = currentState->error - newlinearizedError; + double linearizedCostChange = oldLinearizedError - newlinearizedError; if (verbose) cout << "newlinearizedError = " << newlinearizedError << " linearizedCostChange = " << linearizedCostChange << endl; @@ -188,8 +191,8 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, // cost change in the original, nonlinear system (old - new) costChange = currentState->error - newError; - if (linearizedCostChange > - 1e-20) { // the (linear) error has to decrease to satisfy this condition + if (linearizedCostChange > std::numeric_limits::epsilon() * oldLinearizedError) { + // 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