Cartographer SLAM——submap的建立过程（二）

2.LocalTrajectoryBuilder3D::AddAccumulatedRangeData

此函数对转换到local系下的激光点云数据进行下一步处理，主要是基于submap的匹配和位姿优化，并插入submap。

std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& filtered_range_data_in_tracking)
    {
    ...
    }

1）基于time查询得到当前激光点云数据帧的pos

 const transform::Rigid3d pose_prediction =
      extrapolator_->ExtrapolatePose(time);

2）计算submap下当前帧激光点云的预测pose，作为ceres优化的初始值使用

根据当前submap的位姿pos和上一步得到的当前激光点云的pos计算submap下当前激光点云数据的预测pos（initial_ceres_pose和 initial_ceres_pose_z0）

const Eigen::Quaterniond base_q = extrapolator_->getnewestRotation();
// 从extrapolator_中拿到predict出来的pose，然后作为初值，继续做scan matching
  std::shared_ptr<const mapping::Submap3D> matching_submap =
      active_submaps_.submaps().front();

  Eigen::Quaterniond sub_map_r = matching_submap->local_pose().rotation();
  //std::cout<<"test_pose submap: "<<sub_map_r.coeffs().transpose()<<std::endl;
  transform::Rigid3d initial_ceres_pose =
      matching_submap->local_pose().inverse() * pose_prediction;

3）高分辨率点云匹配，可选

  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
      options_.high_resolution_adaptive_voxel_filter_options());
  const sensor::PointCloud high_resolution_point_cloud_in_tracking =
      adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns);
  if (high_resolution_point_cloud_in_tracking.empty()) {
    LOG(WARNING) << "Dropped empty high resolution point cloud data.";
    return nullptr;
  }
    if (options_.use_online_correlative_scan_matching()) {
    // We take a copy since we use 'initial_ceres_pose' as an output argument.
    const transform::Rigid3d initial_pose = initial_ceres_pose;
    double score = real_time_correlative_scan_matcher_->Match(
        initial_pose, high_resolution_point_cloud_in_tracking,
        matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

4） 高低分辨率点云滤波，然后在submap中对点云进行匹配

此处只进行低分辨率点云滤波，上一步已经进行高分辨率滤波。

transform::Rigid3d pose_observation_in_submap;
  ceres::Solver::Summary summary;

  sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
      options_.low_resolution_adaptive_voxel_filter_options());
  const sensor::PointCloud low_resolution_point_cloud_in_tracking =
      low_resolution_adaptive_voxel_filter.Filter(
          filtered_range_data_in_tracking.returns);
  if (low_resolution_point_cloud_in_tracking.empty()) {
    LOG(WARNING) << "Dropped empty low resolution point cloud data.";
    return nullptr;
  }
  ceres_scan_matcher_->Match(
      (matching_submap->local_pose().inverse() * pose_prediction).translation(),
      initial_ceres_pose,
      {{&high_resolution_point_cloud_in_tracking,
        &matching_submap->high_resolution_hybrid_grid()},
       {&low_resolution_point_cloud_in_tracking,
        &matching_submap->low_resolution_hybrid_grid()}},
      &pose_observation_in_submap, &summary);

ceres_scan_matcher_->Match完成submap下点云的匹配，输入分别为：
（1）submap坐标系下的的初始translation
（2）待优化位姿的初始值initial_ceres_pose
（3）当前激光点云和submap的高分辨率点云
（4）当前激光点云和submap的低分辨率点云
（5）待优化结果pose_observation_in_submap
（6）优化总结

5） 将优化结果pose_observation_in_submap的误差加到kCeresScanMatcherCostMetric中

这是为了检测和监控优化结果？

kCeresScanMatcherCostMetric->Observe(summary.final_cost);
  double residual_distance = (pose_observation_in_submap.translation() -
                              initial_ceres_pose.translation())
                                 .norm();
  kScanMatcherResidualDistanceMetric->Observe(residual_distance);
  double residual_angle = pose_observation_in_submap.rotation().angularDistance(
      initial_ceres_pose.rotation());
  kScanMatcherResidualAngleMetric->Observe(residual_angle);

6）得到pose_estimation，并估计此时的重力

pose_estimation表示当前laserscan在local系下time时刻的优化出来的位姿。上一步估计出来的是当前laserscan在submap中的位姿。并将pose_estimation添加到extrapolator中，并估计此时的中立。

 Eigen::Quaterniond actual_delta = base_q.conjugate()*(matching_submap->local_pose().rotation()*pose_observation_in_submap.rotation());
  transform::Rigid3d pose_estimate =
      matching_submap->local_pose() * pose_observation_in_submap;
  extrapolator_->AddPose(time, pose_estimate);
  const Eigen::Quaterniond gravity_alignment =
      extrapolator_->EstimateGravityOrientation(time);

7）重新根据local系下的pose_estimate 对激光点云数据进行投影

 sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
      filtered_range_data_in_tracking, pose_estimate.cast<float>());

这样便可以得到local系下的准确激光点云坐标。

8）将激光点云数据插入到当前的与之匹配的submap中

调用函数为InsertIntoSubmap（）

  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, filtered_range_data_in_local, filtered_range_data_in_tracking,
      high_resolution_point_cloud_in_tracking,
      low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);

9）计算slam前端的延迟并进行检测和监控

 auto duration = std::chrono::steady_clock::now() - accumulation_started_;
  kLocalSlamLatencyMetric->Set(
      std::chrono::duration_cast<std::chrono::seconds>(duration).count());

10）返回匹配优化的结果

return common::make_unique<MatchingResult>(MatchingResult{
      time, pose_estimate, std::move(filtered_range_data_in_local),
      std::move(insertion_result), active_submaps_.CurrentSubmap() });

包含：1）激光点云的时刻；2）local pose；3）local下的激光点云数据；4）插入submap的结果；5）local下当前激活的submap。

11）补充说明匹配的基本思想

cost function见上图，hk表示当前的激光点云；Ts表示待求解的submap下的激光点云位姿；Ts*hk可得submap中疑似激光点云位置，对submap中的这些位置（疑似激光点云位置）基于Msmooth-双三次插值法计算障碍概率，也就是点云的概率，点云本身就是检测到障碍的位置。所以使得此概率值最大时，Ts便是优化求解出来的位姿。故cost function的构建是使得（1-p）最小，此时p最大。