cartographer + velodyne64 + IMU建图

从周一晚上开始到，现在是周五晚上，断断续续搞了好几天了，终于跑通了，我也是醉了。不过好的是，一个一个问题，最后还是弄懂了，还是有点激动了。抓紧时间锻炼自己的动手能力吧！！渣渣。

我跑了Kitti的raw data和自己录的数据，下面以自己录的数据整个流程及文件配置。我自己录的数据为Velodyne64 E和IMU原始数据。

0  cartographer_rosbag_validate -bag_filename your_bag.bag

如图，我的数据bag是有问题的，到现在我也不知到这个问题咋解决。不过现在知道了没关系，不影响建图。刚开始的时候可是心里一点底都没有，因为你不知道这个问题会造成什么结果。

 frame_id "velo_link" on topic /velodyne_points has serialization time
1552041177.349049635 but sensor time 57002.398437500 differing by -1.55198e+09 s.

 

1  .lua文件：配置传感器数量

这里需要改的设置的有

velodyne64.lua

1. 四个frame： map_frame = "map",tracking_frame = "imu_link",published_frame = "base_link",odom_frame = "odom",
2. num_point_clouds = 1（三维激光数量）
3. num_subdivisions_per_laser_scan = 1（这个特么我也不知道是干啥的，看着像是2d激光的参数应该设为0，一开始我设的0，结果报错 Check failed: options.num_subdivisions_per_laser_scan >=1）
4. TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1（这个全局变量貌似表示一次扫描有多少次数据

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  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

2  .urdf文件：配置link 和joint，有几个传感器就配置几个。

如下，一个IMU和一个Velodyne。分割线上半部分不知道干啥的，下半部分是设置IMU和Velodyne的frame_id名字，以及他们的父frame_id的。base_link是父frame_id。

velodyne64.urdf

<robot name="cartographer_velodyne64_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="velo_link">
    <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="velodyne64_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="velo_link" />
    <origin xyz="-0.0795 1.2967 0.5389" rpy="0. 0. 0." />
  </joint>

</robot>

3  .launch文件：配置数据bag地址，remap topic为指定名称，在这里包含上述的velodyne64.lua和velodyne64.urdf文件。

1、选用离线模式结点cartographer_offline_node！！！这个一开始没有注意，我在在线模式下不行，希望有朋友能交流下。

2、在录制bag时，传感器数据的frame_id必须和velodyne64.urdf文件里的”_link“一样，要对应起来，如下

sensor_msgs::PointCloud2::Ptr output_msg=boost::make_shared<sensor_msgs::PointCloud2>();
output_msg->header.frame_id="velo_link";

sensor_msgs::Imu imuData;
imuData.header.frame_id="imu_link";

3、topic名字更改使用remap，把自己bag里的topic名字换成需要的topic名字 。

velodyne64.launch

<launch>

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

  <arg name="bag_filename" default="/media/localization/新加卷/school/20190306siyuan_RAWIMU_noFanzhun_gy.bag" />

  <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 velodyne64.lua
          -urdf_filenames $(find cartographer_ros)/urdf/velodyne64.urdf
          -bag_filenames $(arg bag_filename)"
      output="screen">
      <remap from="points2" to="/velodyne_points" />
      <remap from="imu" to="/imu/data" />
  </node>


  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

</launch>

4、运行

roslaunch cartographer_ros velodyne64.launch 

运行完后会在源数据bag同目录下生成.pbstream文件，这个文件用于获得最终地图。

5、存地图。

此时需要配置两个文件 

（1）assert_velodyne64.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 = "cartor_siyuanMap.pcd"
    },

    {
      action = "color_points",
      frame_id = "velo_link",
      color = { 255., 0., 0. },
    },
  }
}

return options

这里主要是 "write_pcd"这条命令，filename就是最终要生成pcd地图的名字。官方上有介绍各种命令，但是给的demo里面没有写这句话，写的都是生成三视图的命令。我一开始也没看懂，还在纳闷为什么只有图片，我也没有用相机啊。。。后来看的蓝鲸的那篇博客才明白了，自己添加一条命令就行。

（2）assets_writer_velodyne64.launch

<launch>
<arg name="bag_filenames" default="/media/localization/新加卷/school/20190306siyuan_RAWIMU_noFanzhun_gy.bag"/>
<arg name="pose_graph_filename" default="$(arg bag_filenames).pbstream"/>

  <node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
      type="cartographer_assets_writer" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename assert_velodyne64.lua
          -urdf_filename $(find cartographer_ros)/urdf/velodyne64.urdf
          -bag_filenames $(arg bag_filenames)
          -pose_graph_filename $(arg pose_graph_filename)"
      output="screen">
  </node>
</launch>

bag_filenames就是数据bag的名字，pose_graph_filename是offline slam模式下生成的最终优化过的位姿文件。

在这个launch文件里面就包含了上面的assert_velodyne64.lua

配置完后运行

roslaunch cartographer_ros assets_writer_velodyne64.launch 

即可生成pcd格式的地图。

最终生成的地图如下
 

 

---

最后在这里记录一下自己写下的Bug，希望能长点记性！我不是在写程序，而是在写bug！

今天下午原本都打算先搁置一段时间再看，太崩溃了，又不要求全部搞懂，只是跑通都没成功，深深对自己动手能力感到无力。结果不经意间发现了蓝鲸机器人的这篇博文，算是找到了救命稻草，把我从崩溃边缘拉回来了（主要是bug 1，看了那篇博文之后才发现，真心感谢！！）

Bug 1

remap写反了，如果要是放在rosbag play 那里的话，是 

  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)"  >  
      <remap from="/velodyne_points" to="points2" />
      <remap from="/imu/data" to="imu" />
 </node>

/velodyne_points 和/imu/data 是我发布的topic名称。但是要在订阅topic的节点里写的话就刚好反过来了！！！这个自己之前就有点混，虽然大概知道，还是吃了这个亏了！

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames velodyne64.lua
          -urdf_filenames $(find cartographer_ros)/urdf/velodyne64.urdf
          -bag_filenames $(arg bag_filename)"
      output="screen">
      <remap from="points2" to="/velodyne_points" />
      <remap from="imu" to="/imu/data" />
  </node>

Bug 2

自己录的数据本身就有问题。存读数据搞反了，无语凝咽，自己写的bug，含着泪也得负责。

1、存数据时按yaw pitch roll顺序存的，然而我一直以为是roll pitch yaw顺序，读的时候就用的roll pitch yaw，导致最后程序虽能跑通，但是效果一直很差。对一个未知的东西，出现了bug，想到的原因有好多，一个一个试都不行，最后才想到用matlab画一下图看看，画了图才发现不只是顺序有问题，数据也有问题！（此时我已经惊了，天大的bug，我还调用这个错误的数据调了一天）。数据问题详细见2。

2、惯导文档上写的加速度需要乘以IMU的输出频率，而角速度没有写（如下图）。所以我输出的数据就是角速度没有乘以输出频率100。但是实际上角速度的值也要乘以IMU的输出频率！！！法克！

 
我录的是围着一栋建筑物走了一圈的正方形轨迹数据，原来错误的数据cartographer跑出来的是大概是一条直线到底，没有明显的90度拐弯，现在想一下应该就是角速度值比实际的小了100倍，基本都为零了，所以不会拐弯。不多当时我一度以为是IMU频率问题或者是时间戳的问题。我发布的IMU实际频率为30HZ，但是cartographer却显示为6HZ；时间戳提示

 这个到现在都没搞明白怎么回事，先记下以后再说吧。