纯点云匹配流程

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

流程概述

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

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

流程详解

预处理流程设计

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

ROI配置

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

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

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

粗匹配，精匹配流程设计

常见流程

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

常见的流程如下(下图中3D粗匹配节点可根据场景需要替换成3D粗匹配v2节点)：

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

3D粗匹配说明

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

3D粗匹配节点和3D粗匹配v2节点是两种粗匹配方法，其中3D粗匹配v2节点是一种多视角方法，其相关参数说明见 HPR视角参数说明

3D精匹配说明

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

后处理流程设计

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

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

参数调优

预处理流程参数调优

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

原理：使用采样的方式，降低点云的密度，得到较为稀疏的点云，降低计算复杂度。
- 不同的采样方式区别请参考 PCU中不同降采样方式的对比。
参数：
- 采样方法：可选参数有，标准降采样，均匀降采样，体素降采样，随机降采样，空间降采样。
- 步长：取值范围为[0，正无穷] 的整数，采样步长N，每个N个点取一个点。
- 体素/标准/空间降采样 体素大小：取值范围为[0，正无穷]，体素格子大小。
- 均匀/随机降采样 随机率：取值范围为[0， 1]，表示降采样的比率。

法向滤波

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

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

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

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

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

点云平面滤波

原理：假设点云大多数点接近一个平面，使用类RANSAC方法去寻找一个最优的拟合平面，再根据这个拟合平面，计算每一个点到平面的距离，利用一个阈值去过滤点。
参数：
- 迭代次数：取值[0，正无穷]，即RANSAC的迭代次数，次数越多精度越高。
- 损失函数阈值：取值 [0，正无穷]，该值越小过滤的越多。
- 随机方法："RANSAC"， "LMedS"， "MSAC"， "MLESAC"， "PROSAC"，实际使用中可以尝试不同的随机方法，选取效果最佳的进行使用。

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

3D粗匹配节点

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

聚类角度阈值，聚类距离阈值，投票过滤阈值，最小得票数都影响最终输出匹配位姿数量，均为调大减少，调小增多。

3D粗匹配V2节点

原理：
- HPR方法原理：
  - 如上左图所示，左侧的物体轮廓关于圆周对称出去得到一个外轮廓；如上右图所示，根据外轮廓上的两个夹角的和来判断是否可见
  - 使用HPR (Hidden Point Removal) 方法在多个离散的视点基础上(使用多个极角和方位角去尝试)，逐个去除那些不可见的点，便得到多个去除不可见点之后的模型点云，然后预先计算模型的PPF特征
  - 对场景点云，参考3D粗匹配的方法进行类似PPF特征的计算的匹配
参数：
- 模型点云文件：点云模型，带法向的点云，形状为m*n*6
- 模型尺度：对模型点云进行降采样的步长，单位为毫米，如果计算过慢或者精度不需太高，可以将该值调大
- 场景尺度：用来对场景点云进行降采样的步长，如果计算过慢或者精度不需太高，可以将该值调大
- 极角最大/小值，极角间隔值：约定了极角的搜索区间以及步长，单位为度。区间越大，步长越小，则搜索精度越高，耗时越长。
- 方位角最大/小值，方位角间隔值：方位角的搜索区间以及步长，单位为度。区间越大，步长越小，则搜索精度越高，耗时越长。
- HPR可视参数：控制HPR中的半径，越大的半径则可视的条件越宽松，会留下来更多的点。
- 最大匹配数：如果匹配数量大于最大匹配数，则保留最大匹配数个姿态
- 可视阈值：用于计算一个点数量阈值，可以过滤掉交集点数量较少的姿态【如果完美匹配，交集点，或者说是匹配上的点对，数量应该最大】，如果结果数量较少，可以把该值调小

3D精匹配节点、3D精匹配节点(无法向)

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

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

结果后处理参数调优

生成抓取点节点

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

抓取点排序节点

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

去除模型匹配点

原理：在场景中将匹配结果进行剔除，保留未被匹配的点云，这个结果会被EpicPro节点用于碰撞检测和路径规划。
参数：
- 模型点云文件：参数匹配中使用的模型点云文件，可与匹配节点用相同的映射名，进而与其模型保持一致。