机器人三维视觉 pcl 点云库 云关键点 /Keypoints Harris3D、 NARF

 机器人三维视觉 pcl 点云库 云关键点 /Keypoints Harris3D、 NARF

博文末尾支持二维码赞赏哦

我们都知道在二维图像上，有Harris、SIFT、SURF、KAZE这样的关键点提取算法，
这种特征点的思想可以推广到三维空间。

关键点的数量相比于原始点云或图像的数据量减小很多，与局部特征描述子结合在一起，
组成　关键点描述子　常用来形成原始数据的表示，而且不失代表性和描述性，
从而加快了后续的识别，追踪等对数据的处理了速度，
故而，关键点技术成为在2D和3D 信息处理中非常关键的技术。

常见的三维点云关键点提取算法有一下几种：ISS3D、Harris3D、NARF、SIFT3D
这些算法在PCL库中都有实现，其中NARF算法是博主见过用的比较多的。

PCL—低层次视觉—关键点检测
关键点检测往往需要和特征提取联合在一起，关键点检测的一个重要性质就是旋转不变性，
也就是说，物体旋转后还能够检测出对应的关键点。不过说实话我觉的这个要求对机器人视觉来说是比较鸡肋的。
因为机器人采集到的三维点云并不是一个完整的物体，没哪个相机有透视功能。
机器人采集到的点云也只是一层薄薄的蒙皮。所谓的特征点又往往在变化剧烈的曲面区域，
那么从不同的视角来看，变化剧烈的曲面区域很难提取到同样的关键点。
想象一下一个人的面部，正面的时候鼻尖可以作为关键点，但是侧面的时候呢？
会有一部分面部在阴影中，模型和之前可能就完全不一样了。

也就是说现在这些关键点检测算法针对场景中较远的物体，
也就是物体旋转带来的影响被距离减弱的情况下，是好用的。
一旦距离近了，旋转往往造成捕获的仅有模型的侧面，关键点检测算法就有可能失效。

---------------------------------------------------------------------------
ISS算法的全程是Intrinsic Shape Signatures，第一个词叫做内部，这个词挺有讲究。
说内部，那必然要有个范围，具体是什么东西的范围还暂定。
如果说要描述一个点周围的局部特征，而且这个物体在全局坐标下还可能移动，
那么有一个好方法就是在这个点周围建立一个局部坐标。
只要保证这个局部坐标系也随着物体旋转就好。


方法1.基于协方差矩阵
协方差矩阵的思想其实很简单，实际上它是一种耦合，把两个步骤耦合在了一起
  1.把pi和周围点pj的坐标相减：本质上这生成了许多从pi->pj的向量，
  理想情况下pi的法线应该是垂直于这些向量的
  2.利用奇异值分解求这些向量的0空间，拟合出一个尽可能垂直的向量，作为法线的估计

协方差矩阵本质是啥？就是奇异值分解中的一个步骤。。。。
 奇异值分解是需要矩阵乘以自身的转置从而得到对称矩阵的。
当然，用协方差计算的好处是可以给不同距离的点附上不同的权重。

方法2.基于齐次坐标
  1.把点的坐标转为齐次坐标
  2.对其次坐标进行奇异值分解
  3.最小奇异值对应的向量就是拟合平面的方程
  4.方程的系数就是法线的方向。
显然，这种方法更加简单粗暴，省去了权重的概念，但是换来了运算速度，不需要反复做减法。
 其实本来也不需要反复做减法，做一个点之间向量的检索表就好。。。

但是我要声明PCL的实现是利用反复减法的。

都会有三个相互垂直的向量，一个是法线方向，另外两个方向与之构成了在某点的局部坐标系。
在此局部坐标系内进行建模，就可以达到点云特征旋转不变的目的了。

ISS特征点检测的思想也甚是简单：
1.利用方法1建立模型
2.其利用特征值之间关系来形容该点的特征程度。
显然这种情况下的特征值是有几何意义的，特征值的大小实际上是椭球轴的长度。
  椭球的的形态则是对邻近点分布状态的抽象总结。
  试想，如果临近点沿某个方向分布致密则该方向会作为椭球的第一主方向，
  稀疏的方向则是第二主方向，法线方向当然是极度稀疏（只有一层），那么则作为第三主方向。

如果某个点恰好处于角点，则第一主特征值，第二主特征值，第三主特征值大小相差不会太大。
如果点云沿着某方向致密，而垂直方向系数则有可能是边界。
总而言之，这种局部坐标系建模分析的方法是基于特征值分析的特征点提取。
-----------------------------------------------------------------------
Trajkovic关键点检测算法

角点的一个重要特征就是法线方向和周围的点存在不同，
而本算法的思想就是和相邻点的法线方向进行对比，判定法线方向差异的阈值，
最终决定某点是否是角点。并且需要注意的是，本方法所针对的点云应该只是有序点云。
本方法的优点是快，缺点是对噪声敏感。
手头没有有序点云，不做测试了。

除去NARF这种和特征检测联系比较紧密的方法外，一般来说特征检测都会对曲率变化比较剧烈的点更敏感。
Harris算法是图像检测识别算法中非常重要的一个算法，其对物体姿态变化鲁棒性好，
对旋转不敏感，可以很好的检测出物体的角点。
甚至对于标定算法而言，HARRIS角点检测是使之能成功进行的基础。
HARRIS算法的思想还是很有意思的。很聪明也很trick.
--------------------------------------------------------------------
Harris 算法　
其思想及数学推导大致如下：
1.在图像中取一个窗 w (矩形窗，高斯窗，XX窗，各种窗，
  某师姐要改标定算法不就可以从选Harris的窗开始做起么）

2.获得在该窗下的灰度 I
3.移动该窗，则灰度会发生变化，平坦区域灰度变化不大，
  边缘区域沿边缘方向灰度变化剧烈，角点处各个方向灰度变化均剧烈

4.依据3中条件选出角点

1.两个特征值都很大==========>角点（两个响应方向）
2.一个特征值很大，一个很小=====>边缘（只有一个响应方向）
3.两个特征值都小============>平原地区（响应都很微弱）
---------------------------------------------------------------------------
3DHarris　 方块体内点数量变化确定角点
在2DHarris里，我们使用了 图像梯度构成的 协方差矩阵。 
  图像梯度。。。嗯。。。。每个像素点都有一个梯度，
  在一阶信息量的情况下描述了两个相邻像素的关系。显然这个思想可以轻易的移植到点云上来。

想象一下，如果在 点云中存在一点p
1、在p上建立一个局部坐标系：z方向是法线方向，x,y方向和z垂直。
2、在p上建立一个小正方体，不要太大，大概像材料力学分析应力那种就行
3、假设点云的密度是相同的，点云是一层蒙皮，不是实心的。
a、如果小正方体沿z方向移动，那小正方体里的点云数量应该不变
b、如果小正方体位于边缘上，则沿边缘移动，点云数量几乎不变，沿垂直边缘方向移动，点云数量改
c、如果小正方体位于角点上，则有两个方向都会大幅改变点云数量

如果由法向量x,y,z构成协方差矩阵，那么它应该是一个对称矩阵。
而且特征向量有一个方向是法线方向，另外两个方向和法线垂直。
那么直接用协方差矩阵替换掉图像里的M矩阵，就得到了点云的Harris算法。
其中，半径r可以用来控制角点的规模
r小，则对应的角点越尖锐（对噪声更敏感）
r大，则可能在平缓的区域也检测出角点

----------------------------------------------------------------------
NARF　
1. 边缘提取
对点云而言，场景的边缘代表前景物体和背景物体的分界线。
所以，点云的边缘又分为三种：

前景边缘，背景边缘，阴影边缘。

三维点云的边缘有个很重要的特征，
就是点a 和点b 如果在 rangImage 上是相邻的，然而在三维距离上却很远，那么多半这里就有边缘。
由于三维点云的规模和稀疏性，“很远”这个概念很难描述清楚。
到底多远算远？这里引入一个横向的比较是合适的。这种比较方法可以自适应点云的稀疏性。
所谓的横向比较就是和 某点周围的点相比较。 这个周围有多大？不管多大，
反正就是在某点pi的rangeImage 上取一个方窗。
假设像素边长为s. 那么一共就取了s^2个点。
接下来分三种情况来讨论所谓的边缘：


1.这个点在某个平面上，边长为 s 的方窗没有涉及到边缘
2.这个点恰好在某条边缘上，边长 s 的方窗一半在边缘左边，一半在右边
3.这个点恰好处于某个角点上，边长 s 的方窗可能只有 1/4 与 pi 处于同一个平面
如果将 pi 与不同点距离进行排序，得到一系列的距离，d0 表示与 pi 距离最近的点，显然是 pi 自己。

ds^2 是与pi 最远的点，这就有可能是跨越边缘的点了。 
选择一个dm，作为与m同平面，但距离最远的点。
也就是说，如果d0~ds^2是一个连续递增的数列，那么dm可以取平均值。

如果这个数列存在某个阶跃跳动（可能会形成类似阶跃信号）
那么则发生阶跃的地方应该是有边缘存在，不妨取阶跃点为dm(距离较小的按个阶跃点）
原文并未如此表述此段落，原文取s=5, m=9 作为m点的一个合理估计。
-------------------------------------------------------------------------------
关键点提取

  在提取关键点时，边缘应该作为一个重要的参考依据。
但一定不是唯一的依据。对于某个物体来说关键点应该是表达了某些特征的点，而不仅仅是边缘点。
所以在设计关键点提取算法时，需要考虑到以下一些因素：
边缘和曲面结构都要考虑进去；
关键点要能重复；
关键点最好落在比较稳定的区域，方便提取法线。

对于点云构成的曲面而言，某处的曲率无疑是一个非常重要的结构描述因素。
某点的曲率越大，则该点处曲面变化越剧烈。
在2D rangeImage 上，去 pi 点及其周边与之距离小于2deta的点，
进行PCA主成分分析。可以得到一个 主方向v，以及曲率值 lamda. 
注意， v 必然是一个三维向量。

那么对于边缘点，可以取其 权重 w 为1 ， v 为边缘方向。
对于其他点，取权重 w 为 1-(1-lamda)^3 ， 方向为 v 在平面 p上的投影。 
平面 p 垂直于 pi 与原点连线。
到此位置，每个点都有了两个量，一个权重，一个方向。
将权重与方向带入下列式子 I 就是某点 为特征点的可能性。

 

 

１】NARF

代码地址

/* 
NARF　
从深度图像(RangeImage)中提取NARF关键点
1. 边缘提取
对点云而言，场景的边缘代表前景物体和背景物体的分界线。
所以，点云的边缘又分为三种：
前景边缘，背景边缘，阴影边缘。
就是点a 和点b 如果在 rangImage 上是相邻的，然而在三维距离上却很远，那么多半这里就有边缘。
在提取关键点时，
边缘应该作为一个重要的参考依据。
但一定不是唯一的依据。
对于某个物体来说关键点应该是表达了某些特征的点，而不仅仅是边缘点。
所以在设计关键点提取算法时，需要考虑到以下一些因素：
边缘和曲面结构都要考虑进去；
关键点要能重复；
关键点最好落在比较稳定的区域，方便提取法线。
图像的Harris角点算子将图像的关键点定义为角点。
角点也就是物体边缘的交点，
harris算子利用角点在两个方向的灰度协方差矩阵响应都很大，来定义角点。
既然关键点在二维图像中已经被成功定义且使用了，
看来在三维点云中可以沿用二维图像的定义
不过今天要讲的是另外一种思路，简单粗暴，
直接把三维的点云投射成二维的图像不就好了。
这种投射方法叫做range_image.
*/
#include <iostream>//标准输入输出流
#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>// RangeImage 深度图像
#include <pcl/io/pcd_io.h>//PCL的PCD格式文件的输入输出头文件
#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/console/parse.h>//解析 命令行 参数

//定义别名
typedef pcl::PointXYZ PointType;
// --------------------
// -----参数　Parameters-----
// --------------------
//参数 全局变量
float angular_resolution = 0.5f;//角坐标分辨率
float support_size = 0.2f;//感兴趣点的尺寸（球面的直径）
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;//坐标框架：相机框架（而不是激光框架）
bool setUnseenToMaxRange = false;//是否将所有不可见的点 看作 最大距离

// --------------
// -----打印帮助信息　Help-----
// --------------
//当用户输入命令行参数-h，打印帮助信息
void 
printUsage (const char* progName)
{
  std::cout << "\n\n用法　Usage: "<<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 as maximum range readings\n"
            << "-s <float>   support size for the interest points (diameter of the used sphere - "
            <<                                                     "default "<<support_size<<")\n"
            << "-h           this help\n"
            << "\n\n";
}

//void 
//setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
//{
  //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]);
//}

// --------------
// -----Main-----
// --------------
int 
main (int argc, char** argv)
{
  // --------------------------------------
  // ----- 解析 命令行 参数 Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)//help参数
  {
    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";
  }
  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);
  
  // ------------------------------------------------------------------
  // -----Read pcd file or create example point cloud if not given-----
  // ------------------------------------------------------------------
  //读取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 ());//仿射变换
  //检查参数中是否有pcd格式文件名，返回参数向量中的索引号
  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
  if (!pcd_filename_indices.empty())
  {
    std::string filename = argv[pcd_filename_indices[0]];
    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)//如果指定了pcd文件，读取pcd文件
    {
      std::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//没有指定pcd文件，生成点云，并填充它
  {
    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;
  }
  
  // -----------------------------------------------
  // -----Create RangeImage from the PointCloud-----
  // -----------------------------------------------
  // 从点云数据，创建深度图像
  // 直接把三维的点云投射成二维的图像
  float noise_level = 0.0;
//noise level表示的是容差率，因为1°X1°的空间内很可能不止一个点，
//noise level = 0则表示去最近点的距离作为像素值，如果=0.05则表示在最近点及其后5cm范围内求个平均距离
//minRange表示深度最小值，如果=0则表示取1°X1°的空间内最远点，近的都忽略
  float min_range = 0.0f;
//bordersieze表示图像周边点 
  int border_size = 1;
  boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);//创建RangeImage对象（智能指针）
  pcl::RangeImage& range_image = *range_image_ptr; //RangeImage的引用  
  //从点云创建深度图像
//rangeImage也是PCL的基本数据结构
//pcl::RangeImage rangeImage;
// 球坐标系
//角分辨率
//float angularResolution = (float) (  1.0f * (M_PI/180.0f));  //   1.0 degree in radians　弧度
//phi可以取360°
//  float maxAngleWidth     = (float) (360.0f * (M_PI/180.0f));  // 360.0 degree in radians
//a取180°
//  float maxAngleHeight    = (float) (180.0f * (M_PI/180.0f));  // 180.0 degree in radians
//半圆扫一圈就是整个图像了
  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 ();
  
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  // 3D点云显示
  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 ());
  
  // --------------------------
  // -----Show range image-----
  // --------------------------
  //显示深度图像（平面图）
  pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
  range_image_widget.showRangeImage (range_image);
  
  // --------------------------------
  // -----Extract NARF keypoints-----
  // --------------------------------
  // 提取NARF关键点
  pcl::RangeImageBorderExtractor range_image_border_extractor;//创建深度图像的边界提取器，用于提取NARF关键点
  pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor);//创建NARF对象
  narf_keypoint_detector.setRangeImage (&range_image);//设置点云对应的深度图
  narf_keypoint_detector.getParameters ().support_size = support_size;// 感兴趣点的尺寸（球面的直径）
  //narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
  //narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;
  
  pcl::PointCloud<int> keypoint_indices;//用于存储关键点的索引
  narf_keypoint_detector.compute (keypoint_indices);//计算NARF关键
  std::cout << "Found找到关键点： "<<keypoint_indices.points.size ()<<" key points.\n";

  // ----------------------------------------------
  // -----Show keypoints in range image widget-----
  // ----------------------------------------------
 //在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);
  
  // -------------------------------------
  // -----Show keypoints in 3D viewer-----
  // -------------------------------------
  //在3D图形窗口中显示关键点
  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");
  
  //--------------------
  // -----Main loop-----
  //--------------------
  while (!viewer.wasStopped ())
  {
    range_image_widget.spinOnce ();  // process GUI events　　 处理 GUI事件
    viewer.spinOnce ();
    pcl_sleep(0.01);
  }
}

２】3DHarris　 方块体内点数量变化确定角点

代码地址

/* 
3DHarris　 方块体内点数量变化确定角点
　　在2DHarris里，我们使用了 图像梯度构成的 协方差矩阵。 
    图像梯度。。。嗯。。。。每个像素点都有一个梯度，
    在一阶信息量的情况下描述了两个相邻像素的关系。显然这个思想可以轻易的移植到点云上来。
想象一下，如果在 点云中存在一点p
　　1、在p上建立一个局部坐标系：z方向是法线方向，x,y方向和z垂直。
　　2、在p上建立一个小正方体，不要太大，大概像材料力学分析应力那种就行
　　3、假设点云的密度是相同的，点云是一层蒙皮，不是实心的。
　　a、如果小正方体沿z方向移动，那小正方体里的点云数量应该不变
　　b、如果小正方体位于边缘上，则沿边缘移动，点云数量几乎不变，沿垂直边缘方向移动，点云数量改
　　c、如果小正方体位于角点上，则有两个方向都会大幅改变点云数量
　　如果由法向量x,y,z构成协方差矩阵，那么它应该是一个对称矩阵。
而且特征向量有一个方向是法线方向，另外两个方向和法线垂直。
　　那么直接用协方差矩阵替换掉图像里的M矩阵，就得到了点云的Harris算法。
　　其中，半径r可以用来控制角点的规模
　　r小，则对应的角点越尖锐（对噪声更敏感）
　　r大，则可能在平缓的区域也检测出角点
./harris3d ../../Filtering/build/table_scene_lms400_inliers.pcd 
*/
#include <iostream>//标准输入输出流
#include <boost/thread/thread.hpp>
#include <pcl/io/pcd_io.h>//PCL的PCD格式文件的输入输出头文件
#include <pcl/visualization/pcl_visualizer.h>
//#include <pcl/keypoints/narf_keypoint.h>//关键点检测
#include <pcl/keypoints/harris_3d.h>

#include <cstdlib>
#include <vector>
using namespace std;

//定义别名
typedef pcl::PointXYZ PointType;

// --------------
// -----Main-----
// --------------
int 
main (int argc, char** argv)
{

  // ------------------------------------------------------------------
  // -----Read pcd file or create example point cloud if not given-----
  // ------------------------------------------------------------------
  //读取pcd文件；如果没有指定文件，就创建样本点
  pcl::PointCloud<PointType>::Ptr point_cloud_ptr(new pcl::PointCloud<PointType>);//点云对象指针
  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;//引用　上面点云的别名　常亮指针
  //检查参数中是否有pcd格式文件名，返回参数向量中的索引号
  if (argc>=2)//第二个参数为ｐｃｄ文件名
  {
      pcl::io::loadPCDFile (argv[1], point_cloud);
      cout << "load pcd file : "<< argv[1]<< endl;
      cout << "point_cloud has :"<< point_cloud.points.size()<<" n points."<<endl;
  }
  else//没有指定pcd文件，生成点云，并填充它
  {
    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;
  }
  
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  // 3D点云显示
  //pcl::visualization::PCLVisualizer viewer ("3D Viewer");//可视化对象
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);//可视化对象指针
  viewer->setBackgroundColor (1, 1, 1);//背景颜色　白色
  viewer->addPointCloud(point_cloud_ptr);//指针

  // --------------------------------
  // -----Extract Harri keypoints-----
  // --------------------------------
  // 提取Harri关键点
  pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal> harris;
  harris.setInputCloud(point_cloud_ptr);//设置输入点云 指针
  cout<<"input successful"<<endl;
  harris.setNonMaxSupression(true);
  harris.setRadius(0.02f);//　块体半径
  harris.setThreshold(0.01f);//数量阈值
  cout<<"parameter set successful"<<endl;

  //新建的点云必须初始化，清零，否则指针会越界
  //注意Harris的输出点云必须是有强度(I)信息的 pcl::PointXYZI，因为评估值保存在I分量里
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_out_ptr (new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>& cloud_out = *cloud_out_ptr;
  cloud_out.height=1;
  cloud_out.width =100;
  cloud_out.resize(cloud_out.height * cloud_out.width);    
  cloud_out.clear();
  cout << "extracting... "<< endl;
  // 计算特征点
  harris.compute(cloud_out);
  // 关键点
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_harris_ptr (new pcl::PointCloud<pcl::PointXYZ>);//指针
  pcl::PointCloud<pcl::PointXYZ>& cloud_harris = *cloud_harris_ptr;//引用
  cloud_harris.height=1;
  cloud_harris.width =100;
  cloud_harris.resize(cloud_out.height * cloud_out.width);
  cloud_harris.clear();//清空
  int size = cloud_out.size();
  cout << "extraction : " << size << " n keypoints."<< endl;
  pcl::PointXYZ point;
  //可视化结果不支持XYZI格式点云，所有又要导回XYZ格式。。。。
  for (int i = 0; i<size; ++i)
  {    
        point.x = cloud_out.at(i).x;
        point.y = cloud_out.at(i).y;
        point.z = cloud_out.at(i).z;
        cloud_harris.push_back(point);
  }
  // -------------------------------------
  // -----Show keypoints in 3D viewer-----
  // -------------------------------------
  //在3D图形窗口中显示关键点
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> harris_color_handler (cloud_harris_ptr, 0, 255, 0);//第一个参数类型为　指针
  viewer->addPointCloud(cloud_harris_ptr, harris_color_handler,"harris");//第一个参数类型为　指针
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "harris");

  //--------------------
  // -----Main loop-----
  //--------------------
  while (!viewer->wasStopped ())
  {
    viewer->spinOnce ();
    pcl_sleep(0.1);
  }
}