视觉SLAM ch7总结(二)

目录

计算相机运动

一、2D-2D:对极几何

二、三角测量 

三、3D-2D:PnP

四、3D-3D:ICP


计算相机运动

在已经有匹配好的点对基础上来估计相机的运动(其实就是求R和t)

根据相机不同的种类有以下几种情况:

1.相机为单目时,只有2D的像素坐标(缺少了深度信息),因此问题就是根据两组2D点估计相机运动,用对极几何解决。

2.相机为双目、RGB-D时,或者通过某种方式得到了距离信息,问题就是根据两组3D点估计相机运动,通常用ICP解决。

3.如果一组为3D,一组为2D,即我们得到了一些3D点和它们在相机的投影位置,也能估计相机运动,通过PnP解决。

一、2D-2D:对极几何

对极约束的目的是估计相机运动(定位过程)。

1. 对极几何公式推导:

视觉SLAM ch7总结(二)_第1张图片

视觉SLAM ch7总结(二)_第2张图片

注意: x1 x2是归一化坐标 ,p1 p2是像素坐标。

总结:s可以理解为深度

结合书99页公式5.7, 深度Z * Puv = K P(相机坐标)

归一化的坐标= P / Z ,因此归一化坐标 = K^-1 Puv  也就是这里的 x1 = K^-1 p1.

 根据公式相机位姿估计分为两步:

1.根据配对点的像素位置求E或F

2.根据E或F求出R和t。

 E和F只差了相机内参,因此实际求解中求解形式简单的本质矩阵E。

2.本质矩阵的求解

本质矩阵是一个3x3 矩阵,有9个未知数。

本质矩阵的特殊结构:

1.本质矩阵E由对极约束定义而来,由于对极约束是等于0的约束,因此E乘以任意一个非零常数依然是E,即E在不同尺度下是等价的。

2.本质矩阵的内在性质:根据E = t^R,可以证明E的奇异值必定是[x,x,0]的形式  。

3.旋转平移各有三个自由度,t^R本来有6个自由度,但由于cE=E(尺度等价性),因此E只有5个自由度。

理论上可以用5对匹配点求解E,但是由于E的非线性强,估计比较困难,所以可以只用尺度等价性,用8对匹配点(3*3-1)来求解E,称为“八点法”。

单应矩阵: 所有的特征点落在同一个平面上(墙、地面),则可以通过单应进行运动估计。

问题讨论:

1.单目视觉的尺度不确定性

2.初始化的旋转问题:相机发生了纯选择会导致 t 为0,无法求解 R。因此,单目初始化不能只有旋转,必须有一定程度的平移。

3.多于8对点的情况
工程中给定的点数多于8对,可以计算一个最小二乘解。方程构成一个超定方程,可以通过最小化一个二次型来求。

4.在存在错误匹配的情况下使用随机抽样一致算法(Random Sample Concensus,RANSAC),可以处理带有错误匹配的数据。

RANSAC
随机采样一致,RANSAC主要解决样本中的外点问题,可以去除错误匹配,最多可处理50%的外点情况。
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点,并用下述方法进行验证:
N — 样本点个数, K —  求解模型需要最少的点的个数
1. 随机采样K个点
2. 对该K个点拟合模型
3. 计算其它点到拟合模型的距离,小于一定阈值,当作内点,统计内点个数
4. 重复M次,选择内点数最多的模型
5. 利用所有的内点重新估计模型(可选)

关于RANSAC算法的参考链接:(具体代码一般不用手写,OpenCV和PCL库中都有相应函数)

 计算机视觉基本原理——RANSAC

 RANSAC算法介绍

 视觉SLAM ch7总结(二)_第3张图片

 2D-2D对极几何

二、三角测量 

对极几何只是求出相机在两个照片之间的运动,只是定位过程,而与建图无关。想要建图我们还要知道特征点的空间位置,得到深度信息,这就需要用到三角测量的方法。

 问题讨论:

三角测量是通过平移得到的。

三角测量的矛盾可总结为:增大平移时,会导致匹配失败;减少平移量会导致三角化精度不够。这个问题称为“视差”。

三、3D-2D:PnP

PnP是求解3D2D点对运动的方法。

单目视觉里程计:必须先初始化,才能使用PnP;

双目或RGB-D视觉里程计:可以直接使用PnP估计相机运动。

问题描述:已知n个点的空间坐标和它们在相机像素平面的投影坐标,估计相机位姿。

在视觉SLAM中,PnP是最重要的姿态估计算法。因为对于双目相机与RGBD相机来说,可以直接得到特征点的空间坐标,进而直接使用PnP估计相机运动;对于单目相机来说,通过对极几何初始化之后,我们也得到了一些特征点的空间坐标,在之后的SLAM过程中,我们就可以持续使用PnP算法。

PnP求解方法:理论上最少可以用3对点求解PnP,称为P3P方法,除此以外还有DLT(直接线性变换)、EPnP(Efficient PnP)、UPnP。还可以使用非线性优化的方式,构建最小二乘问题并迭代求解,也就是光束法平差(BA)。

1.DTL直接线性变换

已知空间间坐标[X,Y,Z,1] 和归一化坐标(u1,v1,1),求相机的位姿R和t。

通过6对匹配点求解增广矩阵T = [ R | t ] ;多于6对时,使用SVD方法对超定方程求最小二乘解。

对于旋转矩阵R,必须针对DLT估计的T左边3x3的矩阵块,寻找一个最好的旋转矩阵对它进行近似(将3x3矩阵空间投影到SE(3)流形上),使用QR分解完成。

2.P3P

仅使用3对匹配点,当给定的匹配点对于3组时,难以利用更多的信息;如果3D或2D点受到噪声影响,或者存在误匹配,算法失效。

3.最小化重投影误差求解PnP

DTL和P3P是线性方法,还可以把PnP问题构建成一个重投影误差的非线性最小二乘问题。

线性方法:先求相机位姿,再求空间点位置

非线性方法:把相机位姿和空间点位置看作优化变量,放在一起优化。

这一类把相机位姿和空间点放在一起进行最小化的问题,统称为BA。目标是最小化重投影误差,初值是可以是PnP或ICP给出的结果,如果相机是连续运动的(多数SLAM),可以不要初值,直接求解最优的相机位姿和路标点位置。

重投影误差定义:3D点投影到2D像素的位置 与 预测值的差。

使用BA求解,BA思想本身不难,难在应用在重投影中求导的地方(对误差项e求导)。直接对矩阵求导行不通,因为无论是变换矩阵还是旋转矩阵本身都有非常多的约束,再做求导只会越导越复杂。因此引入了李群与李代数,但是李群不满足加法,因此在李群上也没有导数的定义(ch4)故转而在李代数上利用(满足加法)导数的定义求矩阵的导数。使用李代数可以构建无约束的优化问题。所以难点在李代数求导(扰动模型:一般使用左扰动模型)。

总结:光束法平差BA,指把相机和三维点一起优化的方法。已知与3D点匹配的实际2D像素投影位置 u_i ,通过世界坐标系下的3D坐标,利用待优化的变换矩阵 T 、内参 K 、 深度 s_i 转换得到图像上的预测值作差得重投影误差BA优化就是最小化重投影误差,即求误差项关于优化量(相机位姿T空间点P的位置)的导数(等于0),分别对应187式7.46和7.48两个雅可比矩阵。

关于重投影误差 e 对投影点 P' 的导数前面为什么有个负号问题 

e = u_i - u     u_i 是像素坐标的实际值,它是个常量,  u 是我们计算得到的预测值(估计值)。因此误差 e 对 P' 求导,相当于 u 对 P' 求导。最后求导出来的结果也要带符号。u 和 P' 的关系是如下:

u = f_x \frac{X'}{Z'} +c_x                                       

v = f_y \frac{Y'}{Z'} + c_y
​ 186页 式7.43 给出了误差对于投影点的导数。

四、3D-3D:ICP

ICP统指匹配好的两组点间运动估计问题

ICP的求解过程和相机内参没有关系。

已知世界系下3D坐标、匹配的相机系下3D坐标 ---> 求解世界系、相机系间R,t(相机位姿)

问题描述: 第一帧中的特征点 Pi 经过变换得到第二帧的特征点 Pi'.

书中这里是反过来看的,将第二帧的 Pi'变换到第一帧得到 Pi 

 得到公式: Pi = R Pi' + t

误差 error = Pi - R Pi' +t    前者是观测值(实际值),后者是估计值(计算得到的)。

计算雅可比矩阵:  \frac{\partial e}{\partial \xi } = \frac{\partial e}{\partial P } * \frac{\partial P}{\partial \xi }                   (199页给出了计算结果,是一个3*6的矩阵)

最后记得把算出的R和t 变回去(这里的R t是第二帧到第一帧)

线性求解SVD、非线性优化(BA)方法来计算:

  • SVD
  • 非线性优化。类似PnP的BA优化,存在唯一解(求极小值)或无穷多解的问题。

ICP 是由图像特征给定了匹配的情况下进行位姿估计的问题。但现实中更注重匹配未知的情况,在RGD-D SLAM 中,一个像素的深度可能有,也可能测量不到,所以需要混合使用PnPICP :对于深度已知的特征点,建模它们的3D-3D 误差;对于深度未知的特征点,建模它们的3D-2D重投影误差。将所有的误差放在同一个问题解决。

总结:

ch7是这本书内容最多的一个章节,对视觉里程计进行了详细的介绍。重点介绍:

1.特征点是如何提取并匹配的

2.如何通过2D-2D的特征点估计相机运动(对极几何)

3.如果从2D-2D的匹配估计一个点的空间位置(三角测量)

4.3D-2D的PnP问题,线性解法(P3P、DLT、EPnP)和BA解法

5.3D-3D的ICP问题,线性解法(SVD)和BA解法 

你可能感兴趣的:(计算机视觉,slam,opencv)