基于点线特征的单目同步定位与建图初始化方法
下载: 1209次
ing at the problem of poor robustness and accuracy in the initialization of monocular visual simultaneous localization and mapping (SLAM), this paper proposes a robust initialization method based on point and line features. First, the line features are extracted and matched between two frames. Then, the initial rotation matrix and translation matrix between the two frames are optimized by maximizing the overlap length of the projected line features. Finally, a sliding window is used to increase the number of initial image frames, and the initial map and the estimated keyframe pose are optimized based on information and constraint of multi-frame images and the global bundle adjustment method. The test results on the TUM and OpenLORIS datasets show that compared with traditional initialization methods, the method is more robust and accurate, and can quickly complete high-precision initialization in challenging scenarios.
1 引言
视觉同步定位与建图(SLAM)是计算机视觉领域的重要研究方向,广泛应用于无人驾驶、机器人以及增强现实(AR)/虚拟现实(VR)等领域。单目SLAM的成本低、应用场景广泛,但存在尺度不确定以及漂移误差累积等缺点。此外,相比多目SLAM,单目SLAM的初始化难度更大,容易发生初始化失败或初始化精度不高的情况。初始化是单目SLAM中的重要步骤,主要用于设定初始尺度和创建初始地图,而SLAM系统的性能与初始化的精度及鲁棒性息息相关。早期的视觉SLAM系统都是基于扩展的卡尔曼滤波进行初始化,初始化三维地图点时的不确定性较大,需要在后续输入帧中持续更新地图点[1-3]。基于优化的方法则是从增量运动恢复结构(SFM)中恢复位姿并完成SLAM的初始化[4-5]。直接法SLAM系统初始化时基于灰度不变假设,通过最小化光度误差计算初始两帧图像的位姿,如大尺度直接法单目SLAM(LSD-SLAM)[6],但该方法在复杂光照情况下不稳定且图像序列需要经过光度校准[7-8]。非直接法SLAM初始化使用序列的前两帧图像进行初始化,在两帧图像之间提取特征并进行匹配。一些视觉里程计算法在初始化时假设观察到的是一个平面场景,然后通过单应性矩阵初始化位姿[9];另一些算法则使用本质矩阵计算初始位姿,通用性较强。如ORB(Oriented FAST and rotated brief)-SLAM[10]系统用本质矩阵E和单应性矩阵H混合的方法计算位姿,但只有模型的不确定性很低时才能重建成功。该方法对图像帧的要求较高,如果找不到两帧能够有效重建地图的三维点,SLAM系统就无法运行,且SLAM系统初始化的时间较长。
Tang等[11]提出了用秩1矩阵分解法进行初始化的G-SLAM系统,该系统计算的初始地图不涉及模型选择问题,且能同时优化相机位姿和地图点,保证了系统在线性时间内能够快速收敛,但能被秩1矩阵分解法分解的特征必须被所有帧观察到。可用KLT(Kanade-Lucas-Tomasi)光流法进行特征跟踪,但算法的性能表现非常依赖KLT光流法的跟踪精度。Lee等[12-14]借鉴G-SLAM的思想,基于极线约束实现了所有特征在全部帧中的跟踪,并提出了有效计算几何插值基本矩阵的方法。虽然该方法能在全部帧中得到所有特征的像素坐标,但插值法得到的坐标不能提供额外的信息,即对于位姿约束不会有额外贡献。光束平差(BA)法可对关键帧进行优化,通常应用于SLAM系统的后端优化模块中。对地图进行全局BA优化时通常使用位姿图,将优化问题转换成非线性最小二乘问题,从而用g2o(General graph optimization)[15]或Ceres[16]库迭代求解最小化线性目标函数。目前,基于点线特征的SLAM系统在初始化时只使用点特征。但在一些弱纹理结构化的场景中,线特征的数量较多,如果只用点特征计算本质矩阵和单应性矩阵,会导致匹配特征的数量过少、初始化失败。Zhang[17]基于两个视角观察到的线匹配对估计两帧图像之间的相对位姿,在点特征较少的弱纹理环境中大大增加了初始化方法的鲁棒性。
本文提出了一种基于点线特征的单目SLAM多帧图像初始化方法,能在低视差、结构化弱纹理和近似纯旋转等场景中完成初始化。通过一个滑动窗口,用多帧图像的信息完成初始化,并在初始化时用线特征优化旋转矩阵R和平移矩阵t。为了使构建的初始地图更精准,在滑动窗口中用BA法优化位姿以及地图的点、线坐标。在TUM和OpenLORIS数据集上的测试结果表明,相比ORB-SLAM和LSD-SLAM系统的初始化方法,基于点线特征的多帧初始化方法能提高SLAM系统初始化的鲁棒性及精度。
2 基本原理
2.1 基于线特征优化帧间的初始位姿
假设两帧图像中有一对匹配的线段,由点的对极几何可知,三维空间中的点P在第一帧图像中的像素点p1在第二帧图像中对应一条极线l2,如
空间中的一条直线在图像中的像也是一条直线,虽然在第二帧图像中没有检测到s1和e1对应的端点s'1和e'1,但s'1和e'1肯定在以s2和e2为端点连成的直线
式中,s'1和e'1为第一帧图像中检测线段在第二帧图像中投影线段的起点和终点像素坐标,s2和e2为第二帧图像中检测线段的起点和终点像素坐标。
满足约束条件时,第二帧图像中的线段
若第二帧图像中的线段
根据图像的对称性原理,可通过第二帧图像中检测的线段端点计算其在第一帧图像中的投影,得到L。由于长线段和短线段的重叠长度在约束两帧图像之间的位姿时应具有相同的权重,因此对L和L'进行归一化,分别除以检测线段长度l和l',得到优化重叠长度和检测线段长度的比值,对于
文献[ 17]中用下降单纯形法[18]解决该优化问题,而在SLAM中,由于初始化时两帧图像之间根据点特征可计算出本质矩阵E和单应性矩阵H。因此,将E或H作为待优化变量,利用Ceres库[16]最小化函数F,然后将优化后的E或H分解为(R,t)。
2.2 多帧图像的初始化
针对传统初始化方法在部分场景(如近似纯旋转、移动物体干扰较多、小视差场景)下求解误差大的问题,提出了一种基于BA法优化的多帧图像初始化方法。用滑动窗口将用于构建初始地图的关键帧数量增加至N帧。当输入新图像后,将其与滑动窗口内的第一帧(参考帧)图像进行特征匹配,若匹配的特征数量超过设定的阈值σ,则将其加入滑动窗口,并与其他所有帧进行特征匹配。每一帧与第一帧之间都通过点对计算本质矩阵和单应性矩阵,并用2.1节中的线特征优化方法得到(R,t)。然后,重构地图点与地图线的坐标,记录所有关键帧和特征信息,作为初始地图。若输入帧与第一帧图像的匹配特征数量少于σ,则舍弃该帧图像。若连续六帧图像都被舍弃,则表明目前滑动窗口内关键帧包含的信息不够多,舍弃所有关键帧,重新初始化滑动窗口。当滑动窗口内的有效帧数为N时,对整个滑动窗口内的关键帧和地图点进行位姿图优化。
以N=3为例,首先,在每两帧图像之间用ORB特征点进行匹配,并用五点法或八点法粗略计算出帧间的本质矩阵和单应性矩阵。然后,基于LSD[19]提取线特征和线段二进制描述符(LBD)[20]进行线特征匹配,并用线特征优化位姿。最后,重构出特征点和特征线的三维坐标,并将所有三维坐标在以第一帧相机光心为原点的坐标系下进行表示。根据关键帧和三维地图点、线之间的共视关系,将地图点和地图线分为四类,如
所有关键帧的位姿和三维点坐标、线坐标都在第一帧图像坐标系下(上标(0)表示以第一帧相机的光心作为原点),对所有关键帧位姿和地图点坐标进行全局BA优化,优化的状态向量可表示为
式中,

图 6. 重投影误差示意图。(a)点特征;(b)线特征
Fig. 6. Schematic diagram of the reprojection error. (a) Point feature; (b) line feature
对于某个地图点坐标Pj=[Xj,Yj,Zj]T,其在某一关键帧的投影像素坐标
式中,sj为成像平面的深度,K为相机的内参矩阵,ξ∧为地图点坐标所在坐标系到该帧相机坐标系转换矩阵的李代数。可以发现,点特征的重投影误差ep=uj-
式中,pl、ql为检测线段两个端点的像素坐标,l'为投影直线的法向量,l1、l2为法向量l'的第一、第二参数。则整体优化方程可表示为
式中,
3 实验结果及分析
3.1 实验数据集
TUM数据集[21]包含RGB(Red,Green,Blue)深度(RGBD)图像、单目图像、视觉惯性(VI)图像等不同类型的数据集,实验使用的是RGBD图像数据集。TUM-RGBD数据集主要包括室内场景图像,既有纹理丰富的办公室场景,也有弱纹理或结构化信息较少的场景,有些序列中还存在大量的动态物体,因此在TUM-RGBD数据集上进行测试对SLAM系统的鲁棒性提出了很高的要求。
OpenLORIS数据集[22]中的图像全部采集自真实生活环境,分为物体(Object)数据和场景(Scene)数据,可用于物体识别和SLAM系统的评估。OpenLORIS-Scene数据集全部由真实机器人录制,包含办公室、走廊、家庭、咖啡厅和超市五类场景,彩色图像、深度图像、双目鱼眼图像、惯导(IMU)和里程计五类数据以及由动作捕捉系统或高精度激光雷达提供的机器人轨迹真值。相比其他SLAM数据集,OpenLORIS数据集最大的特点在于场景重入,即同一场景会有多段数据,录制间隔从半天到几周不等,包含了日常生活导致的场景变动。
3.2 基于线特征的初始位姿估计结果
为了验证用两帧图像匹配的线特征优化初始位姿的效果,在TUM数据集上进行测试,测试环境为Intel Core i5-8600K CPU@3.60 GHz×6。以单目ORB-SLAM为基础,在其初始化模块加入线特征优化方法,并将其和原始单目ORB-SLAM进行对比。实验中用LSD提取200根线特征,用ORB提取300个点特征,且线特征只用在初始化模块中。
表 1. 线特征优化前后系统的定位误差
Table 1. Positioning of the system before and after line feature optimizationunit: cm
|
3.3 基于点特征的初始化结果
为了验证多帧初始化方法的有效性,将多帧初始化方法加入ORB-SLAM系统中,并与原始ORB-SLAM系统和LSD-SLAM系统在TUM-RGBD数据集上进行了测试实验,实验中只采用了点特征,目的是探究基于滑动窗口的多帧初始化方法对系统初始化性能的影响。ORB-SLAM系统采用传统初始化方法,即通过两帧图像的信息计算本质矩阵或单应性矩阵,进而恢复(R,t);LSD-SLAM系统利用直接法随机初始化地图。
表 2. 不同方法在TUM数据集上的ATE
Table 2. ATE of different algorithms on TUM-RGBD sequencesunit: cm
|

图 7. 两种方法在TUM数据集上的初始化结果。(a)本方法;(b)传统方法
Fig. 7. Initialization results of two methods on the TUM dataset. (a) Our method; (b) traditional method
3.4 融合点线特征的多帧图像初始化结果
将基于点线特征的多帧初始化SLAM系统和传统PL-SLAM(Point and line SLAM)、ORB-SLAM系统进行对比,其中,PL-SLAM系统为在ORB-SLAM系统上加入线特征,且线特征只用于跟踪及优化部分的SLAM系统(与初始化过程无关),也是目前常见的一种PL-SLAM框架。在TUM[21]和OpenLORIS数据集[22]上的测试结果如
表 3. 不同方法在TUM数据集上的ATE
Table 3. ATE of different methods on the TUM dataset unit: cm
|
表 4. 不同方法在OpenLORIS数据集上的ATE Table 4 ATE of different methods on the OpenLORIS datasetunit: cm
Table 4.
|

图 9. 两种方法在cafe2序列上的轨迹图。(a)本方法;(b)传统方法
Fig. 9. Trajectories of the two methods on the cafe2 sequence. (a) Our method; (b) traditional method

图 10. 两种方法在corridor1-1序列上的初始化结果。(a)本方法;(b)传统方法
Fig. 10. Initialization results of the two methods on the corridor1-1 sequence. (a) Our method; (b) traditional method
4 结论
提出了一种基于点线特征的多帧初始化方法,用一个滑动窗口选择多个关键帧进行初始化,并在BA法优化前用两帧图像之间的线特征优化初始位姿。对于两帧图像中的匹配线特征,用最大化重叠长度的方式提高两帧图像之间位姿初值的估计精度。在初始地图上进行全局BA优化时,通过最小化点特征和线特征的重投影误差对初始地图的位姿和点线特征三维坐标进行优化。在TUM数据集和OpenLORIS数据集上的测试结果表明,相比传统非直接法初始化方法以及直接法初始化方法,本方法的效果都更好。这证明了利用多帧图像的信息以及线特征优化初始位姿,可提高SLAM系统在初始化过程中的精度和鲁棒性,进而提高后续跟踪、建图的精度,且本方法对SLAM系统的实时性没有影响。后续还将针对滑动窗口内关键帧的选择算法进行优化,将基于点线特征的多帧初始化方法应用到其他SLAM框架中。
[1] Chiuso A, Favaro P, Jin H L, et al. Structure from motion causally integrated over time[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2002, 24(4): 523-535.
[2] Davison A J, Reid I D, Molton N D, et al. MonoSLAM: real-time single camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2007, 29(6): 1052-1067.
[3] Civera J, Davison A J. Inverse depth parametrization for monocular SLAM[J]. IEEE Transactions on Robotics, 2008, 24(5): 932-945.
[4] KleinG, MurrayD. Parallel tracking and mapping for small AR workspaces[C]∥2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality, November 13-16, 2007, Nara, Japan.New York: IEEE Press, 2007: 225- 234.
[5] RoyerE, BomJ, DhomeM, et al.Outdoor autonomous navigation using monocular vision[C]∥2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, August 2-6, 2005, Edmonton, AB, Canada.New York: IEEE Press, 2005: 1253- 1258.
[6] EngelJ, SchöpsT, CremersD. LSD-SLAM: large-scale direct monocular SLAM[M] ∥Fleet D, Pajdla T, Schiele B, et al. Computer vision-ECCV 2014. Lecture notes in computer science. Cham: Springer, 2014, 8690: 834- 849.
[7] Engel J, Koltun V, Cremers D. Direct sparse odometry[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2018, 40(3): 611-625.
[8] Bergmann P, Wang R, Cremers D. Online photometric calibration of auto exposure video for realtime visual odometry and SLAM[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 627-634.
[9] ForsterC, PizzoliM, ScaramuzzaD. SVO: fast semi-direct monocular visual odometry[C]∥2014 IEEE International Conference on Robotics and Automation (ICRA), May 31-June 7, 2014, Hong Kong, China. New York: IEEE Press, 2014: 15- 22.
[10] Mur-Artal R. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.
[11] Tang CZ, WangO, TanP. GSLAM: initialization-robust monocular visual SLAM via global structure-from-motion[C]∥2017 International Conference on 3D Vision (3DV), October 10-12, 2017, Qingdao, China. New York: IEEE Press, 2017: 155- 164.
[12] Lee SJ, Hwang SS. Elaborate monocular point and line SLAM with robust initialization[C]∥2019 IEEE/CVF International Conference on Computer Vision (ICCV), October 27-November 2, 2019, Seoul, Korea (South).New York: IEEE Press, 2019: 1121- 1129.
[13] HartleyR, ZissermanA. Multiple view geometry in computer vision[M]. Cambridge: Cambridge University Press, 2004.
[14] Goldstein A, Fattal R. Video stabilization using epipolar geometry[J]. ACM Transactions on Graphics, 2012, 31(5): 1-10.
[15] KümmerleR, GrisettiG, StrasdatH, et al.g2o: a general framework for graph optimization[C]∥2011 IEEE International Conference on Robotics and Automation, May 9-13, 2011, Shanghai, China. New York: IEEE Press, 2011: 3607- 3613.
[16] AgarwalS, MierleK. Ceres solver[EB\OL]. [2020-11-28].http: ∥www.ceres-solver.org/.
[17] Zhang Z Y. Estimating motion and structure from correspondences of line segments between two perspective images[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1995, 17(12): 1129-1139.
[18] Nelder J A, Mead R. A simplex method for function minimization[J]. The Computer Journal, 1965, 7(4): 308-313.
[19] von Gioi R G, Jakubowicz J, Morel J M, et al. LSD: a line segment detector[J]. Image Processing on Line, 2012, 2: 35-55.
[20] Zhang L L, Koch R. An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency[J]. Journal of Visual Communication and Image Representation, 2013, 24(7): 794-805.
[21] SturmJ, EngelhardN, EndresF, et al.A benchmark for the evaluation of RGB-D SLAM systems[C]∥2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, October 7-12, 2012, Vilamoura-Algarve, Portugal.New York: IEEE Press, 2012: 573- 580.
[22] ShiX, LiD, ZhaoP, et al.Are we ready for service robots? The OpenLORIS-scene datasets for lifelong SLAM[C]∥2020 IEEE International Conference on Robotics and Automation (ICRA), May 31-August 31, 2020, Paris, France.New York: IEEE Press, 2020: 3139- 3145.
Article Outline
钱琦, 白庭铭, 闭云峰, 乔程昱, 项志宇. 基于点线特征的单目同步定位与建图初始化方法[J]. 光学学报, 2021, 41(12): 1215002. Qi Qian, Tingming Bai, Yunfeng Bi, Chengyu Qiao, Zhiyu Xiang. Monocular Simultaneous Localization and Mapping Initialization Method Based on Point and Line Features[J]. Acta Optica Sinica, 2021, 41(12): 1215002.