[PCL]点云渐进形态学滤波

PCL支持点云的形态学滤波，四种操作：侵蚀、膨胀、开（先侵蚀后膨胀）、闭（先膨胀后侵蚀）

关于渐进的策略，在初始cell_size_ 的基础上逐渐变大。满足如下公式：

$$window\_size =cell\_size *(2*base^{k}+1)$$

$$window\_size =cell\_size *(2*base*(k+1)+1)$$

 // Compute the series of window sizes and height thresholds
   std::vector<float> height_thresholds;
   std::vector<float> window_sizes;
   int iteration = ;
   float window_size = 0.0f;
   float height_threshold = 0.0f;

   while (window_size < max_window_size_)
   {
     // Determine the initial window size.
     if (exponential_)
       window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f);
     else
       window_size = cell_size_ * (2.0f * (iteration+) * base_ + 1.0f);

     // Calculate the height threshold to be used in the next iteration.
     if (iteration == )
       height_threshold = initial_distance_;
     else
       height_threshold = slope_ * (window_size - window_sizes[iteration-]) * cell_size_ + initial_distance_;

     // Enforce max distance on height threshold
     if (height_threshold > max_distance_)
       height_threshold = max_distance_;

     window_sizes.push_back (window_size);
     height_thresholds.push_back (height_threshold);

     iteration++;
   }

在#include <pcl/filters/morphological_filter.h>中定义了枚举类型

  enum MorphologicalOperators
   {
     MORPH_OPEN,
     MORPH_CLOSE,
     MORPH_DILATE,
     MORPH_ERODE
   };

具体实现：

 template <typename PointT> void
 pcl::applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
                                  float resolution, const int morphological_operator,
                                  pcl::PointCloud<PointT> &cloud_out)
 {
   if (cloud_in->empty ())
     return;

   pcl::copyPointCloud<PointT, PointT> (*cloud_in, cloud_out);

   pcl::octree::OctreePointCloudSearch<PointT> tree (resolution);

   tree.setInputCloud (cloud_in);
   tree.addPointsFromInputCloud ();

   float half_res = resolution / 2.0f;

   switch (morphological_operator)
   {
     case MORPH_DILATE:
     case MORPH_ERODE:
     {
       for (size_t p_idx = ; p_idx < cloud_in->points.size (); ++p_idx)
       {
         Eigen::Vector3f bbox_min, bbox_max;
         std::vector<int> pt_indices;
         float minx = cloud_in->points[p_idx].x - half_res;
         float miny = cloud_in->points[p_idx].y - half_res;
         float minz = -std::numeric_limits<float>::max ();
         float maxx = cloud_in->points[p_idx].x + half_res;
         float maxy = cloud_in->points[p_idx].y + half_res;
         float maxz = std::numeric_limits<float>::max ();
         bbox_min = Eigen::Vector3f (minx, miny, minz);
         bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
         tree.boxSearch (bbox_min, bbox_max, pt_indices);

         if (pt_indices.size () > )
         {
           Eigen::Vector4f min_pt, max_pt;
           pcl::getMinMax3D<PointT> (*cloud_in, pt_indices, min_pt, max_pt);

           switch (morphological_operator)
           {
             case MORPH_DILATE:
             {
               cloud_out.points[p_idx].z = max_pt.z ();
               break;
             }
             case MORPH_ERODE:
             {
               cloud_out.points[p_idx].z = min_pt.z ();
               break;
             }
           }
         }
       }
       break;
     }
     case MORPH_OPEN:
     case MORPH_CLOSE:
     {
       pcl::PointCloud<PointT> cloud_temp;

       pcl::copyPointCloud<PointT, PointT> (*cloud_in, cloud_temp);

       for (size_t p_idx = ; p_idx < cloud_temp.points.size (); ++p_idx)
       {
         Eigen::Vector3f bbox_min, bbox_max;
         std::vector<int> pt_indices;
         float minx = cloud_temp.points[p_idx].x - half_res;
         float miny = cloud_temp.points[p_idx].y - half_res;
         float minz = -std::numeric_limits<float>::max ();
         float maxx = cloud_temp.points[p_idx].x + half_res;
         float maxy = cloud_temp.points[p_idx].y + half_res;
         float maxz = std::numeric_limits<float>::max ();
         bbox_min = Eigen::Vector3f (minx, miny, minz);
         bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
         tree.boxSearch (bbox_min, bbox_max, pt_indices);

         if (pt_indices.size () > )
         {
           Eigen::Vector4f min_pt, max_pt;
           pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt);

           switch (morphological_operator)
           {
             case MORPH_OPEN:
             {
               cloud_out.points[p_idx].z = min_pt.z ();
               break;
             }
             case MORPH_CLOSE:
             {
               cloud_out.points[p_idx].z = max_pt.z ();
               break;
             }
           }
         }
       }

       cloud_temp.swap (cloud_out);

       for (size_t p_idx = ; p_idx < cloud_temp.points.size (); ++p_idx)
       {
         Eigen::Vector3f bbox_min, bbox_max;
         std::vector<int> pt_indices;
         float minx = cloud_temp.points[p_idx].x - half_res;
         float miny = cloud_temp.points[p_idx].y - half_res;
         float minz = -std::numeric_limits<float>::max ();
         float maxx = cloud_temp.points[p_idx].x + half_res;
         float maxy = cloud_temp.points[p_idx].y + half_res;
         float maxz = std::numeric_limits<float>::max ();
         bbox_min = Eigen::Vector3f (minx, miny, minz);
         bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
         tree.boxSearch (bbox_min, bbox_max, pt_indices);

         if (pt_indices.size () > )
         {
           Eigen::Vector4f min_pt, max_pt;
           pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt);

           switch (morphological_operator)
           {
             case MORPH_OPEN:
             default:
             {
               cloud_out.points[p_idx].z = max_pt.z ();
               break;
             }
             case MORPH_CLOSE:
             {
               cloud_out.points[p_idx].z = min_pt.z ();
               break;
             }
           }
         }
       }
       break;
     }
     default:
     {
       PCL_ERROR ("Morphological operator is not supported!\n");
       break;
     }
   }

   return;
 }

而渐进形态学滤波则是逐渐增大窗口，循环进行开操作

template <typename PointT> void
pcl::ProgressiveMorphologicalFilter<PointT>::extract (std::vector<int>& ground)
{
  bool segmentation_is_possible = initCompute ();
  if (!segmentation_is_possible)
  {
    deinitCompute ();
    return;
  }

  // Compute the series of window sizes and height thresholds
  std::vector<float> height_thresholds;
  std::vector<float> window_sizes;
  int iteration = ;
  float window_size = 0.0f;
  float height_threshold = 0.0f;

  while (window_size < max_window_size_)
  {
    // Determine the initial window size.
    if (exponential_)
      window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f);
    else
      window_size = cell_size_ * (2.0f * (iteration+) * base_ + 1.0f);

    // Calculate the height threshold to be used in the next iteration.
    if (iteration == )
      height_threshold = initial_distance_;
    else
      height_threshold = slope_ * (window_size - window_sizes[iteration-]) * cell_size_ + initial_distance_;

    // Enforce max distance on height threshold
    if (height_threshold > max_distance_)
      height_threshold = max_distance_;

    window_sizes.push_back (window_size);
    height_thresholds.push_back (height_threshold);

    iteration++;
  }

  // Ground indices are initially limited to those points in the input cloud we
  // wish to process
  ground = *indices_;

  // Progressively filter ground returns using morphological open
  for (size_t i = ; i < window_sizes.size (); ++i)
  {
    PCL_DEBUG ("      Iteration %d (height threshold = %f, window size = %f)...",
               i, height_thresholds[i], window_sizes[i]);

    // Limit filtering to those points currently considered ground returns
    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
    pcl::copyPointCloud<PointT> (*input_, ground, *cloud);

    // Create new cloud to hold the filtered results. Apply the morphological
    // opening operation at the current window size.
    typename pcl::PointCloud<PointT>::Ptr cloud_f (new pcl::PointCloud<PointT>);
    pcl::applyMorphologicalOperator<PointT> (cloud, window_sizes[i], MORPH_OPEN, *cloud_f);

    // Find indices of the points whose difference between the source and
    // filtered point clouds is less than the current height threshold.
    std::vector<int> pt_indices;
    for (size_t p_idx = ; p_idx < ground.size (); ++p_idx)
    {
      float diff = cloud->points[p_idx].z - cloud_f->points[p_idx].z;
      if (diff < height_thresholds[i])
        pt_indices.push_back (ground[p_idx]);
    }

    // Ground is now limited to pt_indices
    ground.swap (pt_indices);

    PCL_DEBUG ("ground now has %d points\n", ground.size ());
  }

  deinitCompute ();
}