PCL 点云特征描述与提取

说明

• 以下均为 Being_young 前辈所写，现转载过来，再加上自己的理解，重新写了一遍，方便自己日后使用
• 博客地址：http://blog.csdn.net/u013019296/article/
• 或者参考另一篇博文：[PCL系列——从深度图像(RangeImage)中提取NARF关键点]

---

· 目录索引

1. PCL 点云特征描述与提取
2. PCL 描述三维特征相关基础
3. PCL 法线估计实例 —— 估计某一点的表面法线
4. PCL 法线估计实例 —— 估计一个点云的表面法线
5. PCL 使用积分图像进行法线估计
6. 点特征直方图（PFH）描述子
7. 快速点特征直方图(FPFH)描述子
8. 估视点特征直方图
9. 从一个深度图像提取NARF特征
10. 特征描述算子算法基准化分析

---

· PCL 点云特征描述与提取

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

相关的概念与算法

3D形状内容描述子（3D shape contexts）

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

旋转图像（spin iamge）

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

涉及的算法相关的资料

3D形状内容描述子
https://en.wikipedia.org/wiki/Shape_context
www.eecs.berkeley.edu/Research/Projects/CS/vision/shape/belongie-pami02.pdf

---

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

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

• 平移选转不变性（刚体变换）：即三维旋转和三维平移变化 不会影响特征向量F估计，即特征向量具有
• 抗密度干扰性（改变采样密度）：原则上，一个局部表面小块的采样密度无论是大还是小，都应该有相同的特征向量值
• 对噪声具有稳定性（噪声）：数据中有轻微噪声的情况下，点特征表示在它的特征向量中必须保持相同或者极其相似的值

通常，PCL中特征向量利用快速 kd-tree 查询 ，使用近似法来计算查询点的最近邻元素，通常有两种查询类型：K邻域查询，半径搜索两中方法

---

· PCL 法线估计实例 —— 估计某一点的表面法线

对应点云P中每一个点p得到p点最近邻元素，计算p点的表面的法线N，检查N的方向是否指向视点如果不是则翻转

---

一旦确定查询点的邻域后，查询点的邻域点可以用来估计一个局部特征描述子，它用查询点周围领域点描述采样面的几何特征，描述几何表面图形的一个重要属性。首先是推断它在坐标系中的方位，也就是估计他的法线（表面法线是表面的一个重要的属性）。

#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);
//视点默认坐标是（0，0，0）可使用 setViewPoint(float vpx,float vpy,float vpz)来更换
while (!viewer.wasStopped ())
{
     viewer.spinOnce ();
}

return 0;
}

PCL 法线估计实例 —— 估计一个点云的表面法线

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

• 使用曲面重建技术，从获取的点云数据中得到采样点对应的曲面，然后从曲面模型中计算出表面法线
• . 直接从点云数据中近似推断表面法线

在确定表面一点法线的问题近似于估计表面的一个相切面法线的问题，因此转换过来就是求一个最小二乘法平面拟合的问题

---

· PCL 使用积分图像进行法线估计

· 注意此方法只适用于有序点云

     #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;
     }

点特征直方图（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）在PCL中的实现是pcl_features模块的一部分。默认PFH的实现使用5个区间分类（例如：四个特征值中的每个都使用5个区间来统计）

#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：

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

---

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

为了简化直方图的特征计算，我们执行以下过程：

• 第一步，对于每一个查询点 ，计算这个点和它的邻域点之间的一个元组 （参考上一节PFH的介绍），第一步结果我们称之为简化的点特征直方图SPFH（Simple Point Feature Histograms）；
• 第二步，重新确定每个点的k邻域，使用邻近的SPFH值来计算的最终直方图（称为FPFH）

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

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

速点特征直方图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

• 得到 p 的邻域元素
• 计算每一对 p 、p_k 的三个角度参数值（其中 p 、p_k 的邻元素）
• 把所有结果统计输出到一个SPFH直方图
• 得到 p 的邻域元素
• 使用 p 的每一个SPFH和一个权重计算式，来计算最终 p 的FPFH

---

· 估视点特征直方图

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

通过统计视点方向与每个法线之间角度的直方图来计算视点相关的特征分量。

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

因此新组合的特征被称为视点特征直方图（VFH）。下图表体现的就是新特征的想法，包含了以下两部分：

• 一个视点方向相关的分量
• 一个包含扩展FPFH的描述表面形状的分量

在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描述子是针对全局的特征描述

 从一个深度图像提取NARF特征

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

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

---

· 特征描述算子算法基准化分析

使用FeatureEvaluationFramework类对不同的特征描述子算法进行基准测试，基准测试框架可以测试不同种类的特征描述子算法，通过选择输入点云，算法参数，下采样叶子大小，搜索阀值等独立变量来进行测试。

使用FeatureCorrespondenceTest类执行一个单一的“基于特征的对应估计测试”执行以下操作：

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