From 42bab8f3e70eee07a2ae591786f3b2a297b6784a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 25 Jul 2022 22:19:29 -0400
Subject: [PATCH] use KeyVector for GNC inliers & outliers

---
 gtsam/nonlinear/GncParams.h              | 12 ++++++++----
 gtsam/nonlinear/nonlinear.i              |  8 ++++----
 python/gtsam/preamble/nonlinear.h        |  2 --
 python/gtsam/specializations/nonlinear.h |  2 +-
 4 files changed, 13 insertions(+), 11 deletions(-)

diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h
index a7ccb88b7..08ae42336 100644
--- a/gtsam/nonlinear/GncParams.h
+++ b/gtsam/nonlinear/GncParams.h
@@ -72,8 +72,12 @@ class GTSAM_EXPORT GncParams {
   double relativeCostTol = 1e-5;  ///< If relative cost change is below this threshold, stop iterating
   double weightsTol = 1e-4;  ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
   Verbosity verbosity = SILENT;  ///< Verbosity level
-  std::vector<size_t> knownInliers = std::vector<size_t>();  ///< Slots in the factor graph corresponding to measurements that we know are inliers
-  std::vector<size_t> knownOutliers = std::vector<size_t>();  ///< Slots in the factor graph corresponding to measurements that we know are outliers
+
+  // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping
+  ///< Slots in the factor graph corresponding to measurements that we know are inliers
+  KeyVector knownInliers = KeyVector();
+  ///< Slots in the factor graph corresponding to measurements that we know are outliers
+  KeyVector knownOutliers = KeyVector();
 
   /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
   void setLossType(const GncLossType type) {
@@ -114,7 +118,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 std::vector<size_t>& knownIn) {
+  void setKnownInliers(const KeyVector& knownIn) {
     for (size_t i = 0; i < knownIn.size(); i++){
       knownInliers.push_back(knownIn[i]);
     }
@@ -125,7 +129,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 std::vector<size_t>& knownOut) {
+  void setKnownOutliers(const KeyVector& knownOut) {
     for (size_t i = 0; i < knownOut.size(); i++){
       knownOutliers.push_back(knownOut[i]);
     }
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index c79333236..4bb80a0d6 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -529,8 +529,8 @@ virtual class GncParams {
   double relativeCostTol;
   double weightsTol;
   Verbosity verbosity;
-  std::vector<size_t> knownInliers;
-  std::vector<size_t> knownOutliers;
+  gtsam::KeyVector knownInliers;
+  gtsam::KeyVector knownOutliers;
 
   void setLossType(const gtsam::GncLossType type);
   void setMaxIterations(const size_t maxIter);
@@ -538,8 +538,8 @@ virtual class GncParams {
   void setRelativeCostTol(double value);
   void setWeightsTol(double value);
   void setVerbosityGNC(const gtsam::This::Verbosity value);
-  void setKnownInliers(const std::vector<size_t>& knownIn);
-  void setKnownOutliers(const std::vector<size_t>& knownOut);
+  void setKnownInliers(const gtsam::KeyVector& knownIn);
+  void setKnownOutliers(const gtsam::KeyVector& knownOut);
   void print(const string& str = "GncParams: ") const;
   
   enum Verbosity {
diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h
index 56a07cfdd..d07a75f6f 100644
--- a/python/gtsam/preamble/nonlinear.h
+++ b/python/gtsam/preamble/nonlinear.h
@@ -10,5 +10,3 @@
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
  */
-
-#include <pybind11/stl.h>
diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h
index d46ccdc66..da8842eaf 100644
--- a/python/gtsam/specializations/nonlinear.h
+++ b/python/gtsam/specializations/nonlinear.h
@@ -9,4 +9,4 @@
  * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
  * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
  * and saves one copy operation.
- */
\ No newline at end of file
+ */