1、 FPFH原理
快速点特征直方图（Fast Point Feature Histograms，简称FPFH）是对PFH（Point Feature Histograms）计算方法的一种简化，具体内容看参考十四节内容。该方法的核心在于独立计算查询点的K邻域内每个点的简化点特征直方图（Simplified Point Feature Histogram，简称SPFH），然后利用特定公式将所有SPFH加权合并，形成最终的FPFH。通过这种方式，FPFH将算法的计算复杂度降低至O(nk)，同时依然保持了PFH的大部分识别特性。FPFH特征描述具体内容为如下。
1、计算简化点特征直方图（SPFH）
首先，只计算每个查询点Pq和它邻域点之间的三元特征，将<α，Φ，Φ，d>特征简化为<α，Φ， Φ>。FPFH只计算查询点和近邻点之间的特征元素。如1图，是PFH计算特征过程，即邻域点所有组合的特征值，图2是FPFH的计算内容，只需要计算Pq（查询点）和紧邻点（图2中红线部分）之间的特征元素。可知图2中的操作降低了复杂度，将其称之为SPFH（simple point feature histograms）。
图1
图2
2、计算FPFH特征
FPFH特征特征描述对K邻域点对的统计进行分类处理，一类是查询点Pq与周围K个点组成的点对，另外一类是每个邻点与其周围K个点组成的点对，第二类的特征量进行加权处理获得最终的FPFH特征值，计算公式如下：
其中 Wk为权重，为询点Pq与给定度量空间中近邻点Pk之间的距离。
3、特征描述向量简化
PFH的特征空间是完全相关联，3个角度特征，每个划分5份，则有5^3=125维，在实际计算中大部分的值都为0，有点浪费空间。FPFH对上述问题进行改进，创建b个不相关特征直方图，每个特征维度对应一个直方图bin，并将合并连接在一起。在PCL库中FPFH特征描述中bin=11，b=3，因此3*11=33，即pcl::FPFHSignature33,FPFH特征描述维度为33维，内容如下。
struct FPFHSignature33
{
float histogram[33] = {0.f};
static constexpr int descriptorSize () { return detail::traits::descriptorSize_v<FPFHSignature33>; }
inline FPFHSignature33 () = default;
friend std::ostream& operator << (std::ostream& os, const FPFHSignature33& p);
};2、主要成员函数和变量
1、主要的成员变量
1)、每个角度特征区间的划分数，默认为11
int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_;2)、点的f1特征描述的占位符
Eigen::MatrixXf hist_f1_;3)、点的f2特征描述的占位符
Eigen::MatrixXf hist_f2_;4)、点的f3特征描述的占位符
Eigen::MatrixXf hist_f3_;5)、点的fpfh特征描述的占位符
Eigen::VectorXf fpfh_histogram_;2、主要成员函数
1）、计算点对的四元特征
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);2）、计算点的SPFH特征
void computePointSPFHSignature (const pcl::PointCloud<PointInT> &cloud,
const pcl::PointCloud<PointNT> &normals, int p_idx, int row,
const std::vector<int> &indices,
void computeSPFHSignatures (std::vector<int> &spf_hist_lookup,
Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3);3）、计算点带权重的SPFH特征。
void
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);4）、设置特征划分区间
inline void
setNrSubdivisions (int nr_bins_f1, int nr_bins_f2, int nr_bins_f3)；3、主要实现代码注解
1、计算点对的4元特征
//1、计算点对的4元特征
/// //
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::FPFHEstimation<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);
}2、计算点SPFH特征
//
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;
// 分别获取三个角特征的划分期间
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);
// 执行p_idx与所有邻域点对，计算SPFH特征信息
for (const auto& index : indices)
{
// Avoid unnecessary returns
if (p_idx == index)
continue;
// 计算点对的4元特征
if (!computePairFeatures(cloud, normals, p_idx, index, pfh_tuple[0], pfh_tuple[1], pfh_tuple[2], pfh_tuple[3]))
continue;
// 三元特征归一化，并且统计直方图信息，存储到hist_f1(row, h_index)
// hist_f2(row, h_index);hist_f3(row, h_index)中
int h_index = static_cast<int> (std::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;
h_index = static_cast<int> (std::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> (std::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;
}
}//3、计算点SPFH特征
//
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures(std::vector<int>& spfh_hist_lookup,
Eigen::MatrixXf& hist_f1, Eigen::MatrixXf& hist_f2, Eigen::MatrixXf& hist_f3)
{
//为邻域搜索分配内存空间
std::vector<int> nn_indices(k_);
std::vector<float> nn_dists(k_);
//spfh查找表大小初始化
std::set<int> spfh_indices;//保存的索引是没有重复的
spfh_hist_lookup.resize(surface_->points.size());
//为需要计算SPFH的点创建一个索引列表spfh_indices
if (surface_ != input_ ||
indices_->size() != surface_->points.size())
{
//只对部分的点（如提取的关键点）计算特征信息
for (const auto& p_idx : *indices_)
{
//关键点需要满足最近邻域搜索的结果才能计算SPFH特征
if (this->searchForNeighbors(p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;
spfh_indices.insert(nn_indices.begin(), nn_indices.end());
}
}
else
{
// 对所有的点都计算特征信息
for (std::size_t idx = 0; idx < indices_->size(); ++idx)
spfh_indices.insert(static_cast<int> (idx));
}
// 初始化存储 SPFH 特征信息对象，初始化为0
std::size_t data_size = spfh_indices.size();
hist_f1.setZero(data_size, nr_bins_f1_);
hist_f2.setZero(data_size, nr_bins_f2_);
hist_f3.setZero(data_size, nr_bins_f3_);
// 为现有的点计算SPFH特征信息
std::size_t i = 0;
for (const auto& p_idx : spfh_indices)
{
// 最近邻域搜索
if (this->searchForNeighbors(*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;
// 估计索引p_id点的SPFH特征信息
computePointSPFHSignature(*surface_, *normals_, p_idx, i, nn_indices, hist_f1, hist_f2, hist_f3);
// 填充查找表，用于将点索引转换为spfh_hist_*矩阵中相应的行
spfh_hist_lookup[p_idx] = i;
i++;
}
}3、计算加权SPFH特征作为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;
// 获取角度信息划分的区间
const auto nr_bins_f1 = hist_f1.cols();
const auto nr_bins_f2 = hist_f2.cols();
const auto nr_bins_f3 = hist_f3.cols();
const auto nr_bins_f12 = nr_bins_f1 + nr_bins_f2;
// fpfh特征描述尺寸计算，为三个角度区间的和
fpfh_histogram.setZero(nr_bins_f1 + nr_bins_f2 + nr_bins_f3);
// Use the entire patch
for (std::size_t idx = 0; idx < indices.size(); ++idx)
{
//跳过自身点
if (dists[idx] == 0)
continue;
// 权重计算，为与邻域距离的倒数
weight = 1.0f / dists[idx];
//将查询点的SPFH与其邻居的SPFH进行加权
for (std::size_t 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 (std::size_t 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 (std::size_t 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;
}
}
//将特征总和归一化到0-100以内
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
//每个特征值在在乘各自的1/sum得到最终的FPFH特征信息
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);
}4、计算FPFH特征整体流程
//
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature(PointCloudOut& output)
{
//为K邻域搜索分配需要的空间
std::vector<int> nn_indices(k_);
std::vector<float> nn_dists(k_);
std::vector<int> spfh_hist_lookup;
computeSPFHSignatures(spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_);
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)
{
for (Eigen::Index d = 0; d < fpfh_histogram_.size(); ++d)
output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN();
output.is_dense = false;
continue;
}
//将点云的索引转换为存储特征信息矩阵中的列，为了获取矩阵中存储的特征信息
//根据 spfh_hist_lookup[p_idx] = i来转换
for (auto& nn_index : nn_indices)
nn_index = spfh_hist_lookup[nn_index];
// 计算fpfh特征信息，根据原理步骤2中的公式
weightPointSPFHSignature(hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// 将计算的fpfh特征信息拷贝到输出的数据中
for (Eigen::Index d = 0; d < fpfh_histogram_.size(); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
}
}
else
{
// 遍历所有点云的索引
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)
{
//无效值或不满足最邻域的特征信息为NAN无效值，设置输出数据为非稠密点云
for (Eigen::Index d = 0; d < fpfh_histogram_.size(); ++d)
output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN();
output.is_dense = false;
continue;
}
//将点云的索引转换为存储特征信息矩阵中的列，为了获取矩阵中存储的特征信息
//根据 spfh_hist_lookup[p_idx] = i来转换
for (auto& nn_index : nn_indices)
nn_index = spfh_hist_lookup[nn_index];
// 计算fpfh特征信息，根据原理步骤2中的公式
weightPointSPFHSignature(hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// 将计算的fpfh特征信息拷贝到输出的数据中
for (Eigen::Index d = 0; d < fpfh_histogram_.size(); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
}
}
}4、使用代码示例
/*****************************************************************//**
* \file PCLFPFHFeaturemain.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/fpfh.h>//包含FPFH计算的模块
using namespace std;
void PCLFPFHFeature()
{
//加载点云数据
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
std::string fileName = "E:/PCLlearnData/15/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.04);
ne.compute(*cloud_normals);
//计算点云数据的FPFH特征信息
//pcl::PFHEstimation PFH特征估计对象
pcl::FPFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> fpfh_estimation;
// 设置需要计算FPFH的点云数据
fpfh_estimation.setInputCloud(cloud);
// 设置需要计算FPFH的点云数据对应的法向量
fpfh_estimation.setInputNormals(cloud_normals);
//设置最近KDtree方法
fpfh_estimation.setSearchMethod(tree);
//创建保存FPFH特征信息的对象fpfh_features
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_features(new pcl::PointCloud<pcl::FPFHSignature33>);
//设置搜索的邻域半径
fpfh_estimation.setRadiusSearch(0.08);
// 计算FPFH特征信息将结构保存到fpfh_features中
fpfh_estimation.compute(*fpfh_features);
// 输出索引为2000的FPFH特征信息
pcl::FPFHSignature33 descriptor = (*fpfh_features)[2000];
std::cout << descriptor << std::endl;
}
int main(int argc, char* argv[])
{
PCLFPFHFeature();
std::cout << "Hello PCL World!" << std::endl;
std::system("pause");
return 0;
}结果：
5、FPFH与PFH的主要区别
1、FPFH未对全互连点的所有邻近点进行计算，这可能导致遗漏对捕获查询点周围几何特征有贡献的重要点对。
2、PFH特征模型专注于查询点周围精确邻域半径内的特征，而FPFH则扩展至半径r范围以外的额外点对（不超过2r范围）。
3、FPFH通过权重计算方式，结合SPFH值，重新捕获邻近重要点对的几何信息。
4、FPFH的复杂性降低，使其具备在实时应用中使用的潜力。
5、FPFH特征描述的维度较小，PFH是125维，而FPFH仅为33维。
至此完成第十五节PCL库点云特征之FPFH点特征直方图学习，下一节我们将进入《PCL库中点云特征之SHOT特征描述》的学习
扫一扫
113
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
