基於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>

說明：

• <remap from="points2" to="/points_raw" />和<remap from="imu" to="/imu_raw" />用於話題的重映射，Cartographer需要的是points和imu，我們提供的是/points_raw和/imu_raw
• cartographer_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