cartographer LocalTrajectoryBuilder2D

所处：

LocalTrajectoryBuilder2D cartographer/mapping/internal/2d/local_trajectory_build_2d.cc /.h中

LocalTrajectoryBuilder2D 类在 map_build.cc 中 AddTrajectoryBuilder 函数中被构造

主要函数：

函数一、

  std::unique_ptr<MatchingResult> AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& range_data);

具体分析：

1.同步不同传感器的点云数据，得到较好的点云数据该点云数据的坐标为 tracking

 range_data_collator_.AddRangeData(sensor_id, unsynchronized_data)

2. 不是用IMU时初始化外推器

3.将点云数据带上时间，同时将点云数据用重力修正。

4.将点云数据按照配置的范围选取最终获得重力对齐范围数据

5.调用下面函数 AddAccumulatedRangeData函数

函数二、

  std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
      common::Time time, const sensor::RangeData& gravity_aligned_range_data,
      const transform::Rigid3d& gravity_alignment);

1. 姿态预测（transform::Rigid2d ）非重力对齐姿势预测 * 重力对齐的逆（gravity_alignment.inverse()）

2.调用匹配函数 ScanMatch 函数，激光匹配后得到较准确的 pose_estimate的位姿。（匹配是跟活跃子图匹配）

3.pose_estimate转化为 Rigid3d 进行重力对齐。

4.将激光数据转化为 local_trajectory 坐标系下 pose_estimate_2d * gravity_aligned_range_data。

5.插入子图，InsertIntoSubmap 函数。插入子图的激光数据的坐标系为local_trajectory

6.返回匹配结果，MatchingResult结构体。

函数三、

  std::unique_ptr<transform::Rigid2d> ScanMatch(
      common::Time time, const transform::Rigid2d& pose_prediction,
      const sensor::RangeData& gravity_aligned_range_data);

1.活跃子图为空时，返回当前估计位姿的指针 return absl::make_unique<transform::Rigid2d>(pose_prediction);

2.匹配的子图为当前活跃子图的第一个子图

3.如果使用在线相关性扫描匹配时，

const double score = real_time_correlative_scan_matcher_.Match(
pose_prediction, filtered_gravity_aligned_point_cloud,
*static_cast<const ProbabilityGrid*>(matching_submap->grid()),
&initial_ceres_pose);
kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);

4. 3中if完成，使用ceres非线性相关性扫描匹配

ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), pose_observation.get(),
&summary);

5.如果扫描获得位姿，则跟新当前时刻的状态，

6.返回 pose_observation

函数四、

  std::unique_ptr<InsertionResult> InsertIntoSubmap(
      common::Time time, const sensor::RangeData& range_data_in_local,
      const sensor::RangeData& gravity_aligned_range_data,
      const transform::Rigid3d& pose_estimate,
      const Eigen::Quaterniond& gravity_alignment);

1.如果累积的运动（线性，旋转或时间）高于 threshold(阈值)，返回false。否则，相对运动被累积并且返回true。

2.如果是true return nullptr;

3.在调用InsertRangeData（range_data_in_local）之前必须在此处查询活动子图，因为查询的值对下次插入有效。

4.过滤重力对齐点云数据。

5.返回 InsertionResult 类指针。