一种基于实例分割和点云配准的六维位姿估计方法

侯大伟

(中国科学技术太学 信息科学技术学院，安徽 合肥230026)

0 引言

点云配准是三维重建、工业测量、机器人抓取等方面的一种常见方法，目标是将不同视角下点云拼接成一块完整的点云数据。随着深度相机的广泛应用，研究人员可以便捷地获取三维空间的点云数据，用以估计物体的六维位姿。 图 1(a)和图 1(b)是在不同视角下的存在交集的两片点云，不断地调整交集部分的点云直到基本重叠，最终两片点云拼接为一个整体(图 1(c))。

以上两片点云配准的过程，本质上得到的是两片点云之间相对位姿的变换矩阵。假设其中一片点云参考目标，即相对世界坐标系的位姿参数均已知，便可推理得到另一片点云的位姿，从而实现目标点云的位姿估计。

图1

目前基于RGB-D 图位姿估计的方法一般先从RGB 图片中获取物体初始姿态，再依据 RGB-D 图用ICP 算法[1]迭代优化得到最终姿态，如HINTERSTOISSER S[2-4]提出的 LINEMOD 算法，它针对物体的3D 模型，从各个不同的视角来提取 RGB 图像和 RGB-D 图像来生成模板，再与实际的图像去匹配，得到初始姿态估计后再用ICP 算法进行优化。CHAI Z Q 等[5]提出利用全卷积网络(Fully Convolutional Networ-ks，FCN)分 割 RGB 图 像 ，与 LINEMOD算法相结合的新方法，实现对图像中目标的快速检测和定位。 陈启军等[6]将 RGB-D 图转换成 HHA 特征图和场景点云后进行物体的实例分割，并依据4PCS 算法和ICP 算法，将分割出的目标点云和目标CAD 模型的模型点云进行匹配和位姿精修，从而得到精确的位姿估计结果。 CHI L 等[7]通过端到端深度学习模型同时分析RGB 图片和 RGB-D 图片，获取初始的姿态后输入3D 信息来优化出最终的六维位姿。 HODAN T 等[8]使用预过滤器来处理输入图像，忽略置信度低的区域，并对其他位置采用匹配特征点的方法生成模板并验证。 但是该方法只能获得近似的对象姿态，还需要随机优化过程来估计准确的六维姿态。

本文采用的是先分割RGB-D 图后进行点云配准来估计六维姿态的方法。 整体的流程如图2 所示：首先使用深度相机拍摄目标所在的三维空间，获取整体环境的RGB-D 图，其次利用Mask R-CNN模型将环境RGB-D 图进行实例分割出目标，转化为点云图，再用Super4PCS 算法与参考目标进行点云配准，实现目标的六维姿态估计。

图2 实验流程图

1 实例分割与点云配准

1.1 Mask R-CNN 模型

Mask R-CNN[9]是由 Fast R-CNN[10]、Faster R-CNN[11]逐渐改进而来的多任务卷积网络模型，可以完成目标检测、目标分类、像素级别的目标分割等多种任务。 在此之前，Faster R-CNN 网络已实现一张图片上物体的目标分类和目标检测功能；全卷积神经网络(Fully Convolutional Networks，FCN)可以得到一张图片上物体的轮廓，并实现语义分割。 因此结合二者的功能，构建了Mask R-CNN 卷积神经网络模型，如图3 所示。 结构主要包括骨干部分的深度残差网络(Deep Residual Network，Res-Net)和特征图金字塔网 络(Feature Pyramid Networ-ks，FPN)，用 于特征提取；头部部分的功能是目标分类、边框回归和Mask 预测。

图 3 Mask R-CNN 结构图

Mask R-CNN 模型主要有三点重要改进：第一，使用 ResNet 和 FPN 来实现图像特征提取；第二，网络输出层添加二进制Mask 语义识别，对输入网络的任意尺寸的图片，均可得到目标的边框位置、分类结果和像素级的二进制Mask 预测，具体做法是在之前 Faster R-CNN 网络中的边框(Bounding Box，BB)识别分支旁边并行添加了一个用于预测目标的Mask 分支，它是作用于每个感兴趣区域(Region Of Interest，ROI)上的一个小型 FCN，用于预测 ROI 每个像素的类别，最终实现准确的实例分割；第三，改进了 Faster R-CNN 网络 ROI Pooling 层在量化时会有像素对不准的问题，Mask R-CNN 网络用ROI Align 层替换的 ROl Pooling 层，先把 feature map 线性插值到 14×14，再 pooling 到 7×7，消除了最终结果Mask 和实际位置之间的微小偏移。 Mask R-CNN模型功能强太，但速度和准确率并没有降低，同时可以灵活地扩展，适用于本文的实例分割。

1.2 Super4PCS 配准算法

点云配准通过将具有重叠的两两点云迭代调整位姿直到一致，最终实现整体拼接。 假设点云P有 m 个点，点云 Q 有 n 个点，可以通过分别各选 3个点来确定二者对应关系。 最初穷举搜索空间的算法复杂度为 O(m3n3)；RANSAC 算法[12]通过多次迭代选择近似最优解，复杂度降为O(m3logn)。 而AIGER D等[13]提出的 4PCS 算法复杂度降低为 O(n2)，是目前速度和准确度均不错的粗配准算法之一。

4PCS 算法的目标是寻找参考点云P 和目标点云Q 之间的一个最优刚性变换，使得两片点云中存在符合最小允许范围内的点最多，具体分三个步骤：

(1)在参考点云 P 中选择点基 C={a，b，c，d}。 先用 RANSAC 算法选择 3 个点，再挑能够与这 3 个点组成的共面的第4 点，该点必须在两片点云的重叠部分且不能距离其他3 点太近，共同组成点基C。

(2)在点云 Q 寻找任意点基 Di={a′，b′，c′，d′}，如图4(b)所示，并且要求满足利用仿射不变性质：

目标点云Q 中所有成立的Di行成的点基组合D，并剔除角度偏差不在阈值允许范围内的一些冗余点基，如图 4(b)中的{a′，b′，c″，d″}等。

图4

(3)最后确定最佳的刚性变换矩阵 T。 计算上述获得的点基C 与点基 D 中每个点基之间的刚体变换矩阵 Ti；把参考点云 P 做刚体变换 Ti得到 TiP 后，使得TiP 与Q 之间所有点的欧式距离平方和最小Ti，即为最佳刚体变换矩阵 T。

4PCS 算法的时间复杂度为二次，并且根据仿射不变性挑选出的点基D，需要穷尽计算每个点基Di中的角度来与点基C 比对，剔除冗余基代价过于昂贵。

Super4PCS[14]针对上述量点不足进行改进：

(1)在点云 Q 找到对应的对构建球壳来提取点对。 以任意为球心，以图 4(a)中 ab 距离为半径构建球壳，半径的误差限为ε，球壳上的点形成一个集合 S1。 同理以 cd 距离为半径球壳上的点形成集合S2(图5(a))，球壳中的每个单元存储一系列向量方向, 通过二剖分球壳来阵列存储所有落在球壳上的 点(图 5(b))。

(2)同4PCS 算法，如图 5(c)利用仿射不变性在集合S1和S2中找在误差允许范围内相等交点e1ij=e2ij并 记 录 对 应 点 对{as1，bs1，cs2，ds2}，其 中 {e1ij，as1，bs1}⊆S1，{e2ij，cs2，ds2}⊆S2，并判断两个相交线对之间的角度 θ是否相等，最终找到同时满足放射比例和交角θ 的点基础 Di={a′，b′，c′，d′}。

上述得到近似全等的点基础对和变换矩阵后，也还需要迭代寻找最优变换矩阵，Super4PCS 算法将复杂度降为O(n)，可以高效配准更高维度的点云数量，较快速实现目标的六维姿态估计。

2 实验验证

为了验证该方法的有效性，本文通过真实采集的数据进行评估，实验均在 CPU 为 Intel®CoreTMi5-10600KF 处 理 器 、16 GB 内 存 、GeForce RTX 3070的64 位Windows 10 系统中运行，采集数据使用Intel RealSense D435 深度相机，满足实验需求。

图5

2.1 制作数据集

采集300 张发动机连杆的图片作为实验对象，对图中的每个对象进行标注，保存对应生成的JSON文件。由于采集的数据集较小，进行数据集的增广，可使训练的模型有较强的泛化能力。 本实验数据集的增广主要是翻转、加噪、模糊化、增减曝光四种方式；相应地也要把之前标注的JSON 文件进行增广，增广后的数据集共有2 000 张图片。 本实验选择一部分(如图 6 所示)作为 Mask R-CNN 模型分割的实验对象，其余的作为数据集用于模型的训练。

图 6 原始 RGB 图

2.2 Mask R-CNN 实例分割

本文的利用Mask R-CNN 模型获取进行RGB-D图像分割的流程主要包含以下几个步骤：

(1)采集 RGB-D 数据集作训练数据源，并对训练样本进行预处理，采用LabelMe 标注工具进行数据标注和Mask 制作；

(2)将其输入到一个预训练网络 ResNet101 中，得到训练样本的特征图，对该图中每一点设定ROI(Region Of Interest)，从而获得多个候选 ROI；

(3)将这些候选的 ROI 送入二值分类(前景或背景)和 BB(Bounding Box)回归，将候选的 ROI 送入 RPN进行二分类(输出为前景或背景)和 BB 回归，过滤掉一部分候选的 ROI，对剩下 ROI 进行 ROI Align操作；

(4)对 这 些 ROI 进 行 分 类 (N 类 别 分 类)、BB 回 归和 MASK 生成；

(5)重复步骤(4)，训练完所有样本，得到最终优化调整的分割模型。

Mask R-CNN 模型的输入一般为 RBD 图像，而RGB-D 图像多一维深度信息，Mask R-CNN 模型网络结构并不需要改变，只需要在开源代码层面上增加一个通道来并行处理深度数据。 首先修改子类输入的配置文件Config.py，图像通道数IMAGE_CHANNEL_C-OUNT 为 4，平均像素值函数 MEAN_PIXEL 与 RGB值并行添加深度信息值；其次修改模型的输入通道为4，并增添第一层网络的深度信息的随机权重参数，其他层的输入参数和预训练权重不变；最后由于上一步添加的参数是随机的，因此需要加入训练层获得较好初始值。 通过修改 Mask R-CNN 模型的源码直接对 RGB-D 图像实例分割。 增广后的数据集共有约 2 000 张图片，训练 30 个 epoch，每个 epoch时间约 25 min。 图7 是有效的目标候选区域RGB 图。

而本次实验采用了单一参考目标，因此只需要分割出来单一的目标的RGB-D 图像，作为分割后的目标与参考目标进行后续位姿配准。 从原图中实例分割出来一个完整有效的目标的RGB-D 图(如图 8 所 示 )。

图 8 目标 RGB-D 图

2.3 点云配准

图9(a)所示为已知位姿参考点云，由于实验模型比较理想，可以对点云的数量处理为12 500，同时增加表面处理便于直观显示(如图 9(b)所示)。用参考点云与分割后的目标点云进行配准，目标是实现基本重合，获得两者之间的变换矩阵，进而求得分割目标的六维位姿。

图9

上述 Mask R-CNN 模型对 RGB-D 图分割出目标的RGB-D 图之后，需要转换成点云图，便于与参考点云配准工作。 RGB-D 图中有 D435 深度相机到目标的距离值， 要转换成目标表面的点云图，核心是相机的坐标系与世界坐标系的关系转化。本次实验的设定的相机内参为camera_factor=1 000、camera_cx=346.502、camera_cy=274.623、camera_fx=648.791、camera_fy=649.105，不断遍历 RGB-D 图并计算每个点的空间坐标后添加到点云中，并把离群的小片点云做去噪处理，便获得了分割后目标的点云图。

同样为了直观显示，增加表面处理，点云数量的规模分为 23 501、38 934、47 558、48 215。 红色目标为图9(b)参考点云，绿色部分为图8 中分割后目标RGB-D 图转化的目标点云，配准的结果如图10所示，并记录变换矩阵和配准时间。

图10 分割目标点云与参考点云配准结果

本实验继续进行参考点云与未分割整体目标环境点云的配准实验(图 11)，同样采用 Super4PCS 算法。 在多次实验后并限定区域处理后，分别挑选出与分割后配准同一目标的配准结果，并作配准的时间对比。

图11 原图点云图与参考点云配准结果

以上的配准结果包括变换矩阵和配准时间，变换矩阵可以实现目标六维位姿估计，配准时间用于做效率分析。

3 结论

如表1 所示，实验结果记录了分割前与分割后配准的点云时间对比。 由于分割后均为单一目标，且点云数量均处理到12 500，4 个分割目标点云配准时间 分别为 0.42 s、0.39 s、0.41 s、0.42 s，取均值0.41 s。

从表1 中可以看出对目标进行分割后配准时间显著降低了60%~80% ， 因此先对目标采用Mask-R CNN 网络实例分割目标， 再用 Super4PCS点云配准算法的方式来估计目标的六维位姿，该方法是高效可行的。

表1 分割后与原图时间对比

由于实验环境较为理想，仅用Super4PCS 粗配准就已经实现了基本重叠，后续也需要在更复杂场景下做多次验证实验。 基于上述分析，后续实验可行优化方向：第一，考虑目标分割的时间代价，并考虑需要获取位姿的目标数量达到如何规模时可采用分割后配准的方式；第二，增加环境的复杂性和目标的多样性，通过Super4PCS 算法粗配准后再用ICP算法精配准，进一步分析配准效率。