From 215682e64f4063911639320dcab209aab2f062fb Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Thu, 7 Apr 2022 21:33:54 -0400
Subject: [PATCH 1/3] FIX: Robust loss error calculation

---
 gtsam/linear/NoiseModel.h |  6 +++++
 tests/testRobust.cpp      | 53 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 59 insertions(+)
 create mode 100644 tests/testRobust.cpp

diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 5c379beb8..5a29e5d7d 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/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;
diff --git a/tests/testRobust.cpp b/tests/testRobust.cpp
new file mode 100644
index 000000000..ad361f6d9
--- /dev/null
+++ b/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);
+}

From 7a47815e0b0c69828a1c5ebf8f43ec9a38502b21 Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Fri, 8 Apr 2022 16:30:40 -0400
Subject: [PATCH 2/3] Fix the test

---
 gtsam/linear/tests/testNoiseModel.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index b974b6cd5..92774a133 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -668,7 +668,7 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
   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);
+                  robust->loss(robust->squaredMahalanobisDistance(error)), 1e-8);
   }
 }
 

From 0d501b6de153bfc13044816480bc5752044e3bf6 Mon Sep 17 00:00:00 2001
From: yetongumich <yetong@umich.edu>
Date: Thu, 30 Jun 2022 23:38:34 -0400
Subject: [PATCH 3/3] fix testNoiseModel

---
 gtsam/linear/tests/testNoiseModel.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index 92774a133..f83ba7831 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -661,14 +661,15 @@ 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->loss(robust->squaredMahalanobisDistance(error)), 1e-8);
+                  robust->squaredMahalanobisDistance(error), 1e-8);
   }
 }