From 60f5ee7cf1196cfc4960a68b3786be133df7e955 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2019 16:13:09 -0400 Subject: [PATCH 1/3] added test for negative error in Huber Robust noise model --- gtsam/linear/tests/testNoiseModel.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 6b130bea0..f8ce1feca 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -452,19 +452,24 @@ TEST(NoiseModel, WhitenInPlace) /* * These tests are responsible for testing the weight functions for the m-estimators in GTSAM. * The weight function is related to the analytic derivative of the residual function. See - * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * for details. This weight function is required when optimizing cost functions with robust * penalties using iteratively re-weighted least squares. */ TEST(NoiseModel, robustFunctionHuber) { - const double k = 5.0, error1 = 1.0, error2 = 10.0; + 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); + 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); + // 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); } TEST(NoiseModel, robustFunctionGemanMcClure) From ba22688ca066c81c361e7daacaf2e2cba6420990 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2019 18:16:17 -0400 Subject: [PATCH 2/3] compare absolute value of error against k for Huber noise model --- gtsam/linear/NoiseModel.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index aa89be982..d4a955fd4 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -629,7 +629,7 @@ namespace gtsam { /** * The mEstimator name space contains all robust error functions. * It mirrors the exposition at - * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. * * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: @@ -681,7 +681,7 @@ namespace gtsam { /* * This method is responsible for returning the weight function for a given amount of error. * The weight function is related to the analytic derivative of the residual function. See - * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * for details. This method is required when optimizing cost functions with robust penalties * using iteratively re-weighted least squares. */ @@ -776,7 +776,7 @@ namespace gtsam { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const { - return (error < k_) ? (1.0) : (k_ / fabs(error)); + return (std::abs(error) < k_) ? (1.0) : (k_ / fabs(error)); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; From 813f169d4a24c9b858b96f833dc6ad690bc2d5f8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2019 20:01:24 -0400 Subject: [PATCH 3/3] 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)