SLAM技术支持的物体6Dof位姿估计的自训练方法

一、背景

Object-based SLAM:

SLAM就是机器人同步定位与建图，通过一些传感器的测量数据同时去建立环境的地图，且利用这个地图对于机器人的状态进行估计，机器人的状态包括机器人的位姿、速度和机器人的参数，比如内参。环境地图包括比如点的位置，线的位置，面的位置。常见的SLAM系统由前端和后端组成，如图1所示，前端一般从一些原始的传感器数据中采集一些特征，后端利用概率的推断模型对采集的模型进行融合生成全局一致的环境地图。

图1 SLAM结构

要知道环境中有哪些物体，就需要进行物体级SLAM，简单而言就是以物体为目标的SLAM系统，对物体和机器人的状态进行估计，如图2所示。一个是对空间中的几何体加上语义信息，对下游的任务有作用，另一个是非常节省存储空间的表示。如果用稠密点云就需要用很多的存储空间，但是基于物体级的SLAM形成的表示是非常轻量化的描述。

图2 物体级SLAM

如何做Object SLAM?

和广泛SLAM类似，首先要在原始的测量数据中提取特征，使用物体的感知模型，包括二维的目标检测，也包括实例分割。今天涉及的是六自由度物体估计目标检测，后端也是用概率推断模型对于多帧进行融合生成全局一致的地图。图3列举了一些目标SLAM的文章和方法。

图3 相关SLAM方法和文章

为什么object SLAM是比较困难的问题？

因为有一些广泛的SLAM具有的挑战，也面临一些新的挑战。广泛的challenge包括ambiguous data assosiation的问题，比如在一个停车场检测到一辆车，那么怎么把真的观测和地图里面的进行关联，那么哪一辆车是当前被观测的车呢？另外一个问题就是动态的问题，比如有一个车在行进，如何判断这辆车是在行进，如何防止这个行进的车对相机跟踪产生影响，然后如何根据这个车的行进去不断地更新地图，这些问题是比较难解决的。新的挑战主要是源于引入了object perception model，这两个模型结合的过程中就会产生一些information瓶颈，比如在deep learning model做出一些预测的时候，很难对不确定性进行量化，很难知道预测是好是坏。

在这种情况下如何去使用深度学习的model，如何给观测赋权重是一个比较困难的问题。另外，一个比较重要的在object SLAM领域中的问题就是domain gap问题，在新的环境中会有性能下降的问题。就是在训练perception model的时候，一般在特定环境中采取数据，给数据添加标注，用这些标注的数据训练网络。但当使用或测试这个网络时，往往在一个新环境中测试，训练和测试环境之间很可能有一些区别，比如光照的不同，背景的不同，噪声情况的不同，这个差异会使测试数据和训练数据造成分布不匹配的问题，这个问题就是一个domain gap的问题。还会导致perception model性能下降的问题。合成数据在真实情况中使用时，它的效果会大打折扣。

希望能够做到的表现是机器人在探索不同环境的时候，能够自动的适应当前的环境，把它的perception model调整到比较好的性能状态。这里对于object SLAM的介绍告一段落，如果感兴趣可以在面4的主页中关注。

图4 主页

二、方法介绍

什么是6自由度物体位姿估计？

如图5所示，图片中有物体，然后通过模型计算物体相对于相机的位姿，这个位姿包括3自由度的旋转和3自由度的平移，所以称它为6自由度的物体位姿估计。具有代表性的工作，比如说CNN和今天会涉及到的方法。

图5 6自由度的物体位姿估计

今天要探讨的不是如何去设计一个更好的6自由度位姿估计，而是在真实场景中的表现如何，把它从文章中拿出来，和其他的位姿模型在同样的benchmark中进行对比它们的表现最终如何。

BOP方法进行6自由度位姿估计，然后这个benchmark它的目标就是这样的表现，模型对应的物体是刚性物体，它们的输入是RGB和RGBD的图像。BOP challenge根据指标对不同的model进行打分，然后分数比较高的就可以获得奖项，每年的会议上都有BOP challenge的workshop，介绍如图6所示。

图6 BOP六维物体位姿估计

BOP challenge在2019年的结果，在这一年有很多方法在一些task上去竞争，表1列举了不同方法的性能比较，按照性能从高到低排列。可以看到这一年的经典方法就是基于这种特征的方法是由于基于深度学习方法的。

表1 BOP challenge性能比较

针对上面的问题给出解释，首先缺乏在真实世界中训练的图片，还有真实的测试图片和通常使用的合成的训练图片之间有比较大的domain gap。这两个原因属于一个问题，就是缺少在真实环境或者测试环境中带有6自由度物体标注的数据。

为了解决这个问题，有哪些方案呢？一种解决方案就是去提高合成数据的真实性，生成更加成熟的数据，另一种方案是可以利用test devirament没有标注的数据去提高表现。需要用到文章使用的self-training。Semi-supervised learning结合一些带有标签和数据和不带有标签的数据去提高模型的预测性能。

为什么这样的事情能够成功呢？为什么能够用不带有标签的数据去提高性能呢？因为不带标签的数据上往往携带了对于预测的task有用的一些信息。比如雾天的数据是不带标签的数据的话，那它就携带了这种background的信息，这样的信息有可能被提出的semi-supervised learning的方法学习到，提高模型的表现。不幸的是，Semi-supervised learning大部分的方法都没有对于收敛的一个保证，很可能越去训练它这个模型的表现越差，因为预报的一些错误的在这个训练过程中会不断的加强自身导致的。

什么是self-training？

self-training是比较早期的方法，用学习模型的预测去提高模型预测的能力。图7是具体的流程图，首先从一些带有标签的数据开始去训练Deep CNN model，然后用模型在不带标签的数据上预测，再把这些预测当做新的标签，这些标签就叫做伪标签，并不是真实的标签，是模型的预测。这些伪标签可能会有好有坏，为了选出好的伪标签，需要使用selection algorithm选出里面高质量的label形成一些带有伪标签的数据。把这些带有伪标签的数据和原始的带有真实标签的预训练的数据结合在一起，微调或重新训练网络。

可以看到，整个流程图中比较重要的一环就是选择算法，如果通过这个算法能够成功的选择出高质量的数据的话，就可以提高性能表现，反之可能会降低性能表现。

图7 self-training流程图

对于文章SLAM-supported self-training for 6D object pose estimation，首先是一些动机，为什么要做6自由度物体的位姿估计，因为它可以给出这些环境中的几何和语义的信息，如图8所示。

图8 环境的几何和语义信息

在一个环境中训练，在另外一个环境中测试，就会存在domain gap问题。这个问题的表现展示了一个video可视化问题，在合成数据上训练，有了真实数据再测试，可以看到它很难对这些物体进行正确的预测。那么如何去解决这样的问题呢？一种最简单的方式就是在测试数据中采集一些数据，给这些数据加上物体位姿的标注，然后微调6自由度位姿估计器。

但是，整个6自由度物体位姿标注的过程非常费时费力，更重要的一点是希望机器人在探索不同环境的时候是不被打断的，假如机器人进入到新的环境，还要去标注这个新环境的数据，那它对机器人的自主运行就是一个很不利的事情。所以希望做的就是机器人能够自己去给它采集到的数据进行标注，做一个self label。

图9 domain gap问题

应运而生，有一些方法来解决问题，一般用合成带有标签的数据和一些真实不带标签的数据一起去提高位姿估计的性能。如图10所示，它们可以分为single-view methods和Multi-view methods，前者输入的数据是无序的，但是一般机器人采集的数据都是按照一定的次序采集的，会有时间和空间上的连续性。single-view不能利用连续性，于是利用Multi-view方法，融合不同视角对于物体位姿的估计来形成更加可靠的对于物体的理解，用这个更加可靠的位姿对一些数据做标注，再微调，但大部分需要高精度相机的运动信息。

图10 single-view方法和multi-view方法

于是，提出了一种用SLAM来支持的方法，通过机器人采集的数据把它放到一个这种鲁棒的物体级SLAM的系统里面，然后生成一个全局一致的，包括相机的位姿和物体的位姿，然后生成一些伪标签，利用一致性的标签作为新的训练数据去微调位姿估计模型，如图11所示。

图11 SLAM支持的方法

方法的流程图如图12所示，从带有标签的图片数据入手，预训练一个6D的物体位姿估计器，把这个估计器放在机器人上，在行进过程中对物体的位姿进行估计，然后联合物体的位姿估计和机器人的里程计形成位姿图。用提出的一些鲁棒的优化方法求解SLAM估计，包括机器人的位姿和物体的位姿，从这些模型所预测的物体位姿和优化的位姿物体之中选出比较高质量的物体位姿作为伪标签，把它和原始的带有真实标签的数据进行融合。整个流程图和self-training是一样的过程，从宏观上来看，方法左半边是在做一个鲁棒的状态估计，形成全局一致的场景地图，右半边实际上是在用semi-supervised learning提高物体位姿估计的性能，方法结合了两方面的一个成果。

图12 方法流程图

如何进行鲁棒的位姿图优化来得到比较可靠的SLAM估计？提出了一种自动协方差调整的位姿图优化，这里如果展开讲可能需要很长时间，在这边只做一个比较宏观的介绍。如果大家有兴趣，可以去文章中的相关章节看到比较细节的公式推导。

首先要考虑为什么要做这样一个自动协方差的调整，一般在做位姿图估计的过程中会假设观测是符合高斯分布的，这样就能把问题转换为一个非线性最小二乘问题去求解，为了指定这样的高斯分布，需要两个量，一个是期望，一个是方差，对于高维的高斯分布需要一个期望和一个协方差矩阵。期望很好得到，可以通过SLAM估计还有测量模型计算每个测量的期望值，但协方差一般都是经验性的给出一个值，在实际中根据对于传感器噪声大小的一个理解去制定这样的协方差值，比如传感器的噪声比较大，给一个比较大的协方差矩阵，反之给一个比较小的协方差矩阵。

现在对于物体位姿的估计都是从深度学习模型得到的，也就是说传感器变成了模型，对噪声没有非常可靠的理解，预测没有办法很好的量化。在这种情况下，如何指定协方差矩阵？提出的方案是不指定协方差矩阵，把协方差矩阵和SLAM的变量进行联合优化，如图13所示，在公式里展示。第一项代表物体位姿的损失值，最后一项是机器人里程计的损失值，第一项是正则化项，目的是防止值跑到正无穷，像零这个方向去正则化。

求解联合优化的问题是用的alternating minimization方法，这个方法有两个优势，第一个优势是对最优的协方差矩阵有一个解析解，第二个好处是可以在分量级别对协方差矩阵进行拟合。得到位姿预测时，对六个自由度的分量进行不同程度的拟合，与传统方法相比更加灵活，也能够拟合更广泛的噪声模型。

图13 自动协方差调整公式推导

如图14所示是hybrid pseudo-labeling方法，在两种位姿中选取高质量的伪标签。

图14 hybrid pseudo-labeling方法

如图15所示，Hybrid model利用了两种数据，一种数据是模型直接在图片上预测的物体位姿，另一种是通过优化得到的物体位姿，为了对位姿进行好坏的评估，有两种评估方法，一种利用几何信息，另一种利用视觉信息，几何信息使用卡方测试，预测的物体位姿是否和优化的物体位姿有显著的差异，如果有显著差异可能是比较差的位姿估计，反之是比较好的位姿估计。视觉检查根据物体位姿估计生成一个渲染图片，把渲染物体和真实物体对比，转换到特征空间，在特征空间上的向量看它们是不是相似。经过这两个check，就能得到比较高质量的位姿标签数据。

图15 Hybrid model

提出方法的结果如图16所示，在两个数据集上进行实验，并测试方法。第一个数据集是一个公开数据集，叫做YCB video experiment。首先用一些合成数据去预训练，然后拿到模型上进行self-training。值得强调的是，在进行self-training时，不去使用这些label标注，完全通过self-training生成标注，最后一步就是把self-training后的放在上面去评估表现。Video展示的是它们在测试集的表现，就是在self-training之前和之后进行的对比，可以看到self-training后性能更加稳定，能够检测出更多物体，也有更少离群的位姿估计。

图16 提出方法的结果

如图17所示，第二个实验是在真实车上面做的实验，把相机放在机器人上，围绕物体进行导航。做真实机器人实验的目的就是为了测试方法对于挑战的可行性，提出方法在运动模糊等情况下依然可以得到比较好的性能，比较多的提高训练后的表现，离群值很少。

图17 真实车实验

三、总结和未来展望

提出的方法是鲁棒性SLAM所支持的6自由度物体位姿估计的自训练方法，目的是希望做优化，方法能够适应新的环境。最重要的一环是提出的自动协方差调整的位姿图优化方法，最后通过实验验证了方法的表现。如图18所示，右上角有文章和代码的链接，感兴趣可以去看一下。

图18 主页和文章链接

最后就是如何去延伸工作，首先可以给系统加入对动态物体的处理，如何考虑它的影响，或者某些物体有对称性的物体位姿如何处理，以及鲁棒性的半监督学习，还有很多更新的方法进一步提高表现，最后可以把对于物体级别的性能提升扩展到对于物体位姿形状和类别进行估计，用更加可靠的结果训练，然后估计。当然可以很多其他的方面，由于时间的分享，讨论告一段落。

四、问题：

1.深度学习对于堆叠物体位姿估计有什么建议吗？

可以看一些对于这块研究的设计方法的文章，如果对这块理解的不是特别深入，也可以看一下深蓝学院的相关课程。

2.在利用深度学习的方法进行6自由度物体位姿估计时，如果要建立RGBD数据集的话，有什么好的方法推荐吗？

我的理解是如何选择采集到的RGBD图片对不对，可以在网上找一些图，然后放在tool中进行学习产生标注。可以使用优化方法只采用标注关键帧的方法，利用优化进行复制，有效快速的标注。

3.得到伪标签以后，再训练网络是不是需要离线进行？这样SLAM是否就要停下了？

是的，这个说法是正确的，采集到伪标签数据时，需要训练位姿估计的模型，需要较长的时间，当前大多数模型都需要较长时间，如果未来有更好的方法在短时间内进行训练，也希望可以在线进行。

4.实验小车的处理器什么配置？方法实现在线实时吗？

在实验中采取的小车是jerk robot，只是去用它作为一个相机支架围绕物体变成目标，如果要了解的话可以了解这个机器人的内部构造。

5.实现的方法是单目还是rgbd的？

方法是单目的。

6.如果SLAM需要停止的话，那么SLAM的作用不就相当于收集数据集吗？

是的，这个方法的提出自动收集数据集的过程，可能传到云端，有服务的话可以很快的训练，也就不需要停止很久。

7.最后检测的物体仍然是预训练里面已有的物体吗？

是的。

8.即使不使用SLAM，手动收集数据集也可以，另外方法对SLAM的过程有哪些提高？

手动收集是可以的，但大部分都是在公开数据集中进行分析，然后对它进行使用。它的问题在于在真实环境中对机器人环境探索，很难有人一直跟着机器人，希望机器人自主做label的。提高主要是自动的产生标签，可以用这个过程优化协方差模型。