From 63ef77156e2c724f9d0b82b47fd96141ef2d2eb8 Mon Sep 17 00:00:00 2001 From: Michael Bosse Date: Tue, 10 Dec 2019 13:05:18 -0800 Subject: [PATCH 1/3] 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 From 1f43d73b34ff9863ec2e5919a299297175aab4ec Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Mon, 16 Dec 2019 15:52:24 -0800 Subject: [PATCH 2/3] make jacobian factor use noise model distance when available --- gtsam/linear/JacobianFactor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 9be010ff5..2310d88f0 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -513,8 +513,10 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const { /* ************************************************************************* */ double JacobianFactor::error(const VectorValues& c) const { - Vector weighted = error_vector(c); - return 0.5 * weighted.dot(weighted); + Vector e = unweighted_error(c); + // Use the noise model distance function to get the correct error if available. + if (model_) return 0.5 * model_->distance(e); + return 0.5 * e.dot(e); } /* ************************************************************************* */ From 1324cfd283d6c183d299c33300e2cd8db33718b1 Mon Sep 17 00:00:00 2001 From: Michael Bosse Date: Thu, 23 Jan 2020 19:13:41 -0800 Subject: [PATCH 3/3] 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