diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 3b30c32fc..5c540caa3 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,8 @@ namespace gtsam { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const { - return (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 6b130bea0..85dc4735d 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -452,19 +452,20 @@ 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); - 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, huber->weight(error3), 1e-8); + DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure)