From f3f72c24a5184e5182c3db088c6419bcebd20e61 Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Wed, 26 Jan 2022 02:29:59 -0500
Subject: [PATCH] Use ExtractIndices filter in place of 2nd CreateDepthSlice
 call for neighbors search

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 .../src/ScienceSensorsSystem.cc               | 20 ++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index f168799c..6380ecb5 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -33,6 +33,7 @@
 #include <ignition/transport/Node.hh>
 
 #include <pcl/conversions.h>
+#include <pcl/filters/extract_indices.h>
 #include <pcl/filters/passthrough.h>
 #include <pcl/PCLPointCloud2.h>
 #include <pcl/point_cloud.h>
@@ -797,13 +798,22 @@ void ScienceSensorsSystemPrivate::FindTrilinearInterpolators(
 
   // Step 2: exclude z slice of 1st NN from further searches.
 
-  // Remove all points in the z slice of the 1st NN, so that the 2nd NN will be
-  // found in another z slice.
-  // Set invert flag to get all but the depth slice.
   pcl::PointCloud<pcl::PointXYZ> cloudExceptZSlice1;
   std::vector<int> indsExceptZSlice1;
-  this->CreateDepthSlice(nnZ, *(this->timeSpaceCoords[this->timeIdx]),
-    cloudExceptZSlice1, indsExceptZSlice1, true);
+
+  // Convert input indices to PCL type
+  pcl::PointIndices::Ptr zSliceInds1pcl(new pcl::PointIndices());
+  zSliceInds1pcl->indices = zSliceInds1;
+
+  // Remove all points in the z slice of the 1st NN, so that the 2nd NN will be
+  // found in another z slice.
+  pcl::ExtractIndices<pcl::PointXYZ> extract;
+  extract.setInputCloud(this->timeSpaceCoords[this->timeIdx]->makeShared());
+  extract.setIndices(zSliceInds1pcl);
+  extract.setNegative(false);
+  extract.filter(cloudExceptZSlice1);
+  extract.filter(indsExceptZSlice1);
+
   igndbg << "Excluding 1st nn z slice. Remaining cloud has "
     << cloudExceptZSlice1.points.size() << " points" << std::endl;