一起自学SLAM算法：13.2 运行SLAM构建地图
连载文章，长期更新，欢迎关注：
在“xiihoo机器人”中推荐使用基于激光的Cartographer和基于视觉的ORB-SLAM2来建图，并且可以利用Cartographer和ORB-SLAM2进行联合建图来提升定位的稳定性。
13.2.1 运行激光SLAM建图
关于Cartographer的安装与运行细节可以参考8.2.3节，这里就再简单回顾一下运行流程。首先启动机器人平台相关的节点，也就是在命令行终端运行一键启动文件xiihoo_all_sensor.launch。
#一键启动
roslaunch xiihoo_bringup xiihoo_all_sensor.launch然后启动Cartographer建图节点，也就是在命令行终端运行建图启动文件xiihoo_mapbuild.launch。关于建图效果调优，可以修改*.lua配置文件中的参数。
#激光建图
roslaunch cartographer_ros xiihoo_mapbuild.launch接下来，就可以遥控机器人在环境中移动，进行地图构建了。不同的机器人支持不同的遥控方法，比如手柄遥控、手机APP遥控、键盘遥控等。这里使用键盘遥控方式来遥控“xiihoo机器人”建图，键盘启动命令如下。
#首次使用键盘遥控，需要先安装对应功能包
sudo apt install ros-melodic-teleop-twist-keyboard
#启动键盘遥控
rosrun teleop_twist_keyboard teleop_twist_keyboard.py在键盘遥控程序终端下，通过对应的按键就能控制底盘移动了。这里介绍一下按键的映射关系，前进（i）、后退（，）、左转（j）、右转（l），而增加和减小线速度对应按键w和x，增加和减小角速度对应按键e和c。
遥控底盘建图的过程中，可以打开rviz可视化工具查看所建地图的效果以及机器人实时估计位姿等信息。
#启动rviz
rviz当环境扫描完成，并路径回环到起始点后，就可以将Cartographer构建的地图结果保存下来。cartographer_ros提供了将建图结果保存为*.pbstream专门的方法，其实就是一条命令。其实就是调用cartographer_ros提供的叫/write_state这个名字的服务，服务传入参数/home/ubuntu/map/carto_map.pbstream为地图的保存路径。
#保存地图
rosservice call /write_state /home/ubuntu/map/carto_map.pbstream由于Cartographer构建的地图是pbstream格式，后续导航中使用到的地图是GridMap格式。所以需要将pbstream格式转换成GridMap格式，转换其实就是下面这一句命令。注意这是一条长命令，不需要换行。
#启动地图格式转换
roslaunch cartographer_ros xiihoo_pbstream2rosmap.launch pbstream_filename:=/home/ubuntu/map/carto_map.pbstream map_filestem:=/home/ubuntu/map/carto_map13.2.2 运行视觉SLAM建图
关于ORB-SLAM2的安装与运行细节可以参考9.1.3节，这里就再简单回顾一下运行流程。首先启动机器人平台相关的节点，也就是在命令行终端运行一键启动文件xiihoo_all_sensor.launch。
#一键启动
roslaunch xiihoo_bringup xiihoo_all_sensor.launch然后启动ORB-SLAM2单目建图节点，也就是在命令行终端运行建图节点并载入视觉词袋模型ORBvoc.txt和配置文件mono.yaml。
#视觉建图
rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt mono.yaml然后与Cartographer一样，也是用键盘遥控机器人完成建图，然后保存地图到本地，这些操作就不再赘述了。
13.2.3 运行激光与视觉联合建图
其实用ros-navigation中的map_server加载Cartographer构建的地图，然后利用amcl进行全局定位，就可以将自主导航跑起来了。由于amcl仅基于2D地图与激光雷达数据的匹配，因此全局定位极易丢失。那么可以将amcl全局定位替换成由Cartographer重定位提供全局定位，也就是先用Cartographer构建地图，然后载入已保存的地图进行重定位。由于Cartographer重定位是基于子图（submap）的匹配，而生成子图需要花费较长时间收集多帧激光雷达数据，也就是说Cartographer重定位实时性较差。可以利用Cartographer和ORB-SLAM2进行联合建图，然后载入已保存的联合地图同时进行Cartographer重定位和ORB-SLAM2重定位，这样ORB-SLAM2重定位可以弥补实时性较差的缺点。如图13-4所示，为联合建图的大致过程。
图13-4 联合建图
使用cartographer和orb-slam2算法分别进行2D栅格地图和3D点云地图的构建，同时记录下两个系统共有的pose_graph约束，得到的2D栅格地图和3D点云地图如图13-5所示。
图13-5 2D栅格地图和3D点云地图
参考文献
【1】 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.