主流的激光SLAM算法有hector、gmapping、karto、cartographer。很多同学使用ROS默认自带的gmapping、hector等比较多，这次带大家一起尝试下传说中的google cartographer，看看效果差别有多大。
一、概述
1.主流激光SLAM算法
- hector：是⼀种结合了鲁棒性较好的扫描匹⽅法2D_SLAM⽅法和使⽤惯性传感系统的导航技术。传感器的要求较⾼，⾼更新频率⼩测量噪声的激光扫描仪，不需要⾥程计。使空中⽆⼈机与地⾯⼩车在不平坦区域运⾏存在运⽤的可能性。作者利⽤现代激光雷达的⾼更新率和低距离测量噪声，通过扫描匹配实时地对机器⼈运动进⾏估计。所以当只有低更新率的激光传感器时，即便测距估计很精确，对该系统都会出现⼀定的问题。
- gmapping：是⼀种基于粒⼦滤波的激光SLAM算法，它已经集成在ROS中，是移动机器⼈中使⽤最多的SLAM算法。基于粒⼦滤波的算法⽤许多加权粒⼦表⽰路径的后验概率，每个粒⼦都给出⼀个重要性因⼦。但是，它们通常需要⼤量的粒⼦才能获得⽐较好的的结果，从⽽增加该算法的的计算复杂性。此外，与PF重采样过程相关的粒⼦退化耗尽问题也降低了算法的准确性。
- karto：是基于图优化的SLAM算法，⽤⾼度优化和⾮迭代cholesky矩阵进⾏稀疏系统解耦作为解。图优化⽅法利⽤图的均值表⽰地图，每个节点表⽰机器⼈轨迹的⼀个位置点和传感器测量数据集，箭头的指向的连接表⽰连续机器⼈位置点的运动，每个新节点加⼊，地图就会依据空间中的节点箭头的约束进⾏计算更新。路标landmark越多,内存需求越⼤,然⽽图优化⽅式相⽐其他⽅法在⼤环境下制图优势更⼤。
- cartographer：是google推出的一套基于图优化的SLAM算法，cartographer采⽤基于google⾃家开发的ceres⾮线性优化的⽅法，cartographer的量点在于代码规范与⼯程化，⾮常适合于商业应⽤和再开发。并且cartographer基于submap⼦图构建全局地图的思想，能有效的避免建图过程中环境中移动物体的⼲扰。并且cartographer⽀持多传感器数据（odometry、IMU、LaserScan等）建图，⽀持2D_SLAM和3D_SLAM建图。
2.google-cartographer建图算法原理分析
Cartographer主要是通过闭环检测来消除构图过程中产生的累积误差。用于闭环检测的基本单元是submap。一个submap是由一定数量的laser scan构成。将一个laser scan插入其对应的submap时，会基于submap已有的laser scan及其它传感器数据估计其在该submap中的最佳位置。submap的创建在短时间内的误差累积被认为是足够小的。然而随着时间推移，越来越多的submap被创建后，submap间的误差累积则会越来越大。因此需要通过闭环检测适当的优化这些submap的位姿进而消除这些累积误差，这就将问题转化成一个位姿优化问题。当一个submap的构建完成时，也就是不会再有新的laser scan插入到该submap时，该submap就会加入到闭环检测中。闭环检测会考虑所有的已完成创建的submap。当一个新的laser scan加入到地图中时，如果该laser scan的估计位姿与地图中某个submap的某个laser scan的位姿比较接近的话，那么通过某种 scan match策略就会找到该闭环。Cartographer中的scan match策略通过在新加入地图的laser scan的估计位姿附近取一个窗口，进而在该窗口内寻找该laser scan的一个可能的匹配，如果找到了一个足够好的匹配，则会将该匹配的闭环约束加入到位姿优化问题中。Cartographer的重点内容就是融合多传感器数据的局部submap创建以及用于闭环检测的scan match策略的实现。
主要由 Local SLAM 和 Global SLAM 两部分组成：
Local SLAM：
- 利用里程计(Odometry)和IMU数据进行轨迹推算，给出小车位姿估计值
- 将位姿估计值作为初值，对雷达数据进行匹配，并更新位姿估计器的值
- 雷达一帧帧数据经过运动滤波后，进行叠加，形成子图(submap)
Global SLAM：
- 回环检测
- 后端优化，全部子图形成一张完整可用的地图
二、cartographer安装
Google开源的代码包含两个部分：cartographer和cartographer_ros。cartographer主要负责处理来自雷达、IMU和里程计的数据并基于这些数据进行地图的构建，是cartographer理论的底层实现。cartographer_ros则基于ros的通信机制获取传感器的数据并将它们转换成cartographer中定义的格式传递给cartographer处理，与此同时也将cartographer的处理结果发布用于显示或保存，是基于cartographer的上层应用。
源码：
- https://github.com/cartographer-project/cartographer
- https://github.com/cartographer-project/cartographer_ros
接下来我们安装cartographer+cartographer_ros，环境为ubuntu20.04+noetic+pi4b：
# 1.安装编译工具wstool、rosdep、ninjd
sudo apt update
# on Noetic
sudo apt install -y python3-wstool python3-rosdep ninja-build stow
# other distribution
sudo apt-get install -y python-wstool python-rosdep ninja-build stow
# 2.下载cartographer_ros源码
mkdir ~/catkin_ws_carto && cd ~/catkin_ws_carto
wstool init src
wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src
# 3.安装依赖包proto3、deb等
sudo rosdep init #如果执⾏报错，可以直接忽略
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
./src/cartographer/scripts/install_debs_cmake.sh #req libs
./src/cartographer/scripts/install_abseil.sh #abseil
./src/cartographer/scripts/install_proto3.sh #proto3
./src/cartographer/scripts/install_ceres.sh #ceres-solver 如果代码下不下来，修改.sh里git clone https://github.com/ceres-solver/ceres-solver.git
# 4.修改配置
vim ~/catkin_ws_carto/src/cartographer_ros/cartographer_ros/configuration_files/revo_lds.lua
tracking_frame = "base_footprint", #底盘坐标frame
published_frame = "odom", #机器人odom frame（tf关联关系：cartographer map -> odom -> base_footprint -> base_laser_link）
provide_odom_frame = false, #是否使用cartographer提供的坐标系
use_odometry = true, #是否使用机器人的odom
# 5.编译和安装cartographer_ros整个项⽬⼯程
cd ~/catkin_ws_carto
catkin_make_isolated --install --use-ninja
source install_isolated/setup.bash
下方是tf关联关系示例（使用带底盘的机器人）：
*注：如果你只是pc直接接上激光雷达进行测试，那么就只把revo_lds.lua里的tracking_frame、published_frame改为laser的frame_id即可。
三、Bag测试
# 2D bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag
# revo_lds bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/revo_lds/cartographer_paper_revo_lds.bag
roslaunch cartographer_ros demo_revo_lds.launch bag_filename:=${HOME}/Downloads/cartographer_paper_revo_lds.bag
四、实车测试
1.配置cartographer slam
vim cartographer.launch
<launch>
<arg name="simulation" default= "false"/>
<arg name="version" default="$(env ROS_DISTRO)"/>
<param name="/use_sim_time" value="$(arg simulation)" />
<!-- cartographer_node -->
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename revo_lds.lua"
output="screen">
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node"/>
</launch>
2.SLAM建图测试
2.1.2D雷达建图
# 无地图场景导航并建图
roslaunch robot_navigation robot_slam_laser.launch slam_methods:=cartographer
# pc端
roslaunch robot_navigation navigation_rviz.launch
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
# 保存地图
roscd robot_navigation/maps
rosrun map_server map_saver -f map
# 使用地图
roslaunch robot_navigation robot_navigation.launch
roslaunch robot_navigation navigation_rviz.launch
2.2. 2D雷达+摄像头联合建图
# 启动激光雷达+base_control
roslaunch robot_navigation robot_lidar.launch
# 启动深度摄像头
roslaunch robot_vslam camera.launch
# 启动rgbd+lidar联合建图
roslaunch robot_vslam rtabmap_rgbd_lidar.launch
# pc端查看
roslaunch robot_vslam rtabmap_rviz.launch
# pc端控制移动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
# 重启rtabmap_rgbd_lidar，打开movebase+自动定位（允许后退）
roslaunch robot_vslam rtabmap_rgbd_lidar.launch localization:=true planner:=dwa move_forward_only:=false
五、结论
从实际测试来看，与gmapping/hector必须控制移动速度并且容易出现衔接问题相比，cartographer即使在非常快速的移动时，建出的图还原度依然很高，基本都是一次到位，形状非常规整，不同区域衔接的地方做的很好，同时建图速度很快，地图文件很小，实在是slam建图的利器！
参考：
