写在前面

四个nodelet，这已经是最后一个了，代码约1000行，包含的内容也是最多的。

4. SLAM建图

在hdl_graph_slam_nodelet.cpp里定义了继承自Nodelet的HdlGraphSlamNodelet类。同样的，也需要实现onInit和cloud_callback。

4.1 onInit()

比起前几个，多定义了一个mt_nh = getMTNodeHandle()，这是个多线程句柄。
参数初始化：
帧id；地图分辨率；最大关键帧更新量；unique_ptr（生产者和订阅者）的reset，stddev的赋值.

sync.reset(new message_fliters::TimeSynchrizer<nav_msgs::Odometry,sensor_msgs::PointCloud2>(*odom_sub,*cloud_sub,32));
sync->registerCallback(boost::bind(&HdlGraohSlamNodelet::cloud_callback,this,_1,_2));

sync是一个最多支持九个传感器的时间同步器，当odom_sub和cloud_sub的Header里的时间戳一致，则触发cloud_callback，bind里的_1和_2是占位符。
给定graph更新间隔；给定map_cloud更新间隔；

subscribers：
odom_sub订阅/odom,
cloud_sub订阅/filtered_points
imu_sub 订阅/gpsimu_driver/imu_data，调用imu_callback
floor_sub订阅/floor_detection/floor_coeffs调用floor_coeffs_callback
如果使能gps，
gps_sub订阅/gps/geopoint；
nmea_sub订阅/gpsimu_driver/nmea_sentence；
navsat_sub订阅/gps/navsat。
publishers：
markers_pub发布/hdl_graph_slam/markers
odom2map发布/hdl_graph_slam/odom2pub
map_points_pub发布/hdl_graph_slam/map_points,信息队列为1。
read_until_pub发布/hdl_graph_slam/read_until

dump_service_server发布/hdl_graph_slam/dump
save_map_service_server发布/hdl_graph_slam/save_map

创建定时器：

optimization_timer = mt_nh.createWallTimer(ros::WallDuration(graph_update_interval),&HdlGraphSlamNodelet::optimization_timer_callback,this)
map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval),&HdlGraphSlamNodelet::map_points_publish_timer_callback,this)

创建一个基于walltime的定时器，三个参数分别是：定时间隔，调用函数，调用对象。
图优化默认3秒一次，调用optimization_timer_callback；建图默认10秒一次，调用map_points_publish_timer_callback。

4.2 简单介绍

由于代码实在太多，而且涉及到的代码频繁跳转。因此，仅介绍每个函数的基本功能，最后介绍这个nodelet的主流程。

4.2.1 callback

1.callback函数是处理msg的回调函数。

cloud_callback对cloud进行处理，筛选合适的关键帧存入key_frame_queue；
nmea_callback，navsat_callback接受卫星导航信息（现存的卫星定位系统有GPS，GALILEO，GLONASS，NAVSAT，北斗），将其转换成gps信息，并调用gps_callback将gps存入gps_queue；
imu_callback 将imu信息存入imu_queue
floor_coeffs_callback 将地面系数存入floor_coeffs_queue
有两个callback函数是受定时器控制的，放后面介绍。

2.flsuh函数用于处理队列中的信息

flush_keyframe_queue() 向graph_slam中添加node和keyframes之间的edge；
flush_gps_queue()找到与每个关键帧时间戳最接近的gps信息，将信息转换到utm中，然后向graph里加入先验xyz边。
flush_imu_queue()找到与每个关键帧时间戳最接近的imu信息，向graph中添加先验的quat_edge和vec_edge，即倾角和加速度信息。
flush_floor_queue()处理地板检测得到的系数，向graph中添加plane_edge
上述处理中，均引入了鲁棒核，以增加鲁棒性，若没有添加相应edge或node，将返回false。

3.两个定时callback函数

optimization_timer_callback 十秒触发一次，进行后端优化。进入后先调用上面的四个flush函数，如果返回的值均为false，则直接终止。若任一为true，则调用detect进行闭环检测，并在graph加入相应边，这里的边与keyframe之间的边是一样的，因此会使用同样的调用函数add_se3_edge。优化以后，生成snapshot，供建图函数使用，并分别调用odom2map_pub和markers_pub（create_marker_array也在此处调用。）因此这个函数就是每十秒一次，将所有需要的约束均加入graph，利用g2o进行优化
map_points_publish_timer_callback 每十秒触发一次，进行建图。过程很简单，就是从读取optimization中产生的snapshot，然后转成cloud_msg发不出去。

4.可视化函数carete_marker_array
就是拿来产生node edge和闭环球的可视化，不过这部分代码很多数字的含义我理解不了，所以暂时不做解读。

5.两个服务函数

dump_service 用于转储程序内部数据 (point clouds, floor coeffs, odoms, and pose graph) 到指定目录。参数为 destination.
save_map_service 用于存储生成的地图到指定目录。参数为 utm resolution destination.

4.3 主流程

其实写到这里整体的流程已经很明显了。原始点云数据raw_points_data发送到prefiltering，被预处理后变成filtered_points发送到scan_matching和floor_detection里，前者利用NDT或ICP等方法进行3D点云配准，向hdl_graph_slam输出弱关键帧的位姿信息（odom代码中其实有两种关键帧，弱关键帧的采样频率更高，供优化使用。后面的强关键帧直接作为建图的素材）；后者进行地面检测，输出地板平面系数至hdl_graph_slam。
由于这是一个松耦合的多传感系统，因此imu和gps是直接传到hdl_graph_slam作为优化边。
到了hdl_graph_slam中，选取合适的强关键帧，利用imu gps 地面系数闭环检测作为优化条件，优化后输出相应的map。输出时为了避免点云重叠过多，使用了octree滤波，分辨率由用户定义。

4.4 扩展资料

ROS多传感器时间同步机制TimeSynchronizer
boost:bind用法
iterator->second解释
boost::optional的用法
std::transfom的用法
dynmic_cast的用法

hdl_graph_slam源码分析(4)——SLAM建图相关推荐

Nginx源码分析：3张图看懂启动及进程工作原理
图一:nginx 启动及内存申请过程分析任何程序都离不开启动和配置解析.ngx 的代码离不开 ngx_cycle_s 和 ngx_pool_s 这两个核心数据结构,所以我们在启动之前先来分析下. 内 ...
一步一步手绘Spring AOP运行时序图（Spring AOP 源码分析）
相关内容: 架构师系列内容:架构师学习笔记(持续更新) 一步一步手绘Spring IOC运行时序图一(Spring 核心容器 IOC初始化过程) 一步一步手绘Spring IOC运行时序图二(基于XM ...
Python 的 heapq 模块源码分析
作者:weapon 来源:https://zhuanlan.zhihu.com/p/54260935 起步 heapq 模块实现了适用于Python列表的最小堆排序算法. 堆是一个树状的数据结构,其中 ...
Python3.5源码分析-内建模块builtins初始化
Python3源码分析本文环境python3.5.2. 参考书籍<<Python源码剖析>> python官网 Python3模块初始化与加载 Python的模块分为内建的模 ...
java观察者模式类图_设计模式（十八）——观察者模式（JDK Observable源码分析）...
1 天气预报项目需求,具体要求以下: 1) 气象站能够将天天测量到的温度,湿度,气压等等以公告的形式发布出去(好比发布到本身的网站或第三方).java 2) 须要设计开放型 API,便于其余第三方也能 ...
jQuery 2.0.3 源码分析 Deferred（最细的实现剖析，带图）
Deferred的概念请看第一篇 http://www.cnblogs.com/aaronjs/p/3348569.html ******************构建Deferred对象时候的流程图* ...
【Android 性能优化】应用启动优化 ( 安卓应用启动分析 | Launcher 应用简介 | Launcher 应用源码简介 | Launcher 应用快捷方式图标点击方法分析 )
文章目录一. Launcher 应用简介二. Launcher 应用源码简介三. Launcher 图标点击方法分析一. Launcher 应用简介 Launcher 应用 : Android ...
jdk1.8 源码分析导图
以下总结全部基于 jdk1.8,详细源码分析见 GitHub 链接:https://github.com/zchen96/jdk1.8-source-code-read 一.非并发幕布导图链接:ht ...
仿爱奇艺视频，腾讯视频，搜狐视频首页推荐位轮播图（二）之SuperIndicator源码分析
转载请把头部出处链接和尾部二维码一起转载,本文出自逆流的鱼:http://blog.csdn.net/hejjunlin/article/details/52510431 背景:仿爱奇艺视频,腾讯视频 ...

hdl_graph_slam源码分析(4)——SLAM建图

文章目录