Merge pull request #116 from borglab/fix/huber-noise-model

Fix Huber mEstimator
borglab · Sep 19, 2019 · 19315cc · 19315cc
2 parents a614e5e + 813f169
commit 19315cc
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 9 deletions.
diff --git 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
@@ -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)