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。

本文已经来到了真正算法的门前了，后面我们将采用两条线来学习，一条前端，一条后端，而且学习过程将和前面有一点不同，这回会对具体的一些实现做深入的讨论。