双目视惯雷达SLAM
点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
本文由知乎作者robot L授权转载，不得擅自二次转载。原文链接：https://zhuanlan.zhihu.com/p/114898412
主要内容
本文将Foster提出的紧耦合VIO（参考【论文阅读11】【论文阅读12】）和Ji Zhang提出的LOAM结合起来，并且加入了LiDAR增强的闭环模块。
IMU预积分+非结构化视觉因子的紧耦合双目VIO
基于LOAM的建图。主要改进，使用IMU频率的VIO输出用于点云去畸变。
LiDAR增强的闭环。在BoW回环检测和PnP回环约束构建之外增加ICP对齐。
由于本文集成了两种现有算法，因此每种算法的描述比较简略，中间有很多跳步，具体细节还要参考Foster和Ji Zhang的原论文。
W. Shao, S. Vijayarangan, C. Li, G. Kantor. Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping. IEEE/RSJ International Conference on Intelligent Robots and Systems, 2019.
摘要
SLAM是移动和空中机器人的基本任务。基于LiDAR的系统被证明优于视觉系统由于其准确性和鲁棒性。除了该优越性，纯LiDAR系统在特定的场景（如通过隧道时）会失效。Shao等人提出了一种双目视觉惯性LiDAR（VIL）SLAM，在退化的例子中表现更好，在其他例子中有相似表现。VIL-SLAM通过紧耦合双目视觉惯性里程计（VIO），LiDAR建图，以及LiDAR增强的视觉回环检测。该系统实时生成回环修正的6维LiDAR位姿和接近实时的1cm体素稠密地图。VIL-SLAM证实了相比于SOTA LiDAR方法提高的鲁棒和准确性表现。
1. 介绍
SLAM解决了建立未知环境地图同时估计机器人状态的问题。虽然SLAM在过去几十年里被积极地研究，Cadena等人[1]注意到在处理不同的环境和长期连续操作时仍存在挑战。SLAM系统在大量传感器模态中操作，每个尝试利用它们的优点。在过去几年里，基于LiDAR的SLAM系统比基于视觉的系统受到了更多的欢迎由于它们对环境中变化的鲁棒性。但是纯基于LiDAR的系统具有缺陷。它们在重复结构例如隧道或者走廊中失效。这些环境对建图和定位具有挑战性，使用所有传感器模态优势的系统需要被部署来前进。我们提出了VIL-SLAM，其使用IMU，双目相机和LiDAR，并同时利用了它们的优势。我们的实验证实了VIL-SLAM和纯LiDAR系统在大多数情形中表现类似，并且在纯LiDAR失效的情况下表现更好。VIL-SLAm达到这个通过整合双目VIO和具有回环的LiDAR建图。据我们所知，这是首个该类型的工作。此外，我们介绍了一种方法来评估建图结果使用一个ToF激光扫描仪（Faro）。我们还提供了VIO验证结果在EuRoc MAV数据集上。
VIL-SLAM使用一个紧耦合的双目VIO，其执行固定长度平滑图优化，使用稀疏3D特征用于地图配准的LiDAR建图，和融合稀疏点云对齐和视觉回环检测的闭环。回环检测优化一个全局位姿图使用一个增量求解器。VIL-SLAM被设计为长期和不同环境中鲁棒地操作。高频率IMU测量产生短期内合理但是长期容易漂移的估计。当使用双目视觉测量进行约束时，我们可以修正偏差并估计准确的相对运动（被称为VIO）。相对运动估计被用于辅助LiDAR扫描匹配，其然后累计高保真度的3D点云来形成一个准确的地图。机器人的状态估计在长期运动中累积误差。闭环解决了该问题通过识别重新访问的区域使用视觉或者LiDAR方法。视觉方法包括BoW[2]来识别地点和PnP算法来估计运动修正。在LiDAR方法中，场景被识别使用基于分割的方法如SegMatch[3]，并且位姿修正被估计使用ICP[4]算法。虽然BoW方法很快很通用，它缺乏很慢但是很鲁棒的使用ICP的LiDAR方法的准确性。VIL-SLAM使用一个混合方法，其中首先它使用BoW技术寻找闭环候选，使用PnP算法生成一个粗糙的位姿修正估计，然后使用ICP精修该粗糙估计。
图1. (a) 实验平台架构 （b）一个室外测试的建图结果。街道被清晰地重构。
2. 相关工作
当前的VIO文献介绍了不同的构架来整合视觉和惯性数据。文献将不同的方法分为紧耦合系统[5]-[7]，其中视觉信息和惯性测量被联合优化，或者松耦合系统[8]-[11]，其中IMU是一个分离的模块，并且和一个纯视觉的状态估计器融合到一起。该方法可能被进一步地划分为基于滤波的方法[11]-[16]或者基于图优化的方法[5]-[7][17][18]。紧耦合的优化方法，利用迭代地最小化残差的优势，经常达到更好的精度和鲁棒性，同时具有更高的计算代价。在我们的工作中，我们限制计算量通过以无结构化方式构建路标，并且只优化固定大小的位姿图来达到实时表现。
当前仅使用激光扫描仪的SOTA的SLAM系统是[19]-[23]，其中一个运动模型是需要的，或者常速度模型或者高斯过程。在[24]中的方法组合了双目相机和一个激光扫描仪。它具有来自VO的运动估计，并通过匹配激光帧来精修。不同于我们系统的是他们使用多分辨率栅格地图表示，我们使用稀疏点云来定位并输出稠密点云。同时，VIO经常更鲁棒和准确相比于VO[25]。VLOAM[26]，其使用一个IMU，一个单目相机和一个激光扫描仪，是与我们最相似的系统。一点不同是我们使用一个紧耦合的VIO作为运动模型来初始化LiDAR建图算法而VLOAM使用松耦合的IMU和相机。虽然我们的VIO更鲁棒，VLOAM具有更加交互的系统其中来自相机和LiDAR模块的信息可以被用于修正IMU偏置。VIL-SLAM具有的额外一项能力是LiDAR增强的闭环。
3. 系统综述
该系统具有四个模块如图2所示。视觉前端取来自双目相机的双目对。它执行帧到帧的跟踪和双目匹配，并且输出双目匹配作为视觉测量。双目VIO取双目匹配和IMU测量，执行IMU预积分和紧耦合的固定窗口平滑在一个位姿图中。该模块以IMU频率和相机频率输出VIO位姿。LiDAR建图模块使用来自VIO的运动估计并执行LiDAR点云去畸变和帧图配准。回环模块执行视觉回环检测和初始的回环约束估计，其进一步通过一个稀疏点云ICP对齐来精修。一个约束了所有LiDAR位姿的全局位姿图被增量优化来获得全局修正的轨迹和一个实时的LiDAR位姿修正。它们被返回LiDAR建图模块用于地图更新和重定位。在后处理中，我们使用最优估计的LiDAR位姿缝补去畸变的LiDAR帧来得到稠密点云结果（图5）。
图2. VIL-SLAM的系统示意图。传感器是灰色，模块是绿色。箭头表示信息在系统中如何流动。黑色加粗箭头表示系统的实时输出。灰色实时箭头表示由接近实时的后处理生成的输出。
4. 视觉前端
视觉前端接收一个双目对，并执行帧帧跟踪和双目匹配用于一组双目匹配的稀疏特征点的生成，即双目匹配。一个双目匹配可能从之前的双目对中跟踪得到或者从当前对中新提取得到。帧帧跟踪表现直接影响时序约束质量，同时双目匹配帮助限制尺度。这两个工作非常关键对于任何双目视觉里程计来说。直接方法展示了鲁棒和高效时序跟踪结果在最近几年[8][27]。因此，我们使用KLT特征跟踪器[28]来跟踪所有在之前的双目对中的特征点，或者在左或者在右相机中。只有当它们被同时跟踪时，我们有一个跟踪的双目匹配并且它被放入到输出中。宽双目极线有助于尺度估计并减少由较远特征导致的退化问题。我们使用基于特征的方法，其比KLT更适合解决宽基线情形。如果被跟踪的双目匹配的数量低于某阈值，我们使用Shi-Tomashi角点检测器[29]执行特征提取，之后进行特征排除过程，其中与已存在特征距离小于某阈值的像素坐标被删除。ORB[30]描述子被计算在所有的幸存特征中，之后是一个纯暴力双目匹配以获得新的双目匹配。这个系统通过在第一个双目对中执行双目匹配来初始化。
5. 双目视觉惯性里程计
双目VIO的目标是提供实时准确的状态估计以一个相对高的频率，作为lIDAR建图算法的运动模型。紧耦合的在一个位姿图上操作的固定长度平滑是精度和效率之间的一个好的权衡。基于优化的方法总的来说允许多次重新线性化来达到全局最小值。一个固定长度位姿图优化器进一步约束了变量的最大数量，因此计算代价被约束。因为差的视觉测量导致收敛问题，我们强加一个严格的外点剔除机制在视觉测量中。系统排出外点透过检测平均的重投影误差，同时在双目和时序中。
提出的VIO具有IMU预积分因子和非结构化视觉因子作为约束。图表示展示在图3中。被优化的变量是窗口内的状态。表示 为
时刻的状态变量。
包含6个自由度系统位姿
（IMU坐标系），相关的线性速度
，加速度偏置
和陀螺仪偏置
。被估计的状态变量的窗口是最近的
个双目帧。过去的状态变量被边缘化，在相关的变量上产生先验因子。
图3. VIO中的固定长度位姿图构架。被优化的状态变量是圆圈，其中i表示当前状态，N是窗口大小。(b)在边缘化后，相关变量的先验因子被加入。
A. IMU预积分因子
我们遵从IMU预积分方法[31][32]来生成 和
之间的相对IMU测量。使用预积分技术，重新线性化能够在优化过程中被高效地执行。由IMU预积分因子表示的残差是
，其由3项组成：位姿残差
，速度残差
和偏置
。
B. 非结构化视觉因子
视觉测量以非结构化的形式被建模，类似[31][33][34]。考虑一个路标 ，它在全局坐标系中的位置是
，被多个状态观测到，表示观测到
的状态集合为
。对于
中的任何状态
，表示通过在左图片中测量
构建的残差为
（
是左相机位姿，被获得通过应用一个IMU-相机变换到
）：
(1)
其中 是
在图片中的像素测量，同时
编码了视角投影。相同的构架被推导用于右相机图片。迭代方法被采用来优化位姿图，并且因此上述残差的线性化被获得。等式(2)展示了路标
的线性化残差。
(2)
其中雅克比 和残差
是来自线性化和通过视觉测量协方差
归一化后的结果。堆叠每个独立分量到一个矩阵，我们有
(3)
为了避免在 上进行优化，我们投影残差到
的零空间：使用
左乘每一项，一个
的正交投影[31]。我们然后有了非结构化视觉因子，对于路标
为
(4)
C. 优化和边缘化
给定残差，位姿图优化是一个最大后验（MAP）问题其最优解为
(5)
其中 是一组窗口内的状态变量。
和
是先验因子和它们相关的协方差。
是IMU测量的协方差。我们使用LM优化器来解决非线性优化问题。最近的
个状态变量被维护在优化器中。舒尔补边缘化[35]被执行在离开窗口的状态变量上。先验因子然后被加入到窗口内的相关的变量中，如图3(b)所示。
6. 激光建图
激光建图使用高频率IMU的VIO位姿作为运动先验来执行LiDAR点云去畸变和帧图配准。表示一帧 为从一个完整LiDAR旋转中得到的点云。包含边缘和平面上点的几何特征从
中被提取在去畸变之前[22][26]。配准然后基于当前帧到地图的特征点（所有之前的特征点），作为一个最小化特征点的欧式距离残差优化问题被解决[22]。
A. LiDAR扫描去畸变
去畸变是需要的当来自一个LiDAR帧的点的时间戳不同时。表示一帧内的任意时刻为 。我们去畸变所有的点到帧的结束时刻
基于IMU频率的VIO位姿。表示一个
时刻的LiDAR点为
，并且去畸变的点为
，我们有
(6)
其中 是通过最近的IMU频率的VIO位姿变换得到的LiDAR帧位姿。
B. 帧到地图配准
来自去畸变帧 的特征点被配准到地图上，用于优化
时刻的LiDAR建图位姿
。表示
的初始估计为
，我们有：
(7)
其中 是之前被优化的LiDAR建图位姿，
是基于IMU频率的VIO位姿获得的相对变换。所有去畸变的特征点然后被变换到世界坐标系中通过
用于配准。
当前帧中边缘特征点的残差 是它自身到地图中由两个最近边缘点行程的线的欧式距离。当前帧表面点的残差
是它自身到地图中最近的三个表面点形成的平面块之间的距离[22]。组合
，我们可以重写上述两个残差为：
(8)
(9)
其中 和
是LiDAR坐标系中的第
个去畸变特征点的3D位置。LM优化器被用于解决上述非线性优化问题，该问题通过堆叠所有特征点的代价函数构成。
7. LiDAR增强的闭环
闭环是关键的对于任何SLAM系统由于长期操作进入误差。闭环的目标是排除漂移通过执行一个全局位姿图优化，其整合了回环约束和来自LiDAR建图的相对变换信息。为了更好地支持LiDAR建图，修正的LiDAR位姿被实时地发送回来以至于来自新帧的特征点被配准到重遍历的地图。我们提出在BoW[2]回环检测和PnP回环约束构建之外增加ICP对齐。系统使用iSAM2[26]，一个增量的求解器用于优化全局位姿图，达到实时表现。
A. 回环检测
双目图片和LiDAR帧被关联使用它们的时间戳。让我们分别表示这些为关键图片和关键帧。为了避免虚假的回环检测，我们限制候选在一定时间阈值内。回环候选被检测通过使用之前图片构建的BoW[2]数据库来测试关键图片。此外，我们匹配左图片的特征描述子和回环候选来滤除假阳性。
B. 回环约束
系统首先获得视觉回环约束为一个初始估计。因为我们使用非结构化构架用于视觉路标，在回环候选中的所有双目匹配的特征中的三角化被执行来获得他们的3D位置。它们和当前关键图片的关联被给定通过描述子匹配。视觉回环约约束然后被计算使用EPNP[37]。为了提高视觉回环约束的准确性，我们在对应LiDAR关键帧的特征点上使用ICP对齐。有了一个坏的初始化或者大的点数，ICP消耗更长时间来收敛并消耗更多的时间资源。但是，视觉回环约束提供一个好的初始化点并且ICP只使用稀疏特征点（第6节），其使得它收敛的更快。
C. 全局位姿图优化
全局位姿图的图表示被展示在图4中。它包含了所有可用的LiDAR建图位姿作为变量，由LiDAR里程计因子和回环约束因子组成，同时是相对变换的测量 其中
和
分别表示帧ID，并且
是对应的位姿。对于LiDAR里程计因子，
是之前帧ID。对于回环约束因子，
是回环的关键帧ID。对于两种情形，
都是当前帧ID。位姿在优化中被表示为6自由度形式。为了实现实时表现，我们使用iSAM2[36]来增量地优化全局位姿图。
图4. 全局位姿图由LiDAR里程计因子和回环约束因子组成。i表示当前帧。
D. 重定位
一旦真实的闭环候选被发现，LiDAR建图缓存特征点（没有配准它们到地图）直到它接收到一个闭环修正。闭环修正包含全局优化的位姿。LiDAR建图更新它的地图，增加缓存的特征点到地图中然后恢复它的操作。我们可以保证实时更新地图因为（a）闭环具有实时表现（b）稀疏特征地图没有消耗太多内存（c）帧图配准是足够快的来追上LiDAR数据频率。
上述内容，如有侵犯版权，请联系作者，会自行删文。
推荐阅读：
那些精贵的3D视觉系统学习资源总结（附书籍、网址与****）
机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划
重磅！3DCVer-知识星球和学术交流群已成立
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导，800+的星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
欢迎加入我们公众号读者群一起和同行交流，目前有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加群或投稿