Cartographer源码阅读1

在agv公司实习，需学习cartographer算法。

参照大佬帖子Cartographer源码阅读_预准备 - 知乎

本贴仅作为记录自己学习cartographer源码的过程，笔记，感想。

cartographer代码主要包括两部分 cartographer_ros，cartographer，以及一个进行非线性优化的库ceres-solver。cartographer_ros是基于ros的通信机制，获取传感器的数据并转换成cartographer内定义的格式传递给cartographer处理。可以理解为基于cartographer的上层应用，对cartographer进行了一层ROS接口的封装。

代码整体框架：

算法大致分为三个部分，local slam（前端），Global SLAM（后端），回环检测LoopClosure。

Local SLAM部分任务是建立子图Submaps，但前端建图会有部分建图误差，且随着时间累计。该部分的参数定义见/src/cartographer/configuration_files/trajectory_builder_2d.lua和/src/cartographer/configuration_files/trajectory_builder_3d.lua中。

Global SLAM部分主要任务是进行回环检测，采用BBA，Branch-and-Bound Approach （BBA）的方法来解决。具体方法见论文Real-Time Loop Closure in 2D LIDAR SLAM。在3Dladar中，该部分还根据IMU惯导的数据找到重力的方向。对BBA算法的理解：搜索窗口表达成一棵树的形式，树上每一个树枝就是这颗树干节点所有解可能性的划分，而树干对每个树枝都有一个上限得分，当有一个解比子节点（树枝）的得分高时，说明这个树枝不是最优解，就剪掉这个树枝，从而大大加快解题速度。并且在实际问题中，大部分分支的得分是非常低的，通过阈值就剪掉了很多分支。该种方法拿存储空间换时间效率，因为需要预先计算并存储子图每个pixel的得分值，当输入一个节点时，直接寻找即可，所以这个程序很吃内存。

总而言之，前端负责生成较好的子图，而后端将不同的子图以最匹配的位姿合在一起成为总图。

cartographer运行后系统ROS节点，topic，service等情况如下图：

cartographer_node订阅传感器数据，运行算法，生成submap_list, 而cartographer_occupancy_grid_node则订阅了submap_list，并发布栅格地图。

cartographer_node
   |===@订阅的Topic
   |---------scan (sensors_msgs/LaserScan):   解释：传感器数据
   |---------echoes (sensors_msgs/MultiEchoLaserScan) 
   |---------points2 (sensors_msgs/PointCloud2) 解释：2D点云数据
   |---------imu (sensors_msgs/Imu) 解释：IMU的数据
   |---------odom (nav_msgs/Odometry) 解释：里程计数据
   |===@发布的Topic
   |---------scan_matched_points2 (sensors_msgs/PointCloud2) 解释：匹配好的2D点云数据，用来scan-to-submap matching
   |---------submap_list (cartographer_ros_msgs/SubmapList) 解释：发布构建好的submap。
   |===@提供的Service
   |---------submap_query (cartographer_ros_msgs/SubmapQuery) 解释：可以提供查询submap的服务，获取到request的submap
   |---------start_trajectory (cartographer_ros_msgs/StartTrajectory) 解释：维护一条轨迹
   |---------finish_trajectory (cartographer_ros_msgs/FinishTrajectory) 解释：Finish一个给定ID的轨迹
   |---------write_state (cartographer_ros_msgs/WriteState) 解释：将当前状态写入磁盘文件中
   |---------get_trajectory_states (cartographer_ros_msgs/GetTrajectoryStates) 解释：获取指定trajectory的状态
   |---------read_metrics (cartographer_ros_msgs/ReadMetrics) 
   |===@Required tf Transforms
   |===@Provided tf Transforms
2. offline_node
   |===可以理解为一个快速版本的cartographer
   |===不监听任何topic,二是直接从数据包中读取传感器数据。发布的Topic与cartographer_node相同，除此以外，还有：
   |===@额外发布的Topic
   |---------~bagfile_progress (cartographer_ros_msgs/BagfileProgress) 解释：可查询处理包的进度等情况
   |===@Parameters
   |---------~bagfile_progress_pub_interval(double, default=10.0) 解释：发布包数据的时间间隔。以s为单位；
3. occupancy_grid_node
   |===description：主要任务是监听submap_list这个Topic然后构建栅格地图并发布
   |===@订阅的Topics
   |---------submap_list (cartographer_ros_msgs/SubmapList) 解释： 由cartographer_node发布
   |===@发布的Topics
   |---------map (nav_msgs/OccupancyGrid) 解释：栅格地图
4. pbstream_map_publisher
   |===description: 快速版的occupancy_grid_node
   |===未订阅任何节点
   |===发布的Topics
   |---------map (nav_msgs/OccupancyGrid) 解释：栅格地图

上面为代码框架，下面开始接触代码本身。通过文件夹来划分功能模块

1. common: 定义了基本数据结构和一些工具的使用接口。例如，四舍五入取整的函数、时间转化相关的一些函数、数值计算的函数、互斥锁工具等。详见Common部分源码分析

2. sensor: 定义了传感器数据的相关数据结构。详见sensor源码分析

3. transform: 定义了位姿的数据结构及其相关的转化。如2d\3d的刚体变换等。详见：transform部分源码分析

4. mapping: 定义了上层应用的调用借口以及局部submap构建和基于闭环检测的位姿优化等的接口。这个文件也是算法的核心。其中mapping_2d和mapping_3d是对mapping接口的不同实现。详见mapping部分源码分析

5. io: 定义了一些与IO相关的工具，用于存读取数据、记录日志等。详见io部分源码分析

另外，/src/cartographer/configuration_files文件夹下是一些配置文件。算法一些参数在这里定义，通过改这里的参数来进行调试。

后面就不放图了，列文件地址，相关功能索引。

\src\cartographer_ros下：

cartographer_ros_msgs里定义了部分msg文件和srv文件

cartographer_rviz估计是与显示相关的函数

/src/cartographer_ros/cartographer_ros下：

configuration_files是一些配置文件

launch里是一些launch文件。暂时不管

进入/src/cartographer_ros/cartographer_ros/cartographer_ros。这里可以找到程序入口node_main.cc

cartographer_ros是一个Package, 包含多个node，每一个node也都有自己的入口main函数。但cartographer_node是其中最重要的一个node。从这个node的入口函数http://node_main.cc开始。

路径/src/cartographer_ros/cartographer_ros/cartographer_ros​​​

1.node_main.cc

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";

  ::ros::init(argc, argv, "cartographer_node");//初始化ROS，定义一个节点名为“cartographer_node”
  ::ros::start();//启动ROS

  cartographer_ros::ScopedRosLogSink ros_log_sink;
  cartographer_ros::Run();
  ::ros::shutdown();
}

 程序开始进行了一些初始化设置，并向rosmaseter注册了一个名为cartographer_node的节点并启动。

 cartographer_node的Run函数

void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  tf2_ros::TransformListener tf(tf_buffer);
//创建tf2_ros::Buffer用于缓存坐标系变换的数据，，时间为10s。tf2_ros::TransformListener订阅tf的话题，自动接收并更新变换数据到tfbuffer。

  NodeOptions node_options;
  TrajectoryOptions trajectory_options;
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
//从Lua配置文件，从FLAGS这两个指定路径加载参数。NodeOptions定义节点级配置（如发布子图的频率，优化参数）TrajectoryOptions定义轨迹级配置（如传感器参数，建图分配律。
**`std::tie`**：这个函数用于创建一个元组的左值引用，将传入的变量绑定到元组的各个元素。当右边的表达式返回一个元组时，元组的元素会被依次赋值给这些变量。例如，如果函数返回`std::tuple<NodeOptions, TrajectoryOptions>`，`std::tie`会将第一个元素赋值给`node_options`，第二个元素赋值给`trajectory_options`//auto关键字：auto可以在声明变量的时候根据变量初始值的类型自动为此变量选择匹配的类型
//初始化mapbuilder负责管理SLAM的全局地图和轨迹
  auto map_builder = cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
          node_options.map_builder_options);//cartographer::common::make_unique定义在common文件夹下的make_unique.h文件中。


  Node node(node_options, std::move(map_builder), &tf_buffer);
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);//加载数据包数据
  }

  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

  ::ros::spin();//阻塞当前线程，持续处理ROS话题回调，进入循环， 一直调用回调函数chatterCallback()，如接收激光雷达，IMU数据。消息回调处理函数;

//结束与后处理：FI停止所有轨迹的数据接收 RUN执行全局优化 Se保存最终状态到文件
  node.FinishAllTrajectories();
  node.RunFinalOptimization();

  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename);
  }
}