作者 | 黄浴
转载自知乎
【导读】SLAM是“Simultaneous Localization And Mapping”的缩写,可译为同步定位与建图。最早,SLAM 主要用在机器人领域,是为了在没有任何先验知识的情况下,根据传感器数据实时构建周围环境地图,同时根据这个地图推测自身的定位。因此本文以简单清晰的文字为大家介绍了视觉 V-SLAM。
简单的V-SLAM介绍,就当入门:)。
首先,从双目立体几何(stereo geometry)原理开始来定义外极(epipolar)约束:两个摄像头光心分别是 c0 和 c1,3-D 空间点 p 在两个图像平面的投影点分别是 x0 和 x1,那么直线 c0c1 和两个图像平面的交点即外极点(epipole)e0 和 e1,pc0c1 平面称为外极平面(epipolar plane),它和两个图像平面的交线 l0 和 l1 即外极线(epipolar line);可以看到两个摄像头坐标系之间的转换满足(R,t),同时说明摄像头 c0 的图像点 x0 在摄像头 c1 的图像对应点 x1 一定落在其外极线 l1,反之依然;
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/bb2f7e1eeb808645374bac344ce5fd85.jpeg)
(a)对应一条光线的外极线 (b)对应的外极线集合和外极平面 外极线约束
这里需要定义一个本质矩阵(essential matrix)E 和一个基础矩阵(fundamental matrix)F:E = [t]×R,其中 [t]× 是反对称矩阵,即定义向量
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/1f55be5ff2d3ba33582c035a4838307c.png)
那么
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/9434ff1eb32398190384e382169e1a5b.jpeg)
而 F = A−TEA−1,其中 A 是摄像头内参数矩阵;对矩阵 E 和矩阵 F 来说,均满足外极约束方程 x1TEx0 =0,x1TFx0 =0
前者是摄像头已标定情况下图像特征点的对应关系,后者是摄像头未标定情况下图像特征点的对应关系;
其次,得到外极线约束(以F矩阵为例,E矩阵同样)如下
l1=Fx0,l0=x1F,
以及,外极点约束(以F矩阵为例,E矩阵同样)如下
Fe0=0,FTe1=0;
根据上面图像特征点的外极约束方程,有 8 点算法求解 F 和 E,以 F 为例,给定一组匹配特征点(n > 7)
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/7060d804e23ab164d859a0f1677aab20.jpeg)
定义F矩阵元素fij(i=1~3,j=1~3),那么线性齐次方程组为
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/0ba0ff6c6cddf1e977fb7bc71279cb27.jpeg)
简记为
Qf = 0
做SVD得到Q = USVT,而最终F的解是V最右的奇异向量(singular vector);
另外,因为E矩阵完全由摄像头外参数(R和t共6个未知参数,但有一个不确定平移尺度量)决定,所以存在 5 点算法求解 E 矩阵;
可以分解E得到摄像头外参数,其步骤如下:
i. 同样对E矩阵做奇异值分解(SVD):
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/0b4ba531f876f073da4ff9e047fc6c72.png)
ii. 那么第一个摄像头投影矩阵简单设为
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/9d527e304e37e9265a3fe30a4c8060c3.png)
而第二个摄像头矩阵P2有四种可能情况,如图(a)-(d)所示:
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/001a3c8deee21ee6ca5325d614dfd048.png)
其中
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/46e0be9ac25831714695561bc2683542.png)
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/00f33eef86d97ce3af037c1f5df64625.jpeg)
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/d34c7069979656569ca8c9f4d2ad9e57.jpeg)
从E矩阵分解得到R和t
根据视图方向与摄像头中心到3-D点的方向之间夹角可以发现,四个可能中只有情况(a)是合理的解;
确定两个视角的姿态之后,匹配的特征点 x,x’可以重建其 3-D 坐标 X,即三角化(triangulation)理论;首先存在一个线性解:设两个摄像头投影矩阵为 P 和 P’,相应的它们列向量为 pi,pi’,i=1~3,则有方程组:AX = 0
其中
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/45b7874948b38b01c0ef34f84ef08dbd.jpeg)
但一些误差干扰的存在,上述线性解是不存在的;所以需要一个非线性的解,这里采用 F 矩阵定义的外极约束方程 xTFx’= 0,得到最小化误差函数为
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/6b82bfc1ab795329048586f6fe35aae7.png)
下面采用外极线 l, l’的来约束定义误差,如图所示,将目标函数重写为
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/cef8238d2b5317ffe5289c378bdb91f7.jpeg)
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/e93e42999d498a1f1f40452a31143e35.jpeg)
外极线定义的误差
另外,在已知重建的 3-D 点集,如何和新视角的 2-D 图像特征点匹配呢?这个问题解法称为 PnP(Perspective n Points),算法如下:
i. 首先,3D 点表示为 4 个控制点的加权和;
ii. 控制点坐标是求解的(12)未知数;
iii. 3D点投影到图像上以控制点坐标建立线性方程;
iv. 控制点坐标表达为零特征向量(null eigenvectors)线性组合;
v. 上面组合的权重(bi)是新未知数(<4);
vi. 增加刚体(rigidity)约束以得到bi二次方程;
vii. 根据bi数目(无论线性化,或重新线性化)求解。
(注:有时候3-D-2-D匹配比3-D之间匹配的精度高)
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/3da75eb6195730ba54dd3eec0e241abc.jpeg)
PnP求解的示意图
这里需要补充两个概念,一是鲁棒估计的随机样本共识法(RANSAC,RANdom SAmple Consensus),另一个是全局优化的集束修正法(BA,bundle adjustment):
i. RANSAC的目的是在包含异常点(outlier)的数据集上鲁棒地拟合一个模型,如图 2-12 所示:
1. 随机选择(最小)数据点子集并实例化(instantiate)模型;
2. 基于此模型,将所有数据点分类为内点(inlier)或异常点;
3. 迭代重复 1-2 步;
4. 选择最大的内地集,以此重新估计最终模型。
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/ce0ba41c6f69bae6a0667e8985ca75d8.jpeg)
RANSAC示意图
ii. BA的目的是优化全局参数估计,对 SLAM 来说,结构重建的 3-D 点 X 和其对应的 2-D 特征点 x ,还有估计的视角变换参数(甚至包括摄像头内参数) P ,位于一个重投影(reprojection)误差函数 D 最小化框架下(如图所示),即
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/36e71d65d79133db210351d7125e358a.jpeg)
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/326ba6af2a180b082ee90c437fd810f8.jpeg)
BA示意图
这里集束(Bundle)指2-D点和3-D点之间的光线集,而修正(Adjustment)是指全局优化过程;其解法是非线性迭代的梯度下降法,如Gauss-Newton 方法和其修正 Levenberg-Marquardt 方法,因为问题自身的特性,这里的雅可比矩阵是非常稀疏的;另外,只取重建 3-D 点修正的话,称为 structure only BA,而只取视角变换修正的话,称为 motion-only BA;
在以上基础之上,再重新审视SLAM过程:
i. 首先要提到概念 VO,即视觉里程计(visual odometry);VO 是 SLAM 的一部分,VO 主要是估计视角之间的变换,或者运动参数,它不需要输出制图(mapping)的结果,而且 BA 就是 motion-only 的模式;
ii. SLAM 方法分两种途径,一种是递推滤波器方法,另一种是批处理估计方法;
a) 滤波器方法,比如卡尔曼滤波递推估计,实际上建立一个状态空间的观测模型和状态转换(运动)模型;观察模型描述当摄像头姿态和地标位置(来自于地图)已知时观测(地标)的概率; 运动模型是系统状态(摄像头姿态)转换的概率分布,即马尔可夫过程; 那么在递归贝叶斯估计中,同时更新系统状态和建立的地图,其中融合来自不同视角的观测来完成制图,而估计系统状态可计算摄像头的姿态,即定位问题;
b) 批处理估计方法,也称“关键帧”方法,其步骤是:
1)首先通过选择的头两(关键)帧,采用双目几何的特征点匹配得到初始的3-D点云重建;
2)正常模式:假设 3D 地图可用,并且估计出摄像头增量运动,跟踪特征点并使用 PnP 进行摄像头姿态估计;
3)恢复模式:假设 3D 地图可用,但跟踪失败故没有增量运动,可相对于先前重建的地图重新定位(re-localize)摄像头姿势;
4)关键帧 BA:保持一个“关键帧”子集,状态向量是所有关键帧的 3D 地标和对应摄像头姿势,BA 可以在与跟踪模块并列的线程中调整状态估计;
(注意:关键帧的选择策略是算法性能很重要的一个因素)
SLAM 中的闭环(loop closure)问题:当摄像头又回到原来位置,称为闭环,需要检测闭环,并以此在 BA 框架下优化结构和运动估计;闭环检测和重定位是类似的,可以基于图像特征的匹配实现,俗称“基于图像的重定位(image-based re-localization),当关键帧子集较大的时候,需要对特征匹配进行压缩和加速,比如词包(bag of words)法和K维-树(KD-tree)数据结构等等;
SLAM 中的传感器可以是单目、双目、深度传感器(RGB-D)甚至激光雷达,也可以和 IMU 融合,称为 VINS(visual inertial navigation system)。
附录:G-N 和 L-M 的非线性最小二乘算法
假设有观测向量 zi’,其预测模型为 zi = zi(x),其中x为模型参数;那么最小二乘(LS)法就是最小化如下代价函数:平方误差加权和(weighted Sum of Squared Error,SSE)
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/d6f24a84d10adf9c78787eaaafd9d58d.jpeg)
其中 Wi 是一个任意对称正定(symmetric positive definite,SPD) 矩阵,特征误差函数为
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/54464b0173ae7cbc490e5e5155a31e85.png)
雅可比矩阵 J 和黑森(Hessian)矩阵 H 的计算为
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/d69d30aa73f9e8b7a3c8044d889e5697.png)
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/911eec3216a52a1e1b2bdc7065ccbdc1.jpeg)
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/4b3b58c2f449f3ea2c34cfe61ef65f59.png)
那么 H 近似为
H ≈ J⊤ W J
作为梯度下降法,其 G-N 迭代的步进量即 z → z + delta z,由下面方程组计算
![640?wx_fmt=jpeg](https://i-blog.csdnimg.cn/blog_migrate/bdf0fd6e39a9db4f783bdc7eb6657840.jpeg)
对于上面 G-N 的迭代步进量计算,可能左边的矩阵不可逆。为此,一种改进的方法是在对角线元素中加入阻尼因子(Damped factor),即 L-M 迭代的步进量计算变成如下方程组
![640?wx_fmt=png](https://i-blog.csdnimg.cn/blog_migrate/136205061bfe6f9646ee167c0888f460.png)
原文链接:
https://zhuanlan.zhihu.com/p/67848295
(*本文为 AI科技大本营转载文章,转载请联系作者)
◆
福利时刻
◆
入群参与每周抽奖~
扫码添加小助手,回复:大会,加入福利群,参与抽奖送礼!
AI ProCon 2019 邀请到了亚马逊首席科学家@李沐,在大会的前一天(9.5)亲授「深度学习实训营」,通过动手实操,帮助开发者全面了解深度学习的基础知识和开发技巧。还有 9大技术论坛、60+主题分享,百余家企业、千余名开发者共同相约 2019 AI ProCon!距离5折优惠票结束还有4天!
推荐阅读
重磅!全球首个可视化联邦学习产品与联邦pipeline生产服务上线
用Python的算法工程师们,编码问题搞透彻了吗?
Python冷知识,不一样的技巧带给你不一样的乐趣
90 后程序员健康现状:掉头发、油腻、腰椎间盘突出……| 程序员有话说
真相了!AI 程序员:我们根本没有 80w 年薪好么?
CSDN & 火星财经, 联手发起Libra超级节点竞选!
干货!学霸用12个决策模型告诉你,如何判断你到底需不需要区块链!
你点的每个“在看”,我都认真当成了喜欢