纯点云匹配流程

本章主要介绍纯点云匹配流程的算法图搭建思路。

流程概述

该流程可以被简单表述为以下步骤：

输入点云信息。
对点云信息进行预处理，进行过滤、筛选、滤波、法向量生成等操作。
粗匹配 + 精匹配或者多视角粗匹配 + 精匹配，基于一个目标的点云模型在输入点云中进行匹配，得到匹配的点云结果。
进行后处理，基于匹配点云生成位姿，抓取点，及对抓取点进行排序和输出。

流程详解

预处理流程设计

预处理的目的是，优化输入的数据，使得后续操作更加快速，精确。

ROI配置

在纯点云识别流程中，较为常见的是ROI的选取，可以使用配置3维ROI节点进行手动设置，也可以通过Epic Pro输入节点进行设置。

由于点云的法向估计等操作受点云数量和0值及nan值数量的影响，在点云处理前的ROI使用点云ROI或点云ROI(带法向)节点进行处理，去掉一些无用点。
点云处理，包括降采样，统计滤波，法向估计，分割等

点云降采样主要是为了减少点云数量，为后续处理加速，所以一般会将此节点放在ROI节点之后，其他点云操作之前。粗匹配和精匹配节点输入均为带法向的点云，需要有一个法向估计节点，但若是调试阶段，也可以用读取点云(带法向)节点直接读取带法向的点云。

粗匹配，精匹配流程设计

常见流程

一般的使用是1个粗匹配节点+1个及以上3D精匹配节点，前者通过匹配算法得到一些粗略的位姿，再通过一个或多个精匹配微调结果位姿，得到更精确的结果。

常见的流程如下：

一般可以直接对点云进行降采样，再进行法向估计，将法向估计的结果输入到粗匹配和精匹配节点得到匹配结果。
在一些场景下，可以在粗匹配阶段输入未经过降采样的点云，在精匹配阶段输入降采样过后的点云，以提高匹配精度和速度，达到更好的效果。
针对表面平整、目标之间有明显缝隙、边缘明显的目标场景，可以对模型点云和场景点云都进行法向计算和边缘计算再进行粗匹配和精匹配，如下图：

3D粗匹配说明

3D粗匹配是本流程的核心节点，连接法向估计节点的输出点云，然后在参数设置中选择一个带法向的模型点云文件，调整参数即可激活并查看匹配输出。3维模板匹配的输出结果较多，以能覆盖全部待匹配物体为宜，但给出的位姿不够精准，因此需要后续进一步配准。

3D精匹配说明

3D精配准节点可输入降采样节点之后的场景点云，即利用稀疏场景和稀疏模型做配准。

后处理流程设计

后处理流程中主要包括的是抓取物位姿变换节点、生成抓取点节点、抓取点排序节点和去除模型匹配点节点，这个流程往往是固定的。

抓取物位姿变换节点主要是用手眼标定矩阵将抓取点结果从相机坐标系转换到机器人坐标系用于控制机器人抓取。
生成抓取点节点是用于抓取点生成，通过输入一个抓取点json文件来生成不同位姿的抓取点，这个文件一般会通过绑定参数组，从Epic Pro映射得到。
抓取点排序节点是通过不同的抓取策略对生成的抓取点进行排序。
去除模型匹配点节点是在场景中将匹配结果进行剔除，保留未被匹配的点云，这个结果会被Epic Pro节点用于碰撞检测和路径规划。

参数调优

预处理流程参数调优

点云降采样 / 点云降采样(带法向)

原理：使用采样的方式，降低点云的密度，得到较为稀疏的点云，降低计算复杂度。
- 不同的采样方式区别请参考 PCU中不同降采样方式的对比。
参数：
- 采样方法：均匀降采样，体素降采样。
- 均匀/体素降采样 体素大小：体素格子大小。

法向滤波

原理：根据一个参考法向量，对每一个点的法向量计算出和参考法向量的夹角，利用阈值进行过滤。
参数：
- 角度阈值：取值范围为 [0，180]，单位是度，该值越小过滤的越多。
- 参考法向：三维实值向量，根据实际使用进行确定。

半径滤波 / 半径滤波(带法向)

原理：通过指定半径，滤除半径内的点数少于设定的最小邻居个数的点。
参数：
- 最小邻居点个数：取值范围为 [1，正无穷]，取值越大越严格。
- 搜索半径：取值范围为 [0，正无穷]，取值越小越严格。

统计滤波 / 统计滤波(带法向)

原理：
- 对每个点到其邻居的平均距离构建高斯分布，滤波标准差大于 std*stdMultiplier 的点。
参数：
- 标准差放大系数：即stdMultiplier，用于调整滤波的严格程度。
- 邻近点数：用于计算距离分布的邻居点个数。

粗匹配，精匹配流程参数调优

3D粗匹配节点

原理：
- 利用模型点云，计算出一些表面的特征，例如PPF，构建哈希表。
- 会先对场景的点云进行降采样，得到较为稀疏的点云。
- 遍历场景中的点，计算出特征，使用哈希表匹配先前计算出来的模型的特征，随后使用投票计算出最佳的匹配位姿，重复迭代多次。
参数：
- 模型点云文件：点云模型，可生成，也可从现场点云中截取。
- 距离离散数量：将Model的diameter离散化的数量，可根据此参数计算得到：distanceDiscreteStep = diameter / distanceDiscreteNum，推荐数值， 15、20、25、30。此参数可以衡量体素降采样程度，越大则体素降采样格子越小。
- 角度离散数量：角度离散的数量，可根据此参数计算得到: angleDiscreteStep = 2 * PI / distanceDiscreteNum 。
- 参考点步长：参考点步长是均匀降采样，即每隔多少个点取一个点参与计算。
- 输出位姿数量上限：当输入为多个点云时，此上限指的是每个点云的匹配结果上限。
- 聚类角度阈值：变换矩阵进行聚类时，判断两个矩阵是否属于同一类的角度阈值，单位为角度值0~180度; 值越大，聚类的个数越少，结果可能越不精确。
- 聚类距离阈值：变换矩阵进行聚类时，判断两个矩阵是否属于同一类的距离阈值，单位为输入点云的单位，一般为mm; 值越大，聚类的个数越少，结果可能越不精确。
- 投票过滤阈值：聚类时相对于最大投票数的投票滤除阈值，小于(最大投票数*阈值)的姿态会被滤除，值越大，匹配出来的结果可能会越少，推荐参数 0.05，当cluster 时间过长时，可适当调大这个参数，比如0.10、0.15等。
- 重叠阈值：判断两个结果是否重合，如果其距离小于该参数值乘以模型直径，则认为是一个结果(后者被去掉)。
- 体素相对距离离散步长的比例：体素大小相对于距离离散值的比例，此值会影响模型与场景，模型与模型重合度的判断，该值越大，容忍的偏差越大，但错误率上升。

聚类角度阈值，聚类距离阈值，投票过滤阈值均为调大减少，调小增多。

3D精匹配节点

输入降采样后的点云时，单次ICP计算速度较快，所以可以把ICP迭代次数调高一些，体素降采样尺寸建议和前面降采样节点内设置的相同或低一些。ICP搜索半径影响匹配质量，建议在密集场景时调低，在稀疏场景时调高。两个提前终止条件影响速度，调的越低速度越慢。置信阈值和ICP得分有效距离影响最终位姿数量，调高减少，调低增加。

原理：
- 基于一些已有的结果，比如粗匹配后的多个位姿，分别使用ICP算法去优化，得到更加精细的位姿
- 会利用阈值对最后的结果进行过滤，因为有一些位姿优化过后还是误差较大
参数：
- 模型点云文件：点云模型，可生成，也可从现场点云中截取，一般跟粗匹配模型保持一致
- 体素降采样尺寸(毫米)：降采样体素边长
- 置信阈值：ICP得分，低于该值的位姿会被过滤掉。得分的计算规则是: ICP迭代终止时有效响应点数量除以降采样模型点数。
- ICP搜索半径： ICP的模型点相对搜索距离, 即超过该参数的场景点将被忽略,若过小将导致偏差大的初始位姿找不到正确的匹配方向,若过大将引入周边场景的干扰；推荐数值：一般取0.1倍模型直径到1倍模型直径。
- ICP得分有效距离：ICP将某个点归为有效响应点的最大距离。
- 开启去重：在ICP结束后对结果进行基于响应点的去重。
- ICP提前终止距离条件：ICP的提前终止迭代阈值，当变换矩阵增量中平移距离该值，且同时满足下面的旋转部分阈值条件时，结束迭代
- ICP提前终止旋转条件：ICP的提前终止迭代阈值，当变换矩阵增量中旋转角度小于该值，且同时满足上面的平移部分阈值条件时，结束迭代
- ICP最大迭代次数：ICP最大迭代步数，推荐数值：20、50、100
- 利用聚类位姿：利用聚类位姿匹配。
- 移动坐标系原点：移动坐标系原点到点云包围盒中心。
- 打印过程信息：是否打印过程信息，开启后会在日志中打印每个结果匹配的中间数据。
- 开启位姿选择：开启选择位姿, 调试用。

结果后处理参数调优

生成抓取点节点

原理：输入一系列抓取物姿态信息来对每个结果生成对应的抓取点，输入可来源于匹配结果，在此流程中基本是指精匹配后的结果。
参数：
- 抓取点文件：一般是从Epic Pro抓取配置设置抓取点后通过绑定参数组映射得到的。

抓取点排序节点

参考纯图像识别流程参数调优。

去除模型匹配点云

原理：在场景中将匹配结果进行剔除，保留未被匹配的点云，这个结果会被EpicPro节点用于碰撞检测和路径规划。
参数：
- 模型点云文件：参数匹配中使用的模型点云文件，可与匹配节点用相同的映射名，进而与其模型保持一致。
- 启用降采样：是否使用降采样。
- 体素降采样尺寸：降采样体素边长, 单位为毫米。
- 反向去除：是否反向去除。
- 利用聚类位姿：利用聚类位姿匹配。
- 合并输入点云：当输入为多块点云时，将多块点云合并后进行计算，一般而言，合并后计算会更快。
- 去除方法：首先将模型点云通过匹配结果位姿转换到场景点云坐标系下，然后再查找对应的重叠点；哈希值映射是将场景点云和模型都进行哈希值映射，相同的坐标点会映射为相同的哈希值，根据哈希值去除重叠点；KDTree搜索是利用KDTree在场景点云中搜索与模型点云重叠的点；基于匹配响应点是计算匹配时模型在场景中的响应点数，相应点数越多，则重叠越大，将对应场景点去掉。
- 缩放因子：缩放因子，将坐标点控制在一个较小的范围。