基于局内点分布的图优化方法

翁秀玲,王云峰,吴　炜,郭东辉

(厦门大学信息科学与技术学院,福建 厦门 361005)

图优化是即时定位与地图构建(simultaneous localization and mapping,SLAM)[1]的后端．它利用序列图像的闭环对前端视觉里程计获得的相机轨迹进行优化,对消除位姿估计的累积误差和构建一致性的地图至关重要．

图优化问题通常被归为非线性最小二乘问题,采用高斯-牛顿法(Gauss-Newton,GN)或列文伯格-马夸尔特法(Levenberg-Marquardt,LM)进行求解．其求解思路为在当前解处对目标函数进行如一阶泰勒展开近似的线性化操作,从而得到线性最小二乘方程．求解线性系统只需令导函数为零,通过迭代直至达到最大迭代次数或收敛．在迭代求解的问题上,研究者发现SLAM问题的稀疏性可以被充分利用,从而提高求解效率．Dellaert等[2]通过稀疏乔列斯基分解对线性系统进行求解．Kaess等[3]对系统的信息矩阵作正交三角分解,并选择性地对其进行增量式更新,从而避免了每次重新计算系统的信息矩阵,提高了求解效率．Konolige等[4]提出了一种根据给定图的约束关系快速构造稀疏矩阵的方法．Kummerle等[5]公开了基于稀疏矩阵分解的通用图优化库G2O,得到了广泛的应用．但迭代求解问题[2-5]对初始值较敏感,易落入局部极值;并且求解效率与稀疏矩阵的稀疏程度相关,最佳情况下时间复杂度为O(n),最坏情况下时间复杂度为O(n3),无法保证实时优化,不适合在线应用．

针对非线性最小二乘问题迭代求解存在的效率和初值敏感问题,Dubbelman等[6-7]提出COP-SLAM方法(closed-form online pose-chain SLAM,COP-SLAM),可以实现实时优化．该方法根据视觉里程计约束的信息矩阵设置位姿链中边的权重,从而将单个闭环约束的误差项分配到视觉里程计的总约束上，直接得到解析解．在保证位姿链优化精度的情况下,大大地缩短了优化所消耗的时间．该方法基于局内点数量设置视觉里程计约束的信息矩阵,但是当局内点分布不均匀即集中位于图像的特定区域时,视觉里程计精度下降,故该信息矩阵不能很好地衡量视觉里程计的精度,进而影响优化精度．

为了进一步提高COP-SLAM方法的优化结果,本研究提出了基于局内点分布的后端图优化方法．定义局内点集的面积表示局内点的分布,并通过局内点分布结合局内点数量设置位姿图中约束边的信息矩阵,提高了信息矩阵对视觉里程计精度的衡量准确度．实验结果表明,该优化方法有效地降低了绝对轨迹误差．

1　COP-SLAM[7]

COP-SLAM[7]属于轻量型的图优化算法,是根据单个闭环约束对环路内的位姿节点进行优化．算法采用位姿链模型,设相机的位姿用4×4的矩阵Ai表示,连续位姿Ai-1和Ai之间的视觉里程计约束用4×4的变换矩阵Mi表示,则n时刻的位姿可由式(1)表示．式中相机的初始位姿A0一般设为4×4的单位矩阵I4×4．

(1)

图1　COP-SLAM的优化示意图Fig.1 Optimization diagram of COP-SLAM

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

Mi←Mi×Ui.

(10)

(11)

2　基于局内点分布的图优化方法

2.1　基于特征点分布的信息矩阵

视觉里程计的精度不仅受局内特征点数目影响,还受局内特征点分布影响,因此本研究根据局内特征点的分布和数目设置信息矩阵．通过局内特征点之间的面积来表示局内特征点的分布,在局内点数量的基础上引入局内点集的面积,提高了对视觉里程计精度的衡量．

COP-SLAM一般根据图像特征局内点数量N按式(12)对信息矩阵的σ2进行设置,当局内点对分布不均匀,即局内点集中分布于图像的特定区域时,视觉里程计精度下降,该σ2构成的信息矩阵则不能很好地衡量视觉里程计的精确程度,影响优化效果．

σ2=1/N.

(12)

对于2帧图像的视觉里程计结果的准确度可由平移误差ε[8]衡量,

(13)

图2　KITTI00匹配特征点Fig.2 Matched feature points of KITTI00

以KITTI数据集[10]为例,采用开源的双目视觉里程计libviso2[11]对相机位姿进行估计．图2为双目数据集KITTI00中2对连续图像,图2(a)和(b)所示的圆点为连续对图像根据斑点和角点算子检测出来并经过匹配的特征点,匹配特征点集中分布在图像中上方,通过随机抽样一致性算法 (random sample consensus,RANSAC)[12]进行位姿求解得到局内点数目为128,根据式(13)得到视觉里程计的平移误差为0.037 m．图2(c)和(d)为另一对分布更加均匀匹配特征点的连续图像对,其局内点数目为125,根据式(13)得到视觉里程计的平移误差为0.011 m．由此可见,尽管图2(a)和(b)中图像设置的σ2小,但里程计的准确度更差．故基于局内点数量得到的信息矩阵并不能很好地衡量视觉里程计的精确程度．

在局内点数量的基础上,本研究考虑局内点占据的平均面积并对信息矩阵进行设置．双目视觉里程计libviso2[11]首先通过斑点和角点算子对连续2幅图像提取特征点并进行匹配,获得当前图像匹配点PMatch,再通过RANSAC[12]对PMatch进行位姿估计,得到局内点集合PIn,如下所示:

PIn={P1,P2,…，PN},

(14)

其中,Pi的坐标为(xi,yi)．

(15)

最后按式(16)设置信息矩阵唯一的参数σ2．

(16)

2.2　基于局内点分布的图优化

基于局内点分布的图优化算法通过闭环检测获得构成闭环的两个位姿节点,采用视觉里程计得到2个位姿节点的变换矩阵,通过式(16)基于局内点的平均分布面积计算该变换矩阵对应的信息矩阵中的σ2．如果2个位姿节点为连续节点,则通过式(1)根据Mi计算最后一个位姿节点的绝对位姿;如果2个位姿节点为非连续节点,则根据Mm,n优化环路内位姿节点．

(17)

其中,R为3×3阶的旋转矩阵,t为3×1阶的平移向量．

由式(7)得到的更新量Ui也可以表示成如式(18)所示的分块矩阵．

(18)

其中,UR为3×3阶的旋转矩阵,Ut为3×1阶的平移向量．

3　实验结果与分析

图像数据集KITTI[10]是双目SLAM系统研究应用最多的图像集,包含4组图像数据集的详细信息,如表1所示．在此图像集合上通过双目视觉里程计libviso2[11]和闭环检测算法[14]实现了基于局内点分布的图优化,最终输出相机运动轨迹,如图3所示．

表1　数据集的详细信息

轨迹的度量标准是绝对轨迹误差(absolute trajectory error,ATE)[15],其计算式如式(19)所示,是对估计轨迹与真实轨迹之间的绝对整体偏差进行度量．

(19)

其中,tOi表示真实位姿的平移向量,tei表示估计位姿的平移向量．

以图2为例,以局内点数目作为信息矩阵的设置方法,优化后图2(a)和(b)的绝对位姿的平移误差分别为3.425和3.426 m,图2(c)和(d)的绝对位姿的平移误差分别为2.231和2.281 m;而以局内点数目结合分布设置信息矩阵,则得到图2(a)和(b)的绝对位姿的平移误差分别为3.268和3.272 m,图2(c)和(d)的绝对位姿的平移误差分别为1.977和2.026 m．因此,基于局内点分布的图优化方法优化效果好于COP-SLAM．

视觉里程计得到的相机运动轨迹、相机的真实轨迹以及本研究优化后得到的相机运动轨迹如图4所示．可以看出,基于局内点分布的图优化得到的运动轨迹更加接近于真实轨迹,取得了优化效果．

基于局内点分布的SLAM后端图优化算法所需时间如表2所示,在KITTI数据集上的平均优化时间约为G2O的1/10,与COP-SLAM相当,故仍属于轻量型后端图优化算法,可实现实时优化．

本研究所提出的图优化算法与其他一些优化算法性能比较如表3所示,优化结果不如LSD-SLAM[16],原因在于非线性迭代优化具有更好的优化效果,但该优化运行速度仅为本研究的1/50[7]．在所有数据集上绝对轨迹误差均比Frost[17]的小,因为Frost采用单目视觉里程计估计相机运动,具有尺度不确定性,而本研究采用双目视觉里程计,无此问题．与同样属于轻量型优化算法的COP-SLAM[7]相比,由于提高了对视觉里程计精度的衡量,因此4组图像的优化效果均有提升．

图4　本研究优化结果Fig.4 Optimization result of this research

数据集时间/msG2OCOP-SLAM本研究KITTI0025030.431KITTI0229614.715KITTI0512514.715KITTI066214.715

表3　KITTI的绝对轨迹误差

4　结　论

图优化作为SLAM的重要环节,对减小移动相机的累积误差,实现地图的一致性至关重要．为了提高优化效果,本研究提出了基于局内点分布的后端图优化方法．在计算信息矩阵时,通过局内特征点之间的面积来表示局内特征点的分布,在局内点数量的基础上引入局内点集的面积,提高了信息矩阵对视觉里程计精确度的衡量度．在标准双目图像数据集上进行了实验,结果表明,提出的优化方法有效地降低了绝对轨迹误差．

参考文献：

[1] CADENA C,CARLONE L,CARRILLO H,et al.Past,present,and future of simultaneous localization and mapping:towards the robust-perception age[J].IEEE Transactions on Robotics,2016,32(6):1309-1332.

[2]DELLAERT F,KAESS M.Square root SAM:simultaneous localization and mapping via square root information smoothing[J].International Journal of Robotics Research,2006,25(12):1181-1203.

[3]KAESS M,RANGANATHAN A,DELLAERT F.ISAM:incremental smoothing and mapping[J].IEEE Transactions on Robotics,2008,24(6):1365-1378.

[4]KONOLIGE K,GRISETTI G,KUMMERLE R,et al.Efficient sparse pose adjustment for 2D mapping[C]∥IEEE International Conference on Intelligent Robots and Systems.Taiwan:IEEE,2010:22-29.

[5]KUMMERLE R,GRISETTI G,STRASDAT H,et al.G2O:a general framework for graph optimization[C]∥IEEE International Conference on Robotics and Automation.Piscataway:IEEE,2011:3607-3613.

[6]DUBBELMAN G,BROWNING B.Closed-form online pose-chain SLAM[C]∥IEEE International Conference on Robotics and Automation.Karlsruhe:IEEE,2013:5190-5197.

[7]DUBBELMAN G,BROWNING B.COP-SLAM:closed-form online pose-chain optimization for visual SLAM[J].IEEE Transactions on Robotics,2015,31(5):1194-1213.

[8]SUNDERHAUF N.Robust optimization for simultaneous localization and mapping[D].Chemnitz:Chemnitz University of Technology,2012:13-193.

[9]THRUN S,LIU Y,KOLLER D,et al.Simultaneous localization and mapping with sparse extended information filters[J].The International Journal of Robotics Research,2004,23(7/8):693-716.

[10]GEIGER A,LENZ P,STILLER C,et al.Vision meets robotics:the KITTI dataset[J].International Journal of Robotics Research,2013,32(11):1231-1237.

[11]GEIGER A,ZIEGLER J,STILLER C.StereoScan:dense 3d reconstruction in real-time[C]∥IEEE Intelligent Vehicles Symposium.Baden-Baden:IEEE,2011:963-968.

[12]HAST A,NYSJÖ J,MARCHETTI A.Optimal RANSAC：towards a repeatable algorithm for finding the optimal set[J].Journal of Wscg,2013,21(1):21-30.

[13]BARBER C B,DOBKIN D P,HUHDANPAA H.The quickhull algorithm for convex hulls[J].Acm Transactions on Mathematical Software,1998,22(4):469-483.

[14]LABBE M,MICHAUD F.Appearance-based loop closure detection for online large-scale and long-term operation[J].IEEE Transactions on Robotics,2013,29(3):734-745.

[15]STURM J,ENGELHARD N,ENDRES F,et al.A benchmark for the evaluation of RGB-D SLAM systems[C]∥IEEE International Conference on Intelligent Robots and Systems.Algarve:IEEE,2012:573-580.

[16]ENGEL J,STÜCKLER J,CREMERS D.Large-scale direct SLAM with stereo cameras[C]∥IEEE International Conference on Intelligent Robots and Systems.Hamburg:IEEE,2015:1935-1942.