Cartographer学习记录：Cartographer地图3D可视化配置（自录数据集版）

在上一篇对Cartographer官方数据集进行可视化配置后，这篇博客将跟各位小伙伴们分享如果利用自己录制的数据包进行地图的3D可视化。因为之前还没有做博客的习惯，没有将我搭建平台和录制数据集的过程记录下来，后面会找个时间重新写一下，这个系列我会持续更新下去！

一、实验平台介绍

采用WHEELTEC四驱车高配版独立悬挂越野室外小车作为移动平台，搭配QUANERGY的M8系列激光（24V供电）和WHEELTEC的N100型IMU进行校园室外建图测试，录制话题名称为/points2的激光数据和/imu的IMU数据，形成包名为2022-01-14-outdoor.bag的数据集，具体过程后面会更新一篇博客专门介绍如何搭建环境来录制自己的数据集。

二、建立launch文件进行离线建图

在这里建立名为my_demo_offline.launch文件，具体内容如下：

<!--my_demo_offline-->

<launch>
  <param name="/use_sim_time" value="true" />

  <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" output="screen" respawn="true" >
  </node>

  <node name="cartographer_start_trajectory" pkg="cartographer_ros" type="cartographer_start_trajectory"
       output="screen" respawn="true">
  </node>
  
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node" args="-resolution 0.05 -publish_period_sec 10.0" respawn="true"/>

  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />
</launch>

<param name="/use_sim_time" value="true" />中的true值表示离线建图，若是改为false则表示实时在线建图，像我的实时在线建图的launch文件是这样的：

<!--my_demo-->

<launch>
  <param name="/use_sim_time" value="false" />

  <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" output="screen" respawn="true" >
  </node>

  <node name="cartographer_start_trajectory" pkg="cartographer_ros" type="cartographer_start_trajectory"
       output="screen" respawn="true">
  </node>
  
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node" args="-resolution 0.05 -publish_period_sec 10.0" respawn="true"/>
</launch>

所以启用离线建图的终端指令也就是：

roslaunch cartographer_ros my_demo_offline.launch bag_filename:=/home/jin/laser_dataset_own/2022-01-14-outdoor.bag

bag_filename后面更改成自己数据集存放的路径+文件名，我这里是放在home目录下的laser_dataset_own文件夹下。

在建图完毕后就是保存地图的指令，利用cartographer_ros提供的服务来保存pbstream文件：

rosservice call /finish_trajectory "trajectory_id: 0"

rosservice call /write_state "filename: '2022-01-14-outdoor.bag.pbstream'                                                                   include_unfinished_submaps: false"

我这里是保存成名为“2022-01-14-outdoor.bag.pbstream”的文件，建议大家做项目时规范化命名方式，以便后续带来管理上的便捷。数据集我一般就统一成日期+主题的形式，给各位小伙伴们做个参考。

三、建立launch、urdf、lua文件进行点云数据的保存。

在上一篇博客里面我已经详细介绍了保存点云数据的方式，这里就不再赘述，主要讲下如何将上一篇提到的assets_writer_backpack_3d.launch文件更改为自己根据实际需要的launch文件，以及launch文件中涉及到的urdf和lua文件。

下面先放上launch文件的对比图：

 左边为自己的assets_writer_pointcloud_3d.launch文件，右边为官方文件，可以看到主要更改的是lua文件的路径文件名和urdf的路径文件名。

下面是lua文件的对比图：

同样，左边为自己的assets_writer_pointcloud_3d.lua文件，因为官方数据集里面3D激光数据是分为横向和竖向的两类话题，我的QUANERGY的M8系列激光传递名为laser的激光数据，所以我更改的部分就是将原来的两部分删减为一部分就可以了。

下面是urdf文件的对比图：

 

 同样左边为自己的pointcloud_3d.urdf文件，urdf文件主要是配置imu和激光对base_link的转换，我是安装时将激光与IMU放在一起，并且坐标轴按照各自说明书的规定将方向统一，所以暂且将各项平移和旋转配置都设置为0。上面图片是为了各位小伙伴能有个清晰的比较认识，下面我放上我的launch、urdf和lua文件。

assets_writer_pointcloud_3d.launch

<!--
  Copyright 2016 The Cartographer Authors

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

<launch>
  <node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
      type="cartographer_assets_writer" args="
          -configuration_directory /home/jin/config/slam_config
          -configuration_basename assets_writer_pointcloud_3d.lua
          -urdf_filename $(find cartographer_ros)/urdf/pointcloud_3d.urdf
          -bag_filenames $(arg bag_filenames)
          -pose_graph_filename $(arg pose_graph_filename)"
      output="screen">
  </node>
</launch>

 pointcloud_3d.urdf

<!-- pointcloud_map -->

<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="imu_link">
    <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="laser">
    <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="imu_link" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>
</robot>

assets_writer_pointcloud_3d.lua

-- pointcloud_map

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,
    },

    -- Gray X-Rays. These only use geometry to color pixels.
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_yz_all",
      transform = YZ_TRANSFORM,
    },
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xy_all",
      transform = XY_TRANSFORM,
    },
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xz_all",
      transform = XZ_TRANSFORM,
    },

    -- Now we recolor our points by frame and write another batch of X-Rays. It
    -- is visible in them what was seen by the horizontal and the vertical
    -- laser.
    {
      action = "color_points",
      frame_id = "laser",
      color = { 255., 0., 0. },
    },

    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_yz_all_color",
      transform = YZ_TRANSFORM,
    },
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xy_all_color",
      transform = XY_TRANSFORM,
    },
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xz_all_color",
      transform = XZ_TRANSFORM,
    },
    {
      action = "write_ply",
      filename = "points.ply"
    },

  }
}

return options

最后启用assets_writer_pointcloud_3d.launch文件生成.ply文件，终端指令如下：

roslaunch cartographer_ros assets_writer_pointcloud_3d.launch bag_filenames:=/home/jin/laser_dataset_own/2022-01-14-outdoor.bag pose_graph_filename:=/home/jin/laser_dataset_own/2022-01-14-outdoor.bag.pbstream

第四步，点云可视化

方式一：

利用pcl_ply2pcd将ply格式转换为pcd格式进行查看，即在终端输入如下命令：

pcl_ply2pcd 2022-01-14-outdoor.bag_points.ply  2022-01-14-outdoor.pcd

查看pcd格式图的命令如下：

pcl_viewer 2022-01-14-outdoor.pcd

PCD格式的图如下所示，有点简化，如果想看详细的建议采用方式二。

 方式二：

利用sdl_viewer进行查看，具体配置可以看我的上一篇博客，有很详细的介绍。下面就进行效果图的展示，各位小伙伴实际运行后可以跟PCD格式的图进行比较，孰高孰低，一眼便分晓。当然，我这个建的图还是初步的，需要优化的地方还很多，这就是后续的工作安排，加油！