From c163e28c311eccfff8a280338beda1bd2f058a82 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Sat, 8 May 2021 17:03:04 -0400
Subject: [PATCH] addeed gnc example

---
 .../examples/GncPoseAveragingExample.cpp      | 91 +++++++++++++++++++
 1 file changed, 91 insertions(+)
 create mode 100644 gtsam_unstable/examples/GncPoseAveragingExample.cpp

diff --git a/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp
new file mode 100644
index 000000000..a5953bcfb
--- /dev/null
+++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp
@@ -0,0 +1,91 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 GncPoseAveragingExample.cpp
+ * @brief example of GNC estimating a single pose from pose priors possibly corrupted with outliers
+ * @date May 8, 2021
+ * @author Luca Carlone
+ */
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/GncOptimizer.h>
+
+#include <string>
+#include <fstream>
+#include <iostream>
+#include <random>
+#include <boost/lexical_cast.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char** argv){
+  cout << "== Robust Pose Averaging Example === " << endl;
+
+  // default number of inliers and outliers
+  size_t nrInliers = 10;
+  size_t nrOutliers = 10;
+
+  // User can pass arbitrary number of inliers and outliers for testing
+  if (argc > 1)
+    nrInliers = atoi(argv[1]);
+  if (argc > 2)
+    nrOutliers = atoi(argv[2]);
+  cout << "nrInliers " << nrInliers << " nrOutliers "<< nrOutliers << endl;
+
+  // Seed random number generator
+  random_device rd;
+  mt19937 rng(rd());
+  uniform_real_distribution<double> uniformOutliers(-10, 10);
+  normal_distribution<double> normalInliers(0.0, 0.05);
+
+  Values initial;
+  initial.insert(0, Pose3::identity()); // identity pose as initialization
+
+  // create ground truth pose
+  Pose3 gtPose = Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) );
+
+  NonlinearFactorGraph graph;
+  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(6,0.05);
+  // create inliers
+  for(size_t i=0; i<nrInliers; i++){
+    Vector6 poseNoise;
+    for(size_t i = 0; i < 6; ++i){
+      poseNoise(i) = normalInliers(rng);
+    }
+    Pose3 poseMeasurement = gtPose.retract(poseNoise);
+    graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
+  }
+
+  // create outliers
+  for(size_t i=0; i<nrOutliers; i++){
+    Vector6 poseNoise;
+    for(size_t i = 0; i < 6; ++i){
+      poseNoise(i) = uniformOutliers(rng);
+    }
+    Pose3 poseMeasurement = gtPose.retract(poseNoise);
+    graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
+  }
+
+  GncParams<LevenbergMarquardtParams> gncParams;
+  auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(graph,
+      initial,
+      gncParams);
+
+  Values estimate = gnc.optimize();
+  Pose3 poseError = gtPose.between( estimate.at<Pose3>(0) );
+  cout << "norm of translation error: " << poseError.translation().norm() <<
+      " norm of rotation error: " << poseError.rotation().rpy().norm() << endl;
+  // poseError.print("pose error: \n ");
+  return 0;
+}