托盘点云分割与配准

一、 模板的预处理

降采样：模板的降采样率可以很高甚至不降采样，主要影响计算速度，精度上并没有太大影响。我这里是没有进行降采样的。
中值滤波：按理说这一步应该和提取场景中的托盘时的滤波保持一致，应该同时对Z方向和X方向都做中值滤波，得到比较平整的托盘面，但是场景中的托盘上下都会有箱体或者地面货架之类的物品，所以Z方向上滤波对托盘面不会有影响，但是单独提取出来的模板直接滤波会导致z方向上有个变形，有时候是会导致配准出问题的，所以模板和识别场景的滤波步骤不完全相同。



二、 场景中的托盘提取

降采样：对场景点云处理之前降采样还是比较必要的，点云数量比较大，不采样会导致计算非常缓慢，降采样率0.005。
滤波：这里主要是为了在平整托盘平面的同时也平整地面，否则地面噪点非常多的情况下厚度就会响应变大，导致删水平面的过程删除了过多有用的托盘面。X和Z方向的滤波强度都取0.1。
消除水平面：这一步拟合平面，将法线方向在[0,0,1]附近的平面删去，允许厚度误差在0.03，角度误差在5°。水平面消得好的话会让点云全都变成竖直而且分开的平面，这主要是为后面簇分割提供便利。

簇分割：把点云之间距离过大的和整个点云簇数量太少的都直接删除，如果上一步红圈里干扰比较小的情况下直接就会剔除干扰，但是干扰比较大的话就需要后面再说了。这一步点云数量小于1000的和点间距大于0.012的都除去。会得到如下图的主要部分：

这时候基本上是可以确定x的大概范围的，因为只剩下大片感兴趣的点云了，都集中在同一片x坐标的区域里。
托盘孔的识别：首先要把剩余的这部分点云投影到二维平面上去做处理，这里由于目前采用的方法都是基于比例的判别，因此具体是投影到哪一个平面在托盘偏转10°到15°以内的范围内没有什么影响，所以这里处理的时候是直接沿x方向投影到一个垂直于XOY的平面上的。后面如果要用到托盘孔洞的距离参数的时候会受到角度偏转影响，代码里也留出来了这一部分，求xy坐标的2x2协方差矩阵，求特征值和特征向量，得到的旋转矩阵可以让点云投影到一个面积最大的平面上，这样距离也会比较好的限制住。
投影后的平面：

这一步有一个比较重要的参数就是投影后的像素大小，这个像素块是宁可尽量大也不能太小的，是因为大了也不会影响孔洞的识别，太小反而会让托盘正面的一些缺失点云对计算造成影响，起到一个膨胀的作用，这里是把整副图投影到80x80的二维图中。
接着按行从左右两边寻找图像的非空边缘：

红色的部分就是删去的不感兴趣的部分，剩下黑色区域里就是所有有可能成为托盘孔的孔。下一步对有孔的部分进行上升下降沿的检测，检测每一个上升沿后三个下降沿之间的占空比，把阈值之外的孔移除，在此例中会找到两个接近托盘孔的位置，用蓝色框出：
接下来就比较这两个占空比哪个更接近标准比例，选一个最接近的就可以了。这里要设置的就是标准比例的具体参数，例如当nomalRatio设置为1.42时，会把右边蓝框里的部分视为托盘：

而当nomalRatio设置为1时，会把左边蓝框里的部分视为托盘。

识别出洞的位置之后z方向的位置基本就可以确定，这里展示一些情形下z坐标识别的精度：
地面上：


货架上：



同理，y坐标的确认类似z坐标，但多一个步骤就是要判断同一高度下哪一部分更接近于托盘的形状。确定了x，y，z的坐标后就可以选出场景中的托盘，进行配准。

提取出的场景中托盘

三、配准过程
NDT的粗配准，这一步重要参数为gridStep的调节，目前设置为0.01，也是尝试出的最鲁棒的结果。

ICP精配准，这个算法非常容易进入局部最优，所以一定是在保证前面分割的比较好的前提下这一步精度才会比较高，否则ICP常常会放飞自我。

四、鲁棒性测试