PCL中点特征描述子PFH、FPFH和VFH简述和示例

---

---

前言

点特征直方图是PCL中非常重要的特征描述子，在点云匹配、分割、重建等任务中起到关键作用，可以对刚体变换、点云密度和噪声均有较强的抑制作用。而不同的描述子拥有不同优劣势，需要根据具体情况选择使用。

---

一、点特征直方图

点特征直方图融合了点云的局部和全局信息，具有旋转平移不变性，以及对采样密度和噪声点的稳健性。

1.1 PFH

PFH（point feature histogram）通过估计查询点和近邻点之间的法线差异，计算得到一个多维直方图来对点的K近邻进行几何描述，计算复杂度为O(nk^2)。
PFH的计算需要先估计法线，然后计算邻域范围内所有两点之间的关系：

1.1.1 法线估计

PCL采用近似估计的方法来计算法线特征，通过NormalEstimation类完成。
计算过程：
通过估计近邻区域的拟合面，再去计算查询点的法线。
拟合过程通过最小二乘法完成，然后通过PCA方法计算得到法向量（构建协方差矩阵，奇异值分解计算矩阵最小特征值所对应的特征向量做为法向量），最后通过计算相邻点法线内积的方法来进行法线定向。
实现过程可以参考：
为什么用PCA做点云法线估计？
源代码：

inline bool
      computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
                          float &nx, float &ny, float &nz, float &curvature)
      {
      //计算协方差矩阵
        if (indices.size () < 3 ||
            computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0)
        {
          nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
          return false;
        }

        // Get the plane normal and surface curvature
        solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);
        return true;
      }

计算法线和曲率，其中nx,ny,nz为法线的xyz分量。

inline void
solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
                      float &nx, float &ny, float &nz, float &curvature)
{
  // Avoid getting hung on Eigen's optimizers
//  for (int i = 0; i < 9; ++i)
//    if (!std::isfinite (covariance_matrix.coeff (i)))
//    {
//      //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
//      nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
//      return;
//    }
  // Extract the smallest eigenvalue and its eigenvector
  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);

  nx = eigen_vector [0];
  ny = eigen_vector [1];
  nz = eigen_vector [2];

  // Compute the curvature surface change
  float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
  if (eig_sum != 0)
    curvature = std::abs (eigen_value / eig_sum);
  else
    curvature = 0;
}

确定法线方向,vp_x,vp_y,vp_z为视点的坐标：

 template <typename PointT> inline void
  flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
                              float &nx, float &ny, float &nz)
  {
    // See if we need to flip any plane normals
    vp_x -= point.x;
    vp_y -= point.y;
    vp_z -= point.z;

    // Dot product between the (viewpoint - point) and the plane normal
    float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);

    // Flip the plane normal
    if (cos_theta < 0)
    {
      nx *= -1;
      ny *= -1;
      nz *= -1;
    }
  }

1.1.2 特征计算

1：计算两点法线的差异角度。
2：计算查询点法线方向与两点连线方向的角度。
3：计算邻域点法线上一点到UW平面的垂线交点与邻域点的直线，再计算直线与U的角度值。
4：计算两点间的距离。

按以上公式，每两个查询点可以计算出4个特征值。PCL中忽略d特征，只保留3个角度特征。
特征的统计方式按照划分子区间，并统计每个子区间的点数目，同时将角度归一化到相同的区间。PCL将每个角度特征划分5个子区间进行统计，最终得到125个浮点元素的特征向量，可以保存在PFHSignature125类型中。
特征计算：

  PCL_EXPORTS bool 
  computePairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, 
                       const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, 
                       float &f1, float &f2, float &f3, float &f4);

直方图计算：

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePointPFHSignature (
      const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
      const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram)

1.2 FPFH

FPFH(Fast Point Feature Histograms)意为快速点特征直方图，该算法对特征的计算进行了简化，并运用特征加权的方式得到最终的FPFH特征。该算法减少了时间复杂度，增加了实时性。
具体的计算方法：
1：计算查询点p邻域范围内的所有点对特征（只与查询点相连的点对），得到PFH中三个角度特征，命名为SPFH特征。
2：计算邻域内其他点的SPFH特征。
3：将邻域内其他所有的SPFH特征加权得到最终的FPFH特征，权重w是用邻域内点的距离来进行度量的。PCL中将三个特征值中的每个按照11个特征子空间进行统计，组合得到一个33个浮点元素的特征向量来表示FPFH特征。

1.3 VFH

为了使计算得到的特征保持尺度不变性和区分不同的位姿，故引入视点变量，计算得到视点特征直方图VFH特征。
其计算方法为：
1：扩展FPFH，使其利用整个点云来进行计算估计，以点云的中心点c与其他点之间的点对作为计算单元。
2: 添加视点方向与每个点估计法线间的统计信息，其做法是在特征计算时将视点变量直接融入法线角度计算中来。

具体可参考：
PCL 估计一点云的VFH特征
计算出的特征由三部分构成：
1：三个角度特征，每个分为45个子区间进行统计。
2：基于质心的点云形状描述子，分为45个子区间进行统计。
3：视角方向与点法线方向的角度差异，分为128个子区间进行统计。

二、示例

2.1 PFH计算

    //读取点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read("plant.pcd", *cloud_ptr);
	//计算法线
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud(cloud_ptr);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>());
	ne.setSearchMethod(tree1);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr(new pcl::PointCloud<pcl::Normal>);
	pcl::PointCloud<pcl::Normal>& cloud_normals = *cloud_normals_ptr;
	ne.setRadiusSearch(0.01);
	ne.compute(cloud_normals);
    //计算pfh特征
	pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;
	pfh.setInputCloud(cloud_ptr);
	pfh.setInputNormals(cloud_normals_ptr);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());
	pfh.setSearchMethod(tree2);
	//输出
	pcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_ptr(new pcl::PointCloud<pcl::PFHSignature125>());
	pfh.setRadiusSearch(0.03);
	pfh.compute(*pfh_ptr);

	//显示
	pcl::visualization::PCLPlotter plotter;
	plotter.addFeatureHistogram(*pfh_ptr, 200); 
	plotter.plot();

2.2 FPFH

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read("plant.pcd", *cloud);
	//法向量计算
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
	n.setInputCloud(cloud);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	n.setSearchMethod(tree);
	n.setNumberOfThreads(4);
	n.setKSearch(30);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	n.compute(*normals);
	//计算特征
	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
	fpfh.setInputCloud(cloud);
	fpfh.setInputNormals(normals);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());
	fpfh.setSearchMethod(tree2);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_fe(new pcl::PointCloud<pcl::FPFHSignature33>());
	//注意：此处使用的半径必须要大于估计表面法线时使用的半径
	fpfh.setRadiusSearch(0.03);
	fpfh.compute(*fpfh_fe);
	cout << "phf feature size : " << fpfh_fe->points.size() << endl;

	 pcl::visualization::PCLPlotter plotter;
	 plotter.addFeatureHistogram(*fpfh_fe, 200);
     plotter.plot();

---

2.3 VFH

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read("plant.pcd", *cloud);
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>());
	ne.setInputCloud(cloud);
	ne.setSearchMethod(tree1);
	ne.setRadiusSearch(0.01);

	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	ne.compute(*normals);

	pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
	vfh.setInputCloud(cloud);
	vfh.setInputNormals(normals);

	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());
	vfh.setSearchMethod(tree2);
	pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_ptr(new pcl::PointCloud<pcl::VFHSignature308>());
	vfh.compute(*vfh_ptr);
	pcl::visualization::PCLPlotter plotter;
	plotter.addFeatureHistogram(*vfh_ptr, 200);
	plotter.plot();