cartographer 代码思想解读（10）- slam前端LocalTrajectoryBuilder2D类（主流程）

前面9节描述和分析了cartographer在slam前端使用的算法、原理以及具体实现。每个知识点都是任何slam中几乎必不可少的，也是slam技术的关键点。而slam则是将每个算法串联起来，构建slam的前端处理流程，从而实现slam（暂时缺少后端闭环处理）。而LocalTrajectoryBuilder2D类即为cartographer的前端核心流程和调用接口，即local slam。完成了传感器预处理、scanmatch、位置估计、submap构建及更新调用。其代码目录：
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

local_trajectory_builder_2d处理流程

引入官方的总体流程图

其中红色框圈起来的则是local_trajectory_builder_2d所处理的所有内容和流程，其输入仅为激光数据（点云）、odometry，Imu DATA。而经过一系列处理后输出结果为cartographer定义的InsertionResult结果类，包含了时间戳、当前机器人位置、submap及在世界坐标的位置、激光点云结合等。即经过前端处理可以得到一系列的submap，如此后续其他模块可将submap进行后端处理拼接形成整个地图。
整体处理流程如下：
1.激光原始数据预处理融合转换成cartographer使用的点云格式；
2.激光点云数据经过两次滤波后进行真正使用；
3.odometry、imu及其匹配后的精确位置构建位置估计器实时估计下刻位置；
4.根据预估位置和点云数据进行scan match获取优化后更精确的位置；
5.经过运动滤波器（目的是降采样）后的位置及其对应的点云进行维护和更新submap；
6.当submap满足一定数量的激光帧时，输出MatchingResult类型结果；

注意：
1.其中Fixed Frame Pose在2DSLAM中暂未找到传入接口。
2.输出的结果类型应该是MatchingResult，而不是图中的InsertionResult

local_trajectory_builder_2d类定义

类含有变量

  const proto::LocalTrajectoryBuilderOptions2D options_;    // 轨迹跟踪器的配置信息
  ActiveSubmaps2D active_submaps_;                          // 维护正在使用的submap,已分析过，包括栅格地图类型，插入，更新

  MotionFilter motion_filter_;                              // 运动滤波器，用来对运动pose进行降采样
  scan_matching::RealTimeCorrelativeScanMatcher2D           // 相关匹配器，已分析过
      real_time_correlative_scan_matcher_;
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_;    // 优化匹配器，已分析过

  std::unique_ptr<PoseExtrapolator> extrapolator_;          // 位置估计器，可根据历史轨迹及其传感器估计下时刻位置

  int num_accumulated_ = 0;                                 // 累计激光点云个数
  sensor::RangeData accumulated_range_data_;                // 预处理后激光点云，也包含激光原点origin坐标，即经过融合，矫正等操作
                                                            // 也是核心算法使用的数据结构，用于map 更新和匹配
  absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
  absl::optional<double> last_thread_cpu_time_seconds_;
  absl::optional<common::Time> last_sensor_time_;

  RangeDataCollator range_data_collator_;                   // 激光数据收集器，几种激光按照时间戳融合

从类成员变量可看出包括
1.submap封装维护类ActiveSubmaps2D
2.相关scan-match类RealTimeCorrelativeScanMatcher2D
3.ceres库匹配类CeresScanMatcher2D
4.位置估计器类PoseExtrapolator
5.激光点云预处理后结果类sensor::RangeData
6.多激光数据融合同步类RangeDataCollator
7.运动滤波器类MotionFilter（较为简单，分析流程会插入分析）
以上除第7项外，均已分析。

输出结果MatchingResult结构体

  // 用于存储submap列表数据的结构 
  // 用于存储submap列表数据的结构 
  struct InsertionResult {
    std::shared_ptr<const TrajectoryNode::Data> constant_data;       // 匹配后的点云结果
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;  // 更新后的submap列表（仅最新的submap）
  };
  // 用于前端匹配的数据结构
  struct MatchingResult {
    common::Time time;                                               // 时间戳
    transform::Rigid3d local_pose;                                   // 在submap中位置
    sensor::RangeData range_data_in_local;                           // 本帧的点云
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;         // 匹配后的submap结果
  };

其中rajectoryNode::Data是后续用于记录和拼接的rajectoryNode定义的结构体，主要记录的是submap对应的属性信息包括位置，重力加速度方向，修正后的水平点云数据等；为2d和3dSLAM公共结构体类型。其代码目录在
cartographer/mapping/trajectory_node.h

// 轨迹节点结构体
struct TrajectoryNode {
  //