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最大。