support melodic

Author: knorth55

jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h

@@ -65,8 +65,11 @@ namespace jsk_pcl_ros
 
     virtual void filter(const sensor_msgs::PointCloud2::ConstPtr& msg);
     virtual void filterCloud(const pcl::PCLPointCloud2::Ptr pcl_cloud,
-                             pcl::PointIndices::Ptr pcl_indices_filtered,
+                             pcl::PCLPointCloud2::Ptr pcl_cloud_filtered,
                              bool keep_organized);
+    virtual void filterIndices(const pcl::PCLPointCloud2::Ptr pcl_cloud,
+                               pcl::PointIndices::Ptr pcl_indices_filtered,
+                               bool keep_organized);
 
     virtual void updateDiagnostic(
       diagnostic_updater::DiagnosticStatusWrapper &stat);

jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp

@@ -82,21 +82,19 @@ namespace jsk_pcl_ros
 
   void OrganizedStatisticalOutlierRemoval::filterCloud(
           const pcl::PCLPointCloud2::Ptr pcl_cloud,
-          pcl::PointIndices::Ptr pcl_indices_filtered,
+          pcl::PCLPointCloud2::Ptr pcl_cloud_filtered,
           bool keep_organized)
   {
-#if PCL_VERSION_COMPARE (<, 1, 9, 0)
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::fromPCLPointCloud2(*pcl_cloud, *cloud);
     if (keep_organized) {
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+      pcl::fromPCLPointCloud2(*pcl_cloud, *cloud);
       // Initialize the spatial locator
       pcl::search::Search<pcl::PointXYZ>::Ptr tree;
       tree.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
       tree->setInputCloud (cloud);
 
       // Allocate enough space to hold the results
       std::vector<int> indices (cloud->size());
-      std::vector<int> indices_filtered;
       std::vector<int> nn_indices (mean_k_);
       std::vector<float> nn_dists (mean_k_);
       std::vector<float> distances (indices.size ());
@@ -142,6 +140,9 @@ namespace jsk_pcl_ros
       double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
 
       // Build a new cloud by neglecting outliers
+      int nr_p = 0;
+      int nr_removed_p = 0;
+      bool remove_point = false;
       for (int cp = 0; cp < static_cast<int> (indices.size ()); ++cp)
       {
         bool remove_point = false;
@@ -153,12 +154,22 @@ namespace jsk_pcl_ros
         {
           remove_point = (distances[cp] > distance_threshold);
         }
-        if (!remove_point)
+
+        if (remove_point)
         {
-          indices_filtered.push_back(indices[cp]);
+          /* Set the current point to NaN. */
+          *(reinterpret_cast<float*>(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+0) = std::numeric_limits<float>::quiet_NaN();
+          *(reinterpret_cast<float*>(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+1) = std::numeric_limits<float>::quiet_NaN();
+          *(reinterpret_cast<float*>(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+2) = std::numeric_limits<float>::quiet_NaN();
         }
+        else
+        {
+          memcpy (&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step],
+                  &pcl_cloud->data[indices[cp] * pcl_cloud_filtered->point_step],
+                  pcl_cloud_filtered->point_step);
+        }
+        nr_p++;
       }
-      pcl_indices_filtered->indices = indices_filtered;
     }
     else
     {
@@ -167,9 +178,16 @@ namespace jsk_pcl_ros
       sor.setMeanK(mean_k_);
       sor.setStddevMulThresh(std_mul_);
       sor.setNegative(negative_);
-      sor.filter(pcl_indices_filtered->indices);
+      sor.filter(*pcl_cloud_filtered);
     }
-#else
+  }
+
+  void OrganizedStatisticalOutlierRemoval::filterIndices(
+          const pcl::PCLPointCloud2::Ptr pcl_cloud,
+          pcl::PointIndices::Ptr pcl_indices_filtered,
+          bool keep_organized)
+  {
+#if PCL_VERSION_COMPARE (>=, 1, 9, 0)
     pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor;
     sor.setInputCloud(pcl_cloud);
     sor.setMeanK(mean_k_);
@@ -190,15 +208,15 @@ namespace jsk_pcl_ros
       NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized.");
     }
     bool keep_organized = keep_organized_ && !msg->is_dense;
+    // filter
+#if PCL_VERSION_COMPARE (>=, 1, 9, 0)
     pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2);
     pcl_conversions::toPCL(*msg, *pcl_cloud);
-
-    // filter
     pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices());
     pcl_indices_filtered->indices.resize(pcl_cloud->data.size());
-    OrganizedStatisticalOutlierRemoval::filterCloud(pcl_cloud,
-                                                    pcl_indices_filtered,
-                                                    keep_organized);
+    OrganizedStatisticalOutlierRemoval::filterIndices(pcl_cloud,
+                                                      pcl_indices_filtered,
+                                                      keep_organized);
     pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
     pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
     ex.setInputCloud(pcl_cloud);
@@ -207,17 +225,22 @@ namespace jsk_pcl_ros
     ex.setIndices(pcl_indices_filtered);
     ex.filter(*pcl_cloud_filtered);
     pcl_conversions::fromPCL(*pcl_cloud_filtered, output);
-#if PCL_VERSION_COMPARE (<, 1, 9, 0)
-    if (keep_organized) {
-      // Copy the common fields
-      output.is_bigendian = msg->is_bigendian;
-      output.fields = msg->fields;
-      output.point_step = msg->point_step;
-      output.data.resize (msg->data.size ());
-      output.width = msg->width;
-      output.height = msg->height;
-      output.row_step = msg->point_step * msg->width;
-    }
+#else
+    pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2);
+    pcl_conversions::toPCL(*msg, *pcl_cloud);
+    pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
+    // Copy the common fields
+    pcl_cloud_filtered->is_bigendian = msg->is_bigendian;
+    pcl_cloud_filtered->fields = msg->fields;
+    pcl_cloud_filtered->point_step = msg->point_step;
+    pcl_cloud_filtered->data.resize (msg->data.size ());
+    pcl_cloud_filtered->width = msg->width;
+    pcl_cloud_filtered->height = msg->height;
+    pcl_cloud_filtered->row_step = msg->point_step * msg->width;
+    OrganizedStatisticalOutlierRemoval::filterCloud(pcl_cloud,
+                                                    pcl_cloud_filtered,
+                                                    keep_organized);
+    pcl_conversions::fromPCL(*pcl_cloud_filtered, output);
 #endif
     output.header = msg->header;
     output.is_dense = !keep_organized;