基于拉普拉斯分布的双目视觉里程计

范涵奇 吴锦河

视觉里程计(Visual odometry,VO)是在未知环境中只通过相机获得的图像信息来实现机器人定位的技术.近些年来,VO 广泛应用于机器人导航[1]、无人机[2-3]和虚拟现实/增强现实[4]等领域.

视觉里程计使用单目或多目相机作为视觉传感器.单目相机为主要传感器的VO 系统虽然成本较低,但面临的主要问题是单目尺度的不确定性,在计算过程中会发生尺度漂移[5-6].该问题通常使用多传感器来解决,例如雷达和多目相机.相比于单目相机,双目相机可以直接测量三维空间点的位置,避免了尺度的不确定性,因此本文使用双目相机作为视觉传感器.

VO 系统分为前端和后端[7].前端通过特征点匹配构造数据关联来为后端优化提供初始位姿.数据关联(Data association)是指在帧与帧之间的特征点、特征点与地图点以及地图点与地图点之间构建特征对应关系[8].数据关联错误是VO 系统失败的主要原因之一.基于图像特征点的VO 系统数据关联方式主要分为两种.一种是通过计算特征点的描述子来构建数据关联.采用描述子匹配特征点的准确性与鲁棒性较高,但是特征描述子的计算非常耗时.Mur-Artal等[9]测试尺度不变特征变换(Scaleinvariant feature teansform,SIFT)[10]和加速健壮特征(Speed-up robust feature,SURF)[11]提取耗时约为300 ms,像这类比较耗时的特征提取算法会影响VO 系统的实时性.为了提高实时性,Mur-Artal等在ORB (Oriented fast and rotated brief)-SLAM(Simultaneous localization and mapping)[9]中选择计算速度较快的ORB 特征点作为图像特征,帧与帧之间特征点通过特征描述子匹配.Cvišić等[12]在SOFT (Stereo odometry based on feature tracking)中提取Corner 角点和Blob 角点同时计算特征描述子,并通过在连续帧中追踪同一特征点,如果该特征能被追踪到则使用初始的描述子来提高数据关联的精确性.由于相机帧率和图像分辨率越来越高,导致特征提取的计算量越来越大,即使使用ORB 这类速度较快的特征描述子也可能会影响VO 系统的实时性.另一种方式只提取角点而不计算描述子,角点之间的匹配关系通过稀疏的光流(Optical flow)跟踪来构建[13-14].稀疏的光流算法计算速度快,但光流容易导致特征点误匹配从而使得数据关联错误,因此本文使用光流来构建数据关联的同时采用其他技术尽可能剔除错误的数据关联.

VO 系统的后端优化前端估计的相机初始位姿,本文在后端只优化相机位姿而不维护一个全局地图.在优化过程中目标函数的设计影响着系统鲁棒性.在经典的Bundle adjustment (BA)和扩展的卡尔曼滤波(Extended Kalman filter,EKF)算法中都假设误差服从高斯分布,优化过程中对噪声敏感,因而导致位姿估计的误差较大.与高斯分布相比,拉普拉斯分布对大噪声不敏感.同时,对于长尾数据(Long tail data)来说,拉普拉斯分布比高斯分布更适合对大幅噪音的似然描述[15],从而对异常点数据更加鲁棒.

本文在VO 后端假设误差服从拉普拉斯分布,进而构造优化问题的目标函数.Casafranca等[16]在此假设下构造因子图优化问题.Bustos等[17]通过构造旋转的平均只优化相机朝向,在相机朝向已知的情况下得到全局最优的相机位置和三维空间点.该方法优化相机朝向时不受相机位置与三维空间点的影响因此更简单并且能够处理纯旋转的相机运动,但是该方法运算速度慢,并且在求解相机位置时由于同时优化三维空间点和相机位置,误差较大的三维点会影响相机位置的求解.与Casafranca等[16]和Bustos等[17]的方法不同,本文在后端分开优化求解相机朝向、三维空间点以及相机平移,在此过程中假设相机位姿与三维空间点的误差都服从拉普拉斯分布.SOFT 等多个不同的算法证实分开估计相机的朝向与位置可以提高相机位姿估计的精确性.

近些年来,基于直接法的双目视觉里程计越来越受研究者的欢迎,例如,Stereo DSO (Stereo direct sparse odometry)[18]、SVO2 (Semidirect visual odometry 2)[19]和FMD stereo SLAM (Fusing MVG (multiple view geometry)and direct SLAM)[20].Stereo DSO 算法通过采用梯度较大的稀疏像素,使用Bundle adjustment优化来得到精度较高的相机位姿.该算法速度较快、鲁棒性好,并且可以生成稠密的三维点云,但是对于场景的光照变化比较敏感.SVO2 扩展了半稠密直接法单目视觉里程计SVO[13],SVO2 算法只对选择的关键帧提取特征点并且采用稀疏图像对齐算法匹配特征点,因此SVO2 速度快适用于计算平台受限场合的定位.SVO 采用深度滤波器估计深度,由于初始深度估计具有较大误差在优化时可能无法收敛到真实深度,进而影响相机位姿估计.FMD stereo SLAM 方法融合了多视角几何和直接法,在前端采用直接法估计初始位姿而在后端采用多视角几何的方法来估计三维结构,这种直接法和多视角几何法融合的方法同时兼顾了速度与精度.与以上几种方法相比,本文算法采用特征点法并且引入了误差服从拉普拉斯分布的假设来优化相机位姿.

本文的组织结构如下:第1 节简要介绍本文算法的框架.第 2 节中详细阐述特征点的提取以及如何剔除错误的特征匹配并筛选稳定的特征点.第3节为相机位姿的估计与优化.第4 节中通过实验验证了本文算法的有效性.第5 节为本文的结论.

1 系统概述

本节主要从整体上概述本文提出的算法.算法主要由数据关联和相机位姿优化估计两部分组成.数据关联是相机位姿估计的预处理过程.在构造数据关联时,特征点的选择与数据关联的准确性影响着相机位姿估计的精度.在兼顾速度与精度的情况下如何选择稳定的特征点并剔除错误的关联是VO 算法重要的一步.位姿估计部分接收筛选的稳定特征点来优化求解相机位姿并重建出稀疏的环境地图.

1.1 特征选择与数据关联

为了提高VO 算法的实时性,需要尽可能快地提取每帧图像的特征点,因此本文选择FAST(Features from accelerated segment test)[21]角点作为图像特征点.数据关联主要通过稀疏光流算法来构建.虽然稀疏的光流计算速度快,但是往往会导致特征点的误匹配,因此本文采用三个策略,即平滑的运动约束、视差一致性检测以及环形匹配来尽可能剔除错误的关联,进而提高算法的鲁棒性,并在此基础上选择稳定的特征点.

1.2 相机位姿优化

本文算法的主要流程如图1 所示.其中,P表示三维空间点的位置,p表示三维空间点对应的在二维图像上的投影坐标,R,t分别表示相机的旋转和平移.下标l,r,k为左右相机和图像帧的索引.在相机位姿估计的过程中,通常对于相机朝向的优化估计是比较困难的,因此本文首先去优化估计相机的朝向以及三维空间点,然后固定已经求得的相机朝向和三维空间点来优化求解相机平移.在光流构建数据关联的过程中,同一特征点可以连续地在多帧中被跟踪.本文基于特征点能被连续跟踪的帧数选择不同的参考帧从而获得当前帧相机位姿的多个估计.对于当前帧的多个相机位姿估计,在相机位姿误差服从拉普拉斯分布的假设下构造优化问题的目标函数,进而得到位姿的最优估计.

2 图像特征匹配

光流是三维运动在二维图像平面上的投影.在使用光流构建数据关联时,需要剔除错误的关联以提高数据关联的精确性,并且需要在此基础上筛选稳定的特征点.如果一个特征点能在连续多帧中跟踪到,那么该特征点是稳定的.

一般而言,相机连续帧之间的运动是平滑的,即前后帧之间的运动量较小,那么图像帧Ik的运动与前一帧Ik-1的运动是高度相似的,这种现象称为平滑的运动约束(Smooth motion constraint,SMC).Badino等[22]将该思想应用到三维空间中来剔除三维点的误匹配,本文应用到二维的图像特征点上以提高光流跟踪的准确性.

由于当前帧的运动与前一帧是相似的,因此可以使用前一帧的运动去预测特征点在当前帧上的位置.即

式中,Pk,Pk-1分别代表同一组三维空间点在当前帧Ik和前一帧Ik-1视角下的位置表示,表示已经估计的前一帧的相对运动,即帧Ik-1相对于帧Ik-2的相对位姿变换,π(·)表示投影函数,将三维空间点投影到二维的图像上,pk为三维空间点Pk在图像帧Ik上的投影.

由于pk更接近于光流收敛的位置,因此本文使用pk作为光流跟踪过程中预测的前一帧图像特征点在当前帧上的初始位置,在特征跟踪过程中可以消除一定特征点的误匹配,从而提供更为可靠的特征点匹配信息.

在平滑的运动约束下单纯使用光流跟踪特征点依然存在误匹配,为了剔除误匹配增加特征点匹配的正确性,本文采用强约束环形匹配(Circle matching)[12].图2 中描述了环形匹配的整个过程.如图2 所示,假设在图像帧上检测到某一特征点x,在连续帧之间的左右图像上使用光流按照的顺序跟踪特征点x,跟踪结束时在帧上的特征点记为x′.本文通过计算特征点x与x′之间的距离d(x,x′)来判断该环能否闭合.如果距离d不大于一个像素,则认为该环能闭合,否则认为该特征点匹配出错,进而剔除误匹配的特征点.

图2 环形匹配 (使用光流依照箭头所示顺序跟踪特征点)Fig.2 Circular matching (Use optical flow to track feature points following the order as arrows direct)

双目相机系统中,视差是一个非常重要的量.在使用以上两种方法剔除特征点的误匹配后,为了进一步提高特征点匹配的精确性,通过检测双目相机的视差来再次剔除误匹配.本文在光流跟踪过程中只计算稀疏特征点之间的视差.对于已经校正好的左右相机的图像帧来说,左右匹配的特征点一定位于水平的极线(Epipolar line)上.从图像特征点的角度来看,假设 (xl,yl)与 (xr,yr)是左右相机上一对匹配的特征点,则视差的y分量应等于0.即:yl=yr,但实际上在图像校正与特征点跟踪过程中由于噪声的存在,等式不是严格成立的.因此,本文采用阈值thd来筛除掉大于阈值的匹配点.此外,本文以左边相机为参考系,则视差在x方向上的分量应该为正.本文使用视差一致性(Disparity consistency)来描述这种现象.即

其中,(xl,yl),(xr,yr)为左右图像上匹配的特征点.如图3 所示,使用特征选择策略后能明显地剔除误匹配,进而提高数据关联的精确性.

图3 特征点匹配的对比Fig.3 Comparison of feature matching

为了精确地估计相机位姿,本文尽可能选择稳定的特征点.特征点的稳定性通过该特征能被观测到的图像帧数来度量.一个特征点能在连续帧中被观测到的次数越多,那么该特征点越稳定.如图4所示,使用光流构建数据关联时同一特征点可以被连续跟踪多帧.SOFT 使用age来描述特征点的稳定性,本文同样采用age来表示一个特征点的稳定性.以往的几项研究[12,23]证实选择稳定的特征点可以提高相机位姿估计的精确性.

图4 光流特征跟踪与age说明(同一特征点可以在连续帧中被跟踪,age值越大该特征点越稳定,T表示两帧之间的位姿变换)Fig.4 Optical flow feature tracking andagedescription(The same feature point can be tracked in consecutive frames.The larger theage,the more stable the feature point,Trepresents the pose transformation between two frames)

3 相机位姿估计

本节主要描述了相机位姿估计与稀疏点云的重建.本文将位姿估计分为两部分.首先,根据帧与帧之间二维的匹配点直接估计相机的相对旋转;然后固定已知的相机旋转来估计平移.

3.1 拉普拉斯分布假设下目标函数的构造

一般而言,SLAM 可以表示为最大后验估计(Maximum a posteriori estimation,MAP)问题.假定要估计的未知变量为X,其中,X包括相机位姿和环境中路标点的位置.由观测方程zj=hj(Xj)+ϵj可得到一个观测集合Z={zj:j=1,2,···,m},其中,Xj⊆X是估计变量的子集,hj(·)为观测模型,ϵj为观测的噪音.

在MAP 估计中,通过最大化后验概率来估计变量X,即

其中,p(Z|X)为似然,p(X)为先验.在没有先验信息的情况下,p(X)是一个常量,MAP 退化为最大似然估计(Maximum likelihood estimation,MLE).假定观测的噪音互不相关,即观测Z相互独立,则式(3)分解为

假设观测的噪音ϵj服从具有信息矩阵 Ωj的零均值拉普拉斯分布,则式(4)中的似然为

由于最大后验估计等价于最小化后验的负对数,则有

由式(6)可知,在噪音服从拉普拉斯分布的假设下构造了以L1范数来度量误差的非线性优化问题.因此在相机位姿与三维点云优化时,本文在拉普拉斯分布的假设下采用L1范数来度量误差.

传统上在高斯分布假设下采用L2范数作为目标函数来优化估计相机位姿.尽管L2范数应用广泛,但是拉普拉斯分布假设下的L1范数提供了另一种选择.Moreno等[24]详细对比了L1范数和L2范数在不同噪音水平下的表现,证实了L1范数对较大的异常值更加鲁棒.

为了提高L2范数对较大误差的鲁棒性,通常结合鲁棒的核函数(例如Huber)来减小误差较大的数据对优化结果的影响.Casafranca等[25]在后端采用L1范数度量误差实现了因子图SLAM,并在实验中证实了L1范数在消除大噪音数据影响方面比Huber 核更加鲁棒.同时相比于Huber 核函数,L1范数在优化时并不需要调整任何内核参数.Bahreinian等[26]研究了在位姿图优化中通过最小化L1范数能有效剔除离群点的影响.综上,尽管与鲁棒Huber 核函数类似,但是L1范数在消除大噪音数据的影响方面仍然更加有效,即L1范数对大噪音数据优化的鲁棒性更好.

3.2 旋转的估计

不同于一般的双目VO 算法只使用左侧图像信息来估计相机位姿,为了尽可能多地估计当前帧的旋转,本文使用左右相机的图像信息来分别估计当前帧的旋转.

如图4 所示,本文根据当前帧上特征点的age值选择不同的参考帧来估计当前帧的相机朝向.对于首次提取的某一特征点的age值记为0,如果该特征点能跟踪到下一帧则age值加1,以此类推,age值越大说明该特征点能被连续跟踪的帧数越多.如果当前帧中某一特征点的age值为n,那么说明该特征点可以在前n帧中被跟踪到,因此本文选择前n个图像帧分别作为参考帧来估计当前帧相机的朝向.对于当前帧Ik相机朝向的n个不同估计,使用集合{1Rk,2Rk,···,nRk}来表示,其中Rk代表当前帧Ik相机的绝对朝向.为了得到相机朝向的最优估计,假设噪声服从拉普拉斯分布,则有

其中,Ri∈{1Rk,2Rk,···,n-1Rk,nRk},Sˆ 为相机旋转.通过采用SO(3)上的迭代加权最小二乘(Iterative reweighted least squares,IRLS)算法获得相机朝向的最优估计S*.算法1 描述了SO(3)上的IRLS 算法.

算法 1.SO(3)上的 IRLS 算法

为了估计当前帧Ik的绝对旋转Rk,本文首先估计选择的参考帧Ik-i与当前帧Ik之间的相对旋转由于参考帧Ik-i的绝对相机旋转Rk-i已经求得,因此可以得到当前帧Ik绝对的相机旋转,即:

对于参考帧与当前帧之间的相对旋转,传统上应用对极约束根据帧与帧之间二维特征点的匹配关系,通过分解本质矩阵(Essential matrix)[27]来恢复,但是对于纯旋转的相机运动来说,无法从本质矩阵中恢复相机的运动.针对这个问题,Fathian等[28]使用四元数来表示相机的相对旋转,提出了QuEst算法来从二维的匹配点中估计相机的运动.该算法需要5 对点来求解相机旋转,但通常匹配特征点的数量多于5 对,因此本文采用QuEst 结合随机采样一致性算法(Random sample consensus,RANSAC)[29]来精确地估计两个相机视角之间相对的旋转变换.

3.3 稀疏点云重建

重建的三维空间点的精确性影响着相机位置的估计.由第3.2 节,在相机运动的过程中基于特征点的age值,可以在前n帧中多次观测到同一三维空间点.如图5 所示,首先根据双目相机的视差可以三角化得到同一三维空间点的n个位置表示.然后由于前n帧的相机位姿已知,因此可以使用左右相机前后帧之间特征点的匹配关系再次三角化得到 2n-2 个同一三维空间的位置,即三角化得到同一三维空间点的 3n-2 个位置.本文使用集合{1P,2P,···,3n-2P}表示同一空间点的多次观测.为了得到最优三维空间点的位置,本文将三维空间点视为欧氏空间 R3中的三维向量,在同一三维空间点的多次观测中得到该三维空间点的最优位置,即

图5 同一三维空间点的多次三角化(基于特征点的age值,在前n帧中根据双目相机的视差以及左右相机连续帧间特征点的匹配关系多次三角化同一三维空间点)Fig.5 Multiple triangulations of the same 3D space point (Based on theageof the features,the same 3D space point is triangulated multiple times in the firstnframes according to the disparity of the stereo camera and the matching relationship between the feature points of the left and right camera consecutive frames)

其中,Pi∈{1P,2P,···,3n-2P}.通过采用 R3上的IRLS 算法优化三维空间点,得到三维空间点的最优位置P*.

3.4 平移的估计

本文使用优化后的三维点云与二维点的匹配关系来求相机的平移.三维空间点投影到二维图像平面的整个过程为

其中,[u,v,1]T为投影点的齐次坐标表示,f为相机焦距,投影函数用π来表示,(αx,αy)为图像的主点,[R|t]为相机两个视角之间的运动.[x,y,z,1]T为三维点云的齐次坐标表示.

本文固定已经估计的旋转和优化后的三维空间点云来优化估计相机的平移,即

其中,l,r表示左右相机.整个过程中IRLS 算法只迭代优化相机的平移,通常仅需迭代少数几次即可收敛.与SO(3)上的 IRLS 算法相比,对于R3上的IRLS 算法,只在于计算误差以及迭代更新的方式不同,R3上直接通过两个向量之差来定义误差.

4 实验结果与分析

本文在KITTI 数据集[30]以及New Tsukuba数据集[31]上测试提出的算法.实验环境为装有Ubuntu 18.04,配置为Intel® CoreTMi7-4 770 CPU,8 GB RAM 的计算机.KITTI 数据集提供了大尺度动态场景下的22 个立体图像序列.其中前11 序列(00～10)提供了真实轨迹.New Tsukuba 数据集为人工合成的小场景的静态室内环境.

本文主要在精度与速度两个方面来评估算法.在精度方面,主要通过评估估计轨迹与真实轨迹之间的均方根误差(Root mean square error,RMSE)、均值(Mean)与标准差(Standard deviation,STD)来反映本文算法的精确性,其中均方根误差代表绝对轨迹误差,均方根误差越小则位姿估计越准确.本文使用光流跟踪特征点选择的窗口块大小为9×9,视差一致性检测的阈值thd=1,验证是否构成回环匹配的阈值d ≤1.

由于VISO2-S[32]与ORB-SLAM2[33]的作者开放了源代码,因此本文选择这两种算法来与本文算法(算法1)进行对比.表1 中对比了本文算法与ORB-SLAM2在KITTI 数据集00～10 序列共11个序列上的测试结果.从表1 中可知,除了01 序列外,本文算法在其余的10 组序列上位姿估计的结果较精确.01 序列采集的是高速公路的场景,由于场景纹理较少并且场景纹理相似,导致提取的特征点分布不均匀,且大部分集中在中间的一条线上,只使用图像特征点在此场景中估计相机位姿是困难的,其他算法在此序列上也存在同样的问题,导致位姿估计的误差较大.相比于ORB-SLAM2,本文算法的RMSE 在多数序列上更小.由于在05和06序列上有闭环,ORB-SLAM2 通过闭环检测与校正来进一步提高位姿估计的准确性,而本文算法没有回环校正.虽然在05和06 序列上ORB-SLAM2的RMSE 较小,但是本文算法的RMSE 非常接近ORB-SLAM2.

表1 中同时对比了本文算法与VISO2-S 在KITTI数据集00～10 序列上估计的相机运动轨迹与真实轨迹的RMSE、Mean和STD.VISO2-S 算法通过最小化重投影误差并采用高斯牛顿法迭代优化求解相机的旋转与平移.通常对于相机的旋转优化是比较困难的,相机旋转影响着相机平移的估计,因此采用迭代法同时优化求解得到相机旋转与平移的误差较大.本文算法通过分开独立的优化相机的旋转与平移提高来位姿估计的精确性,从表中数据可知本文算法优于VISO2-S 算法.

表1 本文算法、ORB-SLAM2 以及VISO2-S 估计的轨迹与真实轨迹之间RMSE、Mean、STD 的对比Table 1 Comparison of RMSE,Mean,STD between the trajectory estimated by ours,ORB-SLAM2,and VISO2-S and the real trajectory

表2 中对比了不同算法在大场景的 KITTI数据集上的运行时间,部分数据来源于文献[34]与KITTI Benchmark[30].本文只统计特征处理与位姿优化估计所耗费的时间.从表中数据可知,由于ORB-SLAM2 采用局部BA 与全局BA 来优化相机位姿,因此ORB-SLAM2 位姿优化估计部分耗时较多.由于VISO2-S 对左右每幅图像提取Coner角点和Blob 角点并计算描述子,通过描述子来构建图像帧之间的数据关联,最后通过最小化重投影误差求解相机位姿,因此VISO2-S 主要耗时在特征处理部分.FB-KLT 方法提取Shi-Tomasi 角点并通过高斯牛顿法求解相机位姿,从表中实验数据看出该方法主要耗时在特征处理部分.本文算法在特征处理部分速度最快,耗时主要在位姿优化估计部分.总体而言,从表2 中可以看出本文算法耗时最少.

表2 计算时间的对比(部分数据摘自KITTI Benchmark[30])(ms)Table 2 Computation time comparison(Partial data from KITTI Benchmark[30])(ms)

在室内场景的New Tsukuba 数据集上,本文算法估计的RMSE 为0.389,ORB-SLAM2 估计的RMSE 为1.015,VISO2-S 估计的RMSE 为0.916,可以看出本文算法表现较好.

在图6 (a)和图7 (a)中,给出了本文算法、ORBSLAM2和VISO2-S 估计的相机运动轨迹与真实相机轨迹在KITTI 数据集上的对比.而图6 (b)和图7 (b)为本文算法在KITTI 数据集上重建的周围环境的稀疏地图.

图6 在 KITTI 数据集序列 00～05 上的实验Fig.6 Experiments on the KITTI sequence 00～05

图7 在 KITTI 数据集序列 06～10 上的实验Fig.7 Experiments on the KITTI sequence 06～10

在图8(a)中,给出了New Tsukuba 数据集上本文算法、ORB-SLAM2 以及VISO2-S 估计的轨迹与真实轨迹的对比.从轨迹对比图中可以看出,本文算法估计的轨迹更接近于真实轨迹.图8(b)为本文算法在New Tsukuba 数据集上重建的稀疏环境地图.

图8 在 New Tsukuba 数据集上的实验Fig.8 Experiments on New Tsukuba sequence

5 结束语

本文基于拉普拉斯分布提出了一种快速精确的双目视觉里程计算法.为了提高相机位姿与三维空间点估计的精确性,在相机旋转、三维空间点以及相机平移这些量的误差服从拉普拉斯分布的假设下来构造优化问题的目标函数.通过在SO(3)以及R3上的优化来得到位姿和三维空间点的最优估计.KITTI 以及New Tsukuba 数据集上的实验结果表明本文提出的算法能够快速准确地估计相机位姿与三维空间点的位置.本文算法尚有一定的局限性,在构建数据关联时只使用了光流跟踪,由于光流自身的原因,对于亮度变化明显的环境不够鲁棒.未来的工作中,将进一步研究提高光流跟踪的精确性并尝试将视觉信息与惯性测量结合起来,增加系统的鲁棒性.