基于视觉的SLAM
按照传统做法分成以下几块:
1. 前端:特征提取,配准。 2. 中间:计算变换矩阵初值。3. 后端:多帧误差最优化。4. 回环优化
1. 前端:特征提取,配准。
图片特征提取匹配有很多种:
1. SIFT:大名鼎鼎,尺度不变特征变换,但是速度太慢。
2. SUFT:在SIFT的基础上优化,基于Hessian矩阵和一阶Haar小波,但还是慢。
3. FAST:只检测像素的灰度值,没有方向和尺度信息。
4. ORB:改进了FAST,使用非最大值抑制,使用图像金字塔保证尺度不变性,灰度质心法保证旋转不变性。
要满足实时性,一般采用ORB的方法,提取完特征后就是配准,opencv提供一个暴力配准的方法:BFMatcher 特征向量两两一一比较,得到匹配关系。
这样得到的匹配关系很乱,一般采用RANSAC的方法剔除异常匹配:
1.通过匹配点计算两个图像之间单应矩阵(findHomography),然后利用重投影误差来判定某一个匹配是不是正确的匹配。
2.剔除异常匹配,用剩下的匹配计算新的单应矩阵,重新判断所有匹配的正确性,一至迭代到稳定的结果。
2. 中间:计算变换矩阵初值。
使用第一步计算的匹配关系计算两帧之间的坐标变换,这里与ICP不同的,匹配关系都叠加上了摄像机成像部分,并没有三维坐标的匹配关系,不过没关系,opencv已经帮我们想好了。
假设我们提前已经标定好相机,详情见opencv张正友标定。一对匹配的像点之间存在这某种约束关系,这种约束被称为两视图的对极约束。
1. 两个相机中心的连线CC'称为基线.
2. X e e' x x' 五点共面。
3. 存在从点x到对极线l′的变换(基础矩阵)
4. 通过匹配点的关联,我们能知道x x'的对应关系,由此得出基础矩阵。
设以第一个相机作为坐标系三维空间的点P=[X,Y,Z],其在两个相机的像点分别为p1,p2。由于第一个相机的中心作为世界坐标系的原点,也就是说第一个相机没有旋转和平移,通过小孔相机模型可得:
K是相机的内参,带入P得:
两边同时左乘K−1K−1,得到
两边同时左乘向量tt的反对称矩阵,消除t
两边再同时左乘
再将x1,x2 换掉
上式是对极约束的另一种表示,该式子中仅包含像点,相机的旋转和平移,中间的矩阵就是基础矩阵F。
提取中间的矩阵可到
E被称为本质矩阵,其和基础矩阵相差相机的内参K。
也就是说,仅通过匹配的点对(最少7对)可以计算出两视图的基础矩阵F,然后再从F中分解得到相机的位姿势。
3. 后端:多帧误差最优化。
通过上面的计算,可以得到相机两两之间的位置关系,但是由于误差累计,肯定会导致出现较大的偏差。
为此我们可以通过全局优化的方式,进行继续优化。
对于多帧而言:
i = [0,n] 为每两帧之间的匹配点,j = [0,m] 为对应不同的帧
为了求所有对应的R 和T,我们求出一个全局最优解:
为此需要用梯度下降的方法,这要求我们对自变量R和T求偏导,但是R是一个3自由的矩阵,为此需要把R变换到李群代数中: