From d6e9889f45c6df3e9410db8e3eadfbe61e75c232 Mon Sep 17 00:00:00 2001
From: achintyamohan <achintyamohan2002@gmail.com>
Date: Sun, 10 Sep 2023 20:07:12 -0400
Subject: [PATCH 1/2] Add cpp example for GNC

---
 examples/GNCExample.cpp | 67 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 67 insertions(+)
 create mode 100644 examples/GNCExample.cpp

diff --git a/examples/GNCExample.cpp b/examples/GNCExample.cpp
new file mode 100644
index 000000000..651e470c8
--- /dev/null
+++ b/examples/GNCExample.cpp
@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 GNCExample.cpp
+ * @brief Simple example showcasing a Graduated Non-Convexity based solver
+ * @author Achintya Mohan
+ */
+
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/nonlinear/GncOptimizer.h>
+#include <gtsam/nonlinear/GncParams.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+int main() {
+  cout << "Graduated Non-Convexity Example\n";
+
+  NonlinearFactorGraph graph;
+
+  // Add a prior to the first point, set to the origin
+  auto priorNoise = noiseModel::Isotropic::Sigma(2, 0.1);
+  graph.addPrior(1, Point2(0.0, 0.0), priorNoise);
+
+  // Add additional factors, noise models must be Gaussian 
+  Point2 x1(1.0, 0.0);
+  graph.emplace_shared<BetweenFactor<Point2>>(1, 2, x1, noiseModel::Isotropic::Sigma(2, 0.2));
+  Point2 x2(1.1, 0.1);
+  graph.emplace_shared<BetweenFactor<Point2>>(2, 3, x2, noiseModel::Isotropic::Sigma(2, 0.4));
+
+  // Initial estimates
+  Values initial;
+  initial.insert(1, Point2(0.5, -0.1));
+  initial.insert(2, Point2(1.3, 0.1));
+  initial.insert(3, Point2(0.8, 0.2));
+
+  // Set options for the non-minimal solver
+  LevenbergMarquardtParams lmParams;
+  lmParams.setMaxIterations(1000);
+  lmParams.setRelativeErrorTol(1e-5);
+
+  // Set GNC-specific options
+  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setLossType(GncLossType::TLS);
+
+  // Optimize the graph and print results
+  GncOptimizer<GncParams<LevenbergMarquardtParams>> optimizer(graph, initial, gncParams);
+  Values result = optimizer.optimize();
+  result.print("Final Result:");
+
+  return 0;
+}
\ No newline at end of file

From 5cb98a6dcdf7cf4b0624f97e526cff99b4a6b85a Mon Sep 17 00:00:00 2001
From: achintyamohan <achintyamohan2002@gmail.com>
Date: Mon, 11 Sep 2023 16:54:31 -0400
Subject: [PATCH 2/2] Modify GNC example, add comments to explain example
 purpose

---
 examples/GNCExample.cpp | 30 +++++++++++++++++++-----------
 1 file changed, 19 insertions(+), 11 deletions(-)

diff --git a/examples/GNCExample.cpp b/examples/GNCExample.cpp
index 651e470c8..8a4f0706f 100644
--- a/examples/GNCExample.cpp
+++ b/examples/GNCExample.cpp
@@ -15,7 +15,14 @@
  * @author Achintya Mohan
  */
 
-#include <gtsam/geometry/Point2.h>
+/**
+ * A simple 2D pose graph optimization example
+ * - The robot is initially at origin (0.0, 0.0, 0.0) 
+ * - We have full odometry measurements for 2 motions
+ * - The robot first moves to (1.0, 0.0, 0.1) and then to (1.0, 1.0, 0.2) 
+ */
+
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GncOptimizer.h>
 #include <gtsam/nonlinear/GncParams.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -34,20 +41,20 @@ int main() {
   NonlinearFactorGraph graph;
 
   // Add a prior to the first point, set to the origin
-  auto priorNoise = noiseModel::Isotropic::Sigma(2, 0.1);
-  graph.addPrior(1, Point2(0.0, 0.0), priorNoise);
+  auto priorNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.addPrior(1, Pose2(0.0, 0.0, 0.0), priorNoise);
 
   // Add additional factors, noise models must be Gaussian 
-  Point2 x1(1.0, 0.0);
-  graph.emplace_shared<BetweenFactor<Point2>>(1, 2, x1, noiseModel::Isotropic::Sigma(2, 0.2));
-  Point2 x2(1.1, 0.1);
-  graph.emplace_shared<BetweenFactor<Point2>>(2, 3, x2, noiseModel::Isotropic::Sigma(2, 0.4));
+  Pose2 x1(1.0, 0.0, 0.1);
+  graph.emplace_shared<BetweenFactor<Pose2>>(1, 2, x1, noiseModel::Isotropic::Sigma(3, 0.2));
+  Pose2 x2(0.0, 1.0, 0.1);
+  graph.emplace_shared<BetweenFactor<Pose2>>(2, 3, x2, noiseModel::Isotropic::Sigma(3, 0.4));
 
   // Initial estimates
   Values initial;
-  initial.insert(1, Point2(0.5, -0.1));
-  initial.insert(2, Point2(1.3, 0.1));
-  initial.insert(3, Point2(0.8, 0.2));
+  initial.insert(1, Pose2(0.2, 0.5, -0.1));
+  initial.insert(2, Pose2(0.8, 0.3, 0.1));
+  initial.insert(3, Pose2(0.8, 0.2, 0.3));
 
   // Set options for the non-minimal solver
   LevenbergMarquardtParams lmParams;
@@ -64,4 +71,5 @@ int main() {
   result.print("Final Result:");
 
   return 0;
-}
\ No newline at end of file
+}
+