Cartographer 3D建图与纯定位（在线建图、保存和纯定位）

1 Cartographer安装（Ubuntu8.04 melodic）

参考链接：

https://blog.csdn.net/YOULANSHENGMENG/article/details/125059416https://blog.csdn.net/YOULANSHENGMENG/article/details/125059416

1.1 安装依赖包

sudo apt-get update

sudo apt-get install -y google-mock libboost-all-dev libeigen3-dev libgflags-dev libgoogle-glog-dev liblua5.2-dev libprotobuf-dev libsuitesparse-dev libwebp-dev ninja-build protobuf-compiler python-sphinx ros-melodic-tf2-eigen libatlas-base-dev libsuitesparse-dev liblapack-dev

sudo apt-get install -y python-wstool python-rosdep ninja-build stow

1.2 安装cartographer、cartographer_ros和ceres_solver

1.2.1 初始化工作空间（这里与参考链接保持一致，命名为catkin_google_ws）

mkdir catkin_google_ws

cd catkin_google_ws

wstool init src

1.2.2 从原作者的gitee上下载安装cartographer和cartographer_ros

cd src

git clone https://gitee.com/liu_xiao_eu/cartographer.git

git clone https://gitee.com/liu_xiao_eu/cartographer_ros.git

1.2.3 在上述创建好的src文件夹下，获取ceres-solver源码

个人感觉没必要在src文件夹下安装，因为安装的ceres-solver会自动写入系统环境中，保持一致，我们在catkin_google_ws/src文件夹下，获取ceres-solver源码，版本要1.13.0。

wget ceres-solver.org/ceres-solver-1.13.0.tar.gz

编译：

cd ceres-solver-1.13.0

mkdir build

cd build

cmake ..

make

测试一下，然后安装

make test

sudo make install

1.2.4 安装cartographer_ros的依赖项（proto3）

在工作空间下打开终端输入：

src/cartographer/scripts/install_proto3.sh

1.2.5 安装cartographer_ros的依赖（安装ros时已经执行过没有问题，所以直接进行下一步骤）

sudo rosdep init

rosdep update

rosdep install

1.2.6 安装absell-cpp library，方法同步骤1.2.4，在终端中输入：

src/cartographer/scripts/install_abseil.sh

1.2.7 编译Cartographer

catkin_make_isolated --install --use-ninja

值得一提的是，在catkin_google_ws工作空间打开终端，刷新环境变量语句是：

source install_isolated/setup.bash

注：安装过程未尽事宜可以参照上面的链接。

2 Cartographer 3D在线建图

2.1 基础介绍

基本使用Cartographer需要对三种文件有一个基本了解，即urdf、.lua、.launch文件，下面对这三种文件展开介绍。

2.1.1 urdf

简单讲一下，Cartographer需要机器人体包括传感器之间的tf坐标树，在其文件目录中有一个单独的urdf文件夹，我们可以在该目录下创建自己的urdf模型。笔者直接在仿真环境中加载了xacro，所以就没有去构建urdf模型，后续在launch文件中对这一部分进行了注释。

2.1.2 .lua文件

笔者理解该文件是一种配置文件，可以对算法参数进行设置。例如我们进行3D建图，需要用到其中的backpack_3d.lua文件，我们就可以复制该文件然后添加到同一目录下，改个名字，例如叫做xxx_3d.lua。

下面是笔者的xxx_3d.lua文件内容：

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

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

其中参数设置原理可以参照下面的链接：

https://zhuanlan.zhihu.com/p/563264225https://zhuanlan.zhihu.com/p/563264225

2.1.3 .launch文件

与.lua文件操作相同，我们在这里创建属于自己的launch文件，命名为xxx_3d.launch。

下面是笔者的xxx_3d.launch文件内容：

<launch>
  <param name="/use_sim_time" value="true" />
  <!-- <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/wangchao_3d.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 xxx_3d.lua"
      output="screen">
    <remap from="points2" to="/velodyne_points" />
    <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" />
  
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
</launch>

注：笔者是在仿真环境中进行实验，所以use_sim_time设置为true，并注释了原算法自带的urdf模型代码，点云话题、IMU话题结合自己的实际情况进行修改。此次介绍中.lua和.launch文件仅适用于3D情况。

2.2 Cartographer 3D在线建图

2.2.1 加载仿真实验环境

2.2.2 启动Cartographer在线3D建图

roslaunch cartographer_ros xxx_3d.launch

启动launch文件前，不要忘记刷新一下环境变量。建图结果如图所示：

tf坐标树关系如下：

注：虽然Cartographer实现了在线建图和定位，但是/odom话题发布和Cartographer没有关系。想要查看Cartographer发布的位姿话题需要我们自行发布，笔者这里发布了carto_odom，来查看Cartographer的定位信息，这将在后续的纯定位环节展开介绍。

2.2.3 地图保存

Cartographer创建的地图格式与Gmapping、Hector_slam不同，无法调用map_saver节点保存地图，需要以下命令：

首先需要在Cartographer工作空间下打开终端，刷新一下环境变量：

source install_isolated/setup.bash

然后请求/finish_trajectory服务，完成轨迹，不再接收数据，在终端中输入：

rosservice call /finish_trajectory 0

请求/write_state服务，保存当前状态，其中路径可以根据需要自行更改，在终端中输入：

rosservice call /write_state "{filename: '${HOME}/Downloads/mymap.pbstream'}"

笔者当时参考的链接：

https://www.cnblogs.com/54lwl/articles/14713852.htmlhttps://www.cnblogs.com/54lwl/articles/14713852.html

3 Cartographer 3D纯定位

3.1 基础配置

见放一下笔者当时参考的链接：

https://blog.csdn.net/qq_40216084/article/details/106086066https://blog.csdn.net/qq_40216084/article/details/106086066**注：上面的链接修改了算法中子地图存储数量和构成一次回环的有效帧数量，避免地图覆盖问题，通过减少有效帧数量可以增加回环检测次数，但也增加了计算消耗，读者可以自行调节。**

当然保持算法默认参数也是可以的，就只需要参照链接进行如下这步修改，即可使用纯定位：

在这里说一下笔者遇到的特殊情况，因为是仿真环境，笔者的tf树关系如图所示：

刷新后：

xacro模型是没有问题的，问题应该出现在加载差速运动模型xacro时，左右轮和根节点base_footprint的关系由gazebo进行坐标变换发布。

出于此原因，笔者将xxx.lua文件中published_frame 改成了 "odom"，稳定了算法纯定位的实现，机器人模型不再跳动。

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu",
  published_frame = "odom",
  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.,
}

3.2 运行及结果

roslaunch cartographer_ros xxx_3d.launch load_state_filename:=/homw/xxx/Downloads/mymap1.pbstream

3.2.1 Cartographer纯定位结果

3.2.3 Gazebo中导航场景（到达目标点）

3.3 定位话题发布

写到这其实已经写不动了，但是前面留坑需要填。先放笔者当时参考的链接：

https://blog.csdn.net/zhaohaowu/article/details/120889458https://blog.csdn.net/zhaohaowu/article/details/120889458笔者选择了方法1，但是上述代码存在部分问题，一些变量的声明应该是忘记给出了，所以一会直接放代码。

3.3.1 修改publish_tracked_pose为true

首先在VScode里面ctrl+f找到node.cc里面的node_options_.publish_tracked_pose，选中publish_tracked_pose按F12，跳转到定义处，在node_options.h文件下，可以看到publish_tracked_pose默认为false，将其修改为true，这样算法可以发布tracked_pose话题。

3.3.2 修改node.h文件

在node.h中，ctrl+f搜索tracked_pose_publisher_，在其下面添加

::ros::Publisher pub，具体如图所示：

3.3.3 修改node.cc文件

在第一个if (node_options_.publish_tracked_pose) 中加入：

pub = node_handle_.advertise<::nav_msgs::Odometry>("carto_odom", 100);

具体如图所示：

在第二个if (node_options_.publish_tracked_pose) 中加入：

        current_time = ros::Time::now();
        double current_x = pose_msg.pose.position.x;
        double current_y = pose_msg.pose.position.y;
        double current_yaw = tf::getYaw(pose_msg.pose.orientation);
        double dt = (current_time - last_time).toSec();
        double dx = current_x - last_x;
        double dy = current_y - last_y;
        double d_yaw = current_yaw - last_yaw;
        double lin_speed = dx/dt;
        double ang_speed = d_yaw/dt;
        ::nav_msgs::Odometry msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "odom";
        msg.child_frame_id = "velodyne";
        msg.pose.pose.position = pose_msg.pose.position;
        msg.pose.pose.orientation = pose_msg.pose.orientation;
        msg.twist.twist.linear.x = lin_speed;
        msg.twist.twist.linear.y = 0.0;
        msg.twist.twist.angular.z = ang_speed;
        pub.publish(msg);
        last_time = ros::Time::now();
        double last_x = pose_msg.pose.position.x;
        double last_y = pose_msg.pose.position.y;
        double last_yaw = tf::getYaw(pose_msg.pose.orientation);