From 1d51c4e64692f0792685d4d50873ba00ef61d238 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 26 Jul 2022 07:54:05 -0400
Subject: [PATCH] Use new GncParams::IndexVector

---
 gtsam/nonlinear/GncParams.h | 12 +++++++-----
 tests/testGncOptimizer.cpp  | 22 +++++++++++-----------
 2 files changed, 18 insertions(+), 16 deletions(-)

diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h
index 08ae42336..e069d5f3d 100644
--- a/gtsam/nonlinear/GncParams.h
+++ b/gtsam/nonlinear/GncParams.h
@@ -73,11 +73,13 @@ class GTSAM_EXPORT GncParams {
   double weightsTol = 1e-4;  ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
   Verbosity verbosity = SILENT;  ///< Verbosity level
 
-  // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping
+  //TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
+  /// Use IndexVector for inliers and outliers since it is fast + wrapping
+  using IndexVector = FastVector<uint64_t>;
   ///< Slots in the factor graph corresponding to measurements that we know are inliers
-  KeyVector knownInliers = KeyVector();
+  IndexVector knownInliers = IndexVector();
   ///< Slots in the factor graph corresponding to measurements that we know are outliers
-  KeyVector knownOutliers = KeyVector();
+  IndexVector knownOutliers = IndexVector();
 
   /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
   void setLossType(const GncLossType type) {
@@ -118,7 +120,7 @@ class GTSAM_EXPORT GncParams {
    * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and
    * only apply GNC to prune outliers from the loop closures.
    * */
-  void setKnownInliers(const KeyVector& knownIn) {
+  void setKnownInliers(const IndexVector& knownIn) {
     for (size_t i = 0; i < knownIn.size(); i++){
       knownInliers.push_back(knownIn[i]);
     }
@@ -129,7 +131,7 @@ class GTSAM_EXPORT GncParams {
    * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
    * and you provide  knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
    * */
-  void setKnownOutliers(const KeyVector& knownOut) {
+  void setKnownOutliers(const IndexVector& knownOut) {
     for (size_t i = 0; i < knownOut.size(); i++){
       knownOutliers.push_back(knownOut[i]);
     }
diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp
index c3335ce20..15d660114 100644
--- a/tests/testGncOptimizer.cpp
+++ b/tests/testGncOptimizer.cpp
@@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
   Values initial;
   initial.insert(X(1), p0);
 
-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
   knownInliers.push_back(0);
   knownInliers.push_back(1);
   knownInliers.push_back(2);
@@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) {
   Values initial;
   initial.insert(X(1), p0);
 
-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
   knownInliers.push_back(0);
   knownInliers.push_back(1);
   knownInliers.push_back(2);
@@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) {
   Values initial;
   initial.insert(X(1), p0);
 
-  std::vector<size_t> knownInliers;
+  GncParams<GaussNewtonParams>::IndexVector knownInliers;
   knownInliers.push_back(0);
   knownInliers.push_back(1);
   knownInliers.push_back(2);
@@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
   // GNC
   // Note: in difficult instances, we set the odometry measurements to be
   // inliers, but this problem is simple enought to succeed even without that
-  // assumption std::vector<size_t> knownInliers;
+  // assumption GncParams<GaussNewtonParams>::IndexVector knownInliers;
   GncParams<GaussNewtonParams> gncParams;
   auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
                                                         gncParams);
@@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
   // nonconvexity with known inliers and known outliers (check early stopping
   // when all measurements are known to be inliers or outliers)
   {
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
     knownInliers.push_back(0);
     knownInliers.push_back(1);
     knownInliers.push_back(2);
 
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
     knownOutliers.push_back(3);
 
     GncParams<GaussNewtonParams> gncParams;
@@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
 
   // nonconvexity with known inliers and known outliers
   {
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
     knownInliers.push_back(2);
     knownInliers.push_back(0);
 
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
     knownOutliers.push_back(3);
 
     GncParams<GaussNewtonParams> gncParams;
@@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
 
   // only known outliers
   {
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
     knownOutliers.push_back(3);
 
     GncParams<GaussNewtonParams> gncParams;
@@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) {
   // initialize weights and also set known inliers/outliers
   {
     GncParams<GaussNewtonParams> gncParams;
-    std::vector<size_t> knownInliers;
+    GncParams<GaussNewtonParams>::IndexVector knownInliers;
     knownInliers.push_back(2);
     knownInliers.push_back(0);
 
-    std::vector<size_t> knownOutliers;
+    GncParams<GaussNewtonParams>::IndexVector knownOutliers;
     knownOutliers.push_back(3);
     gncParams.setKnownInliers(knownInliers);
     gncParams.setKnownOutliers(knownOutliers);