【PCL学习笔记】之快速点特征直方图FPFH - pcl::FPFHSignature33

1. pcl::FPFHSignature33介绍

先来看 pcl::FPFHSignature33 的源码：

  /** \brief A point structure representing the Fast Point Feature Histogram (FPFH).
    * \ingroup common
    */
  struct FPFHSignature33
  {
    float histogram[33];
    static int descriptorSize () { return 33; }

    friend std::ostream& operator << (std::ostream& os, const FPFHSignature33& p);
  };

这里就是一个很简单 struct，重要的数据成员就是一个浮点型数组，存储的是点云中某点的FPFH特征，一共33个值。

那么什么是FPFH特征呢？

点快速特征直方图(Fast Point Feature Histogram, FPFH)通俗地来说就是表示三维点的一种特征，类似二维图像中的SIFT、SURF、ORB特征等，都是携带了某种特定的信息。类似二维图像的配准，FPFH也可以用于三维点云之间的配准。

FPFH在PCL中的实现是pcl_features库的一部分。默认的FPFH实现使用11个统计子区间，特征直方图被分别计算然后合并得出了浮点值的一个33元素的特征向量，这些值就保存在pcl::FPFHSignature33中。

那么FPFH的求解过程又是怎样呢？这里我们需要先来了解以下它的前身——PFH。

2. PFH

2.1 PFH特征介绍

点特征直方图（PFH）是一种的姿态不变的局部特征。其计算方式是通过参数化查询点与邻域点之间的空间差异，并形成一个多维直方图对点的k邻域内的几何属性进行描述。直方图所在的高维超空间为特征表示提供了一个可度量的信息，对点云对应曲面的6维姿态来说它具有不变性，并且在不同的采样密度或邻域的噪音等级下具有鲁棒性。

点特征直方图（PFH）表示法是基于点与其k邻域之间的关系以及它们的估计法线。它考虑估计法线方向之间所有的相互作用，试图捕获最好的样本表面变化情况，以描述样本的几何特征。因此，合成的特征超空间取决于每个点的表面法线估计的质量。简而言之，某点的PFH跟其邻域内每个近邻点以及每个点的法线有关。

图1表示的是一个查询点（p_q）的PFH计算的影响区域。p_q 用红色标注并放在圆球的中间位置，邻域半径为r， p_q的所有k近邻元素（即与点p_q的距离小于半径r的所有点）全部互相连接在一个网络中。最终的PFH描述子需要通过计算邻域内所有两点之间关系而得到，因此其计算复杂度是O(k²) 。
在这里插入图片描述

2.2 PFH计算过程

为点云中的每一个点求得表面法线n；
确定一个邻域半径r，以各点为原点，选择半径r内的所有近邻并计算其PFH特征三要素；

对于每个点 $p_i，p_j，i≠j$ ，在该邻域中具有法线n_i，n_j，接下来一个局部坐标系的坐标轴计算如下：

$\begin{cases} \bold{u=n_i} \\ \bold{v=(p_j-p_i)\times u} \\ \bold{w=u\times v}\\ \end{cases}$

表面法线的角度变换可以由下式确定：
$\begin{cases} \alpha = \bold{v \cdot n_j} \\ \phi = \bold{\frac{u \cdot (p_j - p_i)}{|| p_j - p_i ||}} \\ \theta = \arctan\bold{(w \cdot n_j, u \cdot n_j)} \\ \end{cases}$

这里的 $(\alpha, \phi, \theta )$ 就是某点的PFH特征的三个元素，见图2：
在这里插入图片描述

归一化及特征分配
要生成最终的对每个查询点的PFH特征，需要将这三个元素将放到直方图里面。分发的过程就是把所有的特征(上面的3个角度)平均分割成m小份（PCL内默认是5份），然后数每个子区间上的个数。因为这3个特征是法线间的角度，所以他们可以很好的归一化并把它们放到三角函数的圆里面。一个分发的案例是把每个特征区间进行平均分配，然后我们就有了一个 $5^3=125$ 份分发的柱状图。在这个图里面有一个区间的值不为0就代表着它的这个特征有一定的值。

在PCL的源码中用以下函数来计算点对间的三个角度特征元素：

/** \brief 计算点对的PFH特征的4个特征元素值，包含3个角度值和一个两点间的距离值
  * \param[in] cloud 点云集
  * \param[in] normals 法线集
  * \param[in] p_idx 第一个点（source）在点云中的索引
  * \param[in] q_idx 第二个点（target）在点云中的索引
  * \param[out] f1 第一个角度特征值 (angle between the projection of nq_idx and u)
  * \param[out] f2 第二个角度特征值 (angle between nq_idx and v)
  * \param[out] f3 第三个角度特征值 (angle between np_idx and |p_idx - q_idx|)
  * \param[out] f4 两点间的欧式距离
  */
bool computePairFeatures (const pcl::PointCloud<PointInT> &cloud, 
                          const pcl::PointCloud<PointNT> &normals, 
                          int p_idx, int q_idx, 
                          float &f1, float &f2, float &f3, float &f4);

这里的f1、f2、f3就是点对间PFH特征的三个元素。计算出这三个元素值后，PCL中通过下面这个函数获取该查询点的PFH：

/** \brief 由点的近邻及各自的法线计算其PFH
  * \param[in] cloud 点云集
  * \param[in] normals 法线集
  * \param[in] indices 点云集中的k个近邻的索引
  * \param[in] nr_split 每个角度特征需要划分的份数（默认为5）
  * \param[out] pfh_histogram 查询点最终的PFH
  */
void computePointPFHSignature (const pcl::PointCloud<PointInT> &cloud, 
                               const pcl::PointCloud<PointNT> &normals,
                               const std::vector<int> &indices, int nr_split,
                               Eigen::VectorXf &pfh_histogram);

2.3 PFH的缺点

计算复杂度高。已知点云P中有n个点，假设点云均匀密度，每点在邻域半径r内平均可以选到k个近邻，该算法对每个点来说计算PFH的时间复杂度为O(k^2)，那么它的点特征直方图（PFH）的理论计算复杂度就是 O(nk²)。对于实时应用或接近实时应用中，密集点云的点特征直方图（PFH）的计算，O(nk²)的计算复杂度实在不敢恭维，是一个主要的性能瓶颈。
存在大量的重复计算。在PFH的三个特征元素的计算过程中，邻域内任意两点都需要计算一次三个特征元素值。这对于相邻的点来说，尽管他们的邻域不一定完全相同，但有很大的概率会同时包含某些近邻点（邻域半径的值越大，重复的近邻就越多）。这些被同时包含的近邻点之间的会被重复地配对在一起并计算特征元素值。虽然论文及PCL了中都引入了高速缓存机制，把重复的计算的花销改成查找的花销，但其效率依然没有太大的改观。

3 FPFH

3.1 FPFH特征介绍

快速点特征直方图(Fast Point Feature Histograms, FPFH)是PFH计算方式的简化形式。它的思想在于分别计算查询点的k邻域中每一个点的简化点特征直方图(Simplified Point Feature Histogram,SPFH)，再通过一个公式将所有的SPFH加权成最后的快速点特征直方图。FPFH把算法的计算复杂度降低到了O(nk) ，但是任然保留了PFH大部分的识别特性。
在这里插入图片描述

3.2 FPFH计算过程

为查询点求得它和其k邻域内每个点之间的三个特征元素值，然后统计成一个SPFH；
分别对k邻域中的每个点确定k邻域，按第一步分别形成自己的SPFH；
对邻域中的各个SPFH进行加权统计，公式如下：
$FPFH(p) = SPFH(p) + \frac{1}{k}\sum_{i=1}^{k}(\frac{1}{w_k} \cdot SPFH(p_k))$

其中权重w_k表示查询点p与给定度量空间中的近邻点p_k之间的距离。

3.3 FPFH与PFH的主要区别

FPFH没有对全互连点的所有邻近点的计算参数进行统计，因此可能漏掉了一些重要的点对，而这些漏掉的对点可能对捕获查询点周围的几何特征有贡献。
PFH特征模型是对查询点周围的一个精确的邻域半径内，而FPFH还包括半径r范围以外的额外点对（但不超过2r的范围）；
因为采用权重计算的方式，所以FPFH结合SPFH值，重新捕获邻近重要点对的几何信息；
由于大大地降低了FPFH的整体复杂性，因此FPFH有可能使用在实时应用中；
通过分解三元组，简化了合成的直方图。也就是简单生成d分离特征直方图，对每个特征维度来单独绘制，并把它们连接在一起。

3.4 利用OpenMP提高FPFH的计算速度

对于计算速度要求苛刻的用户，PCL提供了一个FPFH估计的另一实现，它使用多核/多线程规范，利用OpenMP开发模式来提高计算速度。这个类的名称是pcl::FPFHEstimationOMP，并且它的应用程序接口（API）100%兼容单线程pcl::FPFHEstimation，这使它适合作为一个替换元件。在8核系统中，OpenMP的实现可以在6-8倍更快的计算时间内完全同样单核系统上的计算。

3.5 相关源码

/** \brief 根据(f1, f2, f3)三个特征值计算单个SPFH
  * \param[in] cloud    输入点云
  * \param[in] normals  法线信息
  * \param[in] p_idx    查询点(source)
  * \param[in] row      在特征直方图里对应的行号
  * \param[in] indices  k近邻索引
  * \param[out] hist_f1 f1特征矩阵
  * \param[out] hist_f2 f2特征矩阵
  * \param[out] hist_f3 f3特征矩阵
  */
template <typename PointInT, typename PointNT, typename PointOutT> void 
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (
    const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
    int p_idx, int row, const std::vector<int> &indices,
    Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
{
  Eigen::Vector4f pfh_tuple;
  // 从直方图矩阵中获取特征的分隔数bins
  int nr_bins_f1 = static_cast<int> (hist_f1.cols ());
  int nr_bins_f2 = static_cast<int> (hist_f2.cols ());
  int nr_bins_f3 = static_cast<int> (hist_f3.cols ());

  // Factorization constant
  float hist_incr = 100.0f / static_cast<float>(indices.size () - 1);

  // 对近邻点进行迭代计算
  for (size_t idx = 0; idx < indices.size (); ++idx)
  {
    // 对自身不计算
    if (p_idx == indices[idx])
        continue;

    // 对查询点p_idx以及它的某一近邻这对点计算三个特征值
    if (!computePairFeatures (cloud, normals, p_idx, indices[idx], pfh_tuple[0], pfh_tuple[1], pfh_tuple[2], pfh_tuple[3]))
        continue;

    // 归一化f1, f2, f3 并存入直方图中
    int h_index = static_cast<int> (floor (nr_bins_f1 * ((pfh_tuple[0] + M_PI) * d_pi_)));
    if (h_index < 0)           h_index = 0;
    if (h_index >= nr_bins_f1) h_index = nr_bins_f1 - 1;
    hist_f1 (row, h_index) += hist_incr; // 对应的区间中落了值，统计的直方图数量+100/k

    h_index = static_cast<int> (floor (nr_bins_f2 * ((pfh_tuple[1] + 1.0) * 0.5)));
    if (h_index < 0)           h_index = 0;
    if (h_index >= nr_bins_f2) h_index = nr_bins_f2 - 1;
    hist_f2 (row, h_index) += hist_incr;

    h_index = static_cast<int> (floor (nr_bins_f3 * ((pfh_tuple[2] + 1.0) * 0.5)));
    if (h_index < 0)           h_index = 0;
    if (h_index >= nr_bins_f3) h_index = nr_bins_f3 - 1;
    hist_f3 (row, h_index) += hist_incr;
  }
}


/** \brief 加权SPFH以计算最终的FPFH 
  * \param[in] hist_f1 f1特征向量
  * \param[in] hist_f2 f2特征向量
  * \param[in] hist_f3 f3特征向量
  * \param[in] indices 查询点p_idx的k近邻索引
  * \param[in] dists   查询点p_idx到其k近邻的距离
  * \param[out] fpfh_histogram 最后的FPFH
  */
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::weightPointSPFHSignature (
    const Eigen::MatrixXf &hist_f1, const Eigen::MatrixXf &hist_f2, const Eigen::MatrixXf &hist_f3,
    const std::vector<int> &indices, const std::vector<float> &dists, Eigen::VectorXf &fpfh_histogram)
{
  assert (indices.size () == dists.size ());
  double sum_f1 = 0.0, sum_f2 = 0.0, sum_f3 = 0.0;	// 各特征值的总和
  float weight = 0.0, val_f1, val_f2, val_f3;		// 权值和

  // 获取特征分隔数bins
  int nr_bins_f1 = static_cast<int> (hist_f1.cols ());
  int nr_bins_f2 = static_cast<int> (hist_f2.cols ());
  int nr_bins_f3 = static_cast<int> (hist_f3.cols ());
  int nr_bins_f12 = nr_bins_f1 + nr_bins_f2;

  // 清空直方图，这里共33个
  fpfh_histogram.setZero (nr_bins_f1 + nr_bins_f2 + nr_bins_f3);

  // Use the entire patch
  for (size_t idx = 0, data_size = indices.size (); idx < data_size; ++idx)
  {
    // 查询点本身不做计算
    if (dists[idx] == 0)
      continue;

    // 标准权值为查询点到此近邻的距离的倒数
    weight = 1.0f / dists[idx];

    // 对查询点及其近邻的SPFH进行加权
    for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
    {
      val_f1 = hist_f1 (indices[idx], f1_i) * weight;
      sum_f1 += val_f1;
      fpfh_histogram[f1_i] += val_f1;
    }

    for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
    {
      val_f2 = hist_f2 (indices[idx], f2_i) * weight;
      sum_f2 += val_f2;
      fpfh_histogram[f2_i + nr_bins_f1] += val_f2;
    }

    for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
    {
      val_f3 = hist_f3 (indices[idx], f3_i) * weight;
      sum_f3 += val_f3;
      fpfh_histogram[f3_i + nr_bins_f12] += val_f3;
    }
  }

  if (sum_f1 != 0)
    sum_f1 = 100.0 / sum_f1;           // histogram values sum up to 100
  if (sum_f2 != 0)
    sum_f2 = 100.0 / sum_f2;           // histogram values sum up to 100
  if (sum_f3 != 0)
    sum_f3 = 100.0 / sum_f3;           // histogram values sum up to 100

  // 调整最终的FPFH值，乘了100
  for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
    fpfh_histogram[f1_i] *= static_cast<float> (sum_f1);
  for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
    fpfh_histogram[f2_i + nr_bins_f1] *= static_cast<float> (sum_f2);
  for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
    fpfh_histogram[f3_i + nr_bins_f12] *= static_cast<float> (sum_f3);
}


/** \brief 计算FPFH描述子,最终公式 */
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  std::vector<int> nn_indices (k_);	// 存储某点的k个近邻的索引
  std::vector<float> nn_dists (k_);	// 存储某点到其k个近邻的距离

  std::vector<int> spfh_hist_lookup;	// 存储SPFH特征查询表
  // 计算查询点的SPFH特征，得到SPFH特征查询表
  computeSPFHSignatures (spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_); 

  output.is_dense = true;
  // 如果输入点云是dense的，就不用对每个点坐标的有效性进行判断（针对无效值NaN/和无限值Inf） 
  if (input_->is_dense)
  {
    // 对每个近邻进行迭代计算
    for (size_t idx = 0; idx < indices_->size (); ++idx)
    {
      // 找到每个近邻的k近邻
      if (this->searchForNeighbors((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
      {
        // 如果某一近邻的近邻查找失败，则输出点云非dense，FPFH置为NaN
        for (int d = 0; d < fpfh_histogram_.size (); ++d)
          output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
    
        output.is_dense = false;
        continue;
      }

      // 某一近邻的近邻查找成功， 将近近邻索引值映射到SPFH矩阵里的行号
      // instead of indices into surface_->points
      for (size_t i = 0; i < nn_indices.size (); ++i)
        nn_indices[i] = spfh_hist_lookup[nn_indices[i]];

      // 加权计算FPFH特征
      weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);

      // 将FPFH特征输出
      for (int d = 0; d < fpfh_histogram_.size (); ++d)
        output.points[idx].histogram[d] = fpfh_histogram_[d];
    }
  }
  else // 输入点云非dense，加了一个坐标值有效性判断，其他一样
  {
    // Iterate over the entire index vector
    for (size_t idx = 0; idx < indices_->size (); ++idx)
    {
      if (!isFinite ((*input_)[(*indices_)[idx]]) ||
          this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
      {
        for (int d = 0; d < fpfh_histogram_.size (); ++d)
          output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
    
        output.is_dense = false;
        continue;
      }

      // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices 
      // instead of indices into surface_->points
      for (size_t i = 0; i < nn_indices.size (); ++i)
        nn_indices[i] = spfh_hist_lookup[nn_indices[i]];

      // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
      weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);

      // ...and copy it into the output cloud
      for (int d = 0; d < fpfh_histogram_.size (); ++d)
        output.points[idx].histogram[d] = fpfh_histogram_[d];
    }
  }
}

4. 实验情况

实验设备：惠普ProDesk 680 G1 TWR
配置：处理器Intel Corei5 4590，内存4G
操作系统：Ubuntu 14.04

实验源码：

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>             // 法线
#include <pcl/features/fpfh_omp.h>
#include <pcl/visualization/pcl_visualizer.h>   // 可视化
#include <pcl/visualization/pcl_plotter.h>

#include <iostream>
#include <chrono>

using namespace std;

boost::mutex cloud_mutex;
pcl::visualization::PCLPlotter plotter;
//pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::PointNormal, pcl::FPFHSignature33> fpfh_omp;
pcl::FPFHEstimation<pcl::PointXYZ, pcl::PointNormal, pcl::FPFHSignature33> fpfh;
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());

// structure used to pass arguments to the callback function
struct callback_args {
    pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d;
    pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};

// callback function
void pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
{
    plotter.clearPlots();
    struct callback_args* data = (struct callback_args *)args;
    if (event.getPointIndex() == -1)
        return;
    pcl::PointXYZ current_point;
    event.getPoint(current_point.x, current_point.y, current_point.z);
    data->clicked_points_3d->points.clear();
    data->clicked_points_3d->points.push_back(current_point);

    // Draw clicked points in red:
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red(data->clicked_points_3d, 255, 0, 0);
    data->viewerPtr->removePointCloud("clicked_points");
    data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
    data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
    std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;

    int num = event.getPointIndex();
    plotter.addFeatureHistogram<pcl::FPFHSignature33>(*fpfhs, "fpfh", num);
    plotter.plot();
}

int main(int argc, char *argv[]) {
    if (argc < 2) {
        printf(" -- Usage: %s <pointcloud file>\n", argv[0]);
        return -1;
    }

    bool display = true;
    bool downSampling = false;

    // load pcd/ply point cloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr model(new pcl::PointCloud<pcl::PointXYZ>()); // 模型点云
    if (pcl::io::loadPCDFile(argv[1], *model) < 0) {
        std::cerr << "Error loading model cloud." << std::endl;
        return -1;
    }
    std::cout << "Cloud size: " << model->points.size() << std::endl;

//    cloud_mutex.lock();// for not overwriting the point cloud
    if (downSampling) {
        // create the filtering object
        std::cout << "Number of points before downSampling: " << model->points.size() << std::endl;
        pcl::VoxelGrid<pcl::PointXYZ> sor;
        sor.setInputCloud(model);
        sor.setLeafSize(0.01, 0.01, 0.01);
        sor.filter(*model);
        std::cout << "Number of points after downSampling: " << model->points.size() << std::endl;
    }

    //  Normal estimation
    auto t1 = chrono::steady_clock::now();
    pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
    pcl::PointCloud<pcl::PointNormal>::Ptr normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    ne.setInputCloud(model);
    ne.setSearchMethod(tree);
    ne.setKSearch(10);
//    ne.setRadiusSearch(0.03);
    ne.compute(*normals);
    auto t2 = chrono::steady_clock::now();
    auto dt = chrono::duration_cast<chrono::duration<double> >(t2 - t1).count();
    cout << "Time cost of Normal estimation: " << dt << endl;

    // fpfh or fpfh_omp
    fpfh_omp.setInputCloud(model);
    fpfh_omp.setInputNormals(normals);
    fpfh_omp.setSearchMethod(tree);
    fpfh_omp.setNumberOfThreads(8);
    fpfh_omp.setRadiusSearch(0.05);
    fpfh_omp.compute(*fpfhs);
//    fpfh.setInputCloud(model);
//    fpfh.setInputNormals(normals);
//    fpfh.setSearchMethod(tree);
//    fpfh.setRadiusSearch(0.05);
//    fpfh.compute(*fpfhs);
    t1 = chrono::steady_clock::now();
    dt = chrono::duration_cast<chrono::duration<double> >(t1 - t2).count();
    cout << "Time cost of FPFH estimation: " << dt << endl;

    pcl::FPFHSignature33 descriptor;
    for (int i=0; i<10; ++i) {
        int index = i + rand() % model->points.size();
        descriptor = fpfhs->points[index];
        std::cout << " -- fpfh for point "<< index << ":\n" << descriptor << std::endl;
    }

    if (display) {
        plotter.addFeatureHistogram<pcl::FPFHSignature33>(*fpfhs, "fpfh",100);

        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Viewer"));
        viewer->setBackgroundColor(0, 0, 0);
        viewer->addPointCloud<pcl::PointXYZ>(model, "model");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "model");
        viewer->addPointCloudNormals<pcl::PointXYZ, pcl::PointNormal>(model, normals, 10, 0.05, "normals");  // display every 1 points, and the scale of the arrow is 10
        viewer->addCoordinateSystem(1.0);
        viewer->initCameraParameters();

        // Add point picking callback to viewer:
        struct callback_args cb_args;
        pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d(new pcl::PointCloud<pcl::PointXYZ>);
        cb_args.clicked_points_3d = clicked_points_3d;
        cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
        viewer->registerPointPickingCallback(pp_callback, (void*)&cb_args);
        std::cout << "Shift + click on three floor points, then press 'Q'..." << std::endl;

//        viewer->spin();
//        cloud_mutex.unlock();

        while (!viewer->wasStopped()) {
            viewer->spinOnce(100); // Spin until 'Q' is pressed
            boost::this_thread::sleep(boost::posix_time::microseconds(100000));
        }
    }

    return 0;
}

实验一用传统的pcl::FPFHEstimation实现，123870个点计算FPFH耗时0.5s左右。
在这里插入图片描述