Merge pull request #1462 from wkentaro/extract-indices-keep-organized

ExtractIndices: keep_organized option for PCLPointCloud2
PointCloudLibrary · Dec 4, 2015 · f6ef26a · f6ef26a
2 parents 36304ed + 32e85b1
commit f6ef26a
Show file tree

Hide file tree

Showing 2 changed files with 64 additions and 1 deletion.
diff --git a/filters/src/extract_indices.cpp b/filters/src/extract_indices.cpp
@@ -44,7 +44,42 @@
 void
 pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 {
-  // TODO: the PCLPointCloud2 implementation is not yet using the keep_organized_ system -FF
+  if (keep_organized_)
+  {
+    output = *input_;
+    if (negative_)
+    {
+      // Prepare a vector holding all indices
+      std::vector<int> all_indices (input_->width * input_->height);
+      for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
+        all_indices[i] = i;
+
+      std::vector<int> indices = *indices_;
+      std::sort (indices.begin (), indices.end ());
+
+      // Get the diference
+      std::vector<int> remaining_indices;
+      set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
+                      inserter (remaining_indices, remaining_indices.begin ()));
+
+      // Prepare the output and copy the data
+      for (size_t i = 0; i < remaining_indices.size (); ++i)
+        for (size_t j = 0; j < output.fields.size(); ++j)
+          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;
+  }
   if (indices_->empty () || (input_->width * input_->height == 0))
   {
     output.width = output.height = 0;

diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp
@@ -185,6 +185,34 @@ TEST (ExtractIndices, Filters)
   EXPECT_EQ (cloud->points[cloud->points.size () - 2].y, output.points[output.points.size () - 1].y);
   EXPECT_EQ (cloud->points[cloud->points.size () - 2].z, output.points[output.points.size () - 1].z);
 
+  ei2.setNegative (false);
+  ei2.setKeepOrganized (true);
+  ei2.filter (output_blob);
+
+  fromPCLPointCloud2(output_blob, output);
+
+  EXPECT_EQ (output.points.size (), cloud->points.size ());
+  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);
+
+  ei2.setNegative (true);
+  ei2.setKeepOrganized (true);
+  ei2.filter (output_blob);
+
+  fromPCLPointCloud2(output_blob, output);
+
+  EXPECT_EQ (output.points.size (), cloud->points.size ());
+  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);
+
   // Test setNegative on empty datasets
   PointCloud<PointXYZ> empty, result;
   ExtractIndices<PointXYZ> eie;