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 void
setMaximumCacheSize (unsigned int cache_size)；2）、设置是否使用内部缓存机制减少冗余计算。
inline void
setUseInternalCache (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)
{
//距离特征d
Eigen::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 p2
n1_copy = n2;
n2_copy = n1;
n1_copy[3] = n2_copy[3] = 0.0f;
dp2p1 *= (-1);
f3 = -angle2;
}
else
f3 = angle1;
//创建 Darboux 坐标系 u-v-w
// u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
Eigen::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 v
v /= 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 system
f1 = std::atan2(w.dot(n2_copy), n1_copy.dot(n2_copy)); // @todo optimize this
return (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 neighborhood
for (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 vector
for (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_features
pcl::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特征描述》的学习。