三维计算机视觉（四）--关键点

关键点又称为感兴趣的点，是低层次视觉通往高层次视觉的捷径，抑或是高层次感知对低层次处理手段的妥协。

RangeImage

1.关键点，线，面

　　关键点=特征点；

　　关键线=边缘；

　　关键面=foreground；

　　上述三个概念在信息学中几乎占据了统治地位。比如1维的函数（信号），有各种手段去得到某个所谓的关键点，有极值点，拐点...二维的图像，特征点提取算法是标定算法的核心（harris),边缘提取算法更是备受瞩目（canny,LOG.....)，当然，对二维的图像也有区域所谓的前景分割算法用于提取感兴趣的区域，但那属于较高层次的视觉，本文不讨论。 由此可以推断，三维视觉应该同时具备：关键点，关键线，关键面三种算法。本质上，关键面算法就是我们之前一文中讨论的分割算法（三维点云不是实心的）。

　　ok,在这里我们了解到了，要在n维信息中提取n-1维信息是简单的，但n-2维信息会比n-1维要不稳定或者复杂的多。很容易想象，图像的边缘处理算法所得到的结果一般大同小异，但关键点提取算法的结果可以是千差万别的。主要原因是降维过大后，特征的定义很模糊，很难描述清楚对一幅图像来说，到底怎样的点才是关键点。所以，对3维点云来说，关键点的描述就更难了。点云也有1维边缘检测算法，本文不做讨论。单说说关键点提取。

2.来自点云的降维打击

　　图像的Harris角点算子将图像的关键点定义为角点。角点也就是物体边缘的交点，harris算子利用角点在两个方向的灰度协方差矩阵响应都很大，来定义角点。既然关键点在二维图像中已经被成功定义且使用了，看来在三维点云中可以沿用二维图像的定义...不过今天要讲的是另外一种思路，简单粗暴，直接把三维的点云投射成二维的图像不就好了。这种投射方法叫做range_image.

　　首先放上一张range_imge和点云图像的合照：

　　看起来像个眼睛的那玩意就是range_image. 至于它为什么像个眼睛，就要从它的出生开始说起了。三维点云有多种采集方式，最为著名的是结构光，飞秒相机，双目视觉。简而言之，采集都离不开相机。用相机拍照当然就存在相机的光心坐标原点 Oc 以及主光轴方向 Z. 从这个点，有一种办法可以将三维数据映射到2维平面上。首先，将某点到光心Oc的距离映射成深度图的灰度或颜色（灰度只有256级但颜色却可接近连续变化）。除此之外，再定义一下怎样将点云映射到图像的横纵坐标上就可以了。

　　任意一点都要和光心进行连线.....这么听起来很熟悉....好像有点像球坐标的意思。球坐标长下面这张图这样。在数学里，球坐标系（英语：Spherical coordinate system）是一种利用球坐标（r, θ，φ）表示一个点 p 在三维空间的位置的三维正交坐标系。右图显示了球坐标的几何意义：原点与点 P 之间的径向距离 r ，原点到点 P 的连线与正 z-轴之间的天顶角

θ

以及原点到点 P 的连线，在 xy-平面的投影线，与正 x-轴之间的方位角φ

。

　　

　　深度图中的横，纵坐标实际上是θ和φ，如果要保证沿着场景中某条直线移动，φ线性变化θ却先增大后减小。这也就造成了深度图像一个眼睛一样。但这并不妨碍什么，φ没有定义的地方可以使用深度无限大来代替。

　　将点云转成深度图，只需要确定一个直角坐标系，角分辨率，θ范围，φ范围即可。毕竟这只是一个直角坐标转球坐标的工作而已。

　　这样做显然是有好处的，首先，这是一种除了八叉树，kd_tree之外，能够将点云的空间关系表达出来的手段。每个点云都有了横，纵，深，三个坐标，并且这种坐标原点的设定方式，在理论上是不会存在干涉的（从原点出发的一条线理论上不会遇到多余1个点）。于是点云的空间关系就自然的被编码与深度图中。

　　显然，图像中的关键点检测算子就可以被移植到点云特征点求取中来了。

3.基于PCL的点云-深度图转换

//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
//半圆扫一圈就是整个图像了

//传感器朝向
  Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
//除了三维相机模式还可以选结构光模式
  pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
//noise level表示的是容差率，因为1°X1°的空间内很可能不止一个点，noise level = 0则表示去最近点的距离作为像素值，如果=0.05则表示在最近点及其后5cm范围内求个平均距离
  float noiseLevel=0.00;
//minRange表示深度最小值，如果=0则表示取1°X1°的空间内最远点，近的都忽略
  float minRange = 0.0f;
//bordersieze表示图像周边点  
  int borderSize = 1; //基本数据结构直接打印是ok的 std::cout << rangeImage << "\n";

NARF描述子 

关键点检测本质上来说，并不是一个独立的部分，它往往和特征描述联系在一起，再将特征描述和识别、寻物联系在一起。关键点检测可以说是通往高层次视觉的重要基础。但本章节仅在低层次视觉上讨论点云处理问题，故所有讨论都在关键点检测上点到为止。NARF 算法实际上可以分成两个部分，第一个部分是关键点提取，第二个部分是关键点信息描述，本文仅涉及第一个部分。

　　在文章开始之前，有非常重要的一点要说明，点云中任意一点，都有一定概率作为关键点。关键点也是来自原始点云中的一个元素。和图像的边缘提取或者关键点检测算法追求n次插值，最终求的亚像素坐标不同，点云的关键点只在乎找到那个点。

1. 边缘提取

　　首先声明本文所有思想算法公式均来自：Point Feature Extraction on 3D Range Scans Taking into Account Object Boundaries 

　　在正式开始关键点提取之前，有必要先进行边缘提取。原因是相对于其他点，边缘上的点更有可能是关键点。和图像的边缘不同（灰度明显变化），点云的边缘有更明确的物理意义。对点云而言，场景的边缘代表前景物体和背景物体的分界线。所以，点云的边缘又分为三种：前景边缘，背景边缘，阴影边缘。

　　rangeImage 是一个天然适合用于边缘提取的框架。在这里需要做一些假设：每个rangeImage像素中假设都只有一个点（显然在生成rangeImage的时候点云是被压缩了的，压缩了多少和rangeImage的分辨率有关，分辨率不能太小，否则rangeImage上会有"洞”，分辨率太大则丢失很多信息）。

　　三维点云的边缘有个很重要的特征，就是点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点的一个合理估计。

　　对任意一个点，进行打分，来判断该点作为边缘点有多大可能性。首先，边缘可能会在某点的：上，下，左，右四个方向。

　　所以只要把pi 和 pi 右边的点求相对距离。 并把这个相对距离和dm进行比较，就可以判断边缘是不是在该点右边。如果距离远大于dm，显然该点右边的邻点就和pi不是同一个平面了。

　　

　　为了增加对噪声的适应能力，取右边的点为右边几个点的平均数。接下来依据此信息对该点进行打分。

　　其中deta 就是dm. dright = || pi pright ||. 

　　最后再取大于0.8的Sright，并进行非极大值抑制。就可以得到物体的边缘了

　　

2. 关键点提取

 　  在提取关键点时，边缘应该作为一个重要的参考依据。但一定不是唯一的依据。对于某个物体来说关键点应该是表达了某些特征的点，而不仅仅是边缘点。所以在设计关键点提取算法时，需要考虑到以下一些因素：

　　i) it must take information about borders and the surface structure into account;

 　　边缘和曲面结构都要考虑进去

     ii) it must select positions that can be reliably detected even if the object is observed from another perspective;

       关键点要能重复

　　iii) the points must be on positions that provide stable areas for normal estimation or the descriptor calculation in general.

　　　关键点最好落在比较稳定的区域，方便提取法线　

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

　　那么对于边缘点，可以取其 权重 w 为1 ， v 为边缘方向。

　　对于其他点，取权重 w 为 1-(1-lamda)^3 ， 方向为 v 在平面 p上的投影。 平面 p 垂直于 pi 与原点连线。

　　到此位置，每个点都有了两个量，一个权重，一个方向。

　　将权重与方向带入下列式子 I 就是某点 为特征点的可能性。

　　最后进行极大值抑制，就可以得到一些特征点了。

Harris3D

除去NARF这种和特征检测联系比较紧密的方法外，一般来说特征检测都会对曲率变化比较剧烈的点更敏感。Harris算法是图像检测识别算法中非常重要的一个算法，其对物体姿态变化鲁棒性好，对旋转不敏感，可以很好的检测出物体的角点。甚至对于标定算法而言，HARRIS角点检测是使之能成功进行的基础。

1.Harris 算法　

　　其思想及数学推导大致如下：

　　1.在图像中取一个窗 w

　　2.获得在该窗下的灰度  I

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

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

　　当然啦，如果Harris算子的实现也和它的思想这么平淡那我就不表扬他聪明了，Harris算子的具体实现方法，利用的是图像偏微分方程的思想。

　　先给出抽象数学表达式：

　　其中 w 代表窗函数，某个x,y为图像坐标，u,v是一个移动向量（既反应移动方向，也反应移动大小）。

 

　　Ix表示图像沿x方向的差分，Iy表示图像沿y方向的差分。

　　

　　显然，E(u,v)可以用另外一种形式来表示了。最终可以表达为协方差矩阵的形式。

　　OK，在这里我们有了数学中最优雅的表达——Matrix，especially symmetric Matrix. Nothing is better than that.

　　

2.矩阵的方向性

　　显然，E(u,v)的值和u,v有关。。。很有关。。

　　1.可以取一组u,v，让E(u,v)的值最小。

　　2.还可以取一组u,v，让E(u,v)的值最大。

　　这些u,v怎么取，显然就和矩阵M的方向有关。

　　平面内的一个矩阵乘以一个向量v，大概简单的写成   Mv

　　它会使得这个向量发生一个作用：旋转，拉伸，平移.....总之，这种作用叫做  线性变换

　　矩阵的左边好像也是一个向量，只不过是横着写的（[u v]），换而言之，那就是 vT（v的转置）。

　　vT（Mv)......这是啥？

　　意思好像是。。。。v先旋转+拉伸一下，然后再在它自己身上投影，最终的 E(u,v)本质上来说，就是这个投影的长度。。。嗯，对，投影的长度

　　好了。我们现在明确了 E(u,v) 的数学几何意义，再回过头来想想，要怎样才能让这个投影的长度达到最大或者最小呢？

　　显然，答案就是矩阵的特征值与特征向量，当[u v]T 取特征向量方向的时候，矩阵M只有拉伸作用，而没有旋转作用，这时的投影长度是最长的（如果反向投则是负的最长）。

　　到此为止，我们已经知道了 E(u,v）的最大和最小值了（笨办法是求出特征向量方向再带进去，聪明的方法是直接看矩阵特征值，特征值就是放大倍数）。并且，分析可以知道，特征值越大，那么说明 E(u,v)越大。

　　1.两个特征值都很大==========>角点（两个响应方向）

　　2.一个特征值很大，一个很小=====>边缘（只有一个响应方向）

　　3.两个特征值都小============>平原地区（响应都很微弱）

　　基于上述特征，有很多人设计了角点的快速判据。

　　有 det(M) - trace(M)^2

　　有 det(M)/trace(M)

　　.....等等很多，但是这不重要，思想都是一样的。

3. 3DHarris　

　　在2DHarris里，我们使用了 图像梯度构成的 协方差矩阵。 图像梯度。。。嗯。。。。每个像素点都有一个梯度，在一阶信息量的情况下描述了两个相邻像素的关系。显然这个思想可以轻易的移植到点云上来。

　　OOPS，糟糕，点云木有灰度的概念啊，一般的点云也木有强度的概念啊。。。这可如何是好？？？？？？

　　别紧张，pcl 说这样能行，那就肯定能行咯，先定性的分析一下Harris3D的理念。

　　想象一下，如果在 点云中存在一点p

　　1、在p上建立一个局部坐标系：z方向是法线方向，x,y方向和z垂直。

　　2、在p上建立一个小正方体，不要太大，大概像材料力学分析应力那种就行

　　3、假设点云的密度是相同的，点云是一层蒙皮，不是实心的。

　　　　a、如果小正方体沿z方向移动，那小正方体里的点云数量应该不变

　　　　b、如果小正方体位于边缘上，则沿边缘移动，点云数量几乎不变，沿垂直边缘方向移动，点云数量改变

　　　　c、如果小正方体位于角点上，则有两个方向都会大幅改变点云数量

　　OK，我们已经有了Harris3D的基本准则，接下来要思考的是怎样优雅的解决这个问题

　　两个和z相互垂直的方向。。。。嗯。。。。perpendicular。。。。

　　如果由法向量x,y,z构成协方差矩阵，那么它应该是一个对称矩阵。而且特征向量有一个方向是法线方向，另外两个方向和法线垂直。

　　那么直接用协方差矩阵替换掉图像里的M矩阵，就得到了点云的Harris算法。

　　其中，半径r可以用来控制角点的规模

　　r小，则对应的角点越尖锐（对噪声更敏感）
　　r大，则可能在平缓的区域也检测出角点

 4.PCL对Harris算法的实现

　　根据以上分析，在PCL的API文档的帮助下，我尝试了一下 Harris3D 算法。感谢山大的毕同学提供的点云，该点云是场景点云而不是一般的物体点云。总体感觉是慢，因为针对每个点云，需要计算它的法线，算完之后又要针对每个点进行协方差矩阵的计算，总而言之，整个过程还是非常耗时的。并且说实话。。。算法的效果一般般。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;



int
main()
{
    pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
    pcl::io::loadPCDFile ("F:\\PCL\\PCD\\both.pcd", *cloud);
    boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer);
    viewer->addPointCloud(cloud,"all_cloud");
        //注意Harris的输出点云必须是有强度(I)信息的，因为评估值保存在I分量里
    pcl::PointCloud::Ptr cloud_out (new pcl::PointCloud);
    pcl::HarrisKeypoint3D harris;
    harris.setInputCloud(cloud);
    cout<<"input successful"<<endl;
    harris.setNonMaxSupression(true);
    harris.setRadius(0.04f);
    harris.setThreshold(0.02f);
    cout<<"parameter set successful"<<endl;
        //新建的点云必须初始化，清零，否则指针会越界
    cloud_out->height=1;
    cloud_out->width =100;
    cloud_out->resize(cloud_out->height*cloud->width);    
        cloud_out->clear();
    harris.compute(*cloud_out);
    int size = cloud_out->size();

    pcl::PointCloud::Ptr cloud_harris (new pcl::PointCloud);
    cloud_harris->height=1;
    cloud_harris->width =100;
    cloud_harris->resize(cloud_out->height*cloud->width);
    cloud_harris->clear();

    pcl::PointXYZ point;
        //可视化结果不支持XYZI格式点云，所有又要导回XYZ格式。。。。
    for (int i = 0;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);
    }



    pcl::visualization::PointCloudColorHandlerCustom harris_color_handler (cloud_harris, 0, 255, 0);
    viewer->addPointCloud(cloud_harris,harris_color_handler,"harris");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "harris");




    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);

    }
    
        system("pause");
}    

　　由于我选择的搜索半径比较大，所以找到的角点都不是太"角”,关于参数设置大家可以多多探索，但我认为，特征点检测算法实在太慢，对实时机器人系统来说是远远达不到要求的。这种先算法线，再算协方差的形式真心上不起。。。。实际上这种基于领域法线的特征点检测算法有点类似基于 CRF的语义识别算法，都只使用了相邻信息而忽略了全局信息。也可能相邻信息包含的相关性比较大，是通往高层次感知的唯一路径吧，谁又知道呢？

ISS&Trajkovic

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

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

1.ISS算法

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

　　方法1.基于协方差矩阵

　　协方差矩阵的思想其实很简单，实际上它是一种耦合，把两个步骤耦合在了一起

　　　　1.把pi和周围点pj的坐标相减：本质上这生成了许多从pi->pj的向量，理想情况下pi的法线应该是垂直于这些向量的

　　　　2.利用奇异值分解求这些向量的0空间，拟合出一个尽可能垂直的向量，作为法线的估计

　　协方差矩阵本质是啥？就是奇异值分解中的一个步骤。。。。奇异值分解是需要矩阵乘以自身的转置从而得到对称矩阵的。

　　当然，用协方差计算的好处是可以给不同距离的点附上不同的权重。

　　方法2.基于齐次坐标

　　　　1.把点的坐标转为齐次坐标

　　　　2.对其次坐标进行奇异值分解

　　　　3.最小奇异值对应的向量就是拟合平面的方程

　　　　4.方程的系数就是法线的方向。

　　显然，这种方法更加简单粗暴，省去了权重的概念，但是换来了运算速度，不需要反复做减法。其实本来也不需要反复做减法，做一个点之间向量的检索表就好。。。

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

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

　　ISS特征点检测的思想也甚是简单：

　　1.利用方法1建立模型

　　2.其利用特征值之间关系来形容该点的特征程度。

　　显然这种情况下的特征值是有几何意义的，特征值的大小实际上是椭球轴的长度。椭球的的形态则是对邻近点分布状态的抽象总结。试想，如果临近点沿某个方向分布致密则该方向会作为椭球的第一主方向，稀疏的方向则是第二主方向，法线方向当然是极度稀疏（只有一层），那么则作为第三主方向。

　　如果某个点恰好处于角点，则第一主特征值，第二主特征值，第三主特征值大小相差不会太大。

　　如果点云沿着某方向致密，而垂直方向系数则有可能是边界。

　　总而言之，这种局部坐标系建模分析的方法是基于特征值分析的特征点提取。

　　最后补充，Intrisic指的就是这个椭球的内部。

PCL实现

pcl::PointCloud::Ptr model (new pcl::PointCloud ());;
pcl::PointCloud::Ptr model_keypoints (new pcl::PointCloud ());
pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ());
// Fill in the model cloud
double model_resolution;
// Compute model_resolution
pcl::ISSKeypoint3D iss_detector;
iss_detector.setSearchMethod (tree);
iss_detector.setSalientRadius (6 * model_resolution);
iss_detector.setNonMaxRadius (4 * model_resolution);
iss_detector.setThreshold21 (0.975);
iss_detector.setThreshold32 (0.975);
iss_detector.setMinNeighbors (5);
iss_detector.setNumberOfThreads (4);
iss_detector.setInputCloud (model);
iss_detector.compute (*model_keypoints);

2.Trajkovic关键点检测算法

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

　　本方法的优点是快，缺点是对噪声敏感。

另外，在二维图像中的BRISK描述子，AGAST描述子，SIFT特征，SUAN特征，都 已经移植为对应的3D特征。这里不再啰嗦。