ORB_SLAM1 论文_代码
1 框架:
：ORB_SLAM1 是一个基于特征（ORB）单目SLAM系统，拥有回环检测、重定位功能。该算法的系统框架：
2 主要贡献有：
- 该系统主要包含：跟踪、建图、重定位、回环四个线程，均使用同一种ORB特征，而且具有视角不变性和光照不变性。
- 使用共视图(covisibility graph)，将特征点的跟踪和建图主要集中在局部共视区域，而与全局地图的大小无关，因此在大的环境下实时执行。
- 采用基于位姿图(pose graph)优化的实时闭环，文中成为本质图(Essential Graph)。由系统维护的生成树(spanning tree)、回环链(loop closure links)和共视图的强边构成。
- 拥有重定位模块，实现实时相机重定位，这使得系统跟踪丢失后恢复，还增强了地图重用。
- 采用基于模型选择的自动的、鲁棒的初始化方法，可以创建平面(planar)和非平面(non-planar)场景的初始地图。Fundamental矩阵和homograph矩阵。
- 采用优胜劣汰(survival of the fittest)的方法选择关键帧和地图点，即宽选取、严剔除。
3 数据结构：
对每个图像进行特征提取，判断是否是关键帧，每个关键帧存储以下数据，每个关键帧具有很多的ORB特征点，每个特征点又包含2中数据。采用”宽进严出“的策略，”宽进”：降低了跟丢的风险。”严出“对冗余关键帧/特征点进行剔除降低了地图/关键帧的重复，降低计算量。
- 关键帧：
相机位姿Tiw，可以将世界坐标系的点转换为相机坐标系下。
相机内参，包括焦距和住点。
从图像帧提取的所有ORB特征，不管其是否已经关联了地图云点， 这些ORB特征点都经过畸变模型矫正过。 - 地图点：
- 它在世界坐标系中的3D位置Xw,i
视角方向，所有视角方向的平均单位向量（将该点与观察该点关键帧的光学中心连接的光线）。
ORB特征描述子Di，在匹配特征点可以通过计算描述子的汉明距离得到。
根据ORB特征的尺度不变性，可观测的地图点的最大距离dmax和最小距离dmin - 共视图和本质图
关键帧：
共视图：关键帧之间的共视信息，用于局部BA/和localmaping BA等
生成树：共视图的最小生成树
本质图：共视图中具有强共视关系的节点连接形成的图
4：关键模块：
目标是计算两帧之间的相对位姿，以三角化一组初始地图点,同时计算两个几何模型，一个是假设为平面场景的单应矩阵，另一个是假设为非平面场景的基础矩阵。然后使用一种启发式方法来选择其中一个模型，并通过该模型恢复出相对位姿。
A：地图自动初始化
- 进行初始的特征匹配 KF_current->KF_reference,如果没有匹配到足够的特征对，则重置参考帧为当前帧。
- 并行计算两个几何模型（单应矩阵和基础矩阵）基于RANSAC方案，基础矩阵使用8个点，单应矩阵使用4个点。每次迭代时我们对每个模型M（单应矩阵H或基础矩阵F）计算一个分数SM：
dcr2和drc2是从某帧到另一帧的对称的转换误差(transfer errors)。TM是局外点去除阈值，该阈值基于χ2测试值为95%（T_H=5.99，T_F=3.84，假设测量误差的标准差为1像素）。Γ被定义为等于T_H，这样两个模型在它们的内部区域中对同一个d的得分相等，也是为了两个模型一致。 - 模型选择 R_H>0.45则选择单应建模，否则选择基础矩阵
- 恢复运动和结构
选择好一个模型后，就可以从中恢复出运动即计算R/t
单应矩阵：直接三角化这8个解对应的二维点，然后检查是否有一种解可以使得所有的地图点都在两个相机的前面，而且重投影误差较小。如果还是没有得到一个可用的解，那么放弃本次初始化且回到步骤
基础矩阵：采用校准矩阵K (calibration matrix)将其转换为本质矩阵：Erc=KTFrcK，然后奇异值分解(singular value decomposition)获得4个解。我们对得到的四个解进行三角化，然后像上面单应矩阵的做法那样获得一个可用解
光束法平差(BA) 3D->2D重投影误差。
B：跟踪tracking线程
-
特征提取:
a) 在8层图像金字塔上提取FAST角点，金字塔尺度因子为1.2，ComputePyramid()。
b) 为了确保特征点均匀分布，将每层图像划分为35*35网格，每格提取至少5个角点。然后检测每格角点数量，如果数量不够则降低提取阈值。
c) 采用四叉树将提取的特征点进行均匀分布。
d) 使用灰度值心法计算每个检测到的特征点的方向，先获取1/4圆的u/v坐标，然后再通过特殊技巧获取整个圆坐标； -
b:根据上一帧/关键帧对位姿进行初始估计；
a) 如果运动模型是空的，imu未初始化或者刚刚完成重定位（参考帧经过优化后位姿比较准）则执行参考帧跟踪：bTrackReferenceKeyFrame()。否则使用速率恒定模型来预测当前相机的位置：TrackWithMotionModel()。失败执行参考帧跟踪；
b) 然后搜索上一帧图像中的特征点在地图点与当前帧图像的匹配点 最后利用搜索到的匹配点对当前相机的位姿进行优化g2o::PoseOptimization 但是如果没有找到足够的匹配点（比如，运动模型失效，非匀速运动），加大搜索范围，搜索地图点附近的点，然后通过寻找到的对应匹配点对来优化当前时刻的相机位姿 -
通过全局重定位对相机位姿进行初始估计
上述两种都跟踪失败则如果跟踪丢失，将当前帧转换为词袋向量，然后在查询识别数据库(recognition datebase)得到若干关键帧作为候选帧。除上述情况外在进行纯重定位时也会执行,执行流程：
a) 对每个候选帧计算与地图点对应的ORB特征，ORBmatcher；
b) 对每个备选关键帧轮流执行MLPnP算法[41]计算当前帧的位姿（RANSAC迭代求解）。
c) 如果我们找到一个姿态能涵盖足够多的有效点，则搜索该关键帧对应的更多匹配地图点。
d)最后，基于找到的所有匹配点对相机位置进一步优化，如果有效数据足够多，则跟踪程序将持续执行。 -
局部地图跟踪
B中2-3步骤整体思路就是：通过一个模型先获得当前帧位姿的一个初始估计值，然后将当前帧与参考帧特征点进行匹配，对初始估计值进行第一次优化仅仅优化pose g2o::PoseOptimization。获得较为准确的pose数据；该步骤就是对优化后的pose数据与地图点进一步进行scan-submap的匹配优化的也是位姿。 获取的局部地图包含一组关键帧K1，他们与当期帧有共同的地图点，还包含在共视图中关键帧K1周围的一组关键帧K2。该局部地图还拥有一个参考帧Kref ∈K1，该参考帧与当前帧共视最多的地图点。现在对于K1，K2中可见视图点，在当前帧中进行如下操作：
a) 计算地图点在当前帧图像中的投影点x。如果投影位置超出图像边缘，就将对应的地图云点删除。
b) 计算当前视图射线v和地图点平均视图方向n的夹角。如果v*n<cos(60)，就删除对应地图点。
c) 计算地图点到相机中心的距离d。如果它不在地图点的尺度不变区间内，即d∉[dmin,dmax]，就删除该地图点。
d) 计算该帧图像的尺度比 d/dmin
e) 对比地图点的特征描述子D和当前帧中还未匹配的ORB特征，在预测的尺度层和靠近x的地图点作最优匹配。
相机位姿通过在该帧的所有的地图点得到优化。 -
新关键帧判定
判定当前帧是否是关键帧。局部地图有一个机制删除多余的关键帧，我们尝试尽可能快的插入关键帧，这使得系统对相机运动更加鲁棒，比如旋转运动。插入关键帧的条件如下：
距离上一次全局重定位后需要超过20帧图像。 确保一个好的重定位，
局部地图构建处于空闲状态，或距上一个关键帧插入后，已经有超过20帧图像。如果局部地图构建处于忙状态的时候插入关键帧，就会发信号去暂停局部BA，这样就可以尽可能快地去处理新的关键帧
当前帧跟踪少于50个地图云点。保证好的跟踪。
当前帧跟踪少于参考关键帧K_ref云点的90%。
C：局部建图LocalMapping线程
该线程是对每个关键帧进行处理构建局部地图的线程，对于经过Tracking线程处理并完成关键帧检测的关键帧，要将其加入到地图中，数据处理流程：
- 关键帧插入 ProcessNewKeyFrame()
首先计算该关键帧的词袋向量.
更新共视图，增加一个新的节点Ki，检查与Ki由共视地图点关系的其它关键帧节点，并添加相应的边。
然后更新生成树，将Ki连接到与它有最多共视点的节点上。 - 地图点剔除 MapPointCulling()
在插入地图点之前，执行严格的地图点剔除策略，每个地图点必须满足两个条件：
跟踪线程必须在超过25%的图像中找到该特征点
如果创建地图点后经过了超过一个关键帧，那么它至少被3个关键帧观测到。
一旦一个地图云点通过测试，它只能在被少于3个关键帧观测到的情况下移除。这样的情况在关键帧被删除以及局部BA排除异值点的情况下发生。这个策略使得我们的地图包含很少的无效数据 - 地图点创建
产生新的地图点，利用当前帧和共视帧进行匹配，单目情况下利用三角化产生地图点，双目则根据视差大小确定直接恢复还是三角化恢复；同时将那些不满足对极约束的匹配点删除。ORB特征点对三角化后，需要满足两个相机坐标均正深度、视差、重投影误差、尺度一致性等条件，通过后则该点被插入地图。起初，一个地图云点通过2个关键帧观测，但它在其他关键帧中也有对应匹配点，所以它可以映射到其他相连的关键帧中 - 局部BA
局部BA优化当前关键帧Kc和在共视图中与Kc连接的其它关键帧Kr，以及被这些关键帧观测到的地图点。看到这些地图点但是没有连接到当前关键帧的其它关键帧保持固定。在有imu数据情况下检测时间和位移是否足够，不够则认为运动幅度过小，不足以初始化imu，在没有imu或者imu设置为badimu时执行LocalInnertialBA()否则执行LocalBundleAdjuestment()
LocalInnertialBA()优化位姿和地图点，加入imu约束
LocalBundleAdjuestment()用于纯视觉或者imu未初始化，优化两轮第一次迭代5次，第二次迭代10次。 - 局部关键帧删除
如果关键帧Kc中90%的点都可以被其他至少三个关键帧同时观测到，那认为Kc的存在是冗余的，我们则将其删除。
该部分的代码流程如图所示：
D：回环检测
回环检测线程接收Kc，Kc是局部建图处理的上一个关键帧，回环线程尝试检测和闭合回环。步骤如下。
- 回环候选检测
首先计算关键帧Ki与Ki在共视图上相邻关键帧(θmin = 30)的相似性，通过词袋向量计算，保留最低分值Smin。然后我们检索图像识别数据库，丢弃所有与Ki相似性分值低于Smin的关键帧。这和DBoW2中归一化分值的操作类似，可以获得好的鲁棒性，DBoW2中计算的是前一帧图像，而我们是使用的共视信息。另外，所有连接到Ki的关键帧都会从结果中删除。为了获得候选回环，我们必须检测到3个连续的候选回环（共视图中相连的关键帧）。如果对Ki来说环境样子都差不多，就可能有几个候选回环。 - 计算相似性变换
单目SLAM系统由7个自由度，3个平移自由度，3个旋转自由度和一个尺度自由度[6]，这可能会导致地图漂移。因此，闭合回环，我们需要计算从当前关键帧Ki到回环关键帧Kl的相似变换，以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。
我们首先将当前帧和回环候选帧进行特征匹配，这个过程请参阅第III-E节。此时我们可以得到3D到3D对匹配。我们对每个候选回环帧执行RANSAC迭代，尝试使用Horn[42]的方法找到相似性变换。如果有足够的内点，我们就能找到相似性变换Sil，然后我们优化它(具体看附录)，然后搜索更多的对应关系。如果Sil有更多的内点，我们将再次优化它，知道Kl被回环接受。 - 回环融合
回环修正的第一步是融合重复的地图点，在共视图中插入新的边。首先通过相似变换Sil矫正当前关键帧的位姿Tiw，这种矫正方法应用于所有与Ki相邻的关键帧，这样回环两端就可以对齐。然后，回环关键帧及其近邻能观测到的所有地图点都映射到Ki及其近邻中，并在映射的区域附近小范围内搜索它的对应匹配点，如第5部分D节所述。所有匹配的地图点和计算Sil过程中的有效数据进行融合。融合过程中所有的关键帧将会更新它们在共视图中的边缘，创建的新边缘将用于回环检测。 - 本质图优化
为了高效的闭合回环，我们在本质图(Essential Graph)上执行一个位姿图优化，如第III-D节描述的那样。这样可以将回环误差分散到各个关键帧上。优化程序通过相似变换校正尺度漂移[6]。误差项和代价计算如附录所示。优化过后，每一个地图点都根据关键帧的校正进行变换。
ORB_SLAM2 算法框架解析： https://blog.csdn.net/weixin_43503355/article/details/121618694
参考：
https://blog.csdn.net/qq_37394634/article/details/105494755
https://zhuanlan.zhihu.com/p/351384972
https://blog.csdn.net/a2392008643/article/details/81781766
扫一扫
301
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
