视觉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】【2】

1. 对极几何公式推导：

注意： 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算法介绍

 

 2D-2D对极几何

二、三角测量 

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

 问题讨论：

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

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

三、3D-2D:PnP

PnP是求解3D到2D点对运动的方法。

单目视觉里程计:必须先初始化，才能使用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像素投影位置  ,通过世界坐标系下的3D坐标，利用待优化的变换矩阵 T 、内参 K 、 深度  转换得到图像上的预测值，作差得重投影误差。BA优化就是最小化重投影误差，即求误差项关于优化量(相机位姿T和空间点P的位置)的导数（等于0），分别对应187式7.46和7.48两个雅可比矩阵。

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

      是像素坐标的实际值，它是个常量，   是我们计算得到的预测值（估计值）。因此误差  对  求导，相当于  对  求导。最后求导出来的结果也要带符号。 和  的关系是如下：

                                       

​ 186页 式7.43 给出了误差对于投影点的导数。

四、3D-3D:ICP

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

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

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

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

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

 得到公式： 

误差     前者是观测值（实际值），后者是估计值（计算得到的）。

计算雅可比矩阵：                     (199页给出了计算结果，是一个3*6的矩阵)

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

线性求解SVD、非线性优化（BA）方法来计算：

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

ICP 是由图像特征给定了匹配的情况下进行位姿估计的问题。但现实中更注重匹配未知的情况，在RGD-D SLAM 中，一个像素的深度可能有，也可能测量不到，所以需要混合使用PnP和 ICP ：对于深度已知的特征点，建模它们的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解法