From c59fbc825fe2f77c96d2e8a1bd7c509c3bf7189c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 5 Jan 2022 23:01:57 -0500 Subject: [PATCH] ruled out corner case where weights are outside [0,1] in TLS --- gtsam/nonlinear/GncOptimizer.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3025d2468..9601e5f8c 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -439,13 +439,11 @@ class GTSAM_EXPORT GncOptimizer { for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual - if (u2_k >= upperbound) { + weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; + if (u2_k >= upperbound || weights[k] < 0) { weights[k] = 0; - } else if (u2_k <= lowerbound) { + } else if (u2_k <= lowerbound || weights[k] > 1) { weights[k] = 1; - } else { - weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - - mu; } } }