浅谈FPFH算法实现原理及其在点云配准中的应用

一. 引言

在点云计算应用中，我们希望提取一些特征点，以实现对点云的一些概述性理解。最直观的想法是，我们计算点云内那些曲率变化显著的角点或基于某种几何特征的极值点，来作为特征点。这些点通常带有显著的几何信息，并且与人视觉上的感知保持一致。但是，当我们真的用这种直观的方法尝试提取特征点时，就会发现非常严重的问题：即大量的无效点被错误的识别为特征点。究其原因，其问题在于我们无法保证点云内部点的分布是光滑的，或点云内部曲率变化保持连续。一些异常的点不可避免的存在于点云，并破坏了附近的点云的曲率变化。基于曲率或法向直接定义特征点，那些异常点就会以很大的概率被错误的识别为特征点。为了解决该问题，统计分析方法被引入。

统计分析方法在看一个点是否为特征点时，除了考虑该点的几何特征，同时还会考虑该点附近的区域，一些几何特征的分布情况。这样做的好处就是避免了一些异常的点被错误识别的情况。其原理在于，一个或少数几个点带有异常值，会破坏特征识别，但是我们分析这些异常点的邻域，对邻域内的点做统计计算，就会显著降低异常权重，使得对该点或者该区域的几何特征表述，更加稳定。FPFH算法正是基于该思想所提出。下面，我们将具体展开来介绍下FPFH算法的实现以及在点云配准中的应用。

二. PFH（Point Feature Histogram）

在介绍FPFS算法之前，我们需要先了解PFH(Point Feature Histogram)[1][2]算法。两者原理相同，而前者是后者的一种简化，目的在于提升计算效率。如上所述，对点云进行统计分析计算，即对点的邻域，针对某一种几何特征，进行统计，进而得到一种统计特征。PFH算法的设计完全基于该思路，即在点云定义一个固定的邻域，然后在这个邻域来计算一系列的特征值，建立针对特征值的直方图，已得到统计特征。下面我们来看一下具体的实现。

首先，我们需要定义一个局部坐标系。设Ps点为当前需要计算PFH的点，Pt点为它的邻域内一点。基于法向量于两线之间的向量，定义局部坐标系：

该局部坐标三个轴的计算方法如下：

ns为Ps点的法向。根据上面的公式，我们就得到了基于点Ps的一个局部坐标系。根据该坐标系，我们计算Pt点与Ps点之间的三元组特征表示：

这样，我们就得到了以Ps为源点，对Pt的三元组特征表示。PFH计算在某一点邻域内，所有点对的三元组特征，并根据这些特征，建立直方图表示，进而得到PFH特征表述子。

 如上图所示，点Pq为源点，在定义的邻域内有五个点。这六个点的点对数量为C62，即

换句话说，在邻域内共有15条线，对应15组三元组数据。对这15组三元组数据进行三维的直方图统计，以得到一个125维的特征向量，即为PFH。可以看到，PFH除了考虑源点与邻点的关系，还考虑了邻域内邻点之间的关系。可以说，对于邻域的几何特征表述是足够充分的。但是，这样的描述带来了极大的计算开销，其时间复杂度为C(nk^2),其中k为邻域内点的平均值。为了提高效率，我们希望把时间复杂度降为C(nk)，为此FPFH被提出。 

三. FPFH（Fast Point Feature Histogram）

FPFH的核心计算过程和PFS基本一致，都要计算三元组。所不同的是，FPFH不计算邻域内邻点之间的三元组，这样能够显著的提升计算效率。但是，如果只计算源点和邻点之间的三元组，那么就会因减少了对局部邻域的完整描述，降低特征的表示精度。

为了弥补该问题，FPFH将邻点的邻点考虑进来，以扩大统计分析的范围。这样，在对源点附近区域进行特征统计分析时，能够在更大的范围获取三元组信息，以弥补FPFH对PFH特征描述的缺陷。需要注意的是，邻点对于源点的影响权重是不同的，权重应该与邻点与源点的距离成正比。因此，在考虑邻点的邻点时，需要加上基于距离的权重。

如上图所示，当我们在计算源点Pq点三元组的时候，第一步我们只计算源点与邻点的三元组，即五组数据（标记为粉色线），之后，分别计算邻点在其邻域的三元组，并且赋以基于距离的权重，最后将三元组组合，赋值给Pq，使用与PFH同样的方法，计算直方图，得到统计特征。

基于提取的特征点，FPFH结合一个成为SAMPLE CONSENSUS INITIAL ALIGNMENT的方法，即一个贪心搜索，以建立点云之间的初步对应关系，进而实现基于FPFH的点云配准应用。实验证明，FPFH在点云配准任务中具有优越的性能。

代码如下（基于PCL库）：

#include <pcl/point_types.h>
#include <pcl/features/fpfh.h>

{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());

  ... read, pass in or create a point cloud with normals ...
  ... (note: you can create a single PointCloud<PointNormal> if you want) ...

  // Create the FPFH estimation class, and pass the input dataset+normals to it
  pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
  fpfh.setInputCloud (cloud);
  fpfh.setInputNormals (normals);
  // alternatively, if cloud is of tpe PointNormal, do fpfh.setInputNormals (cloud);

  // Create an empty kdtree representation, and pass it to the FPFH estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<PointXYZ>::Ptr tree (new pcl::search::KdTree<PointXYZ>);

  fpfh.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());

  // Use all neighbors in a sphere of radius 5cm
  // IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!!
  fpfh.setRadiusSearch (0.05);

  // Compute the features
  fpfh.compute (*fpfhs);

  // fpfhs->size () should have the same size as the input cloud->size ()*
}

四. 实验结果

五. 讨论与总结

相比传统的ICP，对于FPFS算法的改进工作不是很多，高质量的文章也比较少。即使在2022年，基于ICP的改进方法仍然能在TPAMI上发表，而基于FPFH的技术路线，在质量不能与之相比。我认为，限制FPFS应用存在两个重要的问题。第一，FPFH的提取需要法线计算，这提高了计算开销，并且使的算法本身对点云的质量敏感。第二，FPFH在实时计算上没有ICP算法和深度学习框架优秀。另外，对于有断裂的，点分布不均匀的点云，FPFH受限于特征提取的过程，必然会进一步受到影响。不过，在我最近的工作中，通过实验对比，我发现FPFH在对于具有显著姿态差异的配准任务中，能够获得非常不错的效果，可见基于FPFH的技术路线仍然具有研发潜力。我已经对改善FPFH的实现与性能提升计划有了思路，未来文章发表后，将在第一时间与各位分享，欢迎持续关注我的博客。

Reference

[1] R. B. Rusu, Z. C. Marton, N. Blodow, and M. Beetz, “Learning Informative Point Classes for the Acquisition of Object Model Maps,” in Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV), Hanoi, Vietnam, December 17-20, 2008.

[2] R. B. Rusu, N. Blodow, Z. C. Marton, and M. Beetz, “Aligning Point Cloud Views using Persistent Feature Histograms,” in Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Nice, France, September 22-26, 2008.

[3] Rusu R B, Blodow N, Beetz M. Fast point feature histograms (FPFH) for 3D registration[C]//2009 IEEE international conference on robotics and automation. IEEE, 2009: 3212-3217.