PCL点云库入门——PCL库点云特征之PFH点特征直方图（Point Feature Histograms -PHF）

1、算法原理

        PFH点（Point Feature Histogram）特征直方图的原理涉及利用参数化查询点与邻域点之间的空间差异，并构建一个多维直方图以捕捉点的k邻域几何属性。这个高维超空间为特征表示提供了一个可度量的信息空间，对于点云对应曲面的六维姿态，它具有不变性，并且在不同的采样密度或邻域噪声水平下表现出鲁棒性。PFH特征描述表示法基于点与其k邻域之间的关系以及它们的估计法线，简而言之，它考虑了所有估计法线方向之间的相互作用，试图捕捉最佳的样本表面变化情况，以描述样本的几何特征。因此，合成特征超空间依赖于每个点的表面法线估计质量。图1展示了查询点Pq的PFH计算影响区域，Pq以红色标记并置于圆球中心，半径为r，所有与Pq的距离小于半径r的k邻元素（即所有点）都相互连接在一个网络中。最终的PFH特征描述子是通过计算邻域内所有点对之间关系得到的直方图，公用有K*(K+1)/2个点对。

 图1 

        PFH特征描述实现的步骤如下：

        第1步：构建局部坐标系

        为了描述两点Ps和Pt及与它们对应的法线Ns和Nt之间的相对偏差，我们在其中一个点上定义一个固定的 Darboux标系，为了使该坐标架被唯一的确定，如图2所示。

图2 

        其中u,v,w具体如下：

        其中，Ps被定义为源点，Pt被定义为目标点。源点即Ps的选择是使得其关联的法线与连接两点的直线之间的角度最小。 

        第2步：特征描述

        在第1步中的vuw坐标系下，法线Ns和Nt之间的相对偏差可以用一组角度特征描述，定义为下：

        其中α和ϕ为单位向量的内积，取值范围为[-1,1]，θ为角度，取值范围[-π/2，π/2]；d为两点欧式距离，取值范围为（0，2r）。Pq点的PFH特征为α，θ，ϕ，d组成的四元特征。

        第3步：特征编码：

        为计算查询点Pq的PFH特征，根据点的四元特征（α，θ，ϕ，d）构建频率直方图，具体而言，先将<α，θ，ϕ，d>中的每个特征值范围划分为b个子区间（默认为5，可自行计算并设置），则4个特征共有b^4个区间，然后，统计对应特征值落在每个子区间的点的数目，由于<α,ϕ,θ>三个特征都是法线之间的角度息，则它们可归为同一个空间，最后，计算每一个点对的特征值，直方图中对应于该点对四个特征值区间的统计个数加1,效果图3所示。

图3 

在实际工作中距离特征d通常跟设备捕获2.5D深度数据是相关的，临近点的距离d是从视点开始递增的，在不同视角下，这些值的变化意义不大，所以在扫描局部点密度影响特征时，省略距离d效果会更好。在PCL 点云库中 FPH 特征描述只用了三个角度特征，因而特征矢量是5^3=125 维,对应PCL中的pcl::PFHSignature125数据类型。内容如下：

  struct PFHSignature125{float histogram[125] = {0.f};static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<PFHSignature125>; }inline PFHSignature125 () = default;friend std::ostream& operator << (std::ostream& os, const PFHSignature125& p);};

 2、主要成员函数和变量

        1、主要成员变量

         1)、每个角度特征区间的划分数，默认为5。

    int nr_subdiv_;

          2)、点的PFH特征描述的占位符。

    Eigen::VectorXf pfh_histogram_;

          3)、PFH的4元特征的占位符。

    Eigen::Vector4f pfh_tuple_;

          4）、map数据类型，用于优化冗余计算的效率。

    std::map<std::pair<int, int>, Eigen::Vector4f, std::less<>,Eigen::aligned_allocator<std::pair<const std::pair<int, int>, Eigen::Vector4f> > > feature_map_;

        5）、中间计算结果保存的对队列。

     std::queue<std::pair<int, int> > key_list_;

       6）、设置最大内部缓存大小。

      unsigned int max_cache_size_;

       7）、设置为true表示使用缓存中间计算结果，减少冗余计算。

     bool use_cache_;

        2、主要成员函数

        1）、设置最大内部缓存大小。默认值为2GB。

 inline voidsetMaximumCacheSize (unsigned int cache_size)；

        2）、设置是否使用内部缓存机制减少冗余计算。

inline voidsetUseInternalCache (bool use_cache)；

        3）、计算点三个角度和两点之间的距离的4元特征。

 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);

        4）、基于具有法线的三维点的空间邻域，估计给定点的三个角（f1, f2, f3）特征的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);

3、主要部分代码注解

        1、计算点对的四元特征

//1、计算点对的四元特征
//
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::PFHEstimation<PointInT, PointNT, PointOutT>::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)
{pcl::computePairFeatures(cloud.points[p_idx].getVector4fMap(),  normals.points[p_idx].getNormalVector4fMap(),cloud.points[q_idx].getVector4fMap(),  normals.points[q_idx].getNormalVector4fMap(),f1, f2, f3, f4);return (true);
}

//算法具体实现，对应1中第2步的公式；
bool pcl::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)
{//距离特征dEigen::Vector4f dp2p1 = p2 - p1;dp2p1[3] = 0.0f;f4 = dp2p1.norm();if (f4 == 0.0f){PCL_DEBUG("[pcl::computePairFeatures] Euclidean distance between points  is 0!\n");f1 = f2 = f3 = f4 = 0.0f;return (false);}//计算ϕ角度特征Eigen::Vector4f n1_copy = n1,n2_copy = n2;n1_copy[3] = n2_copy[3] = 0.0f;float angle1 = n1_copy.dot(dp2p1) / f4;// 根据角度大小来选择源点Ps，选择角度最小的为Ps点，std::acos是递减函数float angle2 = n2_copy.dot(dp2p1) / f4;if (std::acos(std::fabs(angle1)) > std::acos(std::fabs(angle2))){// switch p1 and p2n1_copy = n2;n2_copy = n1;n1_copy[3] = n2_copy[3] = 0.0f;dp2p1 *= (-1);f3 = -angle2;}elsef3 = angle1;//创建 Darboux 坐标系 u-v-w// u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x vEigen::Vector4f v = dp2p1.cross3(n1_copy);v[3] = 0.0f;float v_norm = v.norm();if (v_norm == 0.0f){PCL_DEBUG("[pcl::computePairFeatures] Norm of Delta x U is 0!\n");f1 = f2 = f3 = f4 = 0.0f;return (false);}// Normalize vv /= v_norm;Eigen::Vector4f w = n1_copy.cross3(v);// Do not have to normalize w - it is a unit vector by construction//计算θ精度特征v[3] = 0.0f;f2 = v.dot(n2_copy);w[3] = 0.0f;//计算α角度特征 4元特征（α，θ，ϕ，d）// Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w  coordinate systemf1 = std::atan2(w.dot(n2_copy), n1_copy.dot(n2_copy)); // @todo optimize thisreturn (true);
}

        2、计算单点PFH特征信息

//2、计算单点PFH特征信息
//
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)
{int h_index, h_p;//清楚上一次的PFH结果pfh_histogram.setZero();// 分解常数float hist_incr = 100.0f / static_cast<float> (indices.size() * (indices.size()  - 1) / 2);std::pair<int, int> key;bool key_found = false;// Iterate over all the points in the neighborhoodfor (std::size_t i_idx = 0; i_idx < indices.size(); ++i_idx){for (std::size_t j_idx = 0; j_idx < i_idx; ++j_idx){// 无序点检测，是则跳过if (!isFinite(cloud.points[indices[i_idx]]) ||  !isFinite(cloud.points[indices[j_idx]]))continue;//使用缓存来加速计算if (use_cache_){// 创建map需要的键值，为点云的索引int p1, p2;//      if (indices[i_idx] >= indices[j_idx])//      {p1 = indices[i_idx];p2 = indices[j_idx];//      }//      else//      {//        p1 = indices[j_idx];//        p2 = indices[i_idx];//      }key = std::pair<int, int>(p1, p2);// 检查是否已经在map中已经估计的点对，就不需要重新计算，避免重复计算std::map<std::pair<int, int>, Eigen::Vector4f,  std::less<>, Eigen::aligned_allocator<std::pair<const std::pair<int, int>,  Eigen::Vector4f> > >::iterator fm_it = feature_map_.find(key);if (fm_it != feature_map_.end()){//已经有计算的直接用计算的结果pfh_tuple_ = fm_it->second;key_found = true;}else{//没有被估计过，需要计算，这对新的点对的4元特征if (!computePairFeatures(cloud, normals,  indices[i_idx], indices[j_idx],pfh_tuple_[0], pfh_tuple_[1],  pfh_tuple_[2], pfh_tuple_[3]))continue;key_found = false;}}else//不使用缓存，直接计算所有两对点之间的4元特征if (!computePairFeatures(cloud, normals, indices[i_idx],  indices[j_idx],pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2],  pfh_tuple_[3]))continue;// 将f1， f2， f3特征归一化，并将它们存储到直方图f_index_[0] = static_cast<int> (std::floor(nr_split *  ((pfh_tuple_[0] + M_PI) * d_pi_)));if (f_index_[0] < 0)         f_index_[0] = 0;if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;f_index_[1] = static_cast<int> (std::floor(nr_split *  ((pfh_tuple_[1] + 1.0) * 0.5)));if (f_index_[1] < 0)         f_index_[1] = 0;if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;f_index_[2] = static_cast<int> (std::floor(nr_split *  ((pfh_tuple_[2] + 1.0) * 0.5)));if (f_index_[2] < 0)         f_index_[2] = 0;if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;// 特征直方图统计h_index = 0;h_p = 1;for (const int& d : f_index_){h_index += h_p * d;h_p *= nr_split;}//统计直方图结果pfh_histogram[h_index] += hist_incr;//缓存新计算点对的4元特征if (use_cache_ && !key_found){//保存4元特征信息到map中，根据Key由点的索引值feature_map_[key] = pfh_tuple_;// 将计算过的点对保存起来，为了防止冗余计算key_list_.push(key);// 如果缓存的量操过设置的最大值，需要清除最先缓存的值if (key_list_.size() > max_cache_size_){//删除最先缓存的元素。feature_map_.erase(key_list_.front());key_list_.pop();}}}}
}

        3、计算所有点的PFH特征信息

//3、计算所有点的PFH特征信息
//
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computeFeature(PointCloudOut&  output)
{// 清除特征map信息feature_map_.clear();std::queue<std::pair<int, int> > empty;std::swap(key_list_, empty);//初始化PFH特征维度大小5^3=125，并初始化为{0}pfh_histogram_.setZero(nr_subdiv_ * nr_subdiv_ * nr_subdiv_);//分配需要参数vector的大小std::vector<int> nn_indices(k_);std::vector<float> nn_dists(k_);output.is_dense = true;// 稠密点云数据if (input_->is_dense){// 对每一个索引值进行遍历执行for (std::size_t idx = 0; idx < indices_->size(); ++idx){//点的最近邻域搜索if (this->searchForNeighbors((*indices_)[idx],  search_parameter_, nn_indices, nn_dists) == 0){//如果最近邻域搜索失败，则将该点的PFH值重置为NAN无效值，跳过该点for (Eigen::Index d = 0; d < pfh_histogram_.size(); ++d)output.points[idx].histogram[d] =  std::numeric_limits<float>::quiet_NaN();output.is_dense = false;continue;}// 估计每个patch的PFH特征computePointPFHSignature(*surface_, *normals_, nn_indices,  nr_subdiv_, pfh_histogram_);// 将PFH特征特征信息拷贝到输出的数据中for (Eigen::Index d = 0; d < pfh_histogram_.size(); ++d)output.points[idx].histogram[d] = pfh_histogram_[d];}}else{// Iterating over the entire index vectorfor (std::size_t idx = 0; idx < indices_->size(); ++idx){//点的最近邻域搜索和无效值判断if (!isFinite((*input_)[(*indices_)[idx]]) ||this->searchForNeighbors((*indices_)[idx],  search_parameter_, nn_indices, nn_dists) == 0){//如果最近邻域搜索失败，则将该点的PFH值重置为NAN无效值，跳过该点for (Eigen::Index d = 0; d < pfh_histogram_.size(); ++d)output.points[idx].histogram[d] =  std::numeric_limits<float>::quiet_NaN();output.is_dense = false;continue;}// 估计每个patch的PFH特征computePointPFHSignature(*surface_, *normals_, nn_indices,  nr_subdiv_, pfh_histogram_);// 将PFH特征特征信息拷贝到输出的数据中for (Eigen::Index d = 0; d < pfh_histogram_.size(); ++d)output.points[idx].histogram[d] = pfh_histogram_[d];}}
}

4、算法使用示例

/*****************************************************************//**
* \file   PCLPFHFeaturemain.cpp
* \brief  
*
* \author YZS
* \date   January 2025
*********************************************************************/
#include<iostream>
#include <vector>
#include <ctime>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/auto_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/pfh.h>//包含PFH计算的模块
using namespace std;
void PCLPFHFeature()
{//加载点云数据pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new  pcl::PointCloud<pcl::PointXYZRGB>());std::string fileName = "E:/fragment.pcd";pcl::io::load(fileName, *cloud);std::cout << "Cloud Size:" << cloud->points.size() << std::endl;//计算点云数据的法向量pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;ne.setInputCloud(cloud);pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new  pcl::search::KdTree<pcl::PointXYZRGB>());ne.setSearchMethod(tree);pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new  pcl::PointCloud<pcl::Normal>);ne.setRadiusSearch(0.02);ne.compute(*cloud_normals);//计算点云数据的PFH特征信息//pcl::PFHEstimation PFH特征估计对象pcl::PFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHSignature125>  pfh_estimation;// 设置需要计算PFH的点云数据pfh_estimation.setInputCloud(cloud);// 设置需要计算PFH的点云数据对应的法向量pfh_estimation.setInputNormals(cloud_normals);//设置最近KDtree方法pfh_estimation.setSearchMethod(tree);//创建保存PFH特征信息的对象pfh_featurespcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_features(new  pcl::PointCloud<pcl::PFHSignature125>);//设置搜索的邻域半径pfh_estimation.setRadiusSearch(0.08);//设置启用缓存计算结果，加快速度pfh_estimation.setUseInternalCache(true);// 计算PFH特征信息将结构保存到pfh_features中pfh_estimation.compute(*pfh_features);// 输出索引为2000的PFH特征信息pcl::PFHSignature125 descriptor = (*pfh_features)[2000];std::cout << descriptor << std::endl;
}
int main(int argc, char* argv[])
{PCLPFHFeature();std::cout << "Hello PCL World!" << std::endl;std::system("pause");return 0;
}

结果：只输出部分

5、PFH特征的的优缺点

1、PFH的优点

        1、该方法的优点在于对噪声和遮挡具有较强的抗干扰能力，适用于复杂场景下的三维物体识别与分类。

2、PFH的缺点：

        1、计算复杂度较高。假定点云P由n个点构成，且点云分布均匀，每个点在半径为r的邻域内平均能找到k个近邻点。那么，对于每个点计算点特征直方图（PFH）的时间复杂度为O(k²)，因此，整个点云的PFH理论计算复杂度为O(nk²)。在实时或接近实时的应用场景中，对于密集点云的PFH计算，O(nk²)的计算复杂度显然不尽如人意，成为了一个显著的性能瓶颈。

        2、存在大量重复计算。在计算PFH的三个特征元素时，邻域内任意两点都需要分别计算其三个特征元素值。即便相邻点的邻域可能不完全相同，但它们往往包含许多相同的近邻点（随着邻域半径的增加，这种重复的近邻点会更多）。这些共同的近邻点会被重复配对并计算特征元素值。尽管相关论文和PCL库中引入了高速缓存机制，将重复计算的开销转化为查找开销，但整体效率提升并不显著。

        至此完成第十四节PCL库点云特征之PFH点特征直方图学习，下一节我们将进入《PCL库中点云特征之FPFH特征描述》的学习。