ORB-SLAM2详解（三）自动地图初始化

最新推荐文章于 2023-03-21 16:55:40 发布

Channon_

最新推荐文章于 2023-03-21 16:55:40 发布

阅读量767

点赞数 1

分类专栏： SLAM 文章标签： slam 计算机视觉

原文链接：https://blog.csdn.net/u010128736/article/details/53218140?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522159902838619725250338784%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=159902838619725250338784&biz_id=

版权

SLAM 专栏收录该内容

8 篇文章 5 订阅

订阅专栏

转自http://blog.csdn.net/u010128736/

系统的第一步是初始化，ORB_SLAM使用的是一种自动初始化方法。这里同时计算两个模型：用于平面场景的单应性矩阵H和用于非平面场景的基础矩阵F，然后通过一个评分规则来选择合适的模型，恢复相机的旋转矩阵R和平移向量t。

一、找到初始对应点

在当前帧Fc中提取ORB特征点，与参考帧Fr进行匹配。如果匹配点对数过少，就重置参考帧。这一步骤在Tracking.cc中的Tracking::MonocularInitialization函数中。

int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

nmatches表示匹配到的对应点对数。

二、同时计算两个模型

在找到对应点之后，开始调用Initializer.cc中的Initializer::Initialized函数进行初始化工作。为了计算R和t，ORB_SLAM为了针对平面和非平面场景选择最合适的模型，同时开启了两个线程，分别计算单应性矩阵Hcr和基础矩阵Fcr。如下所示：

thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

这里的H和F分别满足下列关系：

$\begin{array}{l} x_{c}=H_{c r} x_{r} \\ x_{c}^{T} F_{c r} x_{r}=0 \end{array}$

线程threadH调用Initializer::FindHomography函数，计算单应性矩阵H，采用归一化的直接线性变换（normalized DLT）。线程threadF调用Initializer::FindFundamental函数计算基础矩阵F，使用归一化8点法。为了评估哪个模型更合适，文中使用 $S_H$ 和 $S_F$ 来计算各自的分值。分别调用Initializer::CheckHomography函数和Initializer::CheckFundamental函数进行计算，计算的方法如下所示，其中 $S_M$ 统一表示 $S_H$ 和 $S_F$ ：

$\begin{array}{c} S_{M}=\sum_{i}\left(\rho_{M}\left(d_{c r}^{2}\left(x_{c}^{i}, x_{r}^{i}, M\right)+\rho_{M}\left(d_{r c}^{2}\left(x_{c}^{i}, x_{r}^{i}, M\right)\right)\right.\right. \\ \rho_{M}\left(d^{2}\right)=\left\{\begin{array}{l} \tau-d^{2} \text { if } d^{2}<T_{M} \\ 0 \text { if } d^{2} \geq T_{M} \end{array}\right. \end{array}$

其中， $d_{c r}^{2}$ 和 $d_{ r c}^{2}$ 表示对称的转换误差，分别是从当前帧到参考帧的变换误差和参考帧到当前帧的变换误差。这里：

$\begin{array}{c} T_{H}=5.99, \quad T_{F}=3.84 \\ \tau=T_{H} \end{array}$

三、模型选择

文中认为，当场景是一个平面、或近似为一个平面、或者视差较小的时候，可以使用单应性矩阵H，而使用基础矩阵F恢复运动，需要场景是一个非平面、视差大的场景。这个时候，文中使用下面所示的一个机制，来估计两个模型的优劣：

$R_{H}=\frac{S_{H}}{S_{H}+S_{F}}$

当 $R_H$ 大于0.45时，选择从单应性变换矩阵还原运动。不过ORB_SLAM2源代码中使用的是0.4作为阈值，如下：

    // Compute ratio of scores
    float RH = SH/(SH+SF);

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    if(RH>0.40)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

四、运动恢复（sfm）

选择好模型后，就可以恢复运动。
（1）从单应性变换矩阵H中恢复
在求得单应性变化H后，本文使用FAUGERAS的论文[1]的方法，提取8种运动假设。这个方法通过可视化约束来测试选择合理的解。但是如果在低视差的情况下，点云会跑到相机的前面或后面，测试就会出现错误从而选择一个错误的解。文中使用的是直接三角化8种方案，检查两个相机前面具有较少的重投影误差情况下，在视图低视差情况下是否大部分云点都可以看到。如果没有一个解很合适，就不执行初始化，重新从第一步开始。这种方法在低视差和两个交叉的视图情况下，初始化程序更具鲁棒性。程序中调用Initializer::ReconstructH函数恢复运动。

(2)从基础矩阵F中恢复
在得到基础矩阵F，并且一直摄像机内参K的情况下，可以计算得到本质矩阵E，然后使用[2]中的方法，恢复出4个运动假设的解。这一部分的理论推导在之前博客做过介绍。其中基础矩阵F得到本质矩阵E的公式如下所示：

$E_{r c}=K^{T} F_{r c} K$

同样的，这4个解中只有一个是合理的，可以使用可视化约束来选择，本文使用与单应性矩阵做sfm一样的方法，即将4种解都进行三角化，然后从中选择出最合适的解。这里使用的是Initializer::ReconstructF函数。

五、集束调整

最后使用一个全局集束调整（BA)，优化初始化结果。这一部分是在Tracking.cc中的CreateInitialMapMonocular()函数中，使用了如下语句：

Optimizer::GlobalBundleAdjustemnt(mpMap,20);

如下图所示，PTAM和LSD_SLAM在这个数据集中，会将所有点初始化在一个平面上，而ORB_SLAM会一直等到有足够的视差，才使用基础矩阵，得到最正确的初始化。由于ORB-SLAM对初始化的要求较高，因此初始化时可以选择一个特征丰富的场景，移动摄像机给它提供足够的视差。另外，由于坐标系会附着在初始化成功的那帧图像的位置，因此每次初始化不能保证在同一个位置。

这里写图片描述

引用

[1]. O.D.Faugeras and F.Lustman. “Motion and structure from motion in a piecewise planar environment.” International Journal of Pattern Recognation and Artificial Intelligence, vol.2, no.03,pp.485-508,1988.
[2]. R.Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, 2nd ed. Cambridge University Press,2004.

Channon_

关注

1
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
ORB-SLAM2详解（三）自动地图初始化

转自http://blog.csdn.net/u010128736/系统的第一步是初始化，ORB_SLAM使用的是一种自动初始化方法。这里同时计算两个模型：用于平面场景的单应性矩阵H和用于非平面场景的基础矩阵F，然后通过一个评分规则来选择合适的模型，恢复相机的旋转矩阵R和平移向量t。一、找到初始对应点在当前帧Fc中提取ORB特征点，与参考帧Fr进行匹配。如果匹配点对数过少，就重置参考帧。这一步骤在Tracking.cc中的Tracking::MonocularInitialization函数.
复制链接

扫一扫