orbslam2-基础理论（二)

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

这篇将要讲orbslam2相机初始位置估计的另一种方法：单应矩阵Ｈ（使用场景：平面，视差小）和map point的求法

（一）：单应矩阵

在计算机视觉中，平面的单应性被定义为一个平面到另外一个平面的投影映射。因此一个二维平面上的点映射到摄像机成像仪上的映射就是平面单应性的例子。如果点Q到成像仪上的点q的映射使用齐次坐标，这种映射可以用矩阵相乘的方式表示。若有一下定义：

Q=[X Y Z  1]T

q=[x y  1]T

则可以将单应性简单的表示为：

q=sHQ

这里引入参数s，它是任意尺度的比例（目的是使得单应性定义到该尺度比例）。通常根据习惯放在H的外面。

H有两部分组成：用于定位观察的物体平面的物理变换和使用摄像机内参数矩阵的投影。

物理变换部分是与观测到的图像平面相关的部分旋转R和部分平移t的影响之和，表示如下

W=[R t]

这里R为3*3大小的矩阵，t表示一个一个3维的列矢量。

摄像机内参数矩阵用M表示，那么我们重写单应性如下：

q=sWMQ  where W是相机内参矩阵

我们知道单应性研究的是一个平面上到另外一个平面的映射，那么上述公式中的~Q，就可以简化为平面坐标中的~Q'，即我们使Z=0。即物体平面上的点我们用x,y表示，相机平面上的点，我们也是用二维点表示。我们去掉了Z方向的坐标，那么相对于旋转矩阵R，R可以分解为R=[r1 r2 r3]，那么r3也就不要了。

其中H为：

H=sM[r1 r2 t]

是一个3×3大小的矩阵.

 故最终的单应性矩阵可表示如下：

q=sHQ

我们可以从Ｈ矩阵中恢复Ｒ，Ｔ。但是同样存在尺度问题（指Ｔ），所以只有在初始位置估计的时候才用到Ｅ或Ｈ。以后一般都用opencv的ＰＮＰ求解也就是3Ｄ-2Ｄ的映射。

(二）我们除了要求相机的pose以外，还要求map point。下面来讲解map point的求法。

   对于三维空间中的一点  P ，我们假设第一个摄像机坐标系 C1 就是世界坐标系，P在世界坐标系下的表示为 P=(x,y,z,1)T ，这时，摄像机坐标系 C1 的外参数矩阵 M1 为单位矩阵。 点 P 和光心的连线交第一个图像平面于点 p1  ,注意这里的 p1 是在摄像机坐标系的坐标表示，不是在图像坐标系下，这在上一篇博客中已经强调。同理，和第二个摄像机光心连线交第二幅图像于点 p2  。  pi 在各自摄像机坐标系中的表示为：
p1=⎛⎝⎜x1y11⎞⎠⎟c1   和   p2=⎛⎝⎜x2y21⎞⎠⎟c2 摄像机坐标系 C2 的外参数矩阵为 M2 ，由于摄像机坐标系 C1 就是世界坐标系，所以有 在推导本征矩阵 E 的时候，我们说 R 是从坐标系 C2 到坐标系 C1 的旋转变换矩阵，即 12R 。t是平移，更确却的说是光心 C2 在 C1 中的坐标表示,即 1t2 。所以在通过8点法求出的R,T以后，得到的从摄像机坐标2变换到到摄像机坐标系1的变换矩阵为
12H=[R3×30t3×11] 而这里的外参数矩阵 M2 是将世界坐标系中的一点P投影到摄像机坐标系 C2 。所以 M2=21H=12H−1 ，只需去掉H矩阵的最下面一行齐次坐标就行了。

所以，通过本征矩阵得到R,t以后，要计算摄像机坐标系 C2 外参数矩阵的程序如下：

有了外参矩阵，我们就可以得到这些点坐标的关系：

由于光心 Ci ，三维坐标点P，以及 pi 三点共线，所以向量 Cipi、CiP 的叉乘应该为0，上述方程又可以转化为： 这又是一个要用最小二乘求解的线性方程方程组 ,和求本征矩阵一样，计算矩阵A的SVD分解，然后奇异值最小的那个奇异向量就是三维坐标P的解。

在以上计算P三维坐标的推导过程中，可以看到和本征矩阵E是息息相关的，E和我们的尺度紧密相连，所以计算出来的深度和尺度scale也是直接相关的。同时，根据这种三角法(triangulation )计算的深度，其实是不怎么靠谱的，一般只是拿这个做一个初始值。

两个射线夹角越小，误差协方差越大。所以点到光心连线组成的射线向量在orbslam中是有明确记录的。