ExtractIndices: Fix opposite negative points processing

Sorry. This bug is from PointCloudLibrary#1462 by me and I found the logic is opposite after merged.
wkentaro · Dec 9, 2015 · d34bf24 · d34bf24
1 parent 5f37029
commit d34bf24
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 14 deletions.
diff --git a/filters/src/extract_indices.cpp b/filters/src/extract_indices.cpp
@@ -48,6 +48,14 @@ pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
   {
     output = *input_;
     if (negative_)
+    {
+      // Prepare the output and copy the data
+      for (size_t i = 0; i < indices_->size (); ++i)
+        for (size_t j = 0; j < output.fields.size(); ++j)
+          memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[j].offset],
+                  &user_filter_value_, sizeof(float));
+    }
+    else
     {
       // Prepare a vector holding all indices
       std::vector<int> all_indices (input_->width * input_->height);
@@ -68,14 +76,6 @@ pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
           memcpy (&output.data[remaining_indices[i] * output.point_step + output.fields[j].offset],
                   &user_filter_value_, sizeof(float));
     }
-    else
-    {
-      // Prepare the output and copy the data
-      for (size_t i = 0; i < indices_->size (); ++i)
-        for (size_t j = 0; j < output.fields.size(); ++j)
-          memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[j].offset],
-                  &user_filter_value_, sizeof(float));
-    }
     if (!pcl_isfinite (user_filter_value_))
       output.is_dense = false;
     return;

diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp
@@ -195,9 +195,12 @@ TEST (ExtractIndices, Filters)
   EXPECT_EQ (output.width, cloud->width);
   EXPECT_EQ (output.height, cloud->height);
 
-  EXPECT_EQ (output.points[1].x, cloud->points[1].x);
-  EXPECT_EQ (output.points[1].y, cloud->points[1].y);
-  EXPECT_EQ (output.points[1].z, cloud->points[1].z);
+  EXPECT_EQ (output.points[0].x, cloud->points[0].x);
+  EXPECT_EQ (output.points[0].y, cloud->points[0].y);
+  EXPECT_EQ (output.points[0].z, cloud->points[0].z);
+  EXPECT_EQ (output.points[1].x, std::numeric_limits<float>::quiet_NaN());
+  EXPECT_EQ (output.points[1].y, std::numeric_limits<float>::quiet_NaN());
+  EXPECT_EQ (output.points[1].z, std::numeric_limits<float>::quiet_NaN());
 
   ei2.setNegative (true);
   ei2.setKeepOrganized (true);
@@ -209,9 +212,12 @@ TEST (ExtractIndices, Filters)
   EXPECT_EQ (output.width, cloud->width);
   EXPECT_EQ (output.height, cloud->height);
 
-  EXPECT_EQ (output.points[0].x, cloud->points[0].x);
-  EXPECT_EQ (output.points[0].y, cloud->points[0].y);
-  EXPECT_EQ (output.points[0].z, cloud->points[0].z);
+  EXPECT_EQ (output.points[0].x, std::numeric_limits<float>::quiet_NaN());
+  EXPECT_EQ (output.points[0].y, std::numeric_limits<float>::quiet_NaN());
+  EXPECT_EQ (output.points[0].z, std::numeric_limits<float>::quiet_NaN());
+  EXPECT_EQ (output.points[1].x, cloud->points[1].x);
+  EXPECT_EQ (output.points[1].y, cloud->points[1].y);
+  EXPECT_EQ (output.points[1].z, cloud->points[1].z);
 
   // Test setNegative on empty datasets
   PointCloud<PointXYZ> empty, result;