一起学ORBSLAM2（5）ORBSLAM的单目视觉处理方式

转载请注明原创地址：https://blog.csdn.net/qq_30356613/article/category/6897125

单目相机由于深度是未知的，因此我们需要对其进行初始化，在ORB-SLAM中将其用单独的类来表示，并将它写成单独的文件initializer.cc，注意单目相机即使在进行初始化之后，仍然存在尺度问题，初始化将第一帧的位移作为单位长度，后面的深度和位移都是依据这一标准进行的，所以尺度问题是单目slam的理论缺点。

单目slam初始化需要两帧进行，第一帧作为参考初始化帧，第二帧作为当前帧。在第一帧来临时建立初始化器，并将该帧的特征点存储起来，初始化器Initializer的构造函数中有三个参数，分别是初始化参考帧的引用，标准差sigma和RANSAC迭代次数iterations（选择8点时使用），initializer.cc只存在一个外部调用函数initialize()，此函数通过计算当前帧的相机位姿和匹配点对应的3D点来对单目slam初始化。初始化中通过RANSAC的方法来优化初始化的效果，用8点法来同步计算单应性矩阵H和基础矩阵F，从而计算出两者分别的得分，得分方式是通过计算投影误差的和来确定。得分越高证明当下模型下的矩阵越可信，然后选择得分高的矩阵进行分解，从而得到相机位姿和三维匹配点。

其中主要涉及到的算法分别是：8点-DLT方法计算基础矩阵，8点-DLT方法计算单应矩阵，根据基础矩阵分解得到相机位姿和3D匹配点，根据单应矩阵分解得到相机位姿和3D匹配点。

一、8点DLT方法计算基础矩阵：

（参考高翔slam14讲）

首先应明白基础矩阵的几何意义是什么？它解决的问题是如何通过两幅图像中的像素匹配点来估算两幅图像点的变换矩阵F（基础矩阵）。

设的空间位置为：,在两帧图像中的像素坐标分别为：

则有：，这里K为相机内参数矩阵，R，t为旋转矩阵和平移矩阵。

取齐次坐标有: 

取，这里x1，x2是两个像素点的归一化平面上的坐标。代入上式得：

两边同时左乘，得到：

两边同时左乘，得到：

将p1，p2带入上式得到：

从而得到对极约束的代数表示，其中为基础矩阵F，为本质矩阵E

然后根据8点法求解本质矩阵E，进而求解基础矩阵F

设,分别是匹配点的归一化坐标，，并将E写成向量形式则有：

 

用SVD方法求解上述方程，E的解对应的是最小奇异值对应的奇异向量，证明过程参考《Multiple view geometry in computer vision》.page592 A5.3

由此求得本质矩阵E，进而求取基础矩阵F

二、本质矩阵分解得到旋转矩阵R和平移向量t

（参考 multiple view geometry in computer vision 9.6节的相关知识）

上个算法中通过8点法得到了本质矩阵E，但实际上本质矩阵需要满足自身的约束，对于一个本质矩阵E来讲，它的奇异值有两个相等，并且第三个为零。证明如下：

上面可以看出，其中S是向量t对应的反对称矩阵，形式如下：设t(t1,t2,t3)

则，而却对于任意的反对称矩阵S，我们都有如下结论：

如果一个实反对称矩阵S，则可以将S分解为,B是一个对角块矩阵，,这里,并且S的特征值是纯虚数，奇数阶的反对称矩阵是奇异的。

因此，S可以分解为：（1）,这里的U是正交的，

引入正交矩阵，可以得到（2）

不计尺度得到：，从而有

由于W,U,R都是正交矩阵，因此也为正交矩阵，故可以令

从而得到，故本质矩阵的奇异值为1,1,0

其中根据（1）可得

R是一个旋转矩阵可以被写成,从而有：

其中，由上式（2）得到所以或者

由此可得到：或

由于，并且有,因此存在的约束

同时，在约束下计算该方程的解，用SVD方法可以得到t对应的解是S最小特征值对应的特征向量，即

证明参考multiple view geometry in computer vision A5.3 P592  如下：

由于U是酉矩阵，根据范数的酉不变性因此存在，并且存在，我们需要极小化，在遵从的情况下，令，因此有，由于，并且，所以有，，得到

因此得到x的解为U的最后一行，即U的最小向量。证闭。

上述可以得到，但是符号并不能确定，所以t，-t

综上可得，E可以被分解为四种结果，分别为：

 

将其用几何的形式表示得到：

 

其中bcd图有负的深度值，因此可以舍去，只要a图符合要求。

所以根据在两个相机坐标系下坐标点的深度可以筛选出正确的结果，从而得到唯一正确R和t

三、8点DLT方法计算单应矩阵H：

（参考multiple view geometry in computer vision 第四章 P89）

单应性矩阵与基础矩阵的不同在于，单应性矩阵是针对平面上点集合来讲的，而基础矩阵基于三维点集合来讲

对于二维到二维的点集，有，注意到这里的齐次性，因此我们有：

，  所以存在：，提取为矩阵的形式为：

由于上式矩阵质为2，因此只有两个不相关的向量，因此针对每一对匹配点，都有：

，注意到到这里的为9维向量，9个自由度，由于尺度因此有8个自由度，因此4个点对就能得到h的解，根据SVD方法可以得出。参考上面8点法求基础矩阵中SVD方法。由此求得单应矩阵H

四、单应矩阵分解得到旋转矩阵R和平移向量t

（参考文章：Motion and Structure from Motion in a Piecewise Planar Environment）

（同时参考吴博注释版的ORBSLAM的PPT）

 

 

 

 

 

 

从而得到R和t的8种组合，然后根据匹配点在相机坐标系下的深度值以及重投影误差来得到一组最优值，从而结算出当前相机位姿。

五、三角测量得到匹配三维点坐标

通过基础矩阵和单应性矩阵求解相机位姿过程中并没有利用到匹配点的三维点坐标，但在SLAM中的建图下计算匹配点的三维坐标是必要的，因此，我们用三角测量的方式来得到三维点坐标，如上所示，已经得到了相机的位姿R,t，而且相机的内参数矩阵是已知的，结合两者，我们可以得到由相机像素坐标到三维点坐标点的变换矩阵P=T*K，其中T是由R和t决定的相机位姿变换矩阵，K为相机的内参数矩阵。

因此对于每一匹配点我们都有x=P*X，x’=P’*X，x，x’分别为匹配点在帧1和帧2的像素坐标，P,P’分别为相机变换矩阵，由于存在尺度问题，因此，我们用齐次坐标进行求解，对于齐次坐标存在x叉乘P*X，即：

,其中，，

令，从而得到：，即：

同理可以得到的表达式，结合两者得到：

，通过SVD方法求解该方程组得到三维点坐标的齐次形式，然后将齐次坐标转化为非齐次坐标就得到了匹配点的三维点坐标。