基于SfM单目六自由度位姿估计数据集自动标注

刘 毅，魏东辰，李子豪，严小军

(1.北京航天控制仪器研究所 工艺技术研究室，北京 100854；2.北京易航远智科技有限公司 感知部，北京 100015；3.北京航天控制仪器研究所 测试技术研究室，北京 100854)

0 引 言

物体六自由度位姿估计技术主要提供物体在某个参考系中定位定姿的功能。单目相机下物体的六自由度位姿一般指的是物体在相机坐标系中的平移(x，y，z)与物体相对于相机坐标系的旋转(roll，yaw，pitch)。随着深度学习与机器视觉的融合交叉，传统物体六自由度位姿估计方法有被基于深度学习的方法取代的趋势[1]。深度学习算法作为以数据为驱动的方法，高质量训练数据对于训练结果有着积极作用[2]。

目前业界比较常见的六自由度估计数据集[3]有LineMOD、Occlusion、T-LESS、YCB-Video等，其中以LineMOD数据集形式为主，包含15种物体，每一种物体有1200张图像左右，数据形式分别为：原始图像、物体三维模型及尺寸信息、物体位姿可视化图像、物体掩码mask图像、物体相对于相机的旋转矩阵文本信息和图像对应相机内参。数据集的标注并不是一件容易的事，由于二维图像缺乏深度信息，造成6个自由度中深度、旋转量的人工标注差异性大，精度不高。LineMOD数据集制作借助RGB-D相机，使用基于三维模型模板的方法，在空间中离散选取视点，通过物体颜色梯度和表面法线特征进行匹配求解位姿，但深度信息中法线不稳定，需要复杂的算法保证。物体六自由度位姿标注方法常常基于深度相机或者双目相机[4]，在工业界或高校等硬件受限的场景中，这些带有特殊传感器或者需要精确安装的成像方式并不容易实现。另外也有通过渲染，将三维模型按照特定的位姿渲染到任意图像中制作合成数据集[3，5]，这样的方法忽略真实场景中的光影信息，缺乏真实感，而且无法生成具有遮挡现象的数据集。其次，三维模型往往颜色并不与真实相符，使得合成的数据集仅有几何信息被正确保留。

1 数据集制作流程及SfM算法分析

1.1 数据集制作流程

本文中，指定采集数据集的工作场景为室内小场景。数据集制作流程如图1所示，位姿关系解算过程如图2所示。首先根据CAD模型加工实物，同时采用均匀下采样算法处理物体CAD模型表面，得到模型点云；使用单目RGB相机在实验场景中获取物体多视角图像集；然后使用SfM[6]算法获取拍摄场景恢复尺度的点云信息，并获取每张图像的相机参数；将已获得的模型点云并放置在SfM获取的点云空间中，使用ICP(iterative closet point)算法获得物体点云相对于世界坐标系的位姿关系；最后，通过坐标变换获得每一张图像中物体相对于相机的位姿，从而获得物体六自由度位姿估计训练所需的数据集。

图1 数据集制作流程

图2 位姿解算过程

制作六自由度位姿估计数据集的输入是二维图像，但由于二维图像缺失深度信息，通过人工标注的方法并不可靠。SfM可以通过对输入图像集的处理，得到场景点云模型和图像集中每一张图像对应的相机参数，相机参数中包含了相机相对于点云空间坐标系的位姿。SfM算法的性能对于位姿解算结果的精度具有重要影响。

1.2 SfM

SfM算法目前主要分为3种形式：增量式运动恢复结构(incremental structure from motion，Incremental SfM)、全局式运动恢复结构(global structure from motion，Global SfM)、分层式运动恢复结构[7](hierarchical structure from motion，Hierarchical SfM)。在一般情况下，数据采集过程通过采用同一相机进行连续拍摄得到连续的图像帧完成。另外，增量式运动恢复结构在重建过程中不断使用捆绑调整进行优化场景结构，所以选择精度更高的增量式运动恢复结构更为合适。增量式运动恢复结构算法在每输入一定数量图像后会进行一次捆绑调整(bundle adjustment，BA)优化场景结构，该方法已经在各主流三维重建平台使用，例如VisualSfM、Bundle、Colmap等。

增量式运动恢复结构流程如图3所示，首先利用特征提取、特征匹配和几何验证的方法，确定输入数据集中各图片之间的匹配关系，并将多个视角具有相同描述子的特征点进行连接；按照几何关系准则选取初始相机对，以第一个相机坐标系的原点为坐标系原点，通过三角测量方法建立初始三维点云场景；然后输入新的图像，进行增量式重建，对于后续加入的图像利用多点透视方法(perspective-n-point，PnP)求解图像对应相机的外参，利用三角测量确定点云信息，并根据已加入场景的视图，决定是否将当前场景不存在的新点云添加到场景信息中。在增量式重建中，相机位姿求解和三角测量是两个单独的过程，由于算法具有不确定性，将会带来不同的误差，甚至相互影响。为此引入捆绑调整，通过使重投影误差最小实现优化，从而提高点云信息和相机参数信息的精度。

图3 SfM流程

增量式运动恢复结构最后的输出是场景点云模型和图像对应的相机参数，点云模型通过多张图像中匹配的特征点三角测量得到，相机参数通过相机标定与摄影几何求得，在这个过程中由于错误匹配、优化算法的不稳定性等使得输出结果并不理想。为了提高求解的稳定性，可以通过两方面解决：约束的正确性和约束的充分性。约束的正确性表现在特征提取与匹配的高效正确性，SfM算法的假设条件是物体表面为纯漫反射，而在实际数据集采集过程中，无法确保物体表面的材质，当物体表面为镜面反射时，光线照射在镜面反射区域会直接反射到相机中，导致出现高光区域，丢失了物体原本的表面纹理信息，造成特征提取不准确；对于约束的充分性，可以通过先验约束、多传感器融合等方法提高求解的稳定性，本文将通过具有先验信息的标定板来提高约束的充分性，实现尺度的恢复以及更加准确的位姿估计。

2 数据集自动标注方法

SfM算法首先利用SIFT算法获取图像集每一张图像的特征点和对应的描述子，对图像集中不同图片的描述子使用L2范数进行特征匹配，构建匹配图像对，同时设置图像对中第一帧图像的相机坐标系原点作为原点建立点云坐标系和初始化点云空间。通过增量输入图像的匹配以及使用PnP算法解算，获得不同图像在该点云坐标系下的相机外参。对于新的特征点，使用三角测量进一步扩充点云空间，此时，便可获得拍摄数据集的点云场景模型以及每张图像对相应的相机内外参数。由于三维点云由不具备深度信息的二维图像恢复而成，在不采用任何额外处理时，SfM生成的三维点云存在尺度不准确的问题。此外，相机外参仅能反应三维点云所在坐标系相对于相机的位姿变换，而不是目标物体相对于相机的位姿变换，因此需要在点云中对目标物体的位姿信息进行提取，得到目标物体相对于点云坐标系的坐标变换。为了提高数据集的标注精度，并与LineMOD、YCBVideo的数据标注格式保持一致，需要对SfM算法进行改进，并对结果做进一步处理。

2.1 特征提取与特征匹配

在三维重建过程中，由于场景中存在高光现象、动态物体等，使得图像特征提取不准确，并产生错误的特征匹配，不利于后续三角测量、BA优化等过程，所以需要额外的措施来增加算法的鲁棒性。对于工业室内可控场景，可以避免动态物体的存在，但由于光照影响，高光边缘处会产生错误的特征匹配和错误的颜色信息，使得重建结果周围存在白色噪点。

为了获得计算复杂度低、泛化能力强、鲁棒性好的高光检测阈值算法，采用无高光(Specular-free，SF)模型[8]对朗伯体金属表面建立表面反射模型进行高光区域检测。

假设RGB相机像素值与光强成正比，物体表面光反射由漫反射与镜面反射线性组合而成，将物体表面在RGB图像中的色彩分解成漫反射和镜面反射两部分，对于每一个独立反射部分，都可分解为光谱分量与几何分量，利用图像RGB三通道值信息，建立SF模型

Vi(p)=α(p)Vb，i+β(p)Vs，i

(1)

VSF，i(p)=Vi(p)-Vmin(p)=α(p)(Vb，i-Vb，min)

(2)

其中，Vi(p) 表示像素p的i通道响应值；α(p)、β(p) 分别是漫反射因子、镜面反射因子；Vb，i(p)、Vs，i(p) 分别是材料本身颜色与光源颜色；这里假设光源为白色光板，即响应值为255常量，那么得到VSF，i(p) 不再包含镜像反射因子。此时在SF图像与原始图像漫反射分量SF图像的p像素处，获得的相差一个偏差值τ(p)=Vmin(p)-β(p)Vs，i。 为了使阈值在图像中具有泛化能力，设定阈值为

(3)

其中，μv和σv是所有像素Vmin(p)的均值和方差，η与背景图像对比度、图像的反射程度相关。

SFM算法中经常采用尺度不变特征(scale-invariant feature transform，SIFT)，该特征提取方法是目前最复杂的特征检测和描述方法之一，具有很强的探索性，例如colmap采用的DSP-SIFT。采用SIFT特征点检测的图像输入是单张图像灰度图，然后进行下采样得到尺度缩放后的若干灰度图构成尺度空间。由于在图像尺度缩放过程中，高光区域的像素值发生变化，所以对不同尺度空间的图像进行高光区域检测。

采样后的灰度图通过不同标准差的高斯核函数对其进行采样获得图像金字塔，并在每个尺度空间的相邻图像层做差得到高斯差图像，最后利用相邻高斯差图像进行极值求解获得初始关键点。对于单张获得的初始关键点，将特征点中高斯函数差值具有低对比度特征点进行筛除。由于高亮处像素处对比度较高，所以很容易被识别为特征点并保留。

对于图像中的高光区域，a镜面反射使得物体表面的颜色信息、纹理信息丢失，甚至改变物体表面信息。对于图像信息丢失处，Criminisi算法可以利用周围近似像素进行近似补偿，但这样产生的特征信息并不具有参考意义，甚至导致错误匹配，所以对于高光区域检测到的特征采取直接过滤处理。

2.2 后端处理

通过对图像进行特征提取，获得SIFT特征点进行图像匹配，将具有相同描述子的特征点归纳到统一的集合中。为了获得场景的点云模型，与每一张图像对应相机的参数，需要对前端获得的特征信息进行后端处理。

在六自由度位姿估计数据集中，为了使用物体位姿信息将三维标定框正确锚住物体，则必须知道物体的原尺度以及在图像中的尺度信息，对三维重建点云空间恢复尺度[9]。对于小场景的尺度恢复，采用在场景中布置先验尺度信息标定板的方法，首先对图像中出现的特定标志点提取像点信息，利用三角测量得到像点对应的物点信息，然后使用先验的尺度信息来标定物点之间的距离

xi×TiX=0i=1，2，…，n

(4)

(5)

(6)

式中：T为相机外参组成的旋转矩阵；i为参与三角测量的图像编号；j为标定板上标志点对应的三维点；c为标志点之间的先验尺度信息。

在增量重建过程中，添加的图像会进行PnP和随机抽样一致性处理，将得到的新特征信息进行匹配与重建。为了使得位姿数据精度提高，则在BA过程加入先验物点与像点对应关系的约束，如式(7)所示

(7)

在SfM算法中，除特殊规定外，世界坐标系的原点与选定初始化点云的第一帧图像相机光心重合，得到的世界坐标系位置过于随机。为了方便后面对点云进行旋转、移动，利用sim(3)变换将在初始化点云过程中将世界坐标系转化到标定板特定位置处，并在之后添加图像进行增量重建过程中，基于此坐标系进行三角测量与BA优化。

2.3 点云配准

经过三维重建之后，可以得到数据集采集场景的点云信息和每一张图像对应的相机参数。然而SfM得到求解信息为每个图像的相机内外参数和点云空间模型，无法直接获得目标物体点云在点云空间中的位姿信息。为了获得该信息，需要将物体点云与三维模型下采样得到的模型点云进行配准。

目前已经获得固定坐标系的点云空间，根据目标物体CAD模型的尺寸信息，可以很容易地设定PCL的PassThrough滤波器数值，将xyz轴固定距离范围之外的点直接过滤，为了降低噪点，根据点云欧式距离计算将邻近点数目较少的点过滤，从而将目标物体在点云空间中分割出来。将预先准备的三维模型均匀下采样得到模型点云，导入到处理后的点云空间坐标系中，使得模型点云的中心与点云空间坐标系原点重合，其标定姿态的xyz轴与点云空间坐标系xyz轴平行。利用ICP点云配准算法将三维重建的场景模型配准到三维点云模型中，获得三维重建模型与世界坐标系之间的旋转矩阵。

小场景下点云数量较少，使用ICP点云配准算法[10]进行三维重建点云与三维模型点云之间的配准。其算法思想与SfM类似，首先对点云进行特征点提取与特征描述，然后找到相似特征确定数据的重叠部分。得到一组配准后的3D点后，利用以下等式估算变换矩阵

X={x1，x2，…，xn}，X′={x′1，x′2，…，x′n}

(8)

∀i∈[1，n]，xi=Tx′i

(9)

(10)

对于点云提取特征，ICP算法解决的是一个高维的非线性优化问题，容易陷入局部最优解，所以需要让配准的两个点云尽可能过滤噪点，并具有良好初始位置关系。

2.4 坐标变换

对于物体位姿，一般使用SE(3)来表示，对于目标物体在相机坐标系中的位置使用 {x，y，z} 表示，而对于目标物体的旋转表达有两种形式，第一种是物体相对于自身坐标系的旋转，第二种是物体相对于相机坐标系的旋转。前者经常使用在自动驾驶3D目标检测，后者用在机械臂抓取、增强现实等领域。另外，深度学习参数量庞大，为减小不必要的运算，会采用感兴趣区域处理机制。在对物体位姿估计之前采用平面检测，提取物体二维标定框区域，这就导致物体以相同轮廓出现在图像不同位置时，对应的旋转量不同，这对于数据驱动的深度学习算法是不能接受的，所以使用目标物体相对于物体自身坐标系的表达方式更适合基于检测后的深度学习算法[11]；而基于全图信息的深度学习算法，可以直接采用物体相对于相机坐标系的旋转表达。在六自由度位姿估计中，最后获得的数据应是目标物体相对于相机的位姿，所以第一种表达方式需要在神经网络解算之后，添加摄影几何进行求解最终位姿[12]。

为了获得物体的三维标定框，需要计算物体相对于相机的位姿。目前，通过点云配准获得物体相对于世界坐标系的旋转矩阵、通过SfM获得每一张图像相机相对于世界坐标系的旋转矩阵，那么通过坐标变换就很容易得到物体相对于每一张图像相机的旋转矩阵，即物体相对于相机的位姿

(11)

对于物体的三维标定框，可以在模型点云初始位置处，确定点云匹配前模型三维标定框的8个顶点；然后利用Rco和tco将顶点在图像中的位置求出，按顺序连接便得到可视化三维标定框。

3 实验验证与分析

3.1 数据集制作及分析

在正常光照的室内场景中对奶盒进行拍摄，采用Realsense D455相机单目模式，采集图像大小为1280*800像素，相机参数为自动，选择实验数据集对象为奶盒；采用大小为5×4、棋盘格边长为50 mm的ChArUco图案作为棋盘格标定板，以在恢复三维点云尺度信息时作为先验信息；数据采集过程中相机与物体距离保持在0.5 m左右，进行环绕拍摄，分别制作无遮挡、有遮挡的数据集。在有遮挡的数据集中的实例级语义图，需使用Label Studio等工具手工标注，位姿数据仍可以快速自动获得。

算法改进基于colmap框架，设置相机模型为已知相机内参模型，输入拍摄的图像集进行三维重建；利用本文提出改进方法前后对比如图4所示，经过改进后所得到的点云噪点减少，同时点云空间坐标系固定在标定板上；将物体三维模型下采样为点云模型，由于点云空间坐标系与物体点云具有较好的初始位置关系，则可以通过手动调节为ICP算法提供一个优秀的初始值；使用ICP算法，便获得物体点云在点云空间中的精确位姿。已知每一张图像在点云空间的相机外参和物体相对于空间的位姿，利用坐标变换就得到物体相对于相机的位姿关系。

图4 改进前后的点云模型

Blender是一款开源的三维图形图像软件，提供建模、渲染等功能，支持python控制台输入与脚本运行，结合后台终端可以激活python虚拟环境，从而使用更多的扩展包。为了检验位姿的正确性和获得mask数据，使用Blen-der[13]软件的python控制端口，利用Blender中的bpy库Opencv仿射变换将模型渲染到图像上。将图像集和图像对应的相机参数导入Blender中，设定内置相机参数，可以获得图像拍摄虚拟场景。由于不同的数据集或者计算机视觉算法库对坐标系的定义不同，所以需要注意blender空间坐标系以及位姿参数设置与SfM得到结果之间的转换关系。三维模型导入放置在世界坐标系原点处，将先前ICP得到的旋转矩阵转化成Blender参数的表示方式并输入，使三维模型在点云空间中与物体点云重合如图5所示。使用之前求得的相机模型将模型点云渲染到空白图片中，通过设定像素阈值获得mask文件，利用python控制台扩展获得其它文件。

图5 blender渲染

利用本文提出方法获得的无遮挡可视化数据集如图6所示、有遮挡可视化数据集如图7所示。

图6 数据集可视化，从上往下分别为原图、三维标定框、mask

图7 遮挡数据集可视化，从上往下分别为原图、三维标定框、mask

3.2 训练模型与评估标准

本文采用EfficientPose[14]作为检测算法，EfficientPose属于直接回归六自由度位姿估计算法，使用平面图像为输入，通过神经网络直接回归得到物体六维姿态。EfficientPose位姿估计算法是在EfficientDet[15]二维检测算法基础上添加平移头回归网络和旋转头回归网络，在神经网络训练过程中可以基于EfficientDet已训练好的二维检测模型进行训练，其网络结构如图8所示。选择phi=0的EfficientNet[16]神经网络框架，训练过程中采用tensorflow的Adam优化器，学习率设置为1e-4，训练500个迭代周期。为加速训练，模型训练使用两块NVIDIA RTX2080Ti GPU。

图8 神经网络框架

重投影误差指将目标物体的三维模型点云作为输入，利用物体相对于相机的位姿信息将模型顶点投影到图像平面上，真值与估计值之间误差利用下式确定

(12)

式中：M为点云的集合，K为相机参数。由于尺寸缩放，齐次坐标在计算L2范数之前先归一化处理。

平均距离[17](3D average distance，ADD)是指目标物体的三维模型点云作为输入，通过位姿变换预测值与位姿变换真值分别求得变换后的点云坐标，计算所有点的平均距离，如下所示

(13)

式中：R′为估计得到的旋转量，t′为估计得到的平移量。当ΔADD小于模型直径的10%时，判定估计的姿态是正确的。对于对称性物体，如果表面纹理相同，则在特定角度下会有对称解存在，而平均距离考虑的是唯一解。为了能够处理对称解的情况，引入ADD(-s)准则[18]，其计算式为

(14)

3.3 结果分析

基于Colmap框架对特征提取与后端处理进行改进，改进前后得到点云的均方误差见表1。

表1 改进前后点云重投影误差

通过实验结果可知，对高光处错误特征点处理，可以提高约束的正确性；通过添加充分的先验约束，提高约束的精度。

与经典机器视觉基于特征点几何关系算法有较大区别，EfficientPose网络采用基于神经网络的直接回归算法，从图像像素中学习到位姿估计的规律，这就造成位姿估计过度依赖于图像像素信息，造成泛化能力低，当场景发生较大变化时，算法失效。本实验背景为小场景中物体的位姿估计，场景变化不明显。分别在有标定板、无标定板的图像中测试利用本文训练得到EfficientPose网络，其结果如图9所示。

图9 有无标定板位姿估计对比

在相同的参数设置下，通过实验发现，与LineMOD标准数据集相比，我们通过单目RGB相机获得的数据集，同样可以训练神经网络，并获得较好的位姿估计效果，结果见表2；另外，数据集的大小与数据集采集时间相关，后续处理工作量与数据集采集时间无关，所以可以以更简单的方法获取大量数据集。

表2 神经网络训练在相同环境下的结果

本数据集是用于六自由度位姿估计的训练的数据集，如表3所示与现在的数据集制作方法相比有以下区别：首先，在真实场景中采集数据，仅通过单目RGB相机，利用SfM、ICP等算法，以更简单的方法实现对大数量的图像集进行六自由度位姿快速标注；获取到的数据集反映了目标在场景中真实的位姿、光影效果、表面颜色纹理、物体遮挡等信息；其次，使用具有先验信息、特征信息鲁棒性强的标定板辅助定位，在有遮挡场景、弱纹理物体、对称物体，也可以建立数据集。

表3 真实场景中物体位姿估计数据集对比

4 结束语

本文详细叙述了基于SfM的单目RGB六自由姿态估计数据集的创建过程、SfM的前端针对高光区域特征点的设计、后端增加先验约束处理和数据集数据的获得方式，最后使用数据集训练EfficientPose来验证方法的有效性和可行性。实验结果表明，与现有制作方法相比，本文方法在相同有效性下，降低对设备和场景布置的要求，且针对有遮挡的情况具有很强的鲁棒性。

由于场景中布置标定板，使得神经网络学到的特征中包含标定板显著特征，从而降低模型的泛化能力，因此未来需对不存在显著特征的先验信息进行研究。