3D无序抓取视觉算法

点云配准

方法

包括粗配准和精配准两个阶段。

粗配准

指在点云相对位姿完全未知的情况下对点云进行配准，找到一个可以让两块点云相对近似的旋转平移变换矩阵，进而将待配准点云数据转换到统一的坐标系内，可以为精配准提供良好的初始值。
基于特征匹配的配准算法：通过被测物体本身所具备的形态特性构建点云间的匹配对应，然后采用相关算法对变换关系进行估计。点对特征法（Point Pair Feature）算法精髓：整体建模，局部匹配。
1、Model Globally
定义scene是测得的真实场景（点云），model是物体的真实模型（点云），定义S为真实场景中点云的点，M为模型点云。
1）PPF：Model Globally本质通过定义点对特征（Point Pair Feature）来构建矢量的集合及每个矢量特征对应的点对集作为Global Model Desciption。PPF描述了两个有向点的相对位置和姿态。
2）Global Model Description
第一步：计算model表面所有point pairs的特征矢量F
第二步：构建哈希表，将具有相同F（特征向量feature vector）的point pair放在一起，即hash表的键key为feature vector F, 值value为具有相同特征矢量的点对集A.
2、Match Locally
1）Local Coordinates
i）从scene中选取任意一个参考点sr∈S，假设它在物体的表面上，若假设正确，则在model存在一个点mr∈M与sr对应；
ii）将这两个参考点配准，需同时将点的位置和法向量对齐；
iii）让model绕sr的法向轴转动一定角度与scene配准。
2）voting scheme
通过一种方法找到最优的local coordinates使得scene中落在model表面的点最多，即可求出物体pose。
3）Efficient Voting Loop
4) Pose Clustering

精匹配

指在粗配准的基础上，让点云之间的空间位置差异最小化，得到一个更加精准的旋转平移变换矩阵。该算法的运行速度以及向全局最优化的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。
1、GMM：高斯混合聚类（GMM）是一种聚类算法，可以用来对数据进行分类。GMM算法假设数据点是由一个或多个高斯分布生成的，并通过最大似然估计的方法来估计每个簇的高斯分布的参数。
原理
在GMM中，假设数据是由若干个高斯分布组成的。高斯分布的概率密度函数为：

GMM的目标是最大化数据的似然函数：

使用期望最大化（EM）算法来求解GMM的参数。EM算法流程如下：
1）初始化模型的参数
2）计算所有数据点的响应度
3）更新每个高斯分布的参数
4）计算模型的似然函数
5）判断模型的参数是否已经收敛。如果收敛则结束迭代，否则返回第2步
使用下面的公式来计算每个数据点的响应度：

使用下面的公式来更新每个高斯分布的参数：

通过不断迭代，就可以使用GMM聚类算法来对数据进行聚类。
数据处理过程
1）对数据进行预处理
2）需要建立高斯模型
3）确定每个高斯分布的参数
4）确定每个数据点属于哪个高斯分布
5）使用期望最大化(EM)算法来求解GMM的参数
6）计算所有数据点的响应度
7）更新每个高斯分布的参数
8）计算模型的似然函数
9）不断迭代更新模型的参数，直到模型的参数收敛为止。
2、ICP迭代最近点算法：包括对应点搜索和位姿求解，求解的结果是两点集之间的平移及旋转量。基本原理：搜索最近点、求解变换关系（R、T）、应用变换、重复迭代。