From 813f169d4a24c9b858b96f833dc6ad690bc2d5f8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2019 20:01:24 -0400 Subject: [PATCH] improved code for Huber robust noise model --- gtsam/linear/NoiseModel.h | 3 ++- gtsam/linear/tests/testNoiseModel.cpp | 12 ++++-------- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index d4a955fd4..5aa774026 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -776,7 +776,8 @@ namespace gtsam { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const { - return (std::abs(error) < k_) ? (1.0) : (k_ / fabs(error)); + double absError = std::abs(error); + return (absError < k_) ? (1.0) : (k_ / absError); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index f8ce1feca..85dc4735d 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -461,15 +461,11 @@ TEST(NoiseModel, robustFunctionHuber) { const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k); - const double weight1 = huber->weight(error1), - weight2 = huber->weight(error2), - weight3 = huber->weight(error3), - weight4 = huber->weight(error4); - DOUBLES_EQUAL(1.0, weight1, 1e-8); - DOUBLES_EQUAL(0.5, weight2, 1e-8); + DOUBLES_EQUAL(1.0, huber->weight(error1), 1e-8); + DOUBLES_EQUAL(0.5, huber->weight(error2), 1e-8); // Test negative value to ensure we take absolute value of error. - DOUBLES_EQUAL(0.5, weight3, 1e-8); - DOUBLES_EQUAL(1.0, weight4, 1e-8); + DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); + DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure)