自动驾驶中语义追踪的“正确”打开方式

今天给大家介绍的是一篇比较系统性”集大成”的工作[1]，可能不像之前介绍的那些工作，都是从某点insight出发导出了一些很简单的性质或者结论，这篇工作集成了多个成熟的技术，但是又对问题进行了深入的定制化。同样作为自动驾驶的从业者，我是很欣赏这样的工作，并理解这背后的工作量和艰辛的，所以今天给大家换换口味。

这篇文章想要处理的问题是基于视觉的自动驾驶中的一个核心问题：如何通过双目相机准确追踪道路上车辆准确的位姿和状态？这篇文章给出了一个系统性的解决方案，如下图所示，整个系统分为几个部分：

第一部分是通过标准的Deep Learning方法进行2D Detection和视角分类，并通过相机模型，预测出初始的3D框；第二部分通过提取ORB特征和鲁棒匹配，完成双目深度估计与时序追踪；第三部分，也是整个paper的核心，通过SLAM中常用的Bundle Adjustment技术，联合优化自身位姿、每个车辆的位姿和状态，得到最终结果。下面详细介绍每一部分的工作。由于文章中符号很多，而且有一些描述不很清晰的地方，下面部分将主要用语言来描述每一部分在做的事情和想法。虽然可能不十分精确，但是这样更有益于大家理解主旨，如果有兴趣的同学可以直接查阅原文来看精确的数学描述。

在第一部分中，主要follow了CVPR17的一个工作（原文citation[24]），基本思想是通过预测2D框以及不同的观测视角，同时假设2D框能够紧密包围3D框，那么给定视角，相机内参和车辆dimension的情况下，最紧的3D框和水平旋转角度可以直接通过线性方程组解出。注意这里做了很强的假设，也就是2D框完全准确，已经每种车辆的dimension是完全一致的，所以这个计算出的3D框只是作为后续操作的初始值，在第三部分中，还会重新优化这4个自由度的变量。

在第二部分中，主要任务是建立起可靠的local feature匹配和帧间association。帧间association和双目深度估计都使用了比较成熟的方案或简单的baseline。由于在第一步已经有了初步的距离估计，在双目匹配的过程中可以缩小匹配范围以提升结果。local feature这里使用的是经典的ORB feature。对于每个特征点，分别在左右眼和时序上进行匹配，后用RANSAC做outlier去除。

第三部分，也是整个文章中最重要的部分，把沈老师很熟悉的Bundle Adjustment (BA) 引入到了整个的位姿状态精调中来。这部分优化分为两个步骤，第一部分(Camera BA)是利用前面得到的背景中的匹配的点，联合优化自身相机位姿，以及在上一步匹配过的背景特征点坐标来优化reprojection error，这是在SLAM和SfM中的经典BA。通过这一步，可以得到相对准确的自身位姿为下一步使用。第二部分(Object BA)是针对每一辆车，充分挖掘2D和3D之间关系，以及各种约束。这部分BA分为4个loss term，联合优化了每个车辆自身的位姿，大小，速度和方向盘转角以及在上一步得到的ORB特征点的对应3D位置，下面分别介绍这4部分：

特征点一致性loss：原文中的公式(16) (17)，符号非常复杂，要做的事情其实就是要求特征点对应的3D位置经过各种投射变换之后能够在它在2D图像上的坐标对应。文中分别对于左眼和右眼图像计算了loss，下面以左眼为例分布解释下在做什么：

首先，作者假设车辆是一个刚体，也就意味着在帧间匹配得到的特征点，在车辆坐标系下的相对位置是不会随时间变化，所以公式(16)中的f是没有上标t的。

第一步变换，将特征点的3D位置f由车体坐标系转换到世界坐标系。

将世界坐标系内的f转换到相机坐标系。

将相机坐标系中的f通过相机内参矩阵投影回2D图像。

这样就可以计算这两者之间的一个差值，作为loss function。对于右眼，计算方法类似，只不过需要使用双目标定的外参，将左眼相机坐标系转换到右眼相机坐标系中，其余一致。

2.3D框与2D框一致loss：这一个term和第一部分的原理是类似的，区别在于，在这部分中，车辆的dimension和车辆的位姿被同时作为优化变量联合优化，而不是像第一部分中，车辆的dimension固定。所以通过加入这个term解决了在第一步中由于dimension不准确导致的错误估计。

3.车辆动力学模型loss：除了车辆位姿，我们在自动驾驶中还会关系车辆的状态，包括速度和方向盘转角，这对于后续的车辆意图估计等任务有着极大的帮助。实际的车辆当然不可以抽象成一个无规律的质点的运动，所以在这个loss中，作者使用了一个车辆动力学模型来描述当前时刻与前一时刻的位姿和状态之间的关系。这个动力学模型来源于原论文引用[27]。除了位姿之外，这个loss里还引入了车辆速度、方向盘转角以及车辆轮距（可以从车辆大小的出）作为优化变量。

4.点云对齐loss：这一个loss文中描述得不是很清楚。首先，这里的点云并不是指LiDAR点云，而是指从双目中恢复出来的点云。以我个人的理解，这个loss的意义在于约束每辆车的特征点的3D位置可以落在对应车辆的3D框内。具体的细节文中只提到了minimize the distance of the feature points and their corresponding observed surface. 个人不是很理解具体的这个做法，为什么需要minimize这样的距离，而不是直接根据是否在3D框内计算loss。另外，这样的做法是否假设了所有特征点都在3D框的面上，这也不是很合理。总之，这样的一个loss是有必要的，但是具体实现方式可能会有更好的。

在实验中，作者使用了KITTI数据集，分别对ego-motion和object localization&pose估计进行了比较。在ego-motion中，作者对比了经典的ORB-SLAM，由于这个方法能够很好地去除由于移动物体带来的特征点匹配失败，在一些车辆较多的复杂场景中取得了较大的提升。在object motion&pose中，由于前述引入的诸多先验和约束，对比baseline也有了巨大提升。相信这个方法也可以从更强大的Deep learning方法中得到更准确的2D检测和视角估计而受益。强烈推荐大家观看一下下面在KITTI上的结果视频：https://www.youtube.com/watch?v=5_tXtanePdQhttps://www.https://www.youtube.com/watch?v=5_tXtanePdQ

总结以上，这篇文章应该是我见到的一篇将这么多先验紧耦合在一起的系统性工作。这可能和沈老师之前的SLAM背景很相关。虽说每个模块都有改进的空间，但是这篇文章提供了一套切实可行的框架来解决视觉自动驾驶中的这个难题。相比于诸多暴力使用CNN来预测车辆位姿、状态甚至意图的paper，我个人更欣赏这样能够把high level semantic和geometry进行深度融合的工作，在今年CVPR上也见到这个方向很好的尝试[2]。希望介绍的这个工作能够启发大家更深度地思考问题的本质和结构，而不是将所有问题都扔给CNN做一个黑盒来预测，做出更多有insight的工作。

[1] Li, P., Qin, T., & Shen, S. (2018). Stereo Vision-based Semantic 3D Object and Ego-motion Tracking for Autonomous Driving.ECCV2018

[2] Yin, Z., & Shi, J. (2018, March). GeoNet: Unsupervised Learning of Dense Depth, Optical Flow and Camera Pose. InCVPR2018