其他分享
首页 > 其他分享> > 开源自主导航小车MickX4（七）cartographer 室外3D建图

开源自主导航小车MickX4（七）cartographer 室外3D建图

作者：互联网

开源自主导航小车MickX4（七）cartographer 室外3D建图


首先，在前面我们已经实现了2D建图，为什么还要测试室外的3D建图？

1 cartographer 3D建图demo

3D建图的时候需要提供IMU的信息，这里我们使用的是Xsens MTi-30的IMU

1.1 cartographer 安装

cartographer 的安装可以参考我们上一篇博客中的介绍。

1.2 3D数据集建图

使用3D激光雷达建图的时候我们必须要结合IMU，使用IMU提供的重力方向向量。这里我们直接根据官网[1] 的步骤进行运行，首先你需要去下载这个3D数据包[5]

其次我们需要将官网上的 “offline_backpack_3d.launch”替换为 “demo_backpack_3d.launch”,否则在保存地图的时候会出现无法调用

启动3D激光雷达建图

roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=/media/crp/0E3C06880E3C0688/b3-2016-04-05-13-54-42.bag

<iframe allowfullscreen="true" data-mediaembed="bilibili" id="qp0zNAaJ-1574857417790" src="https://player.bilibili.com/player.html?aid=77234298"></iframe>

Cartography-3D建图

等到数据运行完毕以后调用 write_state 服务来保存地图

rosservice call /write_state ~/demo_3d_local.pbstream

将这个pbstream文件进一步转化成3D的ply点云文件（注意使用绝对路径）

roslaunch cartographer_ros assets_writer_backpack_3d.launch bag_filenames:=/media/crp/0E3C06880E3C0688/b3-2016-04-05-13-54-42.bag pose_graph_filename:=/home/administrator/demo_3d_local.pbstream

在这里插入图片描述

等待一段时间，处理完成后命令会自动退出，此时在bag文件旁边会生成一个.bag_points.ply后缀文件，这个就是点云文件[6].最后利用PCL自带的工具将ply文件转换成pcd文件

pcl_ply2pcd b3-2016-04-05-13-54-42.bag_points.ply test_3d.pcd

在运行的时候，机器人的位姿是发布在TF中的，如下图所示。因此我们可以通过读取odom->map之间的坐标变换来知道机器人的位置
在这里插入图片描述
这个TF关系是通过 目录 “cartographer_ros/cartographer_ros/urdf/backpack_3d.urdf” 这个urdf文件来进行配置的

以下是建图时候所使用的部分配置参数

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_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 = 2, #使用两个点云
  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.,
}

1.3 3D定位

3D定位我们是利用在3.1部分生成的 “***~/3d_local.pbstream***”作为已有地图，将当前激光数据输入进行匹配，估计位置

cartographer_ros demo_backpack_3d_localization.launch load_state_filename:=/home/crp/3d_local.pbstream bag_filename:=/media/crp/0E3C06880E3C0688/b3-2016-04-05-15-52-20.bag

<iframe allowfullscreen="true" data-mediaembed="bilibili" id="AK7kJCsU-1574856513043" src="https://player.bilibili.com/player.html?aid=77234444"></iframe>

Cartography-3D定位

同样在运行定位的时候，机器人的位姿也是发布在TF中的，如下图所示。因此我们可以通过读取odom->map之间的坐标变换来知道机器人的位置。（可以明显看出，定位时候的位姿输出频率要远远低于建图时候的频率）
在这里插入图片描述
定位时候的参数（在建图参数上增加了两行）

include "backpack_3d.lua"

TRAJECTORY_BUILDER.pure_localization = true
POSE_GRAPH.optimize_every_n_nodes = 100

return options

2 小车上的3D建图

2.1 配置TF 关系

3D建图必须要加入IMU设备，IMU的坐标这里于demo的坐标系保持一致（发布imu_link 到 base_link之间的关系），这里我是用URDF 文件传入的

<launch>
   <param name="robot_description" textfile="$(find cartographer_ros)/urdf/mickx4.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 mickx4_mapping_3d.lua"
      output="screen">
    <remap from="points2" to="/rslidar_points" />
    <!-- remap from="points2_1" to="/rslidar_points" /-->
    <!-- remap from="points2_2" to="vertical_laser_3d" /-->
    <remap from="/imu" to="/imu/data" />
    <remap from="/odom" to="/odom" />
    
  </node>

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

在这里插入图片描述

2.2 配置lua参数文件

这里我发现四轮差速的里程计不准，也就没有使用里程计。有里程及计的话TF 树也是这样，只需要吧topic和参数（provide_odom_frame 和 use_odometry）使能就可以了

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_footprint",
  odom_frame = "odom",
  provide_odom_frame = false,
  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
TRAJECTORY_BUILDER_3D.min_range = 0.2
TRAJECTORY_BUILDER_3D.max_range = 150
TRAJECTORY_BUILDER_2D.min_z = 0.1
TRAJECTORY_BUILDER_2D.max_z = 1.0
TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = false

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 4
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 = 20
POSE_GRAPH.constraint_builder.min_score = 0.5
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.55


POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e3
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e3

return options

下面是在实验室门口外运行的视频，场景属于室外小范围场景

<iframe allowfullscreen="true" data-mediaembed="bilibili" id="tLTwwcaU-1611884271184" src="https://player.bilibili.com/player.html?aid=458987631"></iframe>

Cartographer 在机器人上实现3D建图-小场景

等到数据运行完毕以后调用 write_state 服务来保存地图

rosservice call /write_state ~/mickx4_3d_mapping.pbstream

2.3 小结

cartographer 3D 建图的效果不是很理想，总体而言存在很多的噪点。测试中发现：
1）对环境比较敏感（校园里面树木比较多，一般都长在路面两侧，会建筑物的墙面有遮挡，导致结构化的元素比较少。因此在这段区域建图的效果就不是很好。等到了后面的路线中，没有树木了，激光可以扫描到两边的墙体时候建图的效果就好了很多了）
2）对地面也比较敏感，在中间的一段，地面有坑，导致小车会上下颠簸，这种因素也会对建图产生影响。
3）激光打到地面，carto 没有去地面的操作，因此需要我们自己调节激光发射角度，调整激光打到地面上的程度

效果比起LOAM系列的框架还是有点不差距。后续可以尝试使用LOAM系列的框架进行测试
在这里插入图片描述

此外，蓝鲸机器人博客[7]上的资料很详细，对cartography 的演示demo也完全可以复现，因此如果跑不起来也可以参考以下他们的资料[6]。

参考资料

[1] https://www.cnblogs.com/hitcm/p/5939507.html
[2] https://blog.csdn.net/crp997576280/article/details/103279649
[3] https://github.com/cartographer-project/cartographer_ros
[4] https://github.com/cartographer-project/cartographer
[5] https://github.com/ceres-solver/ceres-solver
[6] https://github.com/BluewhaleRobot/cartographer_ros
[7] https://community.bwbot.org/topic/523/%E8%B0%B7%E6%AD%8Ccartographer%E4%BD%BF%E7%94%A8%E9%80%9F%E8%85%BE%E8%81%9A%E5%88%9B3d%E6%BF%80%E5%85%89%E9%9B%B7%E8%BE%BE%E6%95%B0%E6%8D%AE%E8%BF%9B%E8%A1%8C%E4%B8%89%E7%BB%B4%E5%BB%BA%E5%9B%BE

上一篇：开源自主导航小车MickX4（六）cartographer 室外2D建图

欢迎大家点赞在评论区交流讨论（cenruping@vip.qq.com） O(∩_∩)O

或者加群交流（1149897304）
在这里插入图片描述

标签：cartographer,frame,建图,BUILDER,3D,MickX4,3d
来源： https://blog.csdn.net/crp997576280/article/details/111600534