Semantic SLAM with Autonomous Object-Level Data Association 论文笔记

系列文章

1.使用椭球表示物体的语义SLAM的基础知识
2.QuadricSLAM

摘要&介绍

疫情原因封校，好想出去耍耍呀！哎，心情不好，写篇博客发泄一下

这篇论文发布在2021年的ICRA上，也是使用椭球来表示物体。使用BoW词袋向量来完成数据关联，并且提出了一种新的椭球初始化方法。能够完成实时的语义建图。

系统框架

整体上和ORBSLAM2的框架非常的相似，这里不得不说ORBSLAM2太牛逼了，整体架构，代码可读性等等，真的可以说是VSLAM领域的一件优秀的艺术。

追踪部分使用的ORBSLAM2，通过并行线程完成目标检测和SLAM的前端追踪。然后在目标检测生成的包围框内提取ORB特征点，并且转化为词袋向量。然后进行物体级别的数据关联并且判断当前帧是否可以作为关键帧来构建语义地图。
语义建图主要是接受语义关键帧，并且基于带线性约束的二次规划完成物体的创建。然后进行BA。
地图中包含着3D物体和语义关键帧。3D物体记录着被观测到的所有的语义关键帧，语义关键帧记录着该帧中所有观测到的3D物体。类似于ORBSLAM中的MapPoints和KeyFrame，构建相互对应的关系，属实是双向奔赴了。

目标检测和追踪

这部分重点介绍了物体级的数据关联。

目标检测

使用ROS接口的YOLOv3完成实时的目标检测。对于每一个输入的图像${\mathbf{I}}_t$，输出一系列语义测量结果$S_t=z_k$，对于每一个语义测量，包含 z k = { b k , c k , s k } z_k = \{ b_k,c_k,s_k\} zk​={bk​,ck​,sk​}，分别表示包围框，物体类别和检测分数。

对于每一个ROI（bbox），按照相同的密度提取FAST角点，并且根据ROI面积调整提取的角点的数目。然后提取ORB描述子。一旦提取到ORB特征点，我们匹配视觉词袋来将他们转化为BoW向量。

物体级数据关联

这部分是整个论文的一大创新点，提出了frame-to-map的匹配方法。

第一步，得到一系列的候选物体。对于每一个语义观测结果 z k = { b k , c k , s k } z_k = \{ b_k,c_k,s_k\} zk​={bk​,ck​,sk​},只有与其有相同的类别的物体才能有资格加入到候选物体。这是一个最基本的条件，此外，本文还引入了几何检测来剔除具有相似外表但是位置不准确的物体。考虑如下两种情况。
情况1:物体已经成功初始化，即可以使用椭球来表示。在这种情况下，椭球的中心投影到当前图像${\mathbf{I}}_t$中应该位于bbox的内部。如图中的虚线所示
情况2:物体没有被初始化，只记录了一些了被观测到的关键帧，那么，最近被观测到的图像的bbox的中心经过投影后应该位于当前观测的包围框的内部（可以理解为单目相机的三角化测量）。上述前提假设是相机不能有剧烈的视角变化，否则，实际上是同一个物体，但是由于视角的剧烈变化，上一帧的中心可能不在当前帧的bbox内。

第二步：通过上述步骤，我们已经得到了语义观测对应于地图中的一系列候选物体${\tau }_j$，同时，这些候选物体${\tau }_j$包含着一系列被观测到的语义关键帧。这一步要做的就是计算当前语义观测 z k = { b k , c k , s k } z_k = \{ b_k,c_k,s_k\} zk​={bk​,ck​,sk​}的词袋向量与${\tau }_j$的语义关键帧中对应的词袋向量的相似度，论文中的公式为
$$s(v_1,v_2)=1-0.5|\frac{v_1}{|v_1|}-\frac{v_2}{|v_2|}|$$
在语义关键帧中最大的得分$c_{kj}$被视为数据关联的得分，表示$z_k$与地图中的物体${\tau }_j$可能存在关联。得到这个得分之后，数据关联问题就转化为了一个整数规划问题，对于每一个$z_k$与${\tau }_j$之间，定义一个0-1变量

构建整数规划问题,使用的是Or-tools求解。

其中公式(3)中的两个约束条件分别表示为
(1)每个语义观测$z_k$（因为遍历了所有的j)至多与一个物体相关联
(2)每个物体${\tau }_j$（同理，因为遍历了所有的k）只能与当前图像中的一个语义测量$z_k$相关联。

语义建图

这部分原理和QuadricSLAM类似，只不过没有使用SVD分解，而是构建二次规划问题求解。

这里重点记录下新增加的三个约束条件：

1. 初始化后的椭球应当在相机的前方
2. 相机的主平面不应该与椭球相交
3. 椭球的中心投影到当前图像中，对应的像素点应当在包围框的内部。

约束条件1

其中，$O_{K_i}$表示相机中心位置（在世界坐标系下），$z_{K_i}$表示相机的光轴（可以理解为相机坐标系中的指向前方的z轴，即下图中的$z_c$）在世界坐标系中的方向，$O_{{\tau }_j}$表示椭球在世界坐标系中的位置。

公式(9)表示，由相机中心到物体中心的向量与光轴的方向夹角应当为锐角，所以大于零。

对于相机中心（光心）$O_{K_i}$，假设相机的位姿表示为
$$T_{wc}=\left[\begin{array}{cc}{R}_{wc}& t_{wc}\\ 0& 1\end{array}\right]$$
相机的光心在相机坐标系中的位置为$Pc=[0,0,0，1{]}^T$，那么可以得出在世界坐标系中的位置为
$$O_{K_i}=T_{wc}{P}_c=t_{wc}$$
另外，我们记
$$R_{wc}=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]$$
在相机坐标系中，光轴的方向为$[0,0,1]$，那么乘以上述变化矩阵就可以得到世界坐标系中光轴的方向为$[R_{13},R_{23},R_{33}{]}^T$

约束条件2
相机主平面，即在相机模型图像中的$o_c,x_c,y_c$构成的平面。

由于使用的是对偶空间中的椭球表示，所以如果相机主平面与椭球不相交的话，应当是等于零。
在论文中，在世界坐标系中，相机的主平面表示为

下面记录下推导过程
考虑世界坐标系中，相机主平面上的一点$P_w=[x_w,y_w,z_w,1{]}^T$。再考虑在相机坐标系中，相机的主平面应当与图像平面($x-y-p$构成的平面）是平行的，即图像应当在无穷远处相交。那么投影关系可以记作
$$\left[\begin{array}{c}u\\ v\\ 0\end{array}\right]=K[R_{cw}|t_{cw}]P_w$$
其中$t_{cw}=-R_{wc}^T{t}_{wc}$,记$P=K[R_{cw}|t_{cw}]=[P_1,P_2,P_3{]}^T$，那么上述投影关系可以表示为
$$\left[\begin{array}{c}u\\ v\\ 0\end{array}\right]=\left[\begin{array}{c}{P}_1\\ P_2\\ P_3\end{array}\right]P_w=\left[\begin{array}{c}{P}_1{P}_w\\ P_2{P}_w\\ P_3{P}_w\end{array}\right]$$
通过上述公式可以看到，$P_3{P}_w=0$，由于$P_w$表示世界坐标系中主平面上的点，那么P_3则表示世界坐标系中的相机主平面。由于$P_3$表示投影矩阵的第三行，代入$R_{wc},t_{wc}$即可得到论文中的表示。