PCL点云特征描述与提取

171
0
2021年1月16日 09时25分

3D点云特征描述与提取是点云信息处理中最基础也是最关键的一部分,点云的识别。分割,重采样,配准曲面重建等处理大部分算法,都严重依赖特征描述与提取的结果。从尺度上来分,一般分为局部特征的描述全局特征的描述,例如局部的法线等几何形状特征的描述,全局的拓朴特征的描述,都属于3D点云特征描述与提取的范畴,

特征描述与提取相关的概念与算法

 

1.3D形状内容描述子(3D shape contexts)
利用描述子建立曲面间的对应点在3D物体识别领域有广发的应用,采用一个向量描述曲面上指定点及邻域的形状特征,通过匹配向量的值来建立不同曲面点的对应关系,此相邻则则称为指定点的俄描述子,经典描述子的3D形状内容描述子结构简单,辨别力强,且对噪声不敏感,

 

2,旋转图像(spin iamge)
旋转图像最早是由johnson提出的特征描述子,主要用于3D场景中的曲面匹配和模型识别,

 

3,涉及的算法相关的资料
3D形状内容描述子
https://en.wikipedia.org/wiki/Shape_context
www.eecs.berkeley.edu/Research/Projects/CS/vision/shape/belongie-pami02.pdf

 

还有很多中描述子的理论与算法的研究,不再一一列出来
关于理论的部分有待研究,但是暂时我只是学习会用。

 

关于PCL中特征描述与提取模块和相关类的介绍

 

Classes
class pcl::ShapeContext3DEstimation< PointInT, PointNT, PointOutT >
实现3D形状内容描述子算法
class pcl::BOARDLocalReferenceFrameEstimation< PointInT, PointNT, PointOutT >
实现局部坐标系估计的方法 特别是处理点云边缘或有孔洞有特殊的处理方式
class pcl::BoundaryEstimation< PointInT, PointNT, PointOutT >
实现估计一组点集是否处于指定点的投影区域的边缘位置
class pcl::CRHEstimation< PointInT, PointNT, PointOutT >
实现摄像头旋转直方图描述子,利用概算法主要进行刚体对象的位姿估计
class pcl::CVFHEstimation< PointInT, PointNT, PointOutT >
实现聚类视点直方图CVFH描述子的计算 主要是针对解决有残缺的点云识别问题
class pcl::ESFEstimation< PointInT, PointOutT >
实现ESF描述子,主要用于实时对三维场景中的点云模型进行分类而提出的
class pcl::Feature< PointInT, PointOutT > 是所有特征相关模块中其他类的基类
class pcl::FeatureWithLocalReferenceFrames< PointInT, PointRFT >
实现FPFH描述子算法主要针对点云配准过程中对应点而提出的

 

PCL中描述三维特征相关基础

 

理论基础
在原始表示形式下,点的定义是用笛卡尔坐标系坐标 x, y, z 相对于一个给定的原点来简单表示的三维映射系统的概念,假定坐标系的原点不随着时间而改变,这里有两个点p1和p2分别在时间t1和t2捕获,有着相同的坐标,对这两个点作比较其实是属于不适定问题(ill—posed problem),因为虽然相对于一些距离测度它们是相等的,但是它们取样于完全不同的表面,因此当把它们和临近的其他环境中点放在一起时,它们表达着完全不同的信息,这是因为在t1和t2之间局部环境有可能发生改变。一些获取设备也许能够提供取样点的额外数据,例如强度或表面反射率等,甚至颜色,然而那并不能完全解决问题,单从两个点之间来 对比仍然是不适定问题。由于各种不同需求需要进行对比以便能够区分曲面空间的分布情况,应用软件要求更好的特征度量方式,因此作为一个单一实体的三维点概念和笛卡尔坐标系被淘汰了,出现了一个新的概念取而代之:局部描述子(locl descriptor)。文献中对这一概念的描述有许多种不同的命名,如:形状描述子(shape descriptors)或几何特征(geometric features),文本中剩余部分都统称为点特征表示。通过包括周围的领域,特征描述子能够表征采样表面的几何 性质,它有助于解决不适定的对比问题,理想情况下相同或相似表面上的点的特征值将非常相似(相对特定度量准则),而不同表面上的点的特征描述子将有明显差异。下面几个条件,通过能否获得相同的局部表面特征值,可以判定点特征表示方式的优劣:

 

(1)刚体变换—–即三维旋转和三维平移变化 不会影响特征向量F估计,即特征向量具有平移选转不变性。
(2)改变采样密度—–原则上,一个局部表面小块的采样密度无论是大还是小,都应该有相同的特征向量值,即特征向量具有抗密度干扰性。
(3)噪声—数据中有轻微噪声的情况下,点特征表示在它的特征向量中必须保持相同或者极其相似的值,即特征向量对点云噪声具有稳定性。
通常,PCL中特征向量利用快速kd-tree查询 ,使用近似法来计算查询点的最近邻元素,通常有两种查询类型:K邻域查询,半径搜索两中方法

 

法线估计实例

 

一旦确定邻域以后,查询点的邻域点可以用来估计一个局部特征描述子,它用查询点周围领域点描述采样面的几何特征,描述几何表面图形的一个重要属性,首先是推断它在坐标系中的方位,也就是估计他的法线,表面法线是表面的一个重要的属性,在许多领域都有重要的应用,如果用光源来生成符合视觉效果的渲染等,
代码解析:normal_estimation.cpp

 

(实现对输入点云数据集中的点估计一组表面法线)执行的操作是:对应点云P中每一个点p得到p点最近邻元素,计算p点的表面的法线N,检查N的方向是否指向视点如果不是则翻转。
视点默认坐标是(0,0,0)可使用setViewPoint(float vpx,float vpy,float vpz)来更换

 

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>  //法线估计类头文件
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>

int main ()
{
	//打开点云代码
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile ("table_scene_lms400.pcd", *cloud);
	//创建法线估计估计向量
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud (cloud);
	//创建一个空的KdTree对象,并把它传递给法线估计向量
	//基于给出的输入数据集,KdTree将被建立
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
	ne.setSearchMethod (tree);
	//存储输出数据
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
	//使用半径在查询点周围3厘米范围内的所有临近元素
	ne.setRadiusSearch (0.03);
	//计算特征值
	ne.compute (*cloud_normals);
	// cloud_normals->points.size ()应该与input cloud_downsampled->points.size ()有相同的尺寸
	//可视化
	pcl::visualization::PCLVisualizer viewer("PCL Viewer");
	viewer.setBackgroundColor (0.0, 0.0, 0.0);
	viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, cloud_normals);
}
while (!viewer.wasStopped ())
{
     viewer.spinOnce ();
}

return 0;
}

 

运行结果并执行:

 

微信图片_20210117161351

 

(2)估计一个点云的表面法线

表面法线是几何体表面一个十分重要的属性,例如:在进行光照渲染时产生符合可视习惯的效果时需要表面法线的信息才能正常进行,对于一个已经已经知道的几何体表面,根据垂直于点表面的的矢量,因此推推处表面某一点的法线方向比较容易,然而由于我们获取的点云的数据集在真实的物体的表面表现为一组定点的样本,这样就会有两种方法解决:

 

1 . 使用曲面重建技术,从获取的点云数据中得到采样点对应的曲面,然后从曲面模型中计算出表面法线
2. 直接从点云数据中近似推断表面法线
在确定表面一点法线的问题近似于估计表面的一个相切面法线的问题,因此转换过来就是求一个最小二乘法平面拟合的问题

 

(3)使用积分图进行法线估计
使用积分图计算一个有序的点云的法线,注意此方法只适用有序点云
代码解析normal_estimation_using_integral_images.cpp

 

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>
int main ()
{
    //打开点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("table_scene_mug_stereo_textured.pcd", *cloud);
    //创建法线估计向量
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;     /****************************************************************************************
	三种法线估计方法
	COVARIANCE_MATRIX 模式从具体某个点的局部邻域的协方差矩阵创建9个积分,来计算这个点的法线
	AVERAGE_3D_GRADIENT 模式创建6个积分图来计算水平方向和垂直方向的平滑后的三维梯度并使用两个梯度间的向量
	积计算法线
	AVERAGE_DEPTH——CHANGE  模式只创建了一个单一的积分图,从而平局深度变化计算法线   ********************************************************************************************/
    ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);  //设置法线估计的方式AVERAGE_3D_GRADIENT
                
    ne.setMaxDepthChangeFactor(0.02f);   //设置深度变化系数
    ne.setNormalSmoothingSize(10.0f);   //设置法线优化时考虑的邻域的大小
    ne.setInputCloud(cloud);               //输入的点云
    ne.compute(*normals);                    //计算法线
    //可视化
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");   //视口的名称
    viewer.setBackgroundColor (0.0, 0.0, 0.5);    //背景颜色的设置
    viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, normals);  //将法线加入到点云中
    while (!viewer.wasStopped ())
    {
        viewer.spinOnce ();
    }
    return 0;
}

 

运行结果为:

 

微信图片_20210117161426

 

点特征直方图(PFH)描述子

 

正如点特征表示法所示,表面法线和曲率估计是某个点周围的几何特征基本表示法。虽然计算非常快速容易,但是无法获得太多信息,因为它们只使用很少的几个参数值来近似表示一个点的k邻域的几何特征。然而大部分场景中包含许多特征点,这些特征点有相同的或者非常相近的特征值,因此采用点特征表示法,其直接结果就减少了全局的特征信息。那么三维特征描述子中一位成员:点特征直方图(Point Feature Histograms),我们简称为PFH,从PCL实现的角度讨论其实施细节。PFH特征不仅与坐标轴三维数据有关,同时还与表面法线有关。

PFH计算方式通过参数化查询点与邻域点之间的空间差异,并形成一个多维直方图对点的k邻域几何属性进行描述。直方图所在的高维超空间为特征表示提供了一个可度量的信息空间,对点云对应曲面的6维姿态来说它具有不变性,并且在不同的采样密度或邻域的噪音等级下具有鲁棒性。点特征直方图(PFH)表示法是基于点与其k邻域之间的关系以及它们的估计法线,简言之,它考虑估计法线方向之间所有的相互作用,试图捕获最好的样本表面变化情况,以描述样本的几何特征。因此,合成特征超空间取决于每个点的表面法线估计的质量。如图所示,表示的是一个查询点(Pq) 的PFH计算的影响区域,Pq 用红色标注并放在圆球的中间位置,半径为r, (Pq)的所有k邻元素(即与点Pq的距离小于半径r的所有点)全部互相连接在一个网络中。最终的PFH描述子通过计算邻域内所有两点之间关系而得到的直方图,因此存在一个O(k) 的计算复杂性。
下图是查询点的PFH的影响区域

 

微信图片_20210117161504

 

为了计算两点Pi和Pj及与它们对应的法线Ni和Nj之间的相对偏差,在其中的一个点上定义一个固定的局部坐标系,如图2所示。

 

1610871325(1)

 

使用上图中uvw坐标系,法线 和 之间的偏差可以用一组角度来表示,如下所示:

 

微信图片_20210117161622

 

d是两点Ps和Pt之间的欧氏距离微信图片_20210117161644

 

, 。计算k邻域内的每一对点的微信图片_20210117161659四组值,这样就把两点和它们法线相关的12个参数(xyz坐标值和法线信息)减少到4个
为查询点创建最终的PFH表示,所有的四元组将会以某种统计的方式放进直方图中,这个过程首先把每个特征值范围划分为b个子区间,并统计落在每个子区间的点数目,因为四分之三的特征在上述中为法线之间的角度计量,在三角化圆上可以将它们的参数值非常容易地归一到相同的区间内。

 

一个统计的例子是:把每个特征区间划分成等分的相同数目,为此在一个完全关联的空间内创建有 个区间的直方图。在这个空间中,一个直方图中某一区间统计个数的增一对应一个点的四个特征值。如图3所示,就是点云中不同点的点特征直方图表示法的一个例子,在某些情况下,第四个特征量d在通常由机器人捕获的2.5维数据集中的并不重要,因为临近点间的距离从视点开始是递增的,而并非不变的,在扫描中局部点密度影响特征时,实践证明省略d是有益的。

 

微信图片_20210117161729

 

更详细的解释:pointclouds.org/documentation/tutorials/how_features_work.php#rusudissertation

 

估计PFH特征

 

点特征直方图(PFH)在PCL中的实现是pcl_features模块的一部分。默认PFH的实现使用5个区间分类(例如:四个特征值中的每个都使用5个区间来统计),
以下代码段将对输入数据集中的所有点估计其对应的PFH特征。

 

#include <pcl/point_types.h>                  //点类型头文件
#include <pcl/features/pfh.h>                 //pfh特征估计类头文件
...//其他相关操作
pcl::PointCloud<pcl::PointXYZ>::Ptrcloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptrnormals(new pcl::PointCloud<pcl::Normal>());
...//打开点云文件估计法线等
//创建PFH估计对象pfh,并将输入点云数据集cloud和法线normals传递给它
pcl::PFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::PFHSignature125> pfh;
pfh.setInputCloud(cloud);
pfh.setInputNormals(normals);
//如果点云是类型为PointNormal,则执行pfh.setInputNormals (cloud);
//创建一个空的kd树表示法,并把它传递给PFH估计对象。
//基于已给的输入数据集,建立kdtree
pcl::KdTreeFLANN<pcl::PointXYZ>::Ptrtree(new pcl::KdTreeFLANN<pcl::PointXYZ>());
pfh.setSearchMethod(tree);
//输出数据集
pcl::PointCloud<pcl::PFHSignature125>::Ptrpfhs(new pcl::PointCloud<pcl::PFHSignature125>());
//使用半径在5厘米范围内的所有邻元素。
//注意:此处使用的半径必须要大于估计表面法线时使用的半径!!!
pfh.setRadiusSearch(0.05);
//计算pfh特征值
pfh.compute(*pfhs);
// pfhs->points.size ()应该与input cloud->points.size ()有相同的大小,即每个点都有一个pfh特征向量

 

PFHEstimation类的实际计算程序内部只执行以下:

 

对点云P中的每个点p

 

1.得到p点的最近邻元素
2.对于邻域内的每对点,计算其三个角度特征参数值
3.将所有结果统计到一个输出直方图中

 

快速点特征直方图(FPFH)描述子

 

已知点云P中有n个点,那么它的点特征直方图(PFH)的理论计算复杂度是O(nk2),其中 k是点云P中每个点p计算特征向量时考虑的邻域数量。对于实时应用或接近实时应用中,密集点云的点特征直方图(PFH)的计算,是一个主要的性能瓶颈。此处为PFH计算方式的简化形式,称为快速点特征直方图FPFH(Fast Point Feature Histograms)

 

为了简化直方图的特征计算,我们执行以下过程:
第一步,对于每一个查询点 ,计算这个点和它的邻域点之间的一个元组 (参考上一节PFH的介绍),第一步结果我们称之为简化的点特征直方图SPFH(Simple Point Feature Histograms);
第二步,重新确定每个点的k邻域,使用邻近的SPFH值来计算的最终直方图(称为FPFH)
权重 在一些给定的度量空间中,表示查询点 和其邻近点 之间的距离,因此可用来评定一对点( , ),但是如果需要的话,也可以把用 另一种度量来表示。如图1所示可以帮助理解这个权重方式的重要性,它表示的是以点Pq为中心的k邻域影响范围。

 

微信图片_20210117161823

 

以点 Pq为中心的k邻域影响范围图
因此,对于一个已知查询点 ,这个算法首先只利用 和它邻域点之间对应对(上图中以红色线来说明),来估计它的SPFH值,很明显这样比PFH的标准计算少了邻域点之间的互联。点云数据集中的所有点都要执行这一计算获取SPFH,接下来使用它的邻近点 的SPFH值和 点的SPFH值重新权重计算,从而得到 点的最终FPFH值。FPFH计算添加的计算连接对,在上图中以黑色线表示。如上图所示,一些重要对点(与 直接相连的点)被重复计数两次(图中以粗线来表示),而其他间接相连的用细黑线表示。

 

PFH和FPFH的区别

 

PFH和FPFH计算方式之间的主要区别总结如下:

1.FPFH没有对全互连 点的所有邻近点的计算参数进行统计,从图中可以看到,因此可能漏掉了一些重要的点对,而这些漏掉的对点可能对捕获查询点周围的几何特征有贡献。
2.PFH特征模型是对查询点周围的一个精确的邻域半径内,而FPFH还包括半径r范围以外的额外点对(不过在2r内);
3.因为重新权重计算的方式,所以FPFH结合SPFH值,重新捕获邻近重要点对的几何信息;
4.由于大大地降低了FPFH的整体复杂性,因此FPFH有可能使用在实时应用中;
5.通过分解三元组,简化了合成的直方图。也就是简单生成d分离特征直方图,对每个特征维度来单独绘制,并把它们连接在一起

 

估计FPFH特征

 

快速点特征直方图FPFH在点云库中的实现可作为pcl_features库的一部分。默认的FPFH实现使用11个统计子区间(例如:四个特征值中的每个都将它的参数区间分割为11个),特征直方图被分别计算然后合并得出了浮点值的一个33元素的特征向量,这些保存在一个pcl::FPFHSignature33点类型中。以下代码段将对输入数据集中的所有点估计一组FPFH特征值。

 

#include
#include           //fpfh特征估计类头文件声明
...//其他相关操作
pcl::PointCloud<pcl::PointXYZ>::Ptrcloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptrnormals(new pcl::PointCloud<pcl::Normal>());
...//打开点云文件估计法线等
//创建FPFH估计对象fpfh,并把输入数据集cloud和法线normals传递给它。
pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh;
fpfh.setInputCloud(cloud);
fpfh.setInputNormals(normals);
//如果点云是类型为PointNormal,则执行fpfh.setInputNormals (cloud);
//创建一个空的kd树对象tree,并把它传递给FPFH估计对象。
//基于已知的输入数据集,建立kdtree
pcl::search::KdTree<PointXYZ>::Ptrtree(new pcl::search::KdTree<PointXYZ>);
fpfh.setSearchMethod(tree);
//输出数据集
pcl::PointCloud<pcl::FPFHSignature33>::Ptrfpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
//使用所有半径在5厘米范围内的邻元素
//注意:此处使用的半径必须要大于估计表面法线时使用的半径!!!
fpfh.setRadiusSearch(0.05);
//计算获取特征向量
fpfh.compute(*fpfhs);
// fpfhs->points.size ()应该和input cloud->points.size ()有相同的大小,即每个点有一个特征向量

 

FPFHEstimation类的实际计算内部只执行以下操作:
对点云P中的每个点p

第一步:
1.得到:math:p的邻域元素
2. 计算每一对:math:p, p_k的三个角度参数值(其中:math:p_k是:math:p的邻元素)
3.把所有结果统计输出到一个SPFH直方图

 

第二步:
1.得到:math:p的最近邻元素
2.使用:math:p的每一个SPFH和一个权重计算式,来计算最终:math:p的FPFH

 

估计一点云的VFH特征

 

视点特征直方图(或VFH)是源于FPFH描述子(见Fast Point Feature Histograms (PFH) 描述子)。由于它的获取速度和识别力,我们决定利用FPFH强大的识别力,但是为了使构造的特征保持缩放不变性的性质同时,还要区分不同的位姿,计算时需要考虑加入视点变量。我们做了以下两种计算来构造特征,以应用于目标识别问题和位姿估计:1.扩展FPFH,使其利用整个点云对象来进行计算估计(如2图所示),在计算FPFH时以物体中心点与物体表面其他所有点之间的点对作为计算单元。2.添加视点方向与每个点估计法线之间额外的统计信息,为了达到这个目的,我们的关键想法是在FPFH计算中将视点方向变量直接融入到相对法线角计算当中。

 

通过统计视点方向与每个法线之间角度的直方图来计算视点相关的特征分量。注意:并不是每条法线的视角,因为法线的视角在尺度变换下具有可变性,我们指的是平移视点到查询点后的视点方向和每条法线间的角度。第二组特征分量就是前面PFH中讲述的三个角度,如PFH小节所述,只是现在测量的是在中心点的视点方向和每条表面法线之间的角度

 

因此新组合的特征被称为视点特征直方图(VFH)。下图表体现的就是新特征的想法,包含了以下两部分:
1.一个视点方向相关的分量
2.一个包含扩展FPFH的描述表面形状的分量

 

估计VFH特征值

 

视点特征直方图在PCL中的实现属于pcl_features模块库的一部分。对扩展的FPFH分量来说,默认的VFH的实现使用45个子区间进行统计,而对于视点分量要使用128个子区间进行统计,这样VFH就由一共308个浮点数组成阵列。在PCL中利用pcl::VFHSignature308的点类型来存储表示。PFH/FPFH描述子和VFH之间的主要区别是:对于一个已知的点云数据集,只一个单一的VFH描述子,而合成的PFH/FPFH特征的数目和点云中的点数目相同。以下代码段将对输入数据集中的所有点估计一组VFH特征值。

 

#include <pcl/point_types.h>
#include <pcl/features/vfh.h>                     //VFH特征估计类头文件
...//其他相关操作
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
...//打开点云文件估计法线等
//创建VFH估计对象vfh,并把输入数据集cloud和法线normal传递给它
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
vfh.setInputCloud (cloud);
vfh.setInputNormals (normals);
//如果点云是PointNormal类型,则执行vfh.setInputNormals (cloud);
//创建一个空的kd树对象,并把它传递给FPFH估计对象。
//基于已知的输入数据集,建立kdtree
pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
vfh.setSearchMethod (tree);
//输出数据集
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
//计算特征值
vfh.compute (*vfhs);
// vfhs->points.size ()的大小应该是1,即vfh描述子是针对全局的特征描述

 

如何从一个深度图像(range image)中提取NARF特征
代码解析narf_feature_extraction.cpp

 

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>
#include <pcl/features/narf_descriptor.h>
#include <pcl/console/parse.h>

typedef pcl::PointXYZ PointType;

//参数的设置
float angular_resolution = 0.5f;
float support_size = 0.2f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;
bool rotation_invariant = true;

//命令帮助
void 
printUsage (const char* progName)
{
  std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
            << "Options:\n"
            << "-------------------------------------------\n"
            << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
            << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
            << "-m           Treat all unseen points to max range\n"
            << "-s <float>   support size for the interest points (diameter of the used sphere - "
                                                                  "default "<<support_size<<")\n"
            << "-o <0/1>     switch rotational invariant version of the feature on/off"
            <<               " (default "<< (int)rotation_invariant<<")\n"
            << "-h           this help\n"
            << "\n\n";
}

void 
setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)//setViewerPose
{
  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
  viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
                            look_at_vector[0], look_at_vector[1], look_at_vector[2],
                            up_vector[0], up_vector[1], up_vector[2]);
}

int 
main (int argc, char** argv)
{
 // 设置参数检测
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::find_argument (argc, argv, "-m") >= 0)
  {
    setUnseenToMaxRange = true;
    cout << "Setting unseen values in range image to maximum range readings.\n";
  }
  if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)
    cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\n";
  int tmp_coordinate_frame;
  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
  {
    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
    cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
  }
  if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
    cout << "Setting support size to "<<support_size<<".\n";
  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
    cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
  angular_resolution = pcl::deg2rad (angular_resolution);
  
//打开一个磁盘中的.pcd文件  但是如果没有指定就会自动生成
  pcl::PointCloud<PointType>::Ptr    point_cloud_ptr (new pcl::PointCloud<PointType>);
  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;

  pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
  if (!pcd_filename_indices.empty ())   //检测是否有far_ranges.pcd
  {
    std::string filename = argv[pcd_filename_indices[0]];
    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
    {
      cerr << "Was not able to open file \""<<filename<<"\".\n";
      printUsage (argv[0]);
      return 0;
    }
    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
                                                               point_cloud.sensor_origin_[1],
                                                               point_cloud.sensor_origin_[2])) *
                        Eigen::Affine3f (point_cloud.sensor_orientation_);
    std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
    if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)
      std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
  }
  else
  {
    setUnseenToMaxRange = true;
    cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
    for (float x=-0.5f; x<=0.5f; x+=0.01f)   //如果没有打开的文件就生成一个矩形的点云
    {
      for (float y=-0.5f; y<=0.5f; y+=0.01f)
      {
        PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
        point_cloud.points.push_back (point);
      }
    }
    point_cloud.width = (int) point_cloud.points.size ();  point_cloud.height = 1;
  }
  
//从点云中建立生成深度图
  float noise_level = 0.0;    
  float min_range = 0.0f;
  int border_size = 1;
  boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
  pcl::RangeImage& range_image = *range_image_ptr;   
  range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
  range_image.integrateFarRanges (far_ranges);
  if (setUnseenToMaxRange)
    range_image.setUnseenToMaxRange ();
  
  //打开3D viewer并加入点云
  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
  viewer.setBackgroundColor (1, 1, 1);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
  viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
  //viewer.addCoordinateSystem (1.0f, "global");
  //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
  //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
  viewer.initCameraParameters ();
  setViewerPose (viewer, range_image.getTransformationToWorldSystem ());
  //显示
  pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
  range_image_widget.showRangeImage (range_image);
  
  //提取NARF特征
  pcl::RangeImageBorderExtractor range_image_border_extractor;    //申明深度图边缘提取器
  pcl::NarfKeypoint narf_keypoint_detector;                       //narf_keypoint_detector为点云对象

  narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);
  narf_keypoint_detector.setRangeImage (&range_image);
  narf_keypoint_detector.getParameters ().support_size = support_size;    //获得特征提取的大小
  
  pcl::PointCloud<int> keypoint_indices;
  narf_keypoint_detector.compute (keypoint_indices);
  std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";

  // ----------------------------------------------
  // -----Show keypoints in range image widget-----
  // ----------------------------------------------
  //for (size_t i=0; i<keypoint_indices.points.size (); ++i)
    //range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
                                  //keypoint_indices.points[i]/range_image.width);
  
  //在3Dviewer显示提取的特征信息
  pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
  keypoints.points.resize (keypoint_indices.points.size ());
  for (size_t i=0; i<keypoint_indices.points.size (); ++i)
    keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
  viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
  
  //在关键点提取NARF描述子
  std::vector<int> keypoint_indices2;
  keypoint_indices2.resize (keypoint_indices.points.size ());
  for (unsigned int i=0; i<keypoint_indices.size (); ++i) // This step is necessary to get the right vector type
    keypoint_indices2[i]=keypoint_indices.points[i];      ///建立NARF关键点的索引向量,此矢量作为NARF特征计算的输入来使用

  pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);//创建narf_descriptor对象。并给了此对象输入数据(特征点索引和深度像)
  narf_descriptor.getParameters ().support_size = support_size;//support_size确定计算描述子时考虑的区域大小
  narf_descriptor.getParameters ().rotation_invariant = rotation_invariant;    //设置旋转不变的NARF描述子
  pcl::PointCloud<pcl::Narf36> narf_descriptors;               //创建Narf36的点类型输入点云对象并进行实际计算
  narf_descriptor.compute (narf_descriptors);                 //计算描述子
  cout << "Extracted "<<narf_descriptors.size ()<<" descriptors for "   //打印输出特征点的数目和提取描述子的数目
                      <<keypoint_indices.points.size ()<< " keypoints.\n";
  
//主循环函数
  while (!viewer.wasStopped ())
  {
    range_image_widget.spinOnce ();  // process GUI events
    viewer.spinOnce ();
    pcl_sleep(0.01);
  }
}

 

编译运行./narf_feature_extraction -m

这将自动生成一个呈矩形的点云,检测的特征点处在角落处,参数-m是必要的,因为矩形周围的区域观测不到,但是属于边界部分,因此系统无法检测到这部分区域的特征点,选项-m将看不到的区域改变到最大范围读取,从而使系统能够使用这些边界区域。

 

微信图片_20210117161914

 

(2)特征描述算子算法基准化分析

 

使用FeatureEvaluationFramework类对不同的特征描述子算法进行基准测试,基准测试框架可以测试不同种类的特征描述子算法,通过选择输入点云,算法参数,下采样叶子大小,搜索阀值等独立变量来进行测试。
使用FeatureCorrespondenceTest类执行一个单一的“基于特征的对应估计测试”执行以下的操作

 

1.FeatureCorrespondenceTest类取两个输入点云(源与目标) 它将指定算法和参数,在每个点云中计算特征描述子
2.基于n_D特征空间中的最近邻元素搜索,源点云中的每个特征将和目标点云中对应的特征相对照
3 。对于每一个点,系统将把估计的目标点的三维位置和之前已知的实际位置相比
4 。如果这两个点很接近(取决与决定的阀值)那么对应就成功,否则失败
5 计算并保存成功和失败的总数,以便进一步分析

发表评论

后才能评论