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

最新推荐文章于 2024-05-05 23:15:56 发布

鱼尾sama

最新推荐文章于 2024-05-05 23:15:56 发布

阅读量1.3k

点赞数 2

分类专栏： SLAM相关文章标签： c++ 自动驾驶

本文链接：https://blog.csdn.net/a850565178/article/details/105587560

版权

SLAM相关专栏收录该内容

17 篇文章 9 订阅

订阅专栏

文章目录

写在前面
4. SLAM建图

写在前面

四个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滤波，分辨率由用户定义。