简单明了,一文入门视觉SLAM

640?wx_fmt=jpeg

作者 | 黄浴

转载自知乎


【导读】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

(a)对应一条光线的外极线 (b)对应的外极线集合和外极平面 外极线约束

这里需要定义一个本质矩阵(essential matrix)E 和一个基础矩阵(fundamental matrix)F:E = [t]×R,其中 [t]× 是反对称矩阵,即定义向量

640?wx_fmt=png


那么

640?wx_fmt=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

定义F矩阵元素fij(i=1~3,j=1~3),那么线性齐次方程组为

640?wx_fmt=jpeg

简记为

Qf = 0

做SVD得到Q = USVT,而最终F的解是V最右的奇异向量(singular vector);


另外,因为E矩阵完全由摄像头外参数(R和t共6个未知参数,但有一个不确定平移尺度量)决定,所以存在 5 点算法求解 E 矩阵;


可以分解E得到摄像头外参数,其步骤如下:


i. 同样对E矩阵做奇异值分解(SVD):


640?wx_fmt=png


ii. 那么第一个摄像头投影矩阵简单设为


640?wx_fmt=png


而第二个摄像头矩阵P2有四种可能情况,如图(a)-(d)所示:


640?wx_fmt=png

其中


640?wx_fmt=png


640?wx_fmt=jpeg


640?wx_fmt=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

但一些误差干扰的存在,上述线性解是不存在的;所以需要一个非线性的解,这里采用 F 矩阵定义的外极约束方程 xTFx’= 0,得到最小化误差函数为


640?wx_fmt=png

下面采用外极线 l, l’的来约束定义误差,如图所示,将目标函数重写为


640?wx_fmt=jpeg
640?wx_fmt=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

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

RANSAC示意图

ii. BA的目的是优化全局参数估计,对 SLAM 来说,结构重建的 3-D 点 X 和其对应的 2-D 特征点 x ,还有估计的视角变换参数(甚至包括摄像头内参数) P ,位于一个重投影(reprojection)误差函数 D 最小化框架下(如图所示),即


640?wx_fmt=jpeg
640?wx_fmt=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

其中 Wi 是一个任意对称正定(symmetric positive definite,SPD) 矩阵,特征误差函数为

640?wx_fmt=png

雅可比矩阵 J 和黑森(Hessian)矩阵 H 的计算为


640?wx_fmt=png
640?wx_fmt=jpeg
640?wx_fmt=png

那么 H 近似为

H ≈ J⊤ W J

作为梯度下降法,其 G-N 迭代的步进量即 z → z + delta z,由下面方程组计算


640?wx_fmt=jpeg

对于上面 G-N 的迭代步进量计算,可能左边的矩阵不可逆。为此,一种改进的方法是在对角线元素中加入阻尼因子(Damped factor),即 L-M 迭代的步进量计算变成如下方程组

640?wx_fmt=png


原文链接:

https://zhuanlan.zhihu.com/p/67848295

(*本文为 AI科技大本营转载文章,转载请联系作者)

福利时刻



入群参与每周抽奖~

扫码添加小助手,回复:大会,加入福利群,参与抽奖送礼!

640?wx_fmt=jpeg


AI ProCon 2019 邀请到了亚马逊首席科学家@李沐,在大会的前一天(9.5)亲授「深度学习实训营」,通过动手实操,帮助开发者全面了解深度学习的基础知识和开发技巧。还有 9大技术论坛、60+主题分享,百余家企业、千余名开发者共同相约 2019 AI ProCon!距离5折优惠票结束还有4天! 

 

640?wx_fmt=jpeg


推荐阅读

640?wx_fmt=png

你点的每个“在看”,我都认真当成了喜欢

  • 0
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值