Merge pull request #5572 from mvieth/local_maximum_fix

[filters] Fix bug in LocalMaximum

Author: mvieth

filters/include/pcl/filters/impl/local_maximum.hpp

@@ -121,6 +121,14 @@ pcl::LocalMaximum<PointT>::applyFilterIndices (Indices &indices)
     // not be maximal in their own neighborhood
     if (point_is_visited[iii] && !point_is_max[iii])
     {
+      if (negative_) {
+        if (extract_removed_indices_) {
+          (*removed_indices_)[rii++] = iii;
+        }
+      }
+      else {
+        indices[oii++] = iii;
+      }
       continue;
     }
 
@@ -146,7 +154,7 @@ pcl::LocalMaximum<PointT>::applyFilterIndices (Indices &indices)
 
     // Check to see if a neighbor is higher than the query point
     float query_z = (*input_)[iii].z;
-    for (std::size_t k = 1; k < radius_indices.size (); ++k)  // k = 1 is the first neighbor
+    for (std::size_t k = 0; k < radius_indices.size (); ++k) // the query point itself is in the (unsorted) radius_indices, but that is okay since we compare with ">"
     {
       if ((*input_)[radius_indices[k]].z > query_z)
       {
@@ -160,7 +168,7 @@ pcl::LocalMaximum<PointT>::applyFilterIndices (Indices &indices)
     // visited, excluding them from future consideration as local maxima
     if (point_is_max[iii])
     {
-      for (std::size_t k = 1; k < radius_indices.size (); ++k)  // k = 1 is the first neighbor
+      for (std::size_t k = 0; k < radius_indices.size (); ++k) // the query point itself is in the (unsorted) radius_indices, but it must also be marked as visited
       {
         point_is_visited[radius_indices[k]] = true;
       }

test/filters/test_local_maximum.cpp

@@ -45,15 +45,13 @@
 
 using namespace pcl;
 
-PointCloud<PointXYZ> cloud;
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (Filters, LocalMaximum)
 {
   PointCloud<PointXYZ> cloud_in, cloud_out;
 
   cloud_in.height = 1;
-  cloud_in.width = 3;
+  cloud_in.width = 4;
   cloud_in.is_dense = true;
   cloud_in.resize (4);
 
@@ -73,6 +71,31 @@ TEST (Filters, LocalMaximum)
   EXPECT_EQ (3, cloud_out.size ());
 }
 
+TEST (Filters, LocalMaximum2) // Same as the "LocalMaximum" test above, but the points have a different order
+{
+  PointCloud<PointXYZ> cloud_in, cloud_out;
+
+  cloud_in.height = 1;
+  cloud_in.width = 4;
+  cloud_in.is_dense = true;
+  cloud_in.resize (4);
+
+  cloud_in[0].x = 0.5;  cloud_in[0].y = 0.5;  cloud_in[0].z = 1; // this one should be removed
+  cloud_in[1].x = 0;    cloud_in[1].y = 0;    cloud_in[1].z = 0.25;
+  cloud_in[2].x = 0.25; cloud_in[2].y = 0.25; cloud_in[2].z = 0.5;
+  cloud_in[3].x = 5;    cloud_in[3].y = 5;    cloud_in[3].z = 2;
+
+  LocalMaximum<PointXYZ> lm;
+  lm.setInputCloud (cloud_in.makeShared ());
+  lm.setRadius (1.0f);
+  lm.filter (cloud_out);
+
+  EXPECT_EQ (0.25f, cloud_out[0].z);
+  EXPECT_EQ (0.50f, cloud_out[1].z);
+  EXPECT_EQ (2.00f, cloud_out[2].z);
+  EXPECT_EQ (3, cloud_out.size ());
+}
+
 /* ---[ */
 int
 main (int argc, char** argv)