用Robosense跑通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. 常规安装流程

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

2. 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时warning

这部分信息我没保存，大致是和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.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。
我暂时不知道怎么改，不过因为本来就是可选项，也就没怎么花心思。
调用他的的确传入了一个float，不知道怎么改。略过吧。

三、代码解析

这部分可以参考知乎专栏里的解读，链接如下:
hdl_graph_slam源码解读
需要注意的是，大神的解读是针对slam算法的相关步骤，而关于ros的node、topic、tf、rviz等设置都是略过不提的。而实际上，真的想了解这个软件包的运行机制，了解其通信机制和调用也很重要。因此，我其实还需要花更多的功夫去解读这个软件包的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。不过可以通过自己写cpp文件来显示。想要拷贝图到某个路径下，可以cp map.pcd /目标路径
ps:我的路径使用~/catkin_ws/map.pcd报错了，报错信息如下：

ERROR:service [/hdl_graph_slam/save_map] responded with an error :: [pcl::PCDWriter::wirterBinary] Error during open!

我估计原因是因为不识别~。建议直接幅值如下代码（XYW是我的Ubuntu的账户名，可以自行替换）：

rosservice call /hdl_graph_slam/save_map "resolution: 0.05
destination: '/home/XYW/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包内信息，包括时长，类型，topics，信息数。
rosbag play
重新播放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。
将改成。
将改成。

可以直接运行原来的rviz，自己手动调节（把/volodyne_points的点云改成/rslidar_points），然后保存设置即可，直接将rviz文件中的velodyne用rslidar替换理论可行，但实际的运行是存在问题的，比如我遇到的一个问题是关键帧抽取的时候都提示超时了，但我觉得应该不是node和nodelet的差异。
3.运行
直接按着上面的室内建模运行即可。

5. 实时SLAM

这里使用的是Robosense的16线激光雷达，其点云数据的topic为/rslidar，具体的改动方法与前面的bag包改动一致

因为我是在室内录制的，因此修改的hdl_graph_slam_501.launch。
将改成。
将改成。

，不过运行时必须将系统时间变成实际的wall time：rosparam set use_sim_time false。因为通过rqt_graph命令查看node关系，可以发现差别只是rslidar_point由包发布还是由rs_lidar_16.launch里的node发布。

另外，由于有关键帧的筛选的步骤，因此不用担心在一个地方停留过久，导致地图数据爆炸。而使用save_map功能时，最好关掉rslidar16的launch，即切断输入流，否则会报错。

五、算法测评

1.NDT、NDT-OMP、autowar NDTGPU加速。
详细测试见：pcl_ndt ndt_omp ndt_gpu对比测评
总结：pcl自带的ndt实现3.98s，ndt_omp112ms,ndt_gpu 190ms。其中omp是ndt的多线程版本，gpu是ndt的gpu加速版。最后的配准效果，三个应该差不多，因此只需要比较配准时间即可。可以看到多线程情况和gpu的速度相近。
在自己的机子（i7-7820HK [email protected]*8）跑室内bag的ndt-omp，八个核的CPU占有平均每个20-30%，内存占用约0.3G。
测试用的命令：htop (相比自带的top，可以看到每个核的占用率)
安装命令：sudo apt install htop

六、未来的工作

• 研究代码的ros部分
• 实时slam
• 显示pcd的小软件
  - [ ] 优化匹配。

七、在办公室录制的rosbag

由于有很多人需要robosense的包，下面给出百度云链接，大概200多MB，请自取。环境为室内，离地高度约1.7m。
链接：https://pan.baidu.com/s/1oyIIQ9DMA_l_iZCLoBn4xg
提取码：p8zc