Fix filtering to work correctly with negative_ and extract_removed_in…

…dices_.
PointCloudLibrary · Dec 2, 2024 · 62779a5 · 62779a5
1 parent 28efda6
commit 62779a5
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 20 deletions.
diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp
@@ -81,6 +81,7 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
   std::vector<float> nn_dists(mean_k);
   // Set to keep all points and in the filtering set those we don't want to keep, assuming
   // we want to keep the majority of the points.
+  // 0 = remove, 1 = keep, 2 = remove due to inf/nan
   std::vector<std::uint8_t> to_keep(indices_->size(), 1);
   indices.resize (indices_->size ());
   removed_indices_->resize (indices_->size ());
@@ -136,9 +137,9 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
     for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(indices_->size()); i++)
     {
       const auto& index = (*indices_)[i];
-      if (!pcl::isXYFinite((*input_)[index]))
+      if (!pcl::isXYZFinite((*input_)[index]))
       {
-        to_keep[i] = 0;
+        to_keep[i] = 2;
         continue;
       }
 
@@ -156,31 +157,32 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
     }
   }
 
-  if (!negative_)
-  {
     for (index_t i=0; i < static_cast<index_t>(to_keep.size()); i++)
     {
-      if (to_keep[i] == 1)
+      if (to_keep[i] == 2)
       {
-        indices[oii++] = (*indices_)[i];
+        if (extract_removed_indices_)
+          (*removed_indices_)[rii++] = (*indices_)[i];
+        continue;
       }
-      else
+
+      if (!negative_ && to_keep[i] == 0)
       {
-        (*removed_indices_)[rii++] = (*indices_)[i];
-      }
-    }
-  }
-  else
-  {
-    for (index_t i = 0; i < static_cast<index_t>(to_keep.size()); i++) {
-      if (to_keep[i] == 0) {
-        indices[oii++] = (*indices_)[i];
+        if (extract_removed_indices_)
+          (*removed_indices_)[rii++] = (*indices_)[i];
+        continue;
       }
-      else {
-        (*removed_indices_)[rii++] = (*indices_)[i];
+
+      if (negative_ && to_keep[i] != 0)
+      {
+        if (extract_removed_indices_)
+          (*removed_indices_)[rii++] = (*indices_)[i];
+        continue;
       }
+
+      // Otherwise it was a normal point for output (inlier)
+      indices[oii++] = (*indices_)[i];
     }
-  }
 
   // Resize the output arrays
   indices.resize (oii);

diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp
@@ -1529,7 +1529,7 @@ TEST (RadiusOutlierRemoval, Filters)
 
   outremNonDense.setNegative(true);
   outremNonDense.filter(cloud_out_rgb_neg);
-  EXPECT_EQ(cloud_out_rgb_neg.size(), 66399);
+  EXPECT_EQ(cloud_out_rgb_neg.size(), 606);
   //EXPECT_FALSE(cloud_out_rgb_neg.is_dense);
 
   // Test the pcl::PCLPointCloud2 method