跑通hdl_graph_slam开源算法
文章目录
一、整体介绍
hdl_graph_slam是使用3D LIDAR的实时6DOF SLAM的开源ROS软件包。它基于3D Graph SLAM，以及基于NDT扫描匹配的测距法估计和回路检测。它还支持多种图形约束，例如GPS，IMU加速度（重力矢量），IMU方向（磁传感器）和地板（在点云中检测到）。我们已经在室内和室外环境中使用Velodyne（HDL32e，VLP16）和RoboSense（16通道）传感器测试了此封装。
github地址在这里
学习的原因：研究一个开源的完整的SLAM软件包无疑是快速上手SLAM的最好方式。而之所以选择这个算法，主要是因为实验室的雷达就是Robosense 16线雷达，而这个算法的介绍里提到已用该雷达测试过封装。
如果大家对本文中遇到的问题有好的建议，欢迎留言。
二、 环境配置可能遇到的坑
1. 常规安装流程
- 安装依赖
首先按照README.md里的Requirements装需要的库和Ros packages。注意，g2o不可以使用最新的，至少Ubuntu16.04（ros-kinetic）不可以。解决方法同样在README里列出来了。在此，直接引用：
sudo apt-get install libsuitesparse-dev
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
git checkout a48ff8c42136f18fbe215b02bfeca48fa0c67507
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=RELEASE
make -j8
sudo make install
可选项最好也安装了，看样子应该是可以根据处理速度调整播放速度。这个文件我没编译成功，issue上提问了，也还没有收到回复，具体的在下文会详述。
sudo pip install ProgressBar2
- git clone
运行如下代码即可。如果速度过慢，参考我的另一篇博客提速。
cd ~/catkin_ws/src
git clone https://github.com/koide3/hdl_graph_slam.git
source devel/setup.bash
cd ..
catkin_make
2. catkin_make报错
这部分报错信息我没保存，大致是和Eigen有关的warning和note，这里主要的原因是Eigen版本不合适。可以自行去Issue里找解决方案。
系统自带的Eigen是3.2.92，我查到的是要降到3.2.1，但我没找到3.2.1的资源，所以把它升到了Eigen3.3.7，同样编译通过。
查询Eigen版本的方法如下：
cd /usr/include/eigen3/Eigen/src/Core/util
gedit Macros.h
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 92
表示版本为3.2.92。
3. 提示不存在progressbar这个Module
这主要是因为py文件所需要的progress头文件不存在。一般产生的实际原因是，该py文件用python2编译，但你的库安装在了python3里，这种时候，或者切换默认的python版本，或者用python2.7 -m pip install --usr progreebar这样的命令在python2里也安装一个库。
切换python版本的方式见我的另一篇博客
4. bag_palyer.py 编译失败
出错的命令如下：
rosrun hdl_graph_slam bag_player.py hdl_501_filtered.bag
报错信息是：float object is not iterable，即float对象不可迭代。
查看代码后出错位置是：for w in self.widgets
这个出错文件是progreebar.py里的，代码是没问题的，所以应该是调用他的地方传入了一个float对象，而不是list。
我暂时不知道怎么改，不过因为本来就是可选项，也就没怎么花心思。
三、代码解析
这部分可以参考知乎专栏里的解读，链接如下:
hdl_graph_slam源码解读
需要注意的是，大神的解读是针对slam算法的相关步骤，而关于ros的node、topic、tf、rviz等设置都是略过不提的。而实际上，真的想了解这个软件包的运行机制，了解其通信机制和调用也很重要。尤其是，我之前跑这样的近万行代码经验仅限于裸机嵌入式编程，也就是说是单线程的。此外接触的就是java和C#的触发模式。因此，我其实还需要花更多的功夫去解读这个软件包的ros相关设置。
1. vs code 头文件的配置
Ubuntu环境下使用vs code是需要自己写json文件的，默认情况下，只能检索项目文件夹里存在的头文件，这会导致pcl，ros相关的include全都飘红，而且函数跳转功能也无法使用。
解决方法如下：
在code界面按crtl+shift+P，在弹出的搜索框中输入C/C++：Edit，选择 C/C++：Edit Configuration（JSON）项。我的默认文件如下：
{
"configurations": [
{
"name": "Win32",
"includePath": [
"${workspaceFolder}/**"
],
"defines": [
"_DEBUG",
"UNICODE",
"_UNICODE"
],
"windowsSdkVersion": "10.0.18362.0",
"compilerPath": "D:/Program Files/VS/VC/Tools/MSVC/14.25.28610/bin/Hostx64/x64/cl.exe",
"cStandard": "c11",
"cppStandard": "c++17",
"intelliSenseMode": "msvc-x64"
}
],
"version": 4
}
我们需要改动的地方如下：
"includePath": [
"${workspaceFolder}/**",
"/usr/"include/**,
"/opt/ros/kinetic/include/**"
],
"browse": [
"${workspaceFolder}/**",
"/usr/"include/**,
"/opt/ros/kinetic/include/**"
],
这个我其实也是一知半解，总之includePath中包含你本地的头文件库，browse复制includePath内容即可，输入时注意标点符号。里面的/**是正则表达式，表示包含里面的所有，不加其实也可以。
如果不太清楚自己的头文件放在哪里了，使用locate命令即可。比如要找ros/ros.h，则键入locate ros/ros.h，将会返回文件所在路径。
2. 观察topic、node
想要了解通信机制，必须要查看各节点之间的关系，tf的相关信息。因此下面罗列一些ros的相关命令。
#罗列topic
$ rostopic list
#罗列node
$ rosnode list
#用ctrl+C关闭node后，会有一些遗留，可以用cleanup清除。
$ rosnode cleanup
#查看node之间topic的传递方式
$ rosrun rqt_graph rqt_graph
#查看tf的publish关系，将会在当前目录下保存一个frames.pdf。
$ rosrun tf view_frames
#evince是Ubuntu默认安装的pdf浏览器，可以直接打开上面保存的文件。
$ evince frames.pdf
#查看两个frame之间的变换关系：
$ rosrun tf tf_echo[source_frame][target_frame]
#查看当前的tf树：
$ rosrun rqt_tf_tree rqt_tf_tree
#显示当前坐标变换树的信息，主要是名称和实时的时间延时
$ rosrun tf tf_monitor
#以TransformStamped消息类型的数组显示所有父子frame的位姿转换关系
$ rostopic echo /tf
四、hdl_graph_slam运行
1. 增加效率的自动命令
想使用roscd，roslaunch这些功能除了要运行roscore外，还需要source一下bash文件。但由于每打开一个terminal，都需要输入一次其实很麻烦。因此，可以在bashrc里写入这条命令，这样每次打开都会自动source。处理如下：
gedit ~/.bashrc
在打开的文档最末尾添加source ~/catkin_ws/devel/setup.bash保存退出即可。
关于此操作的延伸知识，见链接。
2. 室内demo
具体参看github中的README。命令行如下：
rosparam set use_sim_time true
roslaunch hdl_graph_slam hdl_graph_slam_501.launch
roscd hdl_graph_slam/rviz
rviz -d hdl_graph_slam.rviz
rosbag play --clock hdl_501_filtered.bag
第一行代码很重要，因为这个代码要求使用仿真时间而不是wall time，具体见链接。
第二行就是运行结点。
第三行和第四行是为了访问特定的rviz文件，这里面已经设置好了可视化的参数。
第五行即播放包，要注意必须有 --clock，该参数是在包播放时同时publish时间，以便和前面的sim_time搭配。
软件包作者还写了service，一个是保存内部数据，另一个是保存slam最后建出来的地图。保存地图命令如下：
rosservice call /hdl_graph_slam/save_map "resolution: 0.05
destination: '/full_path_directory/map.pcd'"
注意：destination必须分行输入，方法是反斜杠\，敲回车，/full_path_directory应该改成你希望保存的位置，比如/home/**/catkin_ws ，map.pcd可以改成你喜欢的名字。比较简单的方式是输入到save_map后，双击TAB，这样就只需要填空就可以了。
不过要注意的是，建出来的map.pcd无法用pcl_viewer来显示，我估计原因是因为点云类型是六维度的XYZI点，而一般能显示的点云是四维度的XYZI点。不过可以通过自己写查看类来显示。想要拷贝图到某个路径下，可以cp map.pcd /目标路径
3. 室外建图
同室内。差别是在室外建图中引入了GPS和IMU信息融合，参考系关系也更加复杂。
4. 跑自己的bag
1.录制自己的bag
这里我使用的是Robosen 16线雷达。Ubuntu平台下的rslidar包安装方式见Github，不再赘述。需要注意的是，rslidar的launch里默认的坐标系是rslidar，topic是/rslidar_points，这是后面修改的根据。
录制命令如下：
rslaunch rslidar_pointcloud rs_lidar_16.launch
rosbag record -a
rosbag的相关介绍如下，来自官网：
rosbag record -a
在当前所在文件夹，记录下所有被list的publihed data。
rosbag info <name_bag>
输出当前文件夹内的 name_bag包内信息，包括时长，类型，topics，信息数。
rosbag play <name_bag>
重新播放bag文件，但需要原来接收的那个node在运行中。通常会有0.2s的延迟，这是为了让订阅者可以准备接收。-s 选项用于延迟，-r选项用于改变指令速度，如 -r 2会使得指令速度 翻倍。
有些时候，由于包太大了，信息太多，此时可能需要只记录某些特定的topics。此时可以在后面指定topics，如：
rosbag record -O subset /turtle1/cmd_vel /turtle1/pose
注：-o 是自定义名字，但后面会自带日期。而-O是直接覆盖整个文件名字，即不再自xiugai 动追加日期。
2.修改launch文件和rviz文件
因为我是在室内录制的，因此修改的hdl_graph_slam_501.launch。
将<arg name="points_topic" default="/velodyne_points" />改成<arg name="points_topic" default="/rslidar_points" />。
将<node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />改成<node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link rslidar 10" />。
rviz文件里所有的velodyne改成rslidar，当然也可以直接运行原来的rviz，自己手动调节，然后保存设置即可。
3.运行
直接按着上面的室内建模运行即可。不过要注意，这个方法里在rviz里是看不到建出来的图的，因为没有设置map相对其他节点的转移矩阵。
五、未来的工作
- 研究代码的ros部分
- 实时slam
- 更多的集成功能，如直接显示pcd
- 优化匹配。
推荐阅读