cartographer 代码思想解读（12）- local SLAM和global SLAM链接global_trajectory_builder

从上节map_builder代码分析可看出，cartographer SLAM和传统SLAM一致，分为前端和后端两部分：local_trajectory_builder和pose_graph，而将两者如何结合在一起，则采用了GlobalTrajectoryBuilder类进行封装。其功能是将localslam的一些定位和submap信息，送给后端优化pose_graph，从而完成闭环和优化，从而完成整个SLAM。

GlobalTrajectoryBuilder2D 创建

由于只分析2d SLAM的情况，因此可针对2d的情况进行创建。采用LocalTrajectoryBuilder2D前端类和PoseGraph2D后端类。

	std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
    const TrajectoryBuilderInterface::LocalSlamResultCallback&
        local_slam_result_callback) {
  return absl::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback);
}

GlobalTrajectoryBuilder 构造函数和成员变量

  const int trajectory_id_;                                           // 当前轨迹ID
  PoseGraph* const pose_graph_;                                       // 全局图优化即约束
  std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;  // 局部规划器，即local slam
  LocalSlamResultCallback local_slam_result_callback_;                // 结果回调器

非常清楚简单，完成指定ID轨迹中前端和后端的联系，因此需要获取trajectory的ID。
其中pose_graph_采用的2d的闭环类。
LocalTrajectoryBuilder采用的是2的轨迹跟踪器LocalTrajectoryBuilder2D。

  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 {}

类的创建主要是将参数复制到类中的全局缓存变量中。

加入传感器数据接口

virtual void AddSensorData(const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) = 0;
virtual void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) = 0;
virtual void AddSensorData(const std::string& sensor_id, const sensor::OdometryData& odometry_data) = 0;
virtual void AddSensorData(const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
virtual void AddSensorData(const std::string& sensor_id, const sensor::LandmarkData& landmark_data) = 0;

其中加入点云数据为核心功能，下面将详细分析。其他传感器接口都为选配，非必需项，前端代码已经分析过。

IMU传感器加入接口

  // 加入imu数据
  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    if (local_trajectory_builder_) {
      local_trajectory_builder_->AddImuData(imu_data);
    }
    pose_graph_->AddImuData(trajectory_id_, imu_data);
  }

新来的imu数据会同时进入到前端和后端用于轨迹推算。

odometer 加入接口

  // 加入odometer 数据
  void AddSensorData(const std::string& sensor_id,
                     const sensor::OdometryData& odometry_data) override {
    CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
    if (local_trajectory_builder_) {
      local_trajectory_builder_->AddOdometryData(odometry_data);
    }
    pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
  }

同上

绝对位置传感器加入接口

  // 相当于全局位置信息，因此无需传给local slam
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::FixedFramePoseData& fixed_frame_pose) override {
    if (fixed_frame_pose.pose.has_value()) {
      CHECK(fixed_frame_pose.pose.value().IsValid())
          << fixed_frame_pose.pose.value();
    }
    pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
  }

 // 路标插入
  void AddSensorData(const std::string& sensor_id,
                     const sensor::LandmarkData& landmark_data) override {
    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
  }

  // 前端结果直接给后端，但是不能是当前的前端
  void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
                                  local_slam_result_data) override {
    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
                                         "local_trajectory_builder_ present.";
    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
  }

上面三种均可认为插入已知的全局位置，因此无需插入local slam中，仅用于后端约束优化。

点云传感器传入（核心）

  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override

函数声明中仅有sensorid和对应的点云数据，其中点云数据由cartographer_ros中将激光数据预处理后转换成cartographer的识别的数据的格式。

    CHECK(local_trajectory_builder_)
        << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
    // 调用local slam主流程获取匹配结果
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(
            sensor_id, timed_point_cloud_data);
    if (matching_result == nullptr) {
      // The range data has not been fully accumulated yet.
      return;
    }

调用local slam核心接口local_trajectory_builder_->AddRangeData进行前端匹配，详见cartographer 代码思想解读（10）- slam前端LocalTrajectoryBuilder2D类（主流程）。
输出前端匹配结果matching_result。结果中包含了轨迹中节点的pose、点云和submap的相关信息。前面讲过，匹配记录的节点存在运动滤波等其他操作，因此并非每帧传感器信息均会返回匹配结果。如果未返回匹配结果，则无需处理。

    // 前端匹配的结果，即submap位置和submap
    std::unique_ptr<InsertionResult> insertion_result;
    // 存在前端匹配结果（不是每次加入激光都会存在结果，因为有运动滤波器，只有和上一帧存在一定位移时才会有匹配结果）
    if (matching_result->insertion_result != nullptr) {
      kLocalSlamInsertionResults->Increment();
      // 将匹配后的结果加入图优化节点中
      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_);
      // 重新封装前端匹配的后的submap结果，并增加一个node_id
      insertion_result = absl::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())});
    }

如果前端匹配存在结果时，则将前端匹配的结果加入后端优化pose_graph_中，即后端增加一个新的节点。内容包含匹配的点云，轨迹id和submap信息，并输出后端优化位姿图节点ID。最后重新封装前端匹配结果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));
    }

可采用回调函数将前端匹配的结果信息传递出去。

总结

从代码分析可看出，顶层mapbuilder接口仅是用于创建一个轨迹用于slam，同时添加了相关的配置信息，并创建了本节的内容global_trajectory_builder接口类。此类包含所有传感器顶层输入接口，其中点云传感器输入为核心调用接口。内容调用了前端local_trajectory_builder_->AddRangeData核心接口输入，即所有前端local slam的处理接口。实现了相关匹配、位置估计、submap构建与更新所有所有处理，输出匹配结果。将匹配结果传递后端处理类接口，即pose_graph_类，从而完成整个SLAM。local_trajectory_builder_->AddRangeData已经分析完成，而新引入的pose_graph_->AddNode则为后端图优化顶层接口。