Cartographer代码阅读笔记：前后端的入口(GlobalTrajectoryBuilder)

本人热衷于机器人导航算法研究，希望与志同道合伙伴一起探讨有关技术。

前沿

Local SLAM中的任务是接收激光雷达的扫描数据、进行扫描匹配、估计机器人位姿和将符合条件的扫描数据插入到子图中，而不具备任何闭环检测和全局优化的能力（这些是Global SLAM中的任务）。Local SLAM 和 Global SLAM之间一定存在某种通讯才使得后端（Global SLAM）能够看到前端（Local SLAM）构建的子图和位姿估计等信息，进而实现闭环检测和全局优化的目的。而这一工作被封装在一个GlobalTrajectoryBuilder类型的对象中。

Cartographer有关其他分析文章：回到目录

本文的代码文件在：cartographer/mapping/internal/global_trajectory_builder_2d.cc

接下来探讨一下该文件中关键函数的具体实现过程：

代码解析

GlobalTrajectoryBuilder()函数分析

/** 
 * @brief GlobalTrajectoryBuilder的构造
 * @param local_trajectory_builder 记录了轨迹跟踪器，是前端的核心类型
 * @param trajectory_id 轨迹索引
 * @param pose_estimate 记录了位姿图，是后端核心类型
 * @param local_slam_result_callback 记录了一个std::function类型的回调函数
 */
  GlobalTrajectoryBuilder(
      std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
      const int trajectory_id, PoseGraph* const pose_graph,
      const LocalSlamResultCallback& local_slam_result_callback)
      : trajectory_id_(trajectory_id),
        pose_graph_(pose_graph),
        local_trajectory_builder_(std::move(local_trajectory_builder)),
        local_slam_result_callback_(local_slam_result_callback) {}
  ~GlobalTrajectoryBuilder() override {}

AddSensorData()函数分析

  /** 
 * @brief 处理激光点云数据
 * @param sensor_id 记录了传感器类型
 * @param timed_point_cloud_data 记录了带时间戳的点云数据
 */
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    // 检测前端处理对象是否存在
    CHECK(local_trajectory_builder_)
        << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";

    // 通过local_trajectory_builder_对象的AddRangeData成员函数完成Local SLAM的任务，
    // 并返回前端处理结果，该结果记录了更新后子图信息和子图位姿信息        
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(
            sensor_id, timed_point_cloud_data);
    if (matching_result == nullptr) {
      return;
    }

    // 计数器自增，记录了前端输出的次数
    kLocalSlamMatchingResults->Increment();
    std::unique_ptr<InsertionResult> insertion_result;

    // 判断扫描数据是否成功插入子图
    if (matching_result->insertion_result != nullptr) {
      // 计数器自增，记录了前端成功插入子图的次数
      kLocalSlamInsertionResults->Increment();

      // pose_graph增加一个节点，会进行回环检测和后端优化，
      // 如果一切正常，会返回一个轨迹节点索引，记录到临时对象node_id中
      auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data, trajectory_id_,
          matching_result->insertion_result->insertion_submaps);

      CHECK_EQ(node_id.trajectory_id, trajectory_id_);
      // 结合前端处理的结果和后端输出的节点索引，实例化insertion_result对象
      insertion_result = common::make_unique<InsertionResult>(InsertionResult{
          node_id, matching_result->insertion_result->constant_data,
          std::vector<std::shared_ptr<const Submap>>(
              matching_result->insertion_result->insertion_submaps.begin(),
              matching_result->insertion_result->insertion_submaps.end())});
    }

    // 如果提供了回调函数，就调用该回调函数，同时将前端处理结果和刚构建的insertion_result对象传参
    if (local_slam_result_callback_) {
      local_slam_result_callback_(
          trajectory_id_, matching_result->time, matching_result->local_pose,
          std::move(matching_result->range_data_in_local),
          std::move(insertion_result));
    }
  }

小结

文中出现了两个核心对象，分别是：前端核心对象LocalTrajectoryBuilder2D和后端核心对象PoseGraph2D。同时知道了前后端如何配合完SLAM工作。

返回顶部 or 回到目录