[IROS 2018]移动机器人同时估计自身位姿和物体位姿

Semantic Mapping with Simultaneous Object Detection and Localization

论文地址：https://arxiv.org/abs/1810.11525
论文视频：https://www.youtube.com/watch?v=W-6ViSlrrZg

简介

       作者提出同时进行目标检测和位姿估计，利用一段连续的图像帧，这个图像帧和slam不同之处在于它是对一个场景的扫描，运动的幅度可能很小，就在一个场景附近各个角度扫的一段图形序列，然后在机器人运动的时候通过ORBSLAM定位机器人自己的位置，利用faster-rcnn对物体进行目标检测，通过粒子滤波进行物体的位姿估计，使用提出的CT-Map方法来对检测结果进行纠正，得到更准确的检测结果和物体的位姿。

目标

       使机器人能够在物体层面上对世界进行语义映射，其中世界的表示是对物体类别和位姿的置信度。随着神经网络物体检测的不断进步，作者为语义映射提供了更强大的构建模块。然而，由于训练数据集的偏差和多样性不足，这种物体检测在野外经常是嘈杂的。并且对来自此类网络的错误检测保持鲁棒性。作者将物体类别建模为生成推理的隐藏状态的一部分，而不是对检测器给出的类标签做出选择。
       考虑到运动式RGB-D观测，作者的目标是推断解释观测结果的物体类别和位姿，同时考虑物体之间的上下文关系和物体位姿的时间一致性。在语义映射期间显式地建模物体-物体上下文关系，而不是假设每个物体在环境中是独立的。简单来说就是相同类别的物体比不同类别的物体更容易共同出现。此外，应加强物理合理性，以防止物体相互交叉，以及漂浮在空中。
       物体位姿的时间一致性在语义映射中也起着重要作用。物体可以保留在过去观察的位置，或者随着时间的推移逐渐改变它们的语义位置。在遮挡的情况下，建模时间一致性可能有助于部分观察物体的定位。通过时间一致性建模，机器人可以获得物体永久性的概念，例如，即使没有直接观察物体，也相信物体仍然存在。

创新点

       考虑到语义映射中的环境和时间因素，作者提出了环境时间映射（CT-MAP）方法来同时检测物体并通过运动的RGB-D相机观测定位它们的6D位姿。将语义映射问题利用概率表示为物体类别和位姿的置信度估计问题。使用条件随机场（CRF）来建模物体之间的上下文关系和物体位姿的时间一致性。 CRF模型中的依赖性包括以下方面：
       1）潜在语义映射变量和观测结果之间的兼容性，
       2）物体之间的上下文关系
       3）物体位姿的时间一致性。
作者提出了一种基于粒子滤波的算法，在CT-MAP中进行生成推理。

核心思想

       使用一个个向量（物体的位置和概率）来代表场景中的物体。针对每个物体向量存在的概率，作者用粒子滤波和CRF来更新。基本的思想是利用不同帧之间的同一物体在空间位置的一致性来更新物体存在的置信度。用概率来表示物体存在的置信度的好处就是，即使机器人即使一段时间内没有识别出这个物体，这个物体还是一定概率和粒子的形式再场景中存在的。

方法

       作者提出CT-Map的方法。 CT-Map方法保持了对观察场景中物体类别和位姿的置信度。 假设机器人通过外部定位程序（例如ORB-SLAM）在环境中定位。该语义地图由一组N个物体组成$O$= {$o^1$ ，$o^2$ ，···，$o^N$ }。 每个物体$o^i$= {$o^c$，$o^g$，$o^{\psi }$}包含物体类别$o^c\in C$，物体几何结构$o^g$和物体位姿$o^{\psi }$，其中$C$是物体类别的集合$C$ = {$c_1$，$c_2$，…，$c_n$}。
       在t时刻，机器人的位置为$x_t$。机器人的观测值为$z_t$ = {$I_t$，$S_t$}，其中$I_t$是观察到的RGB-D图像，$S_t$是语义测量值。语义测量值$s_k$= { $s_k^s$，$s_k^b$ } ∈ S t S_{t} S