[游戏开发]cartographer(4) --前后端入口

在?MapBuilder::AddTrajectoryBuilder中

1.看2d的局部slam

?local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(

? ? ? ? ? trajectory_options.trajectory_builder_2d_options(),

? ? ? ? ? SelectRangeSensorIds(expected_sensor_ids));

创建局部SLAM的所有功能，包括位姿估计、扫描匹配等

看构造函数：

::LocalTrajectoryBuilder2D(
    const proto::LocalTrajectoryBuilderOptions2D& options,     //轨迹跟踪器的配置选项
    const std::vector<std::string>& expected_range_sensor_ids) //传感器id
    : options_(options),
      active_submaps_(options.submaps_options()),       //当前正在维护的子图
      motion_filter_(options_.motion_filter_options()), //运动滤波器，对位姿相关的数据进行降采样
      real_time_correlative_scan_matcher_(       //实时相关性分析的扫描匹配器
          options_.real_time_correlative_scan_matcher_options()),
      ceres_scan_matcher_(options_.ceres_scan_matcher_options()), //使用Ceres库将扫描数据放置到地图中的扫描匹配器
      range_data_collator_(expected_range_sensor_ids) {}

2.??来管理和收集传感器数据：CollatedTrajectoryBuilder?将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder

 trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,       
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

构造函数

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    const proto::TrajectoryBuilderOptions& trajectory_options, //配置
    sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
    const std::set<SensorId>& expected_sensor_ids,
    std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
    : sensor_collator_(sensor_collator), //传感器集
      collate_landmarks_(trajectory_options.collate_landmarks()),//landmark集
      collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
      trajectory_id_(trajectory_id),
      wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),//轨迹集
      last_logging_time_(std::chrono::steady_clock::now()) {
  absl::flat_hash_set<std::string> expected_sensor_id_strings;
  for (const auto& sensor_id : expected_sensor_ids) { //遍历传感器
    if (sensor_id.type == SensorId::SensorType::LANDMARK &&
        !collate_landmarks_) {
      continue;//否是不接受Landmark
    }
    if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
        !collate_fixed_frame_) {
      continue;
    }
    expected_sensor_id_strings.insert(sensor_id.id);//插入sensor id
  }
  sensor_collator_->AddTrajectory(
      trajectory_id, expected_sensor_id_strings,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
        HandleCollatedSensorData(sensor_id, std::move(data)); //添加传感器数据回调
      });
}

传感器数据回调

void CollatedTrajectoryBuilder::HandleCollatedSensorData(
    const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
  auto it = rate_timers_.find(sensor_id);//sensor id
  if (it == rate_timers_.end()) {//最后一个传感器
    it = rate_timers_
             .emplace(
                 std::piecewise_construct, std::forward_as_tuple(sensor_id),
                 std::forward_as_tuple(
                     common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
             .first;
  }
  it->second.Pulse(data->GetTime());

  if (std::chrono::steady_clock::now() - last_logging_time_ >
      common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
    for (const auto& pair : rate_timers_) {
      LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();//显示传感器帧率
    }
    last_logging_time_ = std::chrono::steady_clock::now();
  }

  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());///
}

data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());//添加传感器数据

 void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

?trajectory_builder->AddSensorData(sensor_id_, data_);

我们看到GlobalTrajectoryBuilder为各种数据类型重载了了一个叫AddSensorData的函数，有点云的，有imu的，里程计的，gps的等

void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }

  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, imu_data));
  }

  void AddSensorData(const std::string& sensor_id,
                     const sensor::OdometryData& odometry_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, odometry_data));
  }

  void AddSensorData(
      const std::string& sensor_id,
      const sensor::FixedFramePoseData& fixed_frame_pose_data) override {
    if (collate_fixed_frame_) {
      AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
      return;
    }
    wrapped_trajectory_builder_->AddSensorData(sensor_id,
                                               fixed_frame_pose_data);
  }

  void AddSensorData(const std::string& sensor_id,
                     const sensor::LandmarkData& landmark_data) override {
    if (collate_landmarks_) {
      AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
      return;
    }
    wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data);
  }

?3，构建全局轨迹集? ?后端的入口

?CreateGlobalTrajectoryBuilder2D(? std::move(local_trajectory_builder), trajectory_id,

? ? ? ? ? ? static_cast<PoseGraph2D*>(pose_graph_.get()),? ?local_slam_result_callback)));

//	trajectory_id轨迹索引
//PoseGraph	位姿图，后端的核心对象，其数据类型是一个模板参数
//LocalTrajectoryBuilder	位姿跟踪器，前端的核心对象
//LocalSlamResultCallback前端数据更新后的回调函数
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);//进行扫描匹配，估计机器人位姿，并将传感器数据插入子图中，更新子图
}