FIX: Robust loss error calculation

gtsam/linear/NoiseModel.h

@@ -697,6 +697,12 @@ namespace gtsam {
         return robust_->loss(std::sqrt(squared_distance));
       }
 
+      // NOTE: This is special because in whiten the base version will do the reweighting
+      // which is incorrect!
+      double squaredMahalanobisDistance(const Vector& v) const override {
+        return noise_->squaredMahalanobisDistance(v);
+      }
+
       // These are really robust iterated re-weighting support functions
       virtual void WhitenSystem(Vector& b) const;
       void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;

gtsam/linear/tests/testNoiseModel.cpp

@@ -661,12 +661,13 @@ TEST(NoiseModel, robustNoiseDCS)
 TEST(NoiseModel, robustNoiseL2WithDeadZone)
 {
   double dead_zone_size = 1.0;
-  SharedNoiseModel robust = noiseModel::Robust::Create(
+  auto robust = noiseModel::Robust::Create(
       noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
       Unit::Create(3));
 
   for (int i = 0; i < 5; i++) {
-    Vector3 error = Vector3(i, 0, 0);
+    Vector error = Vector3(i, 0, 0);
+    robust->WhitenSystem(error);
     DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
                   robust->squaredMahalanobisDistance(error), 1e-8);
   }

tests/testRobust.cpp

@@ -0,0 +1,53 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   testRobust.cpp
+ *  @brief  Unit tests for Robust loss functions
+ *  @author Fan Jiang
+ *  @author Yetong Zhang
+ *  @date   Apr 7, 2022
+ **/
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+using namespace gtsam;
+
+TEST(RobustNoise, loss) {
+  // Keys.
+  gtsam::Key x1_key = 1;
+  gtsam::Key x2_key = 2;
+
+  auto gm = noiseModel::mEstimator::GemanMcClure::Create(1.0);
+  auto noise = noiseModel::Robust::Create(gm, noiseModel::Unit::Create(1));
+
+  auto factor = PriorFactor<double>(x1_key, 0.0, noise);
+  auto between_factor = BetweenFactor<double>(x1_key, x2_key, 0.0, noise);
+
+  Values values;
+  values.insert(x1_key, 10.0);
+  values.insert(x2_key, 0.0);
+
+  EXPECT_DOUBLES_EQUAL(0.49505, factor.error(values), 1e-5);
+  EXPECT_DOUBLES_EQUAL(0.49505, between_factor.error(values), 1e-5);
+  EXPECT_DOUBLES_EQUAL(0.49505, gm->loss(10.0), 1e-5);
+}
+
+int main() {
+  TestResult tr;
+
+  return TestRegistry::runAllTests(tr);
+}