ExtractIndices: keep_organized option for PCLPointCloud2

PointCloudLibrary · Dec 3, 2015 · 052e475 · 052e475
1 parent c5dc56a
commit 052e475
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 1 deletion.
diff --git a/filters/src/extract_indices.cpp b/filters/src/extract_indices.cpp
@@ -44,7 +44,20 @@
 void
 pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
 {
-  // TODO: the PCLPointCloud2 implementation is not yet using the keep_organized_ system -FF
+  if (keep_organized_)
+  {
+    PCL_WARN("[pcl::Extractindices::applyFilter] keep_organized_: true");
+    output = *input_;
+    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,20 @@ 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);
+
   // Test setNegative on empty datasets
   PointCloud<PointXYZ> empty, result;
   ExtractIndices<PointXYZ> eie;