PCL中点特征描述子PFH、FPFH和VFH简述和示例
文章目录
- 前言
- 一、点特征直方图
- 1.1 PFH
- 1.1.1 法线估计
- 1.1.2 特征计算
- 1.2 FPFH
- 1.3 VFH
- 二、示例
- 2.1 PFH计算
- 2.2 FPFH
- 2.3 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();
Sandy_yin: 图形像素大小应该是像元尺寸/放大倍数,放大倍数是40,则image pixel=4/40=0.1μm
孤海的鲸: 现在4年过去了,网上的资料还是较少
MrDapapa: 现在有什么方法了不
Dear午饭君: 这个计算出来的是division除法的结果,如果想用多项式的面扫描模型,该怎么操作呢,用哪些算子
人狮子: 确实错了,张标定出来的就是像距