Make mEstimator variable names consistent with math

gtsam/linear/NoiseModel.h

@@ -461,6 +461,11 @@ namespace gtsam {
         return MixedVariances(precisions.array().inverse());
       }
 
+      /**
+       * The squaredMahalanobisDistance function for a constrained noisemodel,
+       * for non-constrained versions, uses sigmas, otherwise
+       * uses the penalty function with mu
+       */
       double squaredMahalanobisDistance(const Vector& v) const override;
 
       /** Fully constrained variations */
@@ -689,7 +694,7 @@ namespace gtsam {
       { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
       inline Vector unwhiten(const Vector& /*v*/) const override
       { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
-
+      /// Compute loss from the m-estimator using the Mahalanobis distance.
       double loss(const double squared_distance) const override {
         return robust_->loss(std::sqrt(squared_distance));
       }

gtsam/linear/tests/testNoiseModel.cpp

@@ -662,25 +662,14 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
 {
   double dead_zone_size = 1.0;
   SharedNoiseModel robust = noiseModel::Robust::Create(
-    noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
-    Unit::Create(3));
-
-/*
- * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
- * implement a loss function, and GTSAM calls the weight function to evaluate the
- * total penalty, rather than calling the loss function. The weight function should be
- * used during iteratively reweighted least squares optimization, but should not be used to
- * evaluate the total penalty. The long-term solution is for all mEstimators to implement
- * both a weight and a loss function, and for GTSAM to call the loss function when
- * evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it
- * commented out until the underlying bug in GTSAM is fixed.
- *
- * for (int i = 0; i < 5; i++) {
- *   Vector3 error = Vector3(i, 0, 0);
- *   DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->distance(error), 1e-8);
- * }
- */
+      noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
+      Unit::Create(3));
 
+  for (int i = 0; i < 5; i++) {
+    Vector3 error = Vector3(i, 0, 0);
+    DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
+                  robust->squaredMahalanobisDistance(error), 1e-8);
+  }
 }
 
 TEST(NoiseModel, lossFunctionAtZero)
@@ -707,9 +696,9 @@ TEST(NoiseModel, lossFunctionAtZero)
   auto dcs = mEstimator::DCS::Create(k);
   DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8);
   DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8);
-  // auto lsdz = mEstimator::L2WithDeadZone::Create(k);
-  // DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
-  // DOUBLES_EQUAL(lsdz->weight(0), 1, 1e-8);
+  auto lsdz = mEstimator::L2WithDeadZone::Create(k);
+  DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8);
 }
 
 

gtsam/nonlinear/NonlinearFactor.cpp

@@ -114,7 +114,7 @@ double NoiseModelFactor::weight(const Values& c) const {
     if (noiseModel_) {
       const Vector b = unwhitenedError(c);
       check(noiseModel_, b.size());
-      return 0.5 * noiseModel_->weight(b);
+      return noiseModel_->weight(b);
     }
     else
       return 1.0;