From d34bf24bbf7bb3d0f224e61e7f26022c267221d5 Mon Sep 17 00:00:00 2001
From: Kentaro Wada <www.kentaro.wada@gmail.com>
Date: Wed, 9 Dec 2015 10:59:28 +0900
Subject: [PATCH] ExtractIndices: Fix opposite negative points processing

Sorry. This bug is from https://github.com/PointCloudLibrary/pcl/pull/1462 by me
and I found the logic is opposite after merged.
---
 filters/src/extract_indices.cpp | 16 ++++++++--------
 test/filters/test_filters.cpp   | 18 ++++++++++++------
 2 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/filters/src/extract_indices.cpp b/filters/src/extract_indices.cpp
index 412feaee019..ab88180e93e 100644
--- 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
index 55c909b399a..94a1db3773c 100644
--- 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;