2020 Cartographer 新版本源码解析

根据课程《深蓝学院》课程 总结

首先数据入口在文件global_trajectory_builder中，在其头文件中仅创建两个初始实例化函数：

std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D()
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D()

主要内容在.cc文件中， AddSensorData函数，增加激光点云数据，IMU数据，里程计数据，其中点云数据最为关键，仅仅存在点云数据也可运行。
在增加点云数据时：
1.激光的前端匹配，调用2d文件夹中的local_trajectory_builder_2d

    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(
            sensor_id, timed_point_cloud_data);

2.如果匹配成功，则把当前节点添加到pose-graph中，以及地图中，得到返回信息。

    if (matching_result->insertion_result != nullptr) {
      kLocalSlamInsertionResults->Increment();
      //当前节点添加到pose-graph中
      auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data, trajectory_id_,
      CHECK_EQ(node_id.trajectory_id, trajectory_id_);
      //当前节点添加到地图中 返回的信息
      insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
          node_id, matching_result->insertion_result->constant_data,
          std::vector<std::shared_ptr<const Submap>>(
              matching_result->insertion_result->insertion_submaps.begin(),
              matching_result->insertion_result->insertion_submaps.end())});
    }

1.1 AddRangeData函数中，主要是动态激光点的去除，以及非法激光点的过滤
a. 首先是时间同步，当仅存在激光雷达传感器时，相当于不起作用

auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);

b.得到数据中第一个点云的时间戳

  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time.time);
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

接下来为点云进行运动畸变去除
c.通过位姿插值得到每一个点云的位姿，然后进行运动畸变的去除

range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());

d.在cartographer中并不是所有帧处理，而是累计处理++num_accumulated_，累计帧足够才开始进行处理AddAccumulatedRangeData()

if (num_accumulated_ >= options_.num_accumulated_range_data()){
//估计当前机器人的pitch & roll角度，2D中本身就是0
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));
} 

在处理前，利用pitch和roll把点云投影到平面上，并且只保留一定高度范围内的点云数据。然后利用栅格滤波，有一个点云的稀疏化操作。

1.2 AddAccumulatedRangeData()主要进行前端数据处理的函数

  // Computes a gravity aligned pose prediction. 预测当前机器人位资
  //此时预测的位资为3D
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);
  //通过重力向量消除了roll和pitch
  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
      
  //对数据进行进一步的voxel filter过滤
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
                                  options_.adaptive_voxel_filter_options());

通过scan-matching 估计当前点位姿

  // local map frame <- gravity-aligned frame
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

如果匹配成功得到位姿，便可组成一个3D位姿，因为其位姿插值器为3D，所以的到新的位姿之后便可以更新位姿插值器。

  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  extrapolator_->AddPose(time, pose_estimate);

接着将激光数据转换到局部坐标系中，并将数据插入到local submap中

sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()));
                         
  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());

1.2.1 ScanMatch()前端匹配的核心函数
首先需要得到最近的子图进行匹配

  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();

进行CSM匹配 —— 暴力匹配，如果有里程计的话则不需要使用该搜索。

 if (options_.use_online_correlative_scan_matching()) {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
    //计算CSM位姿得分
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

在CSM的基础上，再进行基于优化的匹配，（基于高斯牛顿的匹配）
即CSM+PL-ICP 或 CSM+基于优化。这里使用了ceres优化器进行优化。

auto pose_observation = absl::make_unique<transform::Rigid2d>();
  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);

此时得到位姿，接下来插入到子图中InsertIntoSubmap。

1.2.2 InsertIntoSubmap()将激光数据插入到子图中
首先判断是否运动了足够的距离，运动够了才进行处理

  if (motion_filter_.IsSimilar(time, pose_estimate)) {
    return nullptr;
  }

然后，插入数据并返回被插入的子图（最近的两个子图，因为两个子图之间存在重合部分）。
如果没有子图，或者到了需要生成新子图的时候，则生成一个新子图。
然后插入数据，如果局部子图帧数已经足够则设置为finish，下次进入到InsertRangeData时，将删掉已经finish的子图，则不会进行帧间匹配。

  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);

最终返回插入的结果。

以上是前端部分的内容，接下来为后端部分。
2.1 后端部分AddNode()，往pose-graph中添加节点位姿，通过调用AppendNode()实现，同时把约束计算函数加入到执行队列中。
首先将local pose转换到 global pose

  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

然后加入节点

  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);

检查是否有 finished submap

 const bool newly_finished_submap =
      insertion_submaps.front()->insertion_finished();

把ComputeConstraintsForNode函数加入到执行队列，为当前节点增加约束&回环检测

  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });

添加的约束主要有：
（1）当前节点和insertion submap之间的约束，帧间连边
（2）计算当前节点和每一个完成的submap之间的约束，即回环检测
（3）如果有新完成的submap，需要计算pose-graph中每一个点和新submap之间的约束，其中pose-graph为老节点，可以理解为一个反向的回环检测