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] 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);
+}