pcl源码分析之高斯滤波（平滑）

---

前言

高斯平滑是点云滤波中常见的方法之一，这里将pcl高斯滤波的源码分析一下，加深对原理和代码的理解。

---

一、调用实例

#include <pcl/filters/convolution_3d.h>

	if (cloud == nullptr) return false;
	if (cloud->points.size() < 10) return false;

	//Set up the Gaussian Kernel
	pcl::filters::GaussianKernel<pcl::PointXYZ, pcl::PointXYZ>::Ptr kernel(new pcl::filters::GaussianKernel<pcl::PointXYZ, pcl::PointXYZ>);
	(*kernel).setSigma(cigma);
	(*kernel).setThresholdRelativeToSigma(4);

	//Set up the KDTree
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	(*kdtree).setInputCloud(cloud);

	//Set up the Convolution Filter
	pcl::filters::Convolution3D <pcl::PointXYZ, pcl::PointXYZ, pcl::filters::GaussianKernel<pcl::PointXYZ, pcl::PointXYZ> > convolution;
	convolution.setKernel(*kernel);
	convolution.setInputCloud(cloud);
	convolution.setSearchMethod(kdtree);
	convolution.setRadiusSearch(radius);
	convolution.setNumberOfThreads(10);//important! Set Thread number for openMP
	convolution.convolve(*cloud_out);

二、源码分析

这里主要看Convolution3D 类，该类中，最最重要的是convolve函数

1、convolve函数

template <typename PointInT, typename PointOutT, typename KernelT> void
pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::convolve (PointCloudOut& output)
{
  //初始化
  if (!initCompute ())
  {
    PCL_ERROR ("[pcl::filters::Convlution3D::convolve] init failed!\n");
    return;
  }
  output.resize (surface_->size ());
  output.width = surface_->width;
  output.height = surface_->height;
  output.is_dense = surface_->is_dense;
  Indices nn_indices;
  std::vector<float> nn_distances;
  
  //循环时用到了多线程
#pragma omp parallel for \
  default(none) \
  shared(output) \
  firstprivate(nn_indices, nn_distances) \
  num_threads(threads_)
  for (std::int64_t point_idx = 0; point_idx < static_cast<std::int64_t> (surface_->size ()); ++point_idx)
  {
    const PointInT& point_in = surface_->points [point_idx];
    PointOutT& point_out = output [point_idx];
    
    //在处理浮点数输入时，可以使用std::isfinite()函数来验证输入是否为有效的有限数
    //检查当前点是否为有效点以及找当前点search_radius_范围内的近邻点
    if (isFinite (point_in) &&
        tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances))
    {
      //进行高斯平滑处理，这里是最核心部分，输出的点即为平滑后的点
      point_out = kernel_ (nn_indices, nn_distances);
    }
    else
    {
      kernel_.makeInfinite (point_out);
      output.is_dense = false;
    }
  }
}

2、initCompute 函数

template <typename PointInT, typename PointOutT, typename KernelT> bool
pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::initCompute ()
{
  //这里调用了PCLBase里的initCompute 函数，其作用主要是：
  //1.查看输入点云是否为空；
  //2.查看输入点云的id是否为空；
  //3.查看输入点云的实际数量与其id是否相等，不对等则重新设置id与输入点云数量保持一致
  if (!PCLBase<PointInT>::initCompute ())
  {
    PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed!\n");
    return (false);
  }
  
  // Initialize the spatial locator
  //kdtree初始化与构造
  if (!tree_)
  {
    if (input_->isOrganized ())
      tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
    else
      tree_.reset (new pcl::search::KdTree<PointInT> (false));
  }
  // If no search surface has been defined, use the input dataset as the search surface itself
  //如果没有定义搜索曲面，则使用输入数据集作为搜索曲面本身
  if (!surface_)
    surface_ = input_;
  // Send the surface dataset to the spatial locator
  //构造kdtree
  tree_->setInputCloud (surface_);
  
  // Do a fast check to see if the search parameters are well defined
  //搜索半径是否设置
  if (search_radius_ <= 0.0)
  {
    PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0\n",
               search_radius_);
    return (false);
  }
  
  // Make sure the provided kernel implements the required interface
  //将kernel_转换成ConvolvingKernel<PointInT, PointOutT>类型，等于1则成功，等于0则失败
  //实际上也就是检测kernel_和ConvolvingKernel是否是同一类型
  if (dynamic_cast<ConvolvingKernel<PointInT, PointOutT>* > (&kernel_) == 0)
  {
    PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed : ");
    PCL_ERROR ("kernel_ must implement ConvolvingKernel interface\n!");
    return (false);
  }
  kernel_.setInputCloud (surface_);
  // Initialize convolving kernel
  //高斯核初始化
  if (!kernel_.initCompute ())
  {
    PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n");
    return (false);
  }
  return (true);
}

3、GaussianKernel函数

这里可以回顾下高斯函数和高斯核函数以及他们之间的区别

template<typename PointInT, typename PointOutT> PointOutT
pcl::filters::GaussianKernel<PointInT, PointOutT>::operator() (const Indices& indices,
                                                               const std::vector<float>& distances)
{
  using namespace pcl::common;
  PointOutT result;
  float total_weight = 0;
  std::vector<float>::const_iterator dist_it = distances.begin ();

  for (Indices::const_iterator idx_it = indices.begin ();
       idx_it != indices.end ();
       ++idx_it, ++dist_it)
  {
    if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it]))
    {
      //从这里看，这里的高斯核函数为：
      //e^(-(x^2 + y^2 + z^2) / 2 * sigma^2)
      //实际上是：分子为当前点和其近邻点之间距离的平方（也即dist_it）
      float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
      result += weight * (*input_) [*idx_it];//点乘以权重相加
      total_weight += weight;//权重相加
    }
  }
  if (total_weight != 0)
    result /= total_weight;//得到平滑后的新坐标
  else
    makeInfinite (result);

  return (result);
}

---

总结

分析了一下pcl里高斯滤波的源码及其原理，有些收获，其实和图像处理中的高斯滤波原理差不多，唯一不同的的是高斯核一个是二维的，一个是三维的。