cartographer 源码解析 (四)

置顶

Philtell

已于 2023-05-20 10:55:33 修改

阅读量1.4k

点赞数 3

CC 4.0 BY-SA版权

分类专栏： slam 文章标签：自动驾驶 c++

于 2021-07-27 22:00:56 首次发布

每一个优秀的人的背后都有一段沉默的时光，那是付出很多努力却不一定有结果的日子，我们把它叫做扎根。

本文链接：https://blog.youkuaiyun.com/CCCrunner/article/details/119154163

本文详细解析cartographer源码中多激光数据同步的过程，包括RangeDataCollator如何收集和同步传感器数据，以及如何处理时间戳进行数据整理。通过对local_trajectory_builder_2d.cc和range_data_collator.cc的逐行解析，阐述了数据同步的关键步骤和逻辑。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

相关链接：

cartographer 源码解析(一)

cartographer 源码解析(二)

cartographer 源码解析(三)

cartographer 源码解析(四)

cartographer 源码解析(五)

cartographer 源码解析(六)

因为第四节用文字表达起来有些难以直观的去理解，所以我录制了讲解的视频：https://www.bilibili.com/video/BV1go4y1D792
这一章主要内容是多激光的数据同步
首先看一下全局的轨迹构建器里面global_trajectory_builder.cc里面的添加传感器数据AddSensorData()
废话不多数，在这是，上代码

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.";
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(
            sensor_id, timed_point_cloud_data); // 第一步往前端里面送，做scan-match
    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();
      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));
    }
  }

我们这里就先只分析激光了。
local_trajectory_builder_2d.cc

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
   
   
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
   
   
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }

  const common::Time& time = synchronized_data.time;
  // Initialize extrapolator now if we do not ever use an IMU.
  if (!options_.use_imu_data()) {
   
   
    InitializeExtrapolator(time);
  }

  if (extrapolator_ == nullptr) {
   
   
    // Until we've initialized the extrapolator with our first IMU message, we
    // cannot compute the orientation of the rangefinder.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return nullptr;
  }

  CHECK(!synchronized_data.ranges.empty());
  // TODO(gaschler): Check if this can strictly be 0.
  CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time.time);
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
   
   
    LOG(INFO)