orbslam2的基础理论（一）

转载声明：本文转载自  金木炎 的博客，仅供个人学习。感谢博主的无私分享，如有侵权，敬请告知。

本篇博客借鉴了许多经典博客的理论。

orbslam2相机pose估计和map point结构恢复

orbslam2的相机初始估计有两种方法：本证矩阵Ｅ和单应矩阵两种方法，具体可以看我之前的博客oerslam2-初始化

（1)再讲相机pose估计之前，我要讲一下相机的数学模型和对极几何。

我觉得首先我们要理解相机模型中的四个平面坐标系的关系：像素平面坐标系（u,v）、像平面坐标系（图像物理坐标第（x,y）、相机坐标系（Xc,Yc,Zc）和世界坐标系（Xw,Yw,Zw）。

1、像素坐标与像平面坐标系之间的关系 

    确定他们的关系之前，我们可以假设每一个像素在u轴和v轴方向上的物理尺寸为dx和dy。仔细看下他们的模型可以推出以下公式（这个还是比较好理解的）：

 

                                            

                                                          公式1

    解释：1、dx,dy,u0,v0其实都是我们假设出来的参数，最终是要我们求的内外参数。（dx，dy是一个像素点的在x,y像平面坐标系下的长度，ｕ0,ｖ0是光心投影到图片上点的像素坐标）

得出这个公式后我们可以运用线性代数的知识把方程用矩阵形式表示：

                             

                                                       公式2

当然我们也可以用另一种矩阵形式表示：

                            

                                                           公式3

2、相机坐标系与世界坐标系之间的关系 

   这两个坐标系之间的关系我们可以旋转矩阵R和平移矩阵T来得到以下关系：

                                

                                                        公式4

  解释：1、在这个公式中，R为3*3矩阵，T为3*1，0为（0，0，0），简化用Lw表示后为4*4矩阵。

3、成像投影关系（相机坐标系与像平面坐标系） 

    在相机模型中我们可以得到以下公式：

                                

                                                      公式5

         解释：1、（其中f是相机焦距）

同样我们用矩阵形式表示：

                                   

                                                           公式6

4、得到公式

而我们可以将以上公式综合一下就可以得到：

  

（2)现在要讲一下对极几何的知识！

两个摄像机的光心 C0、C1 ，三维空间中一点 P ，在两幅图像中的位置为 p0、p1 。如下图所示： 
 
由于 C0、C1、P 三点共面，得到： 
 
p0 在坐标系 C0 中的表示,以及 p1 在坐标系 C1 中的表示为： （像平面坐标系）
   p0=⎛⎝⎜x0y01⎞⎠⎟c0   和   p1=⎛⎝⎜x1y11⎞⎠⎟c1

这时，由共面得到的向量方程可写成：

  (1) 其中，t是两个摄像机光心的平移量;R是从坐标系 C1 到坐标系 C0 的旋转变换，左乘旋转矩阵R的目的是把向量 C1p1→ 在坐标系 C1 下的表示旋转到坐标系 C0 下，注意点 p1 是在坐标系 C1 中的表示。 
  一个向量a [a1 a2 a3]叉乘一个向量b可以表示为一个反对称矩阵乘以向量b的形式这时由向量a表示的反对称矩阵(skew symmetric matrix)如下: 
所以式1可以写成： 我们用 [t]× 来表示t向量组成的反对称矩阵。在之前那篇讲lie的代数中，我们用 t^ 表示的向量t的反对称矩阵，在这边博客中这种表示也会采用(写起来方便)。 
  本征矩阵 E 定义为  E=[t]×R ，它是一个3x3的矩阵： 

一个3x3的矩阵是本征矩阵的充要条件是对它奇异值分解后，它有两个相等的奇异值，并且第三个奇异值为0

计算本征矩阵E、尺度scale的来由

将矩阵相乘的形式拆开得到 
 
上面这个方程左边进行任意缩放都不会影响方程的解： 
   (x0x1x0y1x0y0x1y0y1y0x1y11)E33⎛⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜E11/E33E12/E33E13/E33...1⎞⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟=0  
所以E虽然有9个未知数，但是有一个变量 E33 可以看做是缩放因子，因此实际只有8个未知量，这里就是尺度scale的来由，后面会进一步分析这个尺度。 
  AX=0，x有8个未知量，需要A的秩等于8，所以至少需要8对匹配点。有了匹配点后，就只需要求解最小二乘问题了，上面这个方程的解就是矩阵A进行SVD分解 A=UΣVT 后，V矩阵最右边那一列的值。另外如果这些匹配点都在一个平面上那就会出现A的秩小于8的情况，这时会出现多解，会让你计算的E可能是错误的。

然而在实际计算过程中，匹配点坐标存在误差，这会使得计算出的E可能不会满足之前提到的那条性质，所以我们需要把计算出的E投影到真正的本征矩阵空间，也就是使得它的三个奇异值中两个相等，一个为0 。

投影的方法如下：

从本征矩阵恢复R、T，尺度scale的进一步分析

  我们知道本征矩阵 E 定义为  E=[t]×R或者t^R 。可以用下面的计算式从本征矩阵中恢复R和T， T^ 表示T的反对称矩阵： 

其中 RZ(π/2) 表示绕Z轴旋转90度得到的旋转矩阵： 
从R,T的计算公式中可以看到R,T都有两种情况，组合起来R,T有4种组合方式。由于一组R,T就决定了摄像机光心坐标系C的位姿，所以选择正确R、T的方式就是，把所有特征点的深度计算出来，看深度值是不是都大于0，深度都大于0的那组R,T就是正确的。

这里我们有必要进一步的分析下尺度scale，从R，T的计算公式中，可以发现平移向量 t 和 E 的奇异值 Σ 有关。再回顾一下之前求 E 时候的方程以及提到的伸缩: 

AE=AUΣV=0AsE=AUsΣV=0

所以，之前推导过程中所有对E进行的尺度伸缩实际上都是作用在奇异值 Σ 上。也就是说这个尺度scale实际作用的是两个相机之间的平移，图示如下： 

这个图简单明了的演示了这种平移缩放作用。从图中也可以看出，由于尺度scale的关系，不同的t，决定了以后计算点P的深度也是不同的，所以恢复的物体深度也是跟尺度scale有关的，这就是论文中常说的结构恢复structure reconstruction，只是恢复了物体的结构框架，而不是实际意义的物体尺寸。并且要十分注意，每两对图像计算E并恢复R,T时，他们的尺度都不是一样的，本来是同一点，在不同尺寸下，深度不一样了，这时候地图map它最头痛了，所以这个尺度需要统一。 
  那么如何让scale之间统一呢？如果你一直采用这种2d-2d匹配计算位姿的方式，那每次计算的t都是在不同尺度下的，Davide Scaramuzza的论文《Visual odometry I》的computing the relative scale那部分讲了一种方法使得相邻位姿间的不同的尺度s经过缩放进行统一。我们已经知道出现尺度不一致是由于每次都是用这种计算本征矩阵的方式，而尺度就是在计算E时产生的。所以尺度统一的另一种思路就是后续的位姿估计我不用这种2d-2d计算本征 E 的方式了，也就说你通过最开始的两帧图像计算E恢复了R,T，并通过三角法计算出了深度，那我就有了场景点的3D坐标，后续的视频序列就可以通过3Dto2d(opencv里的solvePnp)来进行位姿估计，这样后续的视频序列就不用计算本征矩阵从而绕过了尺度，所以后续的视频序列的位姿和最开始的两帧的尺度是一样的了。但是，这样计算又会产生新的问题–scale drift。因为，两帧间的位姿总会出现误差，这些误差积累以后，使得尺度不再统一了。

下一篇的将会讲单应矩阵的理论。