暑期研习系列：动态室外场景SLAM研究后续

暑期研习系列：动态室外场景SLAM研究后续

VDO-SLAM：论文、安装与代码阅读

一、VDO-SLAM论文阅读

OK，我们来看下VDO-SLAM（ A Visual Dynamic Object-aware SLAM System ）是什么？其实从名字上就能很清晰的看出，该SLAM系统主要用于解决动态场景的问题。并且，由于具有物体级别的感知功能，能够追踪场景中运动刚体，并计算其速度。此外，该系统开源代码：https://github.com/halajun/vdo_slam。

该系统在室内室外都有测试，但是文章也指出，由于系统原理性设计，使得室内小物体难以识别及追踪，因此该系统主要适用于室外场景下的车辆追踪上面，并在kitti数据集上进行了测试，表现优异。

该系统可以在单目、双目、RGBD版本上应用。但是，由于系统需要视差图，因此单目版本还需要额外的视差估计模块。但是最佳使用的应该是基于基于双目的室外场景。

大致了解这个系统之后，开始看论文：

• 引言。作者认为，现有存在处理动态场景的思路：直接把动态场景当做外点扔掉、使用传统的多目标追踪方法进行处理。这两种思路都存在一定的缺陷。前者仅仅处理静态数据（有点浪费）；后者取决于相机位姿估计的精度，在复杂动态场景下可能会因为缺乏可靠的静态结构而失效（简而言之就是slam与追踪两种功能是分离的，没有相互促进）。VDO-SLAM解决了上述两者的缺陷。作者认为VDO-SLAM具有以下贡献（ 主要内容在第1和第3点 ）：

  1. 设计了统一的估计框架，将机器人位姿、动态点和静态点、运动物体的位姿（object motions）联合起来计算与优化
  2. 对动态物体 SE(3) 位姿变化，以及提取场景中物体速度的方法 进行了精确的估计，并优于最先进的算法。
  3. 能够处理由于非直接遮挡导致的语义分割失败，使算法具有更大的鲁棒性。
  4. 一种在复杂和扣人心弦（compelling，不知道怎么翻译好）的真实场景中的可演示系统。

• 相关工作。作者列举的

  detect moving objects and track them separately using traditional multi-target tracking approaches (Wang et al. (2003); Miller and Campbell (2007); Rogers et al. (2010); Kundu et al. (2011)).

  few aim to utilise information from static and dynamic objects into a single framework to improve the accuracy of the estimation (Bibby and Reid (2007); Judd et al. (2018); Yang and Scherer (2019))

  a multi-frame spectral clustering framework is introduced by Xu et al. (2018) with joint integration of an affine model, a homography model and a fundamental matrix.

  Making use of semantic information has proven to help deal with the issues of degenerate motions and partial occlusions increasing the motion segmentation accuracy (Rubino et al. (2018)).

  data-driven (deep learning) approaches (Milan et al. (2017); Kim et al. (2018)).

  The state-of-the-art STAM-MOT (Chu et al. (2017))

  可以看下。除了最后一个STAM-MOT用于对比实验外，基本与本文无关，仅仅作为扩展阅读。

• 符号说明：详见论文，还是比较详细的。

• 推导：大部分推导就不再赘述，详见论文即可，这里挑几个重点的来说下。
  不知为何博客输入公式总出错，就直接放截图吧
  

• 预处理：

  1. 实例级别的语义分割：采用mask r-cnn网络，由coco数据集训练，未经过微调。
  2. 稠密光流估计：采用PWC-Net网络，由FlyingChairs数据集训练，经过Sintel 和 KITTI 数据集微调。
  3. 立体匹配视差图：单目情况下，使用 MonoDepth2 (Godard et al. (2019)) 网络。数据集：Depth Eigen split

• 流程图：
  

  1. 稠密光流估计作用：第一为语义掩码提供采样点，保证有足够数量和鲁棒的点来传播，使得多物体追踪能够进行；第二，能够在语义分割失败后用于恢复语义掩码。之所以选择稠密光流而非稀疏光流，是因为稠密光流能够保证长期追踪的鲁棒性。

  2. 特征检测与追踪：采用稀疏的改进型FAST角点检测（ Rublee et al. (2011). ），并用光流法进行追踪。

  3. 位姿变换：静态点采用3D-2D方法求解。初始化则计算两种模型：先前运动的传播模型、以及带有RANSAC方法的P3P模型，并计算内点数量，取内点数量多的为初始化标准。

  4. 动态物体的判断：计算空间内某一点m在k、k-1帧时刻位置偏差，得到场景流（scene flow）如果该点的场景流变化大于预先设定的阈值（文中取0.12），则认定该点为动态点。在语义分割后的掩码范围内，计算每个物体场景流的动态点占比是否大于某一阈值（文中取30%），如果大于则判定该物体掩码对应的物体为动态物体。

  5. 动态追踪：实例级别的语义分割仅仅能够提供单阶段（单张图片）的分割结果，但是没法实现追踪，因此需要引入稠密光流进行数据关联。使用光流为每个运动物体进行编号，对受到噪声，图像边界及遮蔽影响的运动物体，当其上一帧周边的点场景流运动标签为0时，重新对其编号。

  6. 运动物体的位姿变换估计：直接法。为了提升速度，每三个点采集一次。

  7. 速度估计：公式24第一行将$H$拆解为$H=\left[\begin{array}{cc}{R}_{3\times 3}& t_{3\times 1}\\ 0_{1\times 3}& 1_{1\times 1}\end{array}\right]$，得到第二行。其中$I_3=I_{3\times 3};I_4=I_{4\times 4}$。

  8. 局部地图及优化：固定尺寸的滑动窗口。在局部地图中仅仅优化静态点及相机位姿，因为优化动态结构并不能够带来精度上的收益。

  9. 全局地图及优化：采用因子图方法优化，并且为了提高准确性和鲁棒性，当空间点在3帧内被观测到时才会将该点记录进入全局地图。因子图结如下。此时，静态特征与动态特征同时被纳入因子图中共同被优化。
     

  10. 从地图到追踪：利用传播模型能够给出动态物体运动的先验情况，辅助解决语义分割失败的问题。

• 实验过程及对比

  1. 室内数据集：Oxford Multimotion Dataset。项目地址为：https://robotic-esp.com/datasets/omd/。数据集的主要内容为几个悬挂在室内的摇摆方块以及玩具小车（在本次实验中没有涉及），不是很偏向于实际应用场景，但是在VDO-SLAM系统中可以用于测试方块的运动变换。实验对比的系统为MVO。两者同时去除了全局优化。结果显示，VDO-SLAM在绝大多数情况下优于MVO。但是在纯旋转方面表现可能较弱，这一问题主要由光流在纯旋转问题上表现不佳造成的。
  2. 室外数据集：KITTI。主要对比了01-06、16、20几个序列。主要用于测试追踪效果。对比系统为CubeSLAM（单目）。效果大幅提升（这是废话，因为增加了立体匹配的时差图）。
  3. 对比实验详见论文。

• 讨论：

  1. 加入光流进行联合优化：能够大幅提升鲁棒性（更多内点）和准确率（误差减少15%-20%）。实验见论文。
  2. 非直接遮挡追踪的鲁棒性探索：利用流程图-10中方法进行数据关联，能够大幅降低分割失败造成的影响。
  3. 全局联合动态特征点及物体进行优化：能够同时增加追踪精度和相机自身的定位精度。
  4. 计算消耗情况：取决于追踪数量多少，一般在5-8 fps左右。

二、VDO-SLAM 编译与安装

项目地址：https://github.com/halajun/vdo_slam。注意，该项目仅提供Oxford Multimotion Dataset与KITTI中03序列的某几帧，并且预先设置好了分割掩码（.txt）、光流文件（.flo）及深度图（.png）。

安装十分简单，首先更新gcc、g++至第9版本。

sudo add-apt-repository ppa:ubuntu-toolchain-r/test
sudo apt-get update
sudo apt-get install gcc-9 g++-9
sudo apt-get upgrade
sudo updatedb && sudo ldconfig
locate gcc | grep -E "/usr/bin/gcc-[0-9]" # 检查gcc现有版本
locate g++ | grep -E "/usr/bin/g\+\+-[0-9]" # 检查g++现有版本
cd /usr/bin
sudo rm gcc
sudo ln -s gcc-9 gcc
sudo rm g++
sudo ln -s g++-9 g++

接着，更新3.1.0版本的Eigen（其他部分版本会有问题），链接如下：https://github.com/eigenteam/eigen-git-mirror/tree/3.1.0

安装如下：

mkdir build && cd build && cmake .. && sudo make install
sudo cp -r /usr/local/include/eigen3 /usr/include 

然后，修改CMakeLists.txt。将76行加上注释，将78行注释消除

# for mac os (default)
#${PROJECT_SOURCE_DIR}/dependencies/g2o/lib/libg2o.dylib
# for linux (ubuntu)
${PROJECT_SOURCE_DIR}/dependencies/g2o/lib/libg2o.so

最后，直接编译安装：

chmod +x build.sh
./build.sh

下载demo数据集（谷歌网盘，详见项目主页readme文档）

输入命令：（最后一个参数为数据集位置）

./example/vdo_slam ./example/kitti-0000-0013.yaml  ../demo-kitti/demo-kitti/

OK

三、VDO-SLAM代码阅读

该部分不是很好写笔记，也没看的太仔细，就简单聊聊吧

整个结构相对比较清晰。简单的聊下比较重要的几个函数：

• GrabImageRGBD()：入口函数
• UpdateMask()：这个是之前提到的第10点，即如果语义分割失败后如何回复掩码
• Frame()：里面特征提取完全仿照ORB-SLAM的思路，但是增加了光流特征。并且，对于存在语义掩码的物体每隔3点采集光流。
• GetInitModelCam()：采用恒速度模型（这里理解为变换矩阵与上一帧相同）和PNP求解两种方式计算变换矩阵，并采用内点较多的那一种模型
• PoseOptimizationFlow2Cam()：光流和位姿联合优化
• DynObjTracking()：主要解决动态物体选取、编码问题。其中sf_norm表示场景流的变化（虽然不知道为什么会表述成只计算x与z？），并根据值的大小为光流编订序号（？？？？），最后，将前后帧的标签相互对应
• RenewFrameInfo()：更新信息，补充光流追踪特征点
• GetStaticTrack()/GetDynamicTrackNew()：获得追踪列表
• PartialBatchOptimization()/FullBatchOptimization()：局部、全局优化。说实话有些没有看懂。