cartographer_learn_4

• 續接前面讨論的話題
• MapBuilder的構造函數及類成員
• 前端構造

續接前面讨論的話題

前面經過不斷的學習（不斷的按F12和Ctrl+Alt+"-"）我們知道了先是啟動Node類，Node調用MapBuilderBridge，MapBuilderBridge調用它的類成員map_builder來實作具體的功能。具體的看map_builder的類型:unique_ptr< cartographer::mapping::MapBuilderInterface >，跳轉到Map_builder_interface.h中可以發現MapBuilderInterface并沒有實作什麼功能，隻是定義了一系列的虛函數。看來這裡使用了c++中的一個小小的知識點，使用基類指針指向派生類。其實map_builder真正的類型時cartographer::mapping::MapBuilder，在檔案src/cartographer/cartographer/mapping/map_builder.h中。接下來我們去那瞧瞧。

走了一段路覺得有必要回顧一下這個map_builder是怎麼來的了，在node_main.cc的Run函數中在加載完配置檔案後緊接着就調用src/cartographer/cartographer/mapping/map_builder.h中的CreateMapBuilder函數生成了一個MapBuilder,然後使用右值引用不斷的轉移所有權，直到轉移到了MapBuilderBridge的成員函數中。

MapBuilder的構造函數及類成員

class MapBuilder : public MapBuilderInterface {
......//一系列的類方法
 private:
  const proto::MapBuilderOptions options_;   //一些設定項
  common::ThreadPool thread_pool_; // 線程池
  std::unique_ptr<PoseGraph> pose_graph_;     //負責後端優化
  std::unique_ptr<sensor::CollatorInterface> sensor_collator_;  //傳感器資料收集？
  std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
      trajectory_builders_;      //負責前端
  std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
      all_trajectory_builder_options_;      //記錄每個前端的一些設定
}
           

一個slam過程大緻上可以分成兩部分，前端與後端，這樣看來這個類最重要的兩個成員是pose_graph_和trajectory_builders_，後面我們也會着重學習這兩部分。至于一些線程池是啥，之前聽過但沒學習過，相信随着學習的深入我們也能更加的了解線程池。

再來看構造函數

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options), thread_pool_(options.num_background_threads()) {
  CHECK(options.use_trajectory_builder_2d() ^
        options.use_trajectory_builder_3d());

  // 2d位姿圖(後端)的初始化
  if (options.use_trajectory_builder_2d()) {
    pose_graph_ = absl::make_unique<PoseGraph2D>(
        options_.pose_graph_options(),
        absl::make_unique<optimization::OptimizationProblem2D>(
            options_.pose_graph_options().optimization_problem_options()),
        &thread_pool_);
  }
  // 3d位姿圖(後端)的初始化
  if (options.use_trajectory_builder_3d()) {
    pose_graph_ = absl::make_unique<PoseGraph3D>(
        options_.pose_graph_options(),
        absl::make_unique<optimization::OptimizationProblem3D>(
            options_.pose_graph_options().optimization_problem_options()),
        &thread_pool_);
  } 

  if (options.collate_by_trajectory()) {
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    sensor_collator_ = absl::make_unique<sensor::Collator>();
  }
}
           

開始首先根據設定産生對應的pose_graph_（有2d和3d的差別），在生成一個CollatorInterface。這裡便有個疑問，前端在哪裡呢？

前端構造

原來前端的構造在AddTrajectoryBuilder這個類方法中，又上一大段代碼

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
  // 軌迹id
  const int trajectory_id = trajectory_builders_.size();
  // 運動過濾器, 運動太小沒必要進行更新
  absl::optional<MotionFilter> pose_graph_odometry_motion_filter;

  if (trajectory_options.has_pose_graph_odometry_motion_filter()) {
    LOG(INFO) << "Using a motion filter for adding odometry to the pose graph.";
    pose_graph_odometry_motion_filter.emplace(
        MotionFilter(trajectory_options.pose_graph_odometry_motion_filter()));
  }


  if (options_.use_trajectory_builder_3d()) {
    // local_trajectory_builder(前端)的初始化
    std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_3d_options()) {
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder3D>(
          trajectory_options.trajectory_builder_3d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    } 

    DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));
    //CollatedTrajectoryBuilder繼承了TrajectoryBuilderInterface
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将3D前端與3D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder3D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph3D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
  } 
  // 2d的軌迹
  else {
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }

    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));

    // CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将2D前端與2D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
  }

  // 是否是純定位模式, 如果是則隻儲存最近3個submap
  MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                  pose_graph_.get());

  // 如果給了初始位姿
  if (trajectory_options.has_initial_trajectory_pose()) {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    
    // 在位姿圖中設定初始位姿
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));
  }

  // 儲存軌迹的配置檔案
  proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
  for (const auto& sensor_id : expected_sensor_ids) {
    *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
  }
  *options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
      trajectory_options;
  all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
  
  CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
  return trajectory_id;
}
           

一來我們就看到了一個trajectory_id，這下子軌迹id就明白了，原來軌迹id就是該軌迹在MapBuilder::trajectory_builders_中的下标。然後是一個運動過濾器，據說可以過濾掉平移，旋轉或者時間間隔過小的運動（相信後面我們會再看它），再接着就是根據2d或者3d的設定生成對應的前端。最後判斷這個軌迹是否具有初始位姿，如果有就要告訴後端的pose_graph_這個id的前端的初始位姿。

來具體看看看前端的生成過程（以2d舉例），先生成一個據說是真正做前端的類LocalTrajectoryBuilder2D（一邊學一邊寫的。。。），然後再利用它生成一個我們前面所看到的做前端的類TrajectoryBuilderInterface。

本文已經來到了真正算法的門前了，後面我們将采用兩條線來學習，一條前端，一條後端，而且學習過程将和前面有一點不同，這回會對具體的一些實作做深入的讨論。