基于RGBD相机的Cartographer调试

硬件

Realsense D435i，Jetson Nano

软件调试

深度图转LaserScan

<?xml version="1.0" ?>
<launch>
	<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
	<param name="camera_info" value="/camera/depth/camera_info"/>
	<remap from="image" to="/camera/depth/image_rect_raw"/>
	<param name="scan_height" value="1"/>
	<param name="range_min" value="0.15"/>
	<param name="range_max" value="5.0"/>
	<remap from="scan" to="/scan"/>
	<param name="output_frame_id" value="horizontal_laser_link"/>
	</node>
</launch>

我有写里面的一些tricks，感兴趣移步： ROS的depthimage_to_laserscan_妄想出头的工业炼药师的博客-CSDN博客当我们想把RGBD相机的深度图转成单线激光雷达来使时，经常考虑到使用depthimage_to_laserscan的ros包。其中有关于通过深度图和相机内参转成基于极坐标系下的单线激光雷达最大和最小角度这两个参数，用到的代码如下：double DepthImageToLaserScan::magnitude_of_ray(const cv::Point3d& ray) const{ return sqrt(pow(ray.x, 2.0) + pow(ray.y, 2.0) + pow(https://blog.csdn.net/z15f34/article/details/123673040

构建坐标系

urdf：

<robot name="cartographer_backpack_2d">
  <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="camera_imu_optical_frame">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="horizontal_laser_link">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link" />

  <joint name="imu_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="camera_imu_optical_frame" />
    <origin xyz="0 0 0" />
  </joint>

  <joint name="horizontal_laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="horizontal_laser_link" />
    <origin xyz="0. 0 0." />
  </joint>

</robot>

注意：

ros中，parent link和child_link之间的关系是我们常用的表示Tchild_parent或Tparent2child,是child基于parent的坐标系表示，跟常规坐标系建立是保持一致的。

http://wiki.ros.org/navigation/Tutorials/RobotSetup/TFhttp://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

Bug

激光雷达数据报错

[ WARN] [1647514464.952742870]: W0317 18:54:24.000000  6064 sensor_bridge.cc:211] Ignored subdivision of a LaserScan message from sensor scan because previous subdivision time 637831013454354019 is not before current subdivision time 637831013454354019

header: 
  seq: 44
  stamp: 
    secs: 1647504542
    nsecs: 806658506
  frame_id: "horizontal_laser_link_joint"
angle_min: -0.690491855145
angle_max: 0.703561544418
angle_increment: 0.00218161731027
time_increment: 0.0
scan_time: 0.0329999998212
range_min: 0.15000000596
range_max: 3.0
ranges: [0.47154831886291504, 0.47094249725341797, 0.4697343111038208, 0.4691319465637207, 0.46724358201026917, 0.4673319160938263, 0.46673426032066345, 0.4655425250530243, 0.4649484157562256, 0.46435555815696716, 0.46317338943481445, 0.46258413791656494, 0.46199607849121094, 0.4608236849308014, 0.46023932099342346, 0.

原因：

time_increment设为0，而num_subdivisions_per_laser_scan = 10,改为num_subdivisions_per_laser_scan=1，即可。

解决办法参考：

https://github.com/cartographer-project/cartographer_ros/issues/1326https://github.com/cartographer-project/cartographer_ros/issues/1326

cartographer build map error · Issue #1093 · cartographer-project/cartographer_ros · GitHubprocess[robot_state_publisher-1]: started with pid [11071] process[cartographer_node-2]: started with pid [11072] process[cartographer_occupancy_grid_node-3]: started with pid [11078] process[base_to_laser_broadcaster-4]: started with pi...https://github.com/cartographer-project/cartographer_ros/issues/1093https://github.com/cartographer-project/cartographer_ros/issues/907https://github.com/cartographer-project/cartographer_ros/issues/907SLAM学习笔记（十七）激光SLAM——Cartographer配置文件参数解释及报错原因_zkk9527的博客-CSDN博客_odometry_sampling_ratiohttps://blog.csdn.net/zkk9527/article/details/109096142

TF树map->base_link变换关系是无限远

用录制的数据建图，建图的结果很好，但其中有TF树的变换关系（一根黄线）从天边连过来的似的，不影响建图，看得很膈应人，主要是打开use_pose_extrapolator，关掉就没了，原因解释链接如下：

2d map and trajectory works, TF map --> base_link does not - using only laserscan data · Issue #758 · cartographer-project/cartographer_ros · GitHubhttps://github.com/cartographer-project/cartographer_ros/issues/758

关于pose extrapolator的作用主要是运动畸变校正，我使用的RGBD是全局快门类型的相机，所以不用做运动畸变校正

Cartographer中对激光雷达运动畸变的处理方法分析 - 尚码园https://www.shangmayuan.com/a/53b8010df47a4f7e8b07f953.html

cartographer的2D建图设置

 cartographer作者原本的参数设置是编译静态库（.a）的时候生成了，正常安装应该在/user/local/目录下，我编译指定文件夹，文件地址：/***/share/cartographer/configuration_files里，这里有两个参数文件比较重要，trajectory_builder_2d.lua和pose_graph.lua,可以看出来不管前端还是后端都用了csm（粗配准），迭代的scan match（精配准，精配准一般都带迭代，可以看下我写的icp：搞懂ICP（最近邻迭代）配准算法 - 2_妄想出头的工业炼药师的博客-CSDN博客_icp迭代），pose_graph.lua里还包含回环检测的参数。

调参的策略是先调前端，再加上后端再调一下，所以首先把以下参数设为0：

POSE_GRAPH.optimize_every_n_nodes = 0 --120

Cartographer调参方法_Cayla梦云的博客-CSDN博客_cartographer调参不幸的是，调试cartographer真的很难。该系统具有许多参数，其中许多参数相互影响。本调试指南试图解释具体示例的原则方法。##示例：调整局部SLAM对于这个例子，我们将从cartographer提交aba4575和cartographer_ros提交99c23b6开始，然后从我们的测试数据集中查看包b2-2016-04-27-12-31-41.bag。在我们的初始配置中，我们看到这个包...https://blog.csdn.net/xmy306538517/article/details/86609497Cartographer调参与系统资源使用情况分析_火种源码的博客-CSDN博客动机：通过Cartographer调参，达到在Rockchip rk3399开发板上实时和精度兼顾的效果。描述：Cartographer的参数众多，真实环境下需要进行大量的调整。仅通过官方文档调整仍比较繁琐。本文通过逐个调参，直观调整参数效果。1. 硬件及OS信息nametypeCPURK3399 双核Cortex-A72(大核)+四核Cortex-A53(小核)Kernel4.4.179ROSMeloclidarToFbaghttps://blog.csdn.net/zhzwang/article/details/107695535

我最终的参数设置：

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "gyro_link", --"camera_imu_optical_frame",
  published_frame = "base_footprint",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = false,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  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.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

--TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians=math.rad(1.57)


TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 10.0

TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false --不用数据丢失时效果更好？

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 200 --70
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 48 --50 51
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 --10
--TRAJECTORY_BUILDER.collate_landmarks = false

--TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.3
--TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
--TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(3.)
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 70
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 300

--TRAJECTORY_BUILDER_2D.submaps.resolution = 0.035
--TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
--后端优化部分/GLOBAL SLAM参数
POSE_GRAPH.optimize_every_n_nodes = 90 --120
--POSE_GRAPH.optimization_problem.huber_scale = 1
POSE_GRAPH.constraint_builder.ceres_scan_matcher.rotation_weight = 48
POSE_GRAPH.constraint_builder.ceres_scan_matcher.translation_weight = 100
POSE_GRAPH.constraint_builder.min_score = 0.6
--POSE_GRAPH.constraint_builder.sampling_ratio = 1.

return options

总脚本

<?xml version="1.0" ?>
<launch>
  <!--深度图转单线激光雷达-->
  <include file="$(find depthimage_to_laserscan)/launch/depthimage_to_laserscan.launch"/>

  <!--建图-->
  <include file="$(find cartographer_ros)/launch/backpack_2d.launch"/>

  <!--可视化-->
  <include file="$(find cartographer_ros)/launch/visualize_pbstream.launch"/>

</launch>

结果展示

最后的结果如下，有点像铅笔画，主要是因为RGBD的远处数据跳动太大：

 

 

附一张激光雷达的结果对比一下，可以看到边缘处干净太多：

地图处理

保存地图

cartographer ros使用指南-保存地图 - 创客智造

加载地图及纯定位

纯定位脚本：

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

  <!--深度图转单线激光雷达-->
  <include file="$(find depthimage_to_laserscan)/launch/depthimage_to_laserscan.launch"/>

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/rgbd_backpack_2d.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 rgbd_backpack_2d_localization.lua
          -load_state_filename $(arg load_state_filename)"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </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_2d.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />
</launch>

 运行加载地图与纯定位指令：

roslaunch cartographer_ros rgbd_demo_backpack_2d_localization.launch load_state_filename:=${HOME}/Downloads/mymap.pbstream bag_filename:=${HOME}/Downloads/2022-03-22-19-34-41.bag

 重定位

手动定位

增加在地图上的起点：

Initial Pose Wiring · Issue #579 · cartographer-project/cartographer_ros · GitHubhttps://github.com/cartographer-project/cartographer_ros/issues/579

自动重定位

类似项目

Accurate SLAM from depth camera possible? · Issue #244 · cartographer-project/cartographer_ros · GitHubhttps://github.com/cartographer-project/cartographer_ros/issues/244

GitHub - cartographer-project/cartographer_turtlebot: Provides TurtleBot integration for Cartographer.https://github.com/cartographer-project/cartographer_turtlebot