Cartographer源码阅读03-local_trajectory_builder.cc(2)-前端匹配 SLAM

在上一篇文章对local_trajectory_builder的部分代码进行了阅读，接下来，将对ScanMatch及InserttoSubmap进行代码的阅读。代码位于Cartographer/mapping/internal/2d/scan_matching/local_trajectory_builder_2d.cc
1.扫描匹配，ScanMatch 在扫描匹配方面，Cartographer采用的是CSM（相关性扫描匹配）＋基于梯度优化的方法（ceres）来实现，CSM可以简单地理解为暴力搜索，即每一个激光数据与子图里的每一个位姿进行匹配，直到找到最优位姿，进行三层for循环，不过谷歌采用多分辨率搜索的方式，来降低计算量，在后续也会进行解读。基于优化的方法是谷歌的ceres库来实现，原理就是构造误差函数，使其对最小化，其精度高，但是对初始值敏感。因此需要结合两种方法

//进行scanMatch，CSM+Ceres， std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( const common::Time time, const transform::Rigid2d& pose_prediction, const sensor::PointCloud& filtered_gravity_aligned_point_cloud) { if (active_submaps_.submaps().empty()) { return absl::make_unique(pose_prediction); }//进行匹配的子图，采用激光数据与子图进行匹配的方法，因此肯定要有子图 std::shared_ptr matching_submap = active_submaps_.submaps().front(); // The online correlative scan matcher will refine the initial estimate for // the Ceres scan matcher. // 初始的位姿，由PoseExtrapolator预测的初始位姿pose_prediction transform::Rigid2d initial_ceres_pose = pose_prediction; //是否进行real-time csm的匹配，默认是fasle但建议使用 //采用CSM（在线相关扫描匹配器将）对Ceres扫描匹配器的初始估计进行细化。返回一个更好的值initial_ceres_poseif (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); kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score); //返回一个更好的值initial_ceres_pose,传给ceres进行优化 }//进行ceres的匹配.--即基于优化的方法来进行匹配. auto pose_observation = absl::make_unique(); 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); // 统计误差，如果扫描获得位姿，则更新当前时刻的状态， if (pose_observation) { kCeresScanMatcherCostMetric->Observe(summary.final_cost); const double residual_distance =//误差 (pose_observation->translation() - pose_prediction.translation()) .norm(); kScanMatcherResidualDistanceMetric->Observe(residual_distance); const double residual_angle = std::abs(pose_observation->rotation().angle() - pose_prediction.rotation().angle()); kScanMatcherResidualAngleMetric->Observe(residual_angle); } return pose_observation; //返回扫描匹配后的位姿 }

阅读代码可以知道，CSM将姿态外推器插值得到的位姿进行细分优化，然后传给ceres进行扫描匹配，来解决基于优化的方法对初始值敏感的缺点。
2.InsertToSubmap ScanMatch获得了最优的匹配位姿，接下来InsertToSubmap将按照获得的最优位姿，把激光数据插入子图。

//插入到局部子图中． std::unique_ptr LocalTrajectoryBuilder2D::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_local, const sensor::PointCloud& filtered_gravity_aligned_point_cloud, const transform::Rigid3d& pose_estimate, const Eigen::Quaterniond& gravity_alignment) {//如果没有被MotionFilter滤掉 if (motion_filter_.IsSimilar(time, pose_estimate)) { return nullptr; }//把当前帧数据,插入到地图中. //调用的插入函数是子图的工具函数 // 把active_submaps_中维护的Submap列表放到insertion_submaps这个向量中 std::vector::shared_ptr> insertion_submaps = active_submaps_.InsertRangeData(range_data_in_local); //返回匹配结果 return absl::make_unique(InsertionResult{ std::make_shared(TrajectoryNode::Data{ time, gravity_alignment, filtered_gravity_aligned_point_cloud, {},// 'high_resolution_point_cloud' is only used in 3D. {},// 'low_resolution_point_cloud' is only used in 3D. {},// 'rotational_scan_matcher_histogram' is only used in 3D. pose_estimate}), std::move(insertion_submaps)}); }

将激光束插入子图实际是调用的子图的插入工具函数，后面我们也将进行阅读。
3.AddImuData加入IMU数据

//加入IMU数据,用来初始化和更新PoseExtrapolator void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) { CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added."; InitializeExtrapolator(imu_data.time); extrapolator_->AddImuData(imu_data); }

4.AddOdometryData

//加入里程计数据,用来初始化更新PoseExtrapolator void LocalTrajectoryBuilder2D::AddOdometryData( const sensor::OdometryData& odometry_data) { if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator we cannot add odometry data. LOG(INFO) << "Extrapolator not yet initialized."; return; }extrapolator_->AddOdometryData(odometry_data); }

5.初始化PoseExtrapolator.

/初始化PoseExtrapolator. void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) { if (extrapolator_ != nullptr) { return; }// We derive velocities from poses which are at least 1 ms apart for numerical // stability. Usually poses known to the extrapolator will be further apart // in time and thus the last two are used. // 用相差至少1ms的两个位姿来计算速度. constexpr double kExtrapolationEstimationTimeSec = 0.001; // TODO(gaschler): Consider using InitializeWithImu as 3D does. extrapolator_ = absl::make_unique( ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), options_.imu_gravity_time_constant()); extrapolator_->AddPose(time, transform::Rigid3d::Identity()); }

小节 【Cartographer源码阅读03-local_trajectory_builder.cc(2)-前端匹配】到目前为止，local_trajectory_builder的核心代码阅读完成，从总体上对前端匹配有了了解，具体的实现还有待进一步探索，不如说位姿外推器，csm相关性扫描匹配，ceres,子图的插入数据的工具函数等。我们依然按照传感器数据从输入到输出的流程来阅读代码，其中也会对其分支进行阅读。当我们按照这一个流程走过一遍，代码也就可以读懂了。