ORBSlam2学习研究(Code analysis)-ORBSlam2中的闭环检测和后端优化LoopClosing

ORBSlam2中的闭环检测和后端优化LoopClosing

LoopClosing在现在的Slam系统中是非常重要的一个部分,VO总是会有累计误差,而LoopClosing通过检测是否曾经来过此处,进行后端优化,可以将这个累计误差缩小到一个可接受的范围内。从而使得Slam系统应对大范围场景时,拥有更高的鲁棒性和可用性。

ORBSlam2中的LoopClosing闭环检测线程主要进行闭环检测,并在检测到闭环的时候计算Sim3变换,进行后端优化。

这里写图片描述
这里写图片描述

检测闭环 DetectLoop

首先我们会检测当前关键帧在Covisibility图中的附近关键帧,并会依次计算当前关键帧和每一个附近关键帧的BoW分值,通过我们所得到分数的最低分,到数据库中查询,查找出所有大于该最低分的关键帧作为候选帧,用以检测闭环。

1. 如果地图中的关键帧数小于10,那么不进行闭环检测

2. 获取共视关键帧,并计算他们和当前关键帧之间的BoW分数,求得最低分

3. 通过上一步计算出的最低分数到数据库中查找出候选关键帧,这一步相当于是找到了曾经到过此处的关键帧们


这一步是非常重要的,相当于是在为闭环检测做前期的预处理

  1. 首先,得到与当前帧的链接的关键帧。
    set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();

  2. 然后,在地图中搜索与当前关键帧共享一个BOW word的关键帧,并排除上一步搜集到的附近关键帧,得到候选帧,这些候选帧基本上都是曾经来到此处看到的。

  3. 统计这些帧中,与当前关键帧的Bow共有Word最多的单词数maxCommonWords

  4. 计算最低共有单词数阈值minCommonWords = maxCommonWords*0.8f,并搜寻候选帧中,共有单词数大于minCommonWords的关键帧,并计算它与当前帧的score分值。

  5. 将这些候选帧进行聚类,相连的候选帧聚为同一类,并计算每一组的累计得分,得到最高累计分的组,并得分数最高组的最高分的关键帧。
    这样的话,会把一些拥有很高分数的独立出来的关键帧给去掉,因为他并没有跟其他关键帧相连,没有连续性,所以这一步就是去除这些得分很高的错误关键帧

  6. 得到最后一个阈值 minScoreToRetain = 0.75f*bestAccScore,再通过这个最低阈值,计算出这些候选帧中比这个分值高的关键帧,并保存起来返回。

4. 对候选关键帧集进行连续性检测(有的文章也翻译为一致性检测)

这一步相当于是检测上一步得到的关键帧集是否是真的可以使用的。
(由于这段网络上很少有介绍,所以这部分我是按照我自己的理解写的,如果有错误,希望有人能够指出)
这里写图片描述

闭环连续性是什么

每次我们在上一步的数据库查询操作里找到了候选关键帧之后,基本上找到的候选关键帧就是我们所要找的闭环关键帧,但是为了防止错误进行闭环检测,我们非常有必要再进行一次连续性检测,连续性检测的意思就是,是否我们在三个当前的关键帧内都同时发现了某一个闭环候选帧的话,那么就表明当前的SLAM系统已经闭环。

详细解释

比方说,在上图中,通过数据库查询,我们可以在A点得到闭环候选关键帧有两个(1,2)。在下一次进入DetectLoop函数的时候,我们当前拿到的关键帧是B,那么在B点我们可以得到的闭环候选关键帧是(1,2,3),以此类推,在再下一次进入DetectLoop函数的时候,也就是在C点的时候,我们这时对比到的闭环候选关键帧是(2,3),所以2这个闭环候选关键帧被检测到了三次。在LoopClosing中,mnCovisibilityConsistencyTh = 3 一致性共视阈值被设为3,并且如果一旦有一个闭环候选关键帧被检测到3次,系统就认为检测到闭环。

在每一次进行完闭环候选连续性检测之后,该线程都会保存在这一关键帧下的计数情况,保存的变量也就是mvConsistentGroups,以供给下一轮循环使用。

计算Sim3 ComputeSim3

既然上一步我们已经检测到了闭环,那么我们现在就需要开始进行后端优化了。该函数主要工作就是在当前关键帧和闭环帧之间找到更多的对应点,并通过这些对应点计算当前关键帧和闭环帧之间的Sim3变换,求解出Rt和s。在这一过程中,共进行了三次对应点的查找工作。

  1. 对每一个闭环帧,通过BoW的matcher方法进行第一次匹配,匹配闭环帧和当前关键帧之间的匹配关系,如果对应关系少于20个,则丢弃,否则构造一个Sim3求解器并保存起来。这一步主要是对效果较好的闭环帧构建Sim3求解器
  2. 对上一步得到的每一个满足条件的闭环帧,通过RANSAC迭代,求解Sim3。
    对于这里的Sim3的Ransac迭代,我在网上也没有找到很多的解释,我们都知道Ransac是根据一组包含异常数据的样本数据集,计算出数据的数学模型参数的方法,在这里使用Ransac的方法,可以大大提高对杂点干扰的鲁棒性。

  3. 通过返回的Sim3进行第二次匹配。
    刚才得到了Sim3,所以现在要利用Sim3再去进行匹配点的查找,本次查找的匹配点数量,会在原来的基础上有所增加。

  4. 使用非线性最小二乘法优化Sim3.
    在拿到了第二次匹配的结果以后,要通过这些匹配点,再去优化Sim3的值,从而再精细化Rt和s。
    const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

  5. 恢复闭环关键帧和其邻居关键帧的MapPoint地图点
    最后一步求解匹配点的时候,将所指的闭环帧和与其链接的关键帧所看到的所有的MapPoint都恢复出来。通过这个方法,可以尽可能得到我们当前关键帧所能看到的所有的地图点,为下一步做投影匹配,得到更多的匹配点做准备。

  6. 使用投影得到更多的匹配点,如果匹配点数量充足,则接受该闭环。
    最后我们通过投影匹配得到了尽可能多的匹配点,通过匹配点的数量判断是否接受闭环。

纠正闭环后端优化 CorrectLoop

在上一步求得了Sim3和对应点之后,就纠正了当前帧的位姿,但是我们的误差不仅仅在当前帧,此前的每一帧都有累计误差需要消除,所以这个函数CorrectLoop就是用来消除这个累计误差,进行整体的调节。

  1. 如果有全局BA运算在运行的话,终止之前的BA运算。
    这里写图片描述

  2. 使用传播法计算每一个关键帧正确的Sim3变换值

    • 得到当前关键帧的附近关键帧的Sim3位姿并用纠正的Sim3位姿与其相乘,保存结果到CorrectedSim3变量中。
      这里写图片描述

    • 使用反向投影的方法,将当前关键帧和邻居观察到的地图点得到三维场景下的位姿,并更新关键帧的位姿
      这里写图片描述

    • 将当前关键帧的地图点进行融合,其实融合就是判断如果是同一个点的话,那么将当前的地图点强制换成原本的地图点。
      这里写图片描述

    • 使用已纠正的位姿,将在循环关键帧附近观察到的地图点投影到当前的关键帧和邻居,融合重复点。
      这里写图片描述

    • 更新链接,检测新连接
      这里写图片描述

  3. 优化图
    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
    使用非线性最小二乘图优化的方法来优化EssentialGraph。

  4. 全局BA优化
    这里写图片描述

参考链接
LoopClosing中的Sim3求解 http://www.cnblogs.com/shang-slam/p/6480863.html
ORBSlam2闭环检测 http://blog.csdn.net/u010128736/article/details/53409199
ORBSlam2原文翻译 http://www.sohu.com/a/154011668_715754
泡泡机器人ORBSlam2解析 http://rosclub.cn/post-505.html

  • 19
    点赞
  • 61
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
### 回答1: ORBSLAM3是一个开源的视觉SLAM系统,可以用于在无GPS信息的情况下实现室内导航和定位。以下是关于ORBSLAM3源码下载的相关信息: 要下载ORBSLAM3源码,你可以访问ORBSLAM3项目的GitHub页面。在GitHub上,你可以浏览到ORBSLAM3的最新源代码,并且可以选择将其克隆到你的本地计算机。 要从GitHub上下载ORBSLAM3源码,你可以按照以下步骤进行: 1. 打开你的web浏览器,访问GitHub的主页。 2. 在GitHub的搜索栏输入&ldquo;ORBSLAM3&rdquo;。 3. 在搜索结果找到ORBSLAM3项目的主页,点击进入。 4. 进入ORBSLAM3项目主页后,你会看到项目的描述、文档和源码。 5. 在主页的右上方,有个绿色的按钮,上面写着&ldquo;Code&rdquo;,点击它。 6. 在弹出的下拉菜单,你可以选择不同的下载方式。如果你熟悉git,你可以选择使用git命令将源码克隆到你的本地计算机。如果不熟悉git,你可以选择直接下载源码的zip压缩包。 7. 选择适合你的方式进行下载,并将源码保存到你的本地计算机。 一旦你完成了下载,你就可以开始使用ORBSLAM3的源码了。你可以解压zip压缩包(如果你选择了zip方式下载)并浏览源码的内容。在源码的根目录,你可以找到主要的源代码文件和其他相关文件。 请注意,源码的下载可能需要一些时间,具体取决于你的网络连接速度。此外,使用ORBSLAM3前,建议你详细阅读相关的文档和使用说明,以了解如何正确配置、编译和运行该系统。 ### 回答2: ORB-SLAM3是一种开源的用于实时单目视觉SLAM算法框架。为了获取ORB-SLAM3的源代码,你可以按照以下步骤进行下载。 1. 首先,打开ORB-SLAM3的官方GitHub页面(https://github.com/UZ-SLAMLab/ORB_SLAM3)。 2. 在该页面上,你可以看到绿色的按钮&ldquo;Code&rdquo;,点击它会弹出下拉菜单。 3. 在下拉菜单,你可以选择&ldquo;Download ZIP&rdquo;选项。点击此选项后,源代码将以ZIP文件的形式下载到你的计算机。 4. 下载完成后,你可以将ZIP文件解压缩到你选择的目录。解压后你会看到源代码的文件夹。 现在你已经成功地下载了ORB-SLAM3的源代码。你可以通过查看源代码、阅读文档、学习示例等方式来了解和使用ORB-SLAM3算法框架。在研究SLAM算法或者开发相关应用时,这个源码将为你提供很好的参考和基础。 记得遵守源代码的许可证,并尊重作者的工作。你可以在源码查找相关许可证和使用条款,以确保你遵循了正确的使用规定。
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值