实战！视觉SLAM中的直接法在智能驾驶中的应用实践！

来源：HorizonRobotics

本期由智能驾驶团队吴佳田、颜沁睿、杨德刚给大家分享他们在研发中对视觉SLAM直接法的应用实践，相关代码及说明文档已在Github上发布，地址为：https://github.com/HorizonAD/stereo_dso

【作者简介】

吴佳田 | 地平线智能驾驶实习生

本科毕业于南京理工大学，研究生就读于卡内基梅隆大学机器人技术专业，主要研究方向为机器人建图定位。2017年在地平线自动驾驶组实习。

颜沁睿 | 地平线智能驾驶算法工程师

比利时荷语鲁汶大学人工智能硕士，电子工程学士（GroupT）。电子科技大学信息显示与光电技术学士。长期致力于研究人工智能技术在机器人领域的应用，包括计算机视觉、机器学习和SLAM。现担任地平线机器人公司智能驾驶部算法工程师。

杨德刚 | 地平线智能驾驶工程师

2014年硕士毕业于西北工业大学模式识别专业，随后作为初始成员加入百度深度学习研究院自动驾驶组，负责高精地图自动化和融合感知系统开发。2016年加入地平线，做过人脸识别、车牌识别、模型定点化等工作，现负责地平线自动驾驶地图和定位。

在自动驾驶地图定位中SLAM技术扮演重要角色，当前视觉SLAM一般有直接法和特征法，两种方法各有优劣。为了使相机能同时完成3D建图、定位、感知等任务，我们选择直接法进行了相应的应用实践，下面对相关内容做一总结。

一、关于直接法

视觉slam的几个关键元素：图像点、图像点特征、空间点、空间点特征、相机姿态，一般空间点和相机姿态由图像点优化得到，而空间点特征来源于图像点。

SLAM求解优化过程中主要优化点的特征和位置匹配代价，在特征法中点的特征匹配由特征描述子确定，求解只优化点特征匹配状态下的位置代价；直接法则是对点的特征（灰度）和位置同时进行优化，是更加end2end的方法，其优势在于避免了绝对特征匹配带来的局部最优问题，同时也省去了特征描述的开销。

因此直接法相对特征法的好处有：1. 速度快；2. 对点特征缺失图像更鲁棒；3. 姿态稳定连续；4. 场景重建较稠密。

直接法应用细节：

1、 只考虑梯度明显的像素点，不明显的点对目标函数贡献不大

2、 稀疏直接法可以将像素附近patch作为整体计算loss

3、 灰度不变假设比较严格，当不能稳定成立的时候需要光度标定

4、 整体目标函数是极度非凸的，需要仔细的初始化

二、DSO

DSO是Direct Sparse Odometry，是一种新的稀疏直接运动估计算法，其利用梯度强度进行像素选择，联合优化获得相机运动，这种方法不仅可以计算出相机的运动，而且可以得到场景的半稠密重建结果。

DSO的前端和LSD-SLAM相似，后端则抛弃了图优化的框架，借鉴了OKVIS中Sliding Window Filter的优化方式，可以取得更快的速度和更好的准确性，在实际应用中也发现DSO比LSD要鲁棒很多。这是因为DSO在联合优化的时候充分利用了相机的光度标定，比如曝光时间，gamma矫正等。同时, 后端优化时的一些trick如 First Estimate Jacobians、Schur Complement和大量的指令集优化使得DSO可以在cpu上达到实时效果。

三、直接法的应用

机器人智能系统需要准确的3D定位和感知，SLAM一般是作为定位的一类方法，比如Odometry进行相对运动的估计、Mapping重建场景准确地图等。过去特征法的输出只能用于定位，因为重建出来的场景过于稀疏，有意义场景物体结构已经不复存在，但直接法则不同，其输出的较稠密场景重建结果不仅可以用于地图构建，同时还可以用于3D感知，比如下图1，我们算法研究上较多的是左边的图像分割任务，自动驾驶需要的是右边的结果；如图2一般研究目标检测任务以左图为目标，而自动驾驶需要的是准确的3D结果，这时直接法重建的较稠密结果就有了额外的用武之地，即可以用于与2D感知结合得到准确的3D感知，比如根据连续几帧直接法输出的相对姿态和目标2D Bounding Box获得3D Bounding Box和速度等。

地平线自动驾驶团队在3D定位和感知方面做了一些尝试，比如DSO算法的单目版本没办法获取准确的物理尺度信息，在自动驾驶应用上有比较大的局限，因此我们对其进行了双目扩展。StereoDSO通过结合双目Stereo的约束到统一的优化框架中不仅可以得到高精度的姿态估计，解决单目DSO的尺度问题; 而且能够在系统跟丢的情况下快速初始化，使系统更加方便实用，最终整个方法能够实时获得鲁棒的相机姿态和场景重建结果。

移动机器人应用比如自动驾驶和服务机器人，其执行任务的基础是需要实时知道自己的位置和姿态，并需要算法在保证精度的前提下具有足够的鲁棒性，因为机器人都是在先验记忆的世界模型中进行运动操作的，世界模型与实时感知之间需要准确的物理尺寸进行对接，StereoDSO正是这样任务的很好的解决方法，首先没有尺度问题，即物理尺寸足够准确，其次本身运行中足够鲁棒，即使丢失也能够快速的重新初始化跟踪，保证机器人任务的连续性。

机器人执行任务除了定位还要实时避开障碍，这就要要求对场景有准确的3D感知，DSO可以在定位的同时获得半稠密的重建结果，结合基于图像的目标检测可以很容易的得到物体的3D BBox，也继而获得准确的目标速度朝向等，即本方法还可以同时完成感知属性重建的部分功能。

我们也将这个StereoDSO的代码实现放到了Github，地址是https://github.com/HorizonAD/stereo_dso，欢迎大家使用和交流。下图是DSO与StereoDSO在Kitti 05数据上的效果，左边是DSO和StereoDSO的对比，可以看到StereoDSO较好的解决了尺度问题，右图是StereoDSO与真值的比较，最终达到约1.1% ~ 4.2%的平移误差和0.001deg/m ~ 0.0053deg/m的旋转误差，更好的满足了自动驾驶所需的准确3D定位和感知要求。

下图是在我们自己的地下停车场数据上的结果，除了能够在这种比较具有挑战性的场景中进行姿态估计，也能够恢复稠密的场景结构，结合图像的2D感知结果即可以进行准确的场景3D建模，直接基于这一结果就可以对自动驾驶车进行决策规划，以完成指定任务。

在自动驾驶中，地面的交通元素是更加重要的，在StereoDSO得到的较稠密重建场景中，如果需要对地面交通元素进行更精确全面的感知建图，可以根据StereoDSO的位姿结果进行地面Mapping，如下图所示，在这样的图上我们可以自动获取或者人工编辑高精地图，以获得结构化的结果。

【参考】：

[1] Direct Sparse Odometry,J. Engel, V. Koltun, D. Cremers, InarXiv:1607.02565, 2016

[2] Large-scale direct SLAM withstereo cameras, J. Engel, J. Stückler, D.Cremers, IROS, 2015

[3] http://blog.csdn.net/Kevin_cc98/article/details/70920700

[4] http://www.cnblogs.com/gaoxiang12/p/5689927.html

[5] https://github.com/HorizonAD/stereo_dso