BLAM源码解析（五）—

上一节介绍了BLAM的帧间匹配和帧图匹配，代码简洁明了。

本节介绍BLAM的回环检测模块。具体代码块如下：

 // Check for new loop closures.bool new_keyframe;if (HandleLoopClosures(msg_filtered, &new_keyframe)) {// We found one - regenerate the 3D map.PointCloud::Ptr regenerated_map(new PointCloud);loop_closure_.GetMaximumLikelihoodPoints(regenerated_map.get());mapper_.Reset();PointCloud::Ptr unused(new PointCloud);mapper_.InsertPoints(regenerated_map, unused.get());// Also reset the robot's estimated position.localization_.SetIntegratedEstimate(loop_closure_.GetLastPose());} else {// No new loop closures - but was there a new key frame? If so, add new// points to the map.if (new_keyframe) {localization_.MotionUpdate(gu::Transform3::Identity());localization_.TransformPointsToFixedFrame(*msg_filtered, msg_fixed.get());PointCloud::Ptr unused(new PointCloud);mapper_.InsertPoints(msg_fixed, unused.get());}}// Visualize the pose graph and current loop closure radius.loop_closure_.PublishPoseGraph();

我们知道视觉SLAM一般通过词袋模型进行回环，激光SLAM中的佼佼者Cartographer通过分支定界+栅格匹配进行回环，等等。BLAM由于构建的是点云地图，不涉及栅格更新，因此通过最简单的ICP进行回环。接下来逐行解析：

首先使用HandleLoopClosures，向位姿图中加入新位姿节点：

  if (!loop_closure_.AddBetweenFactor(localization_.GetIncrementalEstimate(),  // MJYcovariance, &pose_key)) {

接下来使用AddKeyScanPair，向回环检测模块加入关键帧。

  if (!loop_closure_.AddKeyScanPair(pose_key, scan)) {

然后最重要的：在可回环范围内寻找回环，可回环范围由第一节介绍的几个参数决定。

  if (!loop_closure_.FindLoopClosures(pose_key, &closure_keys)) {

寻找回环的过程中将回环边加入位姿图。

让我们回到BlamSlam.cc，继续HandleLoopClosures以下的内容，使用了如下函数将历史关键帧的点云转换到全局坐标系，合并到一起，加入到回环检测模块的mapper_中：

    loop_closure_.GetMaximumLikelihoodPoints(regenerated_map.get());

注意，这个点云是经过回环校正的点云，因此是经过回环检测优化的map。

然后用回环校正的位姿，更新定位模块localization_中的位姿，即：

void PointCloudLocalization::SetIntegratedEstimate(const gu::Transform3& integrated_estimate) {integrated_estimate_ = integrated_estimate;// Publish transform between fixed frame and localization frame.geometry_msgs::TransformStamped tf;tf.transform = gr::ToRosTransform(integrated_estimate_);tf.header.stamp = stamp_;tf.header.frame_id = fixed_frame_id_;tf.child_frame_id = base_frame_id_;tfbr_.sendTransform(tf);
}

最终发布位姿图。没有订阅者是不发布的。

  loop_closure_.PublishPoseGraph();

至此，BLAM源码介绍完毕，接下来尽情调参，实现你自己特定场景的SLAM吧！！！！

BLAM源码解析（五）—— 回环检测相关推荐

cartographer 源码解析 (五)
相关链接: cartographer 源码解析(一) cartographer 源码解析(二) cartographer 源码解析(三) cartographer 源码解析(四) cartograph ...
(Nacos源码解析五)Nacos服务事件变动源码解析
Nacos源码解析系列目录 Nacos 源码编译运行 (Nacos源码解析一)Nacos 注册实例源码解析 (Nacos源码解析二)Nacos 服务发现源码解析 (Nacos源码解析三)Nacos 心 ...
BLAM源码解析（一）—— 模块初始化
接下来写一个开源SLAM算法系列吧,本期介绍BLAM算法.BLAM算法是伯克利的一位大牛写的,首先名字就很有意思,BLAM表示 B(erkeley) L(localization) A(nd) M(a ...
Spring源码解析(五)-Bean的实例化流程(上)
在前面已经完成了对需要实例化bean的收集并封装成BeanDefinition,并且将BeanPostProcess等组件进行了提前实例化.接下来就到了容器启动的最后一步,也是最复杂的一步-实例化be ...
Tomcat源码解析五：Tomcat请求处理过程
前面已经分析完了Tomcat的启动和关闭过程,本篇就来接着分析一下Tomcat中请求的处理过程. 在开始本文之前,咋们首先来看看一个Http请求处理的过程,一般情况下是浏览器发送http请求-> ...
Netty源码解析-Netty内存泄露检测
前言: 在前一篇文章中,我们介绍了ByteBuf的引用计数器的使用,基本所有的ByteBuf都有相关计数的功能,那么这个计数有什么用呢. 实际主要就是做内存泄露检测用的.本文就其如何做检测来进行说明. ...
【NDPI】源码解析之深度包检测分析（一）
(Albert.2019.4.28) 文章目录: 前言: 正文: 一.nDPI深度包检测流程: 二.重要结构体的源码分析 1.ndpi_ethdr.ndpi_iphdr.ndpi_tcphdr.ndp ...
spring源码解析五
2019独角兽企业重金招聘Python工程师标准>>> 1.创建用于承载属性的BeanDefinition 这是一个接口,在spring中存在三种实现:RootBeanDefinit ...
BLAM源码解析（四）—— 基于ICP的位姿更新
第三节我们介绍了定时器的定时回调,实现对激光数据的批量循环处理,在每一个激光数据的循环当中,除了一开始filter_的点云过滤,最重要的其实是下面的基于ICP的位姿更新,即 if (!odometry ...

BLAM源码解析（五）—— 回环检测

至此，BLAM源码介绍完毕，接下来尽情调参，实现你自己特定场景的SLAM吧！！！！

BLAM源码解析（五）—— 回环检测相关推荐

最新文章

热门文章