Catorgrapher源码阅读01-global_trajectory_bulider.cc

对Cartographer的阅读我们按照从数据的（传感器）的输入开始，直到结果的输出为止。因此代码从global_trajectory_bulider.cc开始阅读，里面包含了激光数据、IMU、里程计数据等的输入。
文件在Cartographer/mapping/internal/global_trajectory_builder.cc

1.加入激光数据

初始化函数：

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

加入激光数据：在这里函数的实现非常简单，主要是因为此处多为调用函数，主要分为了前端匹配，调用了 Cartographer/mapping/internal/2d/scan_matching/local_trajectory_builder_2d.cc->AddRangeData（）函数来实现；后端调用pose_graph_->AddNode

local_trajectory_builder_2d.cc:因为研究方向为2d,所以只对2d的代码进行阅读.local_trajectory_builder_相当于局部匹配器,trajectory实际是机器人走过的一段轨迹,cartographer允许分段建立多段轨迹.

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.";

    //进行局部的子图匹配，cartographer采用CSM（相关性匹配）＋基于优化的前端匹配方式，接下来我们将学到
    //调用了LocalTrajectoryBuilder－＞AddRangeData,在接下来将学到,具体的实现函数
    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;
    }
    
    kLocalSlamMatchingResults->Increment();
    
    //如果插入地图成功.
    std::unique_ptr<InsertionResult> insertion_result;
    if (matching_result->insertion_result != nullptr)
    {
      kLocalSlamInsertionResults->Increment();

      //pose-graph增加一个节点,本函数会进行回环检测和后端优化.
      //此处节点就是指的关键帧,一个节点就是一个关键帧数据
      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 = 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())});
    }

    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));
    }
  }

2.加入IMU数据

跟激光数据的加入一样,均调用了local_trajectory_builder及pose_graph_里的函数,因此,接下来就要去查看这两处的代码.

//加入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);
  }

3.加入里程计数据

同上

//加入里程计数据.
  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);
  }

4.加入Landmark

//加入landmark数据.
  void AddSensorData(const std::string& sensor_id,
                     const sensor::LandmarkData& landmark_data) override
  {
    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
  }

5.加入局部slam数据

//加入局部slam数据.
  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_);
  }

总结

由global_trajectory_builder.cc代码阅读之后发现,无论是激光数据还是里程计和IMU数据,它们的前端匹配均调用local_trajectory_builder_里的函数,后端优化均调用pose_graph_里的函数,因此接下来将对这两部分代码进行阅读.