Cartographer 3D中submap的建立过程

2.LocalTrajectoryBuilder3D::AddAccumulatedRangeData1）基于time查询得到当前激光点云数据帧的pos2）计算submap下当前帧激光点云的预测pose，作为ceres优化的初始值使用3）高分辨率点云匹配，可选4） 高低分辨率点云滤波，然后在submap中对点云进行匹配5） 将优化结果pose_observation_in_submap的误差加到kCeresScanMatcherCostMetric中6）得到pose_estimation，并估计此时的重力7）重新根据local系下的pose_estimate 对激光点云数据进行投影8）将激光点云数据插入到当前的与之匹配的submap中9）计算slam前端的延迟并进行检测和监控10）返回匹配优化的结果11）补充说明匹配的基本思想

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