基于Cartographer的3D SLAM（Lidar+IMU）
1 系统要求
虽然 Cartographer 可以在其他系统上运行，但已确认可以在满足以下要求的系统上运行：
- 64-bit, modern CPU (e.g. 3rd generation i7)
- 16 GB RAM
- Ubuntu 14.04 (Trusty) and 16.04 (Xenial)
- gcc version 4.8.4 and 5.4.0
目前支持以下ROS发行版：
- Indigo
- Kinetic
- Lunar
- Melodic
2 编译与安装
为了编译 Cartographer ROS，我们建议使用wstool和rosdep。为了更快的构编译建，我们还建议使用Ninja。
sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build
在catkin_ws中创建一个新的cartographer_ros工作空间。
mkdir catkin_ws
cd catkin_ws
wstool init src
wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src
安装cartographer_ros的依赖项（proto3和deb软件包）。如果您在安装ROS后已经执行了命令sudo rosdep init，将会输出错误，可以忽略此错误。
src/cartographer/scripts/install_proto3.sh
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
编译和安装：
catkin_make_isolated --install --use-ninja
注意：由于某种原因，无法下载ceres-solver，解决办法是在Github上下载ceres-slover并放到指定的路径下。
3 使用自己的bag进行测试
3.1 录制bag
对于3D Cartographer，必须依赖IMU，具体原因参见：https://google-cartographer-ros.readthedocs.io/en/latest/faq.html#why-is-imu-data-required-for-3d-slam-but-not-for-2d 。
因此，要录制包含Velodyne VLP-16和Xsens MTi-300的数据的bag。
另外，还需要做传感器间的标定，在urdf文件中会用到。
3.2 分析bag
使用cartographer_rosbag_validate工具来自动分析bag中的数据。
cartographer_rosbag_validate -bag_filename path_your_bag.bag
以下是我的输出：
I0816 11:15:15.414490 22913 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/imu_raw" (frame_id: "mti/data"):
Count: 23827 Min: 0.004766 Max: 0.015202 Mean: 0.010000
[0.004766, 0.005810) Count: 48 (0.201452%) Total: 48 (0.201452%)
[0.005810, 0.006853) Count: 1 (0.004197%) Total: 49 (0.205649%)
[0.006853, 0.007897) Count: 0 (0.000000%) Total: 49 (0.205649%)
[0.007897, 0.008940) Count: 2 (0.008394%) Total: 51 (0.214043%)
[0.008940, 0.009984) ####### Count: 8915 (37.415539%) Total: 8966 (37.629581%)
[0.009984, 0.011028) ############ Count: 14812 (62.164772%) Total: 23778 (99.794350%)
[0.011028, 0.012071) Count: 0 (0.000000%) Total: 23778 (99.794350%)
[0.012071, 0.013115) Count: 0 (0.000000%) Total: 23778 (99.794350%)
[0.013115, 0.014158) Count: 26 (0.109120%) Total: 23804 (99.903473%)
[0.014158, 0.015202] Count: 23 (0.096529%) Total: 23827 (100.000000%)
E0816 11:15:15.414909 22913 rosbag_validate_main.cc:382] Point data (frame_id: "velodyne") has a large gap, largest is 0.101079 s, recommended is [0.0005, 0.05] s with no jitter.
I0816 11:15:15.415345 22913 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/points_raw" (frame_id: "velodyne"):
Count: 2361 Min: 0.100648 Max: 0.101079 Mean: 0.100864
[0.100648, 0.100691) Count: 3 (0.127065%) Total: 3 (0.127065%)
[0.100691, 0.100734) Count: 9 (0.381194%) Total: 12 (0.508259%)
[0.100734, 0.100777) # Count: 67 (2.837781%) Total: 79 (3.346040%)
[0.100777, 0.100820) ## Count: 274 (11.605252%) Total: 353 (14.951292%)
[0.100820, 0.100864) ####### Count: 860 (36.425243%) Total: 1213 (51.376534%)
[0.100864, 0.100907) ####### Count: 798 (33.799236%) Total: 2011 (85.175774%)
[0.100907, 0.100950) ## Count: 275 (11.647607%) Total: 2286 (96.823380%)
[0.100950, 0.100993) Count: 56 (2.371876%) Total: 2342 (99.195259%)
[0.100993, 0.101036) Count: 17 (0.720034%) Total: 2359 (99.915291%)
[0.101036, 0.101079] Count: 2 (0.084710%) Total: 2361 (100.000000%)
提取关键信息：
- IMU topic：
/imu_raw - IMU frame_id：
mti/data - Velodyne topic：
/points_raw - Velodyne frame_id：
velodyne
3.3 编写.lua文件
my_demo.lua:
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "mti/data",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 0,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 1,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1
MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
return options
说明：
num_subdivisions_per_laser_scan必须大于等于1，否则会报错：Check failed: options.num_subdivisions_per_laser_scan >=1- 不清楚
TRAJECTORY_BUILDER_3D.num_accumulated_range_data的意义，这里设置成1 - 传感器为Velodyne VLP-16和Xsens MTi-300，没有用到里程计，GPS和路标，因此
use_odometry，use_nav_sat和use_landmarks都设置为false，num_laser_scans和num_multi_echo_laser_scans都为0，num_point_clouds为1 tracking_frame必须为IMU，因此这里为mti/data- 其他参数保持默认
3.4 编写urdf文件
URDF是ROS中机器人模型的描述格式，包含对机器人刚体外观、物理属性、关节类型等方面的描述。
my_demo.urdf:
<robot name="cartographer_backpack_3d">
<material name="orange">
<color rgba="1.0 0.5 0.2 1" />
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1" />
</material>
<link name="mti/data">
<visual>
<origin xyz="0.0 0.0 0.0" />
<geometry>
<box size="0.06 0.04 0.02" />
</geometry>
<material name="orange" />
</visual>
</link>
<link name="velodyne">
<visual>
<origin xyz="0.0 0.0 0.0" />
<geometry>
<cylinder length="0.07" radius="0.05" />
</geometry>
<material name="gray" />
</visual>
</link>
<link name="base_link" />
<joint name="imu_link_joint" type="fixed">
<parent link="base_link" />
<child link="mti/data" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<joint name="vlp16_link_joint" type="fixed">
<parent link="base_link" />
<child link="velodyne" />
<origin xyz="0. 0.1 0." rpy="0. 0. 0." />
</joint>
</robot>
说明：
- 根标签设置模型，并定义模型名称
- 标签用来定义外观属性，标签设置颜色，设置RGBA值
- joint为关节，用于连接两个组件，在这里，有两个joint，分别为IMU与base_link和Velodyne与base_link；joint的类型为fixed，表明join是固定的，不允许关节发生运动；标签定义了joint的起点，这里为传感器之间的标定参数
3.5 编写launch文件
编写launch文件，实现多节点的配置和启动。
（1）my_demo.launch
<launch>
<param name="/use_sim_time" value="true" />
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/my_demo.urdf" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
<node name="cartographer_offline_node" pkg="cartographer_ros"
type="cartographer_offline_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basenames my_demo.lua
-urdf_filenames $(find cartographer_ros)/urdf/my_demo.urdf
-bag_filenames $(arg bag_filenames)"
output="screen">
<remap from="points2" to="/points_raw" />
<remap from="imu" to="/imu_raw" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>
说明：
points和imu，我们提供的是/points_raw和/imu_rawcartographer_offline_node速度较快。
执行以下命令开始建图：
roslaunch cartographer_ros my_demo.launch bag_filenames:=/path/to/your_bag.bag
（2）my_demo.launch2
cartographer_offline_node速度较快，看不清建图过程的细节，这时可以使用cartographer_node。
两个节点除了速度上的区别，其他不同之处我还不清楚。
<launch>
<param name="/use_sim_time" value="true" />
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/my_demo.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename my_demo.lua"
output="screen">
<remap from="points2" to="/points_raw" />
<remap from="imu" to="/imu_raw" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>
执行以下命令开始建图：
roslaunch cartographer_ros my_demo2.launch bag_filename:=/path/to/your_bag.bag
bag1：
bag2：
完成后会在bag所在目录下生成.pbstream文件。
使用以下命令查看tf树：
rosrun rqt_tf_tree rqt_tf_tree
3.6 保存地图
保存地图需要两个文件，分别为assert_write_my_demo.lua和assets_writer_my_demo.launch。
（1）assert_write_my_demo.lua
VOXEL_SIZE = 5e-2
include "transform.lua"
options = {
tracking_frame = "base_link",
pipeline = {
{
action = "min_max_range_filter",
min_range = 1.,
max_range = 60.,
},
{
action = "dump_num_points",
},
{
action = "fixed_ratio_sampler",
sampling_ratio = 0.01,
},
{
action = "write_pcd",
filename = "map.pcd"
},
{
action = "color_points",
frame_id = "velodyne",
color = { 255., 0., 0. },
},
}
}
return options
（2）assets_writer_my_demo.launch
<launch>
<node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
type="cartographer_assets_writer" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename assets_writer_my_demo.lua
-urdf_filename $(find cartographer_ros)/urdf/my_demo.urdf
-bag_filenames $(arg bag_filenames)
-pose_graph_filename $(arg pose_graph_filename)"
output="screen">
</node>
</launch>
执行以下命令，生成pcd格式的点云地图。
roslaunch cartographer_ros assets_writer_my_demo.launch bag_filenames:=your_bag.bag pose_graph_filename:=your_bag.bag.pbstream
使用以下命令可视化pcd文件：
pcl_viewer *.pcd
很明显，地图质量很差。
4 总结
本文录制了Lidar+IMU数据，然后使用Cartographer实现3D SLAM。
可以看出，定位效果比较差，可能的原因是需要：
- 继续调参
- 严格标定IMU-Lidar