从零实现无序抓取（三）不同特征描述子的3D位姿估计对比

前言

3D点云的姿态估计有时可以看作是点云配准的过程，其实就是计算场景点云Ps和模版点云Pm之间的位姿变换关系。这种变换一般都是刚性变换，包含平移和旋转。目前最常用的是采用两阶段的算法来进行位姿的精确估计。第一阶段，采用SAI-IA算法进行位姿粗估计，第二阶段，采用ICP迭代优化位姿，进行精确的位姿估计。

一、点云特征描述子

常见的特征描述子：FPFH,SHOT,3DSC具体实现就不写了，后期有时间的话慢慢补上。
这里详细讲一下特征描述子的作用。
首先举个例子，现在你家里人叫你去相亲，但是他们没有女方A的照片，为了防止你认错人，见过她的人给你描述了一番她的体型和长相：黑熊般一身粗肉，铁牛似遍体顽皮（体型），交加一字赤黄眉，双眼赤丝乱系，怒发浑如铁刷，狰狞好似狻猊（长相）。记下这些描述之后，你屁颠屁颠地跑去相亲，到了地方，发现有很多人，所以你把所有人的体型和长相都描述了一番，然后发现有个人的描述和见过A的人的描述很相似，所以你就可以认为她就是女方A了。
我们把这个过程类比到点云特征描述中，现在计算机对模版点云Pm中某一点A按照一定的规则（体型，长相）进行了特征描述（一般是一个一维的向量），然后采用同样的方法对场景点云Ps中的所有点进行了描述，这样给每个点都生成了一个特征描述向量（与A的描述向量长度相同）。接下来就是找哪个点和A的特征描述向量最接近了，很自然地想到，就是计算两个向量之间的差距呗，如何计算呢？最简单的就是求两个向量之间的欧氏距离，也就是两个向量相减然后求平方和。所有的距离中，对应最小距离的那个点或者距离小于一定阈值的点就是场景点云中对应点A的点了。
说到这，应该理解点云特征描述的作用了。但是你还可能会产生如下的问题：
1.如果所有点的距离都很大怎么办？
答：那就说明不存在对应的点
2.如果有很多点距离都很小怎么办？
答：那就说明你的描述不够详细，无法通过给出的这些条件筛选出正确的点。相当于你去了相亲的地方，发现很多人都满足“黑熊般一身粗肉，铁牛似遍体顽皮，交加一字赤黄眉，双眼赤丝乱系，怒发浑如铁刷，狰狞好似狻猊。”那应该咋办呢？很简单，打电话给见过A的人，叫她再对A从别的角度描述几句，直到你可以认出A。
3.为什么要非要描述，不能直接给照片吗？
答：因为在三维的点云空间中，这些点只具备空间位置属性，很难直接配对，生成特征描述相当于将他们提升到了高维空间，在高维空间中他们会和很接近，更加容易配对。

二、SAI-IA粗配准

SAI-IA（采样一致性初始配准算法）的核心思想：
(1) 从待配准点云P中选取n个采样点，为了尽量保证所采样的点具有不同的FPFH特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d。
(2) 在目标点云Q中查找与点云P中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在目标点云Q中的一一对应点。
(3) 计算对应点之间刚体变换矩阵, 然后通过求解对应点变换后的“距离误差和”函数来判断当前配准变换的性能。此处的距离误差和函数多使用Huber罚函数表示。te为设定的阈值。

三、ICP精配准

迭代最近点算法(Iterative Cloest Point, ICP)
ICP算法基于SVD，其大致思路如下:
(1) 将初始配准后的两片点云P′(经过坐标变换后的源点云)和Q，作为精配准的初始点集；
(2) 对源点云P’中的每一点pi，在目标点云Q中寻找距离最近的对应点qi，作为该点在目标点云中的对应点，组成初始对应点对；
(3) 初始对应点集中的对应关系并不都是正确的，错误的对应关系会影响最终的配准结果，采用方向向量阈值剔除错误的对应点对；
(4) 计算旋转矩阵R和平移向量T，使最小，即对应点集之间的均方误差最小；
(5) 设定某一阈值ε=dk-dk-1和最大迭代次数Nmax, 将上一步得到的刚体变换作用于源点云P′，得到新点云P”，计算P”和Q的距离误差,，如果两次迭代的误差小于阈值ε或者当前迭代次数大于Nmax，则迭代结束，否则将初始配准的点集更新为P”和Q，继续重复上述步骤，直至满足收敛条件
在两点云相差较大的情况下，ICP算法容易陷入局部最优解，从而无法得到较好的匹配结果，故需要先给定一个初始变换矩阵，一般由SAI-IA确定。

四、实验结果

为了定量地分析算法的效果，使用PCL对点云进行一个给定的变换。将点云沿着XYZ轴分别平移10mm、20mm和30mm，并绕Z轴旋转90°。这个变换对应的变换矩阵为：

4.1 FPFH特征描述子

绿色为原始点云，红色为进行位姿变化之后的点云，蓝色为采用位姿估计矩阵对原始点云进行变化的结果，可见效果还是很好滴。
变换矩阵结果：

误差：

运行时间：

4.2 SHOT特征描述子

变换矩阵结果：

误差：

运行时间：

4.3 3Dsc特征描述子

变换矩阵结果：

误差：

运行时间：

4.4 结果分析

从上述计算结果可以看出：
耗时：FPFH<SHOT<3Dsc
误差：FPFH<3Dsc<SHOT
代码后续大家点个赞有需要再附上吧。