建图
如ch11中所述,建图是视觉SLAM中的最后一环。原著第13章是一个SLAM系统的设计实践,需要配合程序讲解。原著的第14章是现在与未来,介绍了目前的开源视觉SLAM方案和未来的SLAM话题。这部分内容比较容易,而且当读者完全前12章时对此更是不在话下。基于这些考虑,本篇将是此系列的最后一篇。这里需要声明一下,本系列中的图片,除非特别标注,均为笔者自己绘制、引用高翔博士的大作《视觉SLAM14讲》或在其基础上修改。
一、概述
SLAM过程本身就可以定位路标,为什么还要建图呢?这是因为SLAM得到的只是稀疏图,根据实际应用需要还有半稠密图,稠密图和语义图。地图的作用包括定位、导航、避障、重建和交互等。定位可以用SLAM得到的稀疏地图。导航和避障都要用到稠密地图,其二者的区别是避障中地图的路标可能是运动的。重建是目前的一个研究热门如三维地图重构,它不但要求稠密地图,更希望带纹理以增强现实感。交互需要用到语义地图。例如某应用中命令机器人取桌子上的报纸,除了环境地图外,机器人还需要知道什么叫做桌子,什么叫做报纸。这就需要机器人对地图有更高层面的认知。能满足这种认知或交互需要的地图就称为语义地图。这些地图分类与作用如图1所示
图1:各种地图示意图
从上面分析可以看出,稠密地图具有重要的工程应用,那应该如何建立稠密地图呢?
二、单目稠密重建
在单目SLAM完成后,我们已经知道相机位以及特征点与像素的对应关系。这时候只要根据ch7上所述的三角测量关系就可以确定特征点的深度。然而,这种方法只能用于稀疏地图的重建,这是因为特征点只是图片中像素很小一部分的缘故。为了构建稠密地图或者估计非特征点的深度信息,需要用到极线搜索和快匹配技术。
1.极线搜索技术
如图2所示,空间点在两个相机位像素平面中的投影分别为。我们如果给定
,要如何在另外一个相机位上找到
呢?
图2:极线搜索示意图
第一种思路是根据的灰度在右边相机位像素平面上全平面寻找。很显然这样搜索的范围太大太耗时间。回想起ch7上的对极几何可知,点
一定会在极线
上。这样我们只要在
上搜索匹配点即可,可大大减小搜索范围。具体说,根据ch7上中的式(5)可知
(1)
其中为本质矩阵
的各元素;
为点
的像素坐标;
为点
的像素坐标。在两帧相机位差
和内参
已知的情况下本质矩阵
中各元素也是已知的,这样上式就转化成一个关于未知数
的二元一次方程,即极线
的平面方程。由此可方便地在右边极线平面上通过改变
的值实现在
上查找匹配点的目的。
2.块匹配技术
比较单个两个并不一定稳定可靠,经常的情况是上有许多和
灰度相似的点。这样
就会同时匹配很多点,难以区分哪个是正确的。一种有效的做法是在
周围取一个一定大小的像素块,然后用这个像素块与极线
上同样大小的块进行比较。这种方法可以在一定程度上提高区分性,称为块匹配技术。至于如何判断两个像素块是否匹配,可以将两个像素块的灰度相减并求范数得到。这种方法很多,我们这里介绍一种并非基于范数的匹配算法。这种算法称为归一化互相关算法(NCC, Normalized Cross Correlation),它计算两个小块的相关性。假设两个小块的灰度分别用
和
来表示,其中i,j表示像素块中像素的索引,则两个数据库的相关性可表示为
(2)
的值越大,表示两个像素块越匹配,越小表示它们越不匹配。
计算这种相关性的时候要注意以下几点
(1)计算前都减去像素块的平均值,这样有利于减小因为光照亮度差异而引起的匹配误差。
(2)计算前将两个像素块通过坐标变换变到同一个像素平面坐标系中后再进行相关性计算。由于坐标变换技术我们已经很熟悉,就不在这里展开了。仍不熟悉的读者可参考原著12.3.4节。
(3)像素块尽量取在极线上存在明显梯度的情况,这样更容易匹配准确,如图3所示。从图中可以看出,如果像素块存在极线方向的梯度,随着像素块位置匹配的成功相关性会明显增大。若像素块没有梯度或者如图3右侧所示梯度与极线垂直,则像素块即便到达正确的匹配位置,相关性都没有发生变化,即难以匹配成功。
图3:像素梯度与匹配度的关系示意图
3.高斯分布深度滤波器
根据上一小结中的NCC算法可以得到图4所示的匹配分数曲线。从图4中可以看出,有好多个点都得到了较高的匹配分数,那么实际应用中应该如何采用呢?将图2左边的像素平面称为参考平面,右边的像素平面称为匹配平面。由于相机不断移动,匹配平面会每隔一帧就来一个。
图4:NCC匹配分数随像素块极线移动的变换规律
在匹配平面的第一帧,我们取图4中匹配分的最高点为正确的上匹配位。这样可确定
的位置并根据三角测量关系确定像素点的深度
。随着时间的增加,后面还会有新的匹配帧过来,还可以用同样的办法得到新的深度
,那我们相信哪个深度呢?如果所有的深度均满足同参数的正态分布,那么我们只要按概率论教材中根据样本统计结果的数字特征算法来确定
即可。然而,后面的分析表明每次得到
即便均可假设为正态分布,但其数字特征(均值和方差)并不相同,因此这种方法并不能使用。在正态分布假设下,
和
的分布相乘后可得其联合分布。注意到
和
的分布均表示同一个深度
的分布,此联合分布函数可写成关于
的分布函数。根据下面链接
中☆A a g s√博主的推导可知仍然是高斯分布且均值和方差计算公式位
(3)
其中的下标相当于高博书上公式(12.6)中的下标
,下标
相当于公式(12.6)中无下标的参数,下标
相当于公式(12.6)中的下标
。根据这种算法,我们可以不断更新
和
。当
的值足够小时,认为匹配收敛,也就确定了匹配点的位置。在实际应用中,我们可根据
和
在
范围寻找其所对应
帧中极线
上的线段,图4中的匹配点将只在这个线段范围内寻找,这样可以避免数据异常。
现在剩下的问题是或
如何计算。这种计算是一个典型的几何问题,其过程并不复杂。为了使博文简练,我们略去了原著p312-313中的推导过程。在匹配误差为一个像素的情况下其结果为
(4)
我们对原著中的公式(12.10)进行了改写,使其更方便使用。上式中的各参数含义如图5所示,其中表示参考相机位下空间点与光心的距离(这个不是深度),
为两个相机位的视差,
为参考相机位对空间点视线与视差线的夹角,
为匹配相机位与视差线的夹角,
位匹配像素平面中像素在匹配点附近沿极线
移动一个像素所引起的
角变化量。
图5:不确定性分析
三、小结
本篇主要讨论了单目SLAM的稠密地图重建方法。除了介绍极线搜索技术和块匹配技术外,对深度滤波器进行了阐述并加入了自己的理解,也综合了其他博主的见解。RGB-D相机的稠密重建还包含了许多丰富的内容。考虑到本系列是写给初学者看的,并且这部分内容最好与程序配合讲解,因此把它们放到后续的系列中去。
本系列的许多内容融入了笔者的理解,也修正了高博原著中的一些笔误。如有不妥之处,请留言指正或讨论。