From e99188095fc017233dd0765beadd498c2e8fb1f5 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Fri, 27 Nov 2020 17:14:34 -0500
Subject: [PATCH] stuck on conversion of noise model

---
 tests/testGncOptimizer.cpp | 68 +++++++++++++++++++++++++++++---------
 1 file changed, 53 insertions(+), 15 deletions(-)

diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp
index a31f4b677..a2dcafc81 100644
--- a/tests/testGncOptimizer.cpp
+++ b/tests/testGncOptimizer.cpp
@@ -113,7 +113,15 @@ public:
   GncOptimizer(const NonlinearFactorGraph& graph,
       const Values& initialValues, const GncParameters& params = GncParameters()) :
         nfg_(graph), state_(initialValues), params_(params) {
-    // TODO: Check that all noise models are Gaussian
+
+    // make sure all noiseModels are Gaussian or convert to Gaussian
+    for (size_t i = 0; i < nfg_.size(); i++) {
+      if(nfg_[i]){
+        // NonlinearFactor factor = nfg_[i]->clone();
+        nfg_[i]->
+
+      }
+    }
   }
 
   NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); }
@@ -199,7 +207,7 @@ public:
       for (size_t k = 0; k < nfg_.size(); k++) {
         if(nfg_[k]){
           double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
-          weights[k] = std::pow( ( mu*mu )/( u2_k + mu*mu ) , 2);
+          weights[k] = std::pow( ( mu*params_.barcSq )/( u2_k + mu*params_.barcSq ) , 2);
         }
       }
       return weights;
@@ -259,6 +267,25 @@ TEST(GncOptimizer, gncConstructor) {
   CHECK(gnc.getParams().equals(gncParams));
 }
 
+/* ************************************************************************* */
+TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) {
+  // simple graph with Gaussian noise model
+  auto fg = example::createReallyNonlinearFactorGraph();
+  // same graph with robust noise model
+  auto fg_robust = example::sharedRobustFactorGraphWithOutliers();
+
+  Point2 p0(3, 3);
+  Values initial;
+  initial.insert(X(1), p0);
+
+  LevenbergMarquardtParams lmParams;
+  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
+  auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust, initial, gncParams);
+
+  // make sure that when parsing the graph is transformed into one without robust loss
+  CHECK( fg.equals(gnc.getFactors()) );
+}
+
 /* ************************************************************************* */
 TEST(GncOptimizer, initializeMu) {
   // has to have Gaussian noise models !
@@ -337,22 +364,33 @@ TEST(GncOptimizer, calculateWeights) {
     double mu = 1.0;
     Vector weights_actual = gnc.calculateWeights(initial,mu);
     CHECK(assert_equal(weights_expected, weights_actual, tol));
+
+    mu = 2.0;
+    double barcSq = 5.0;
+    weights_expected[3] = std::pow(mu*barcSq / (50.0 + mu*barcSq),2); // outlier, error = 50
+    gncParams.setInlierThreshold(barcSq);
+    auto gnc2 = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
+    weights_actual = gnc2.calculateWeights(initial,mu);
+    CHECK(assert_equal(weights_expected, weights_actual, tol));
 }
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 TEST(GncOptimizer, makeGraph) {
-  // has to have Gaussian noise models !
-  auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point
-
-  Point2 p0(3, 3);
-  Values initial;
-  initial.insert(X(1), p0);
-
-  LevenbergMarquardtParams lmParams;
-  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
-  auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
-
-//  NonlinearFactorGraph actual = gnc.makeGraph(initial);
+//  // simple graph with Gaussian noise model
+//  auto fg = example::createReallyNonlinearFactorGraph();
+//  // same graph with robust noise model
+//  auto fg_robust = example::sharedRobustFactorGraphWithOutliers();
+//
+//  Point2 p0(3, 3);
+//  Values initial;
+//  initial.insert(X(1), p0);
+//
+//  LevenbergMarquardtParams lmParams;
+//  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
+//  auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust, initial, gncParams);
+//
+//  // make sure that when parsing the graph is transformed into one without robust loss
+//  CHECK( fg.equals(gnc.getFactors()) );
 }
 
 /* ************************************************************************* */