cartographer学习与使用

记录一下在配置和使用cartographer建图时遇到的各种问题吧。

我的数据

配置文件：

my_rslidar.launch

cpp 复制代码

<launch>  
      
<param name="/use_sim_time" value="false" />  
<!--启动建图节点-->
<node name="cartographer_node" pkg="cartographer_ros"  
    type="cartographer_node" args="  
    -configuration_directory $(find cartographer_ros)/configuration_files  
    -configuration_basename my_rslidar2.lua"  
    output="screen">  
    <remap from="scan" to="scan" />  
</node>  
 
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.5" />
 
<node name="rviz" pkg="rviz" type="rviz" required="true"  
          args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />  
</launch>

my_rslidar2.lua

cpp 复制代码

include "map_builder.lua"
include "trajectory_builder.lua"
options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "rslidar",
  published_frame = "rslidar",
  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 = 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
MAP_BUILDER.num_background_threads = 8
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 4
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 15
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 200
TRAJECTORY_BUILDER_2D.min_range = 5
TRAJECTORY_BUILDER_2D.max_range = 100
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

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

参考链接：
Cartographer（一）ubuntu系统Cartographer_ros配置与运行
 关于速腾激光雷达在实际移动平台上配置cartographer实现室内建图定位
 google激光雷达slam算法Cartographer的安装及bag包demo测试
 使用rosbag录制和回放3d激光雷达数据和小强ROS开发平台的里程计IMU数据
 如何使用Cartographer生成的地图
 速腾聚创robosense16线在cartographer下进行２Ｄ建图
 RoboSense（速腾）16线激光雷达数据进行cartographer建图的实践历程
 谷歌cartographer使用速腾聚创3d激光雷达数据进行三维建图
 使用速腾16线激光雷达跑通cartogranpher-2d作为真值,对视觉激光融合结果使用evo评估
 RoboSense（速腾）16线激光雷达数据进行cartographer建图的实践历程
 基于Carto的小车SLAM建图定位与导航

原理上面：

CSM：想象给定一个栅格地图（或者submap子图），拿到当前帧的点云之后，怎样才能知道激光雷达所在的位置呢？最简单的办法，把激光雷达放在地图的每个格子上，计算在这个位置时，点云是否与地图重合，重合程度最高的位置就是激光雷达的位姿；这个过程，就是一个暴力搜索的过程，但是由于前端维护的是一个个子图计算也还好。回环检测需要遍历大面积地图，采用了分枝定界破这个暴力搜索！

回环：FastCSM，分枝定界，先在低分辨率地图上搜索，逐渐在高分辨率地图上搜索，满足条件之后直接剪枝不再浪费cpu去搜索；

前端：scan2map的方式，采用CeresScanMatch(初值不错，效果就不错)，外部传感器提供的初值不佳时（无imu等），采用RealTimeCSM去求得不错的初值；

后端：CeresScanMatch构建约束优化submap位姿；

imu作用：2d时，用加速度矫正激光；3d时提供重力方向（必须！！）。为激光帧间匹配提供位置初值；（cartographer只使用了线加速度和角速度信息）

体素滤波器/自适应：下采样激光，减少激光数据量，一定程度也解决激光近密远疏；

位姿外推器：在前端匹配之前，融合多传感器数据（odom，imu，lidar），提供前端匹配的初值；

来历

2016年10月5日，谷歌宣布开放一个名为cartographer的即时定位与地图建模库，开发人员可以使用该库实现机器人在二维或三维条件下的定位及建图功能。cartograhper的设计目的是在计算资源有限的情况下，实时获取相对较高精度的2D地图。考虑到基于模拟策略的粒子滤波方法在较大环境下对内存和计算资源的需求较高，cartographer 采用基于优化方法。目前cartographer主要基于激光雷达来实现SLAM，谷歌希望通过后续的开发及社区的贡献支持更多的传感器和机器人平台，同时不断增加新的功能。
cartographer 功能包

cartographer 功能包已经与ROS集成，但是还没有提供二进制安装包，所以需要采用源码编译的方式进行安装。为了不与已有功能包冲突，最好为 cartographer专门创建一个工作空间,这里我们新创建了一个工作空间catkin_google_ws，然后使用如下步骤下载源码并完成编译，安装参考https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html。

以下内容仅供参考

cpp 复制代码

sudo apt-get update
sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow

创建工作空间并下载源码

cpp 复制代码

mkdir cartographer_ws

cpp 复制代码

cd cartographer_ws

cpp 复制代码

wstool init src

cpp 复制代码

cd src

cpp 复制代码

git clone https://github.com/googlecartographer/cartographer_ros.git

cpp 复制代码

git clone https://github.com/googlecartographer/cartographer.git

cpp 复制代码

git clone https://github.com/ceres-solver/ceres-solver.git

安装依赖

cpp 复制代码

cd ..

cpp 复制代码

rosdep update

cpp 复制代码

rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

安装abseil-cpp库

cpp 复制代码

src/cartographer/scripts/install_abseil.sh

编译与安装

cpp 复制代码

catkin_make_isolated --install --use-ninja

测试参考

下载2D数据包并运行demo

cpp 复制代码

wget -P ~/cartographer_ws/dataset https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag

cpp 复制代码

source ~/cartographer_ws/install_isolated/setup.bash

cpp 复制代码

roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/cartographer_ws/dataset/cartographer_paper_deutsches_museum.bag

下载3D数据包并运行demo（文件较大）

cpp 复制代码

wget -P ~/cartographer_ws/dataset https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag

cpp 复制代码

source ~/cartographer_ws/install_isolated/setup.bash

cpp 复制代码

roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/cartographer_ws/dataset/b3-2016-04-05-14-14-00.bag

地图保存

cpp 复制代码

rosservice call /write_state "{filename: '${HOME}/cartographer_ws/dataset/cartographer_paper_deutsches_museum.bag.pbstream', include_unfinished_submaps: "true"}"