From 7e29944f95c6afb4f9043b8c912add0f558b8852 Mon Sep 17 00:00:00 2001
From: Frank dellaert <dellaert@gmail.com>
Date: Wed, 25 Nov 2020 11:02:01 -0500
Subject: [PATCH] Initial design

---
 tests/testGncOptimizer.cpp | 115 +++++++++++++++++++++++++++++++++++++
 1 file changed, 115 insertions(+)
 create mode 100644 tests/testGncOptimizer.cpp

diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp
new file mode 100644
index 000000000..d9ba209c5
--- /dev/null
+++ b/tests/testGncOptimizer.cpp
@@ -0,0 +1,115 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testGncOptimizer.cpp
+ * @brief   Unit tests for GncOptimizer class
+ * @author  Jignnan Shi
+ * @author  Luca Carlone
+ * @author  Frank Dellaert
+ */
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <tests/smallExample.h>
+
+/* ************************************************************************* */
+template <class BaseOptimizerParameters>
+class GncParams {
+  using BaseOptimizer = BaseOptimizerParameters::OptimizerType;
+  GncParams(const BaseOptimizerParameters& baseOptimizerParams)
+      : baseOptimizerParams(baseOptimizerParams) {}
+
+  BaseOptimizerParameters baseOptimizerParams;
+
+  /// any other specific GNC parameters:
+};
+
+/* ************************************************************************* */
+template <class GncParameters>
+class GncOptimizer {
+ public:
+  // types etc
+
+ private:
+  FG INITIAL GncParameters params_;
+
+ public:
+  GncOptimizer(FG, INITIAL, const GncParameters& params) : params(params) {
+    // Check that all noise models are Gaussian
+  }
+
+  Values optimize() const {
+    NonlinearFactorGraph currentGraph = graph_;
+    for (i : {1, 2, 3}) {
+      BaseOptimizer::Optimizer baseOptimizer(currentGraph, initial);
+      VALUES currentSolution = baseOptimizer.optimize();
+      if (converged) {
+        return currentSolution;
+      }
+      graph_i = this->makeGraph(currentSolution);
+    }
+  }
+
+  NonlinearFactorGraph makeGraph(const Values& currentSolution) const {
+    // calculate some weights
+    this->calculateWeights();
+    // copy the graph with new weights
+    
+  }
+};
+
+/* ************************************************************************* */
+TEST(GncOptimizer, calculateWeights) {
+}
+
+/* ************************************************************************* */
+TEST(GncOptimizer, copyGraph) {
+}
+
+/* ************************************************************************* */
+TEST(GncOptimizer, makeGraph) {
+  // has to have Gaussian noise models !
+  auto fg = example::createReallyNonlinearFactorGraph();
+
+  Point2 p0(3, 3);
+  Values initial;
+  initial.insert(X(1), p0);
+
+  LevenbergMarquardtParams lmParams;
+  GncParams gncParams(lmParams);
+  auto gnc = GncOptimizer(fg, initial, gncParams);
+
+  NonlinearFactorGraph actual = gnc.makeGraph(initial);
+}
+
+/* ************************************************************************* */
+TEST(GncOptimizer, optimize) {
+  // has to have Gaussian noise models !
+  auto fg = example::createReallyNonlinearFactorGraph();
+
+  Point2 p0(3, 3);
+  Values initial;
+  initial.insert(X(1), p0);
+
+  LevenbergMarquardtParams lmParams;
+  GncParams gncParams(lmParams);
+  auto gnc = GncOptimizer(fg, initial, gncParams);
+  Values actual = gnc.optimize();
+  DOUBLES_EQUAL(0, fg.error(actual2), tol);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */