cartographer代码学习-第一帧雷达数据到来时栅格地图的原点

最新推荐文章于 2024-04-14 16:58:56 发布

原创

最新推荐文章于 2024-04-14 16:58:56 发布 · 667 阅读

11 ·

CC 4.0 BY-SA版权

文章标签：

#学习

在cartographer中，存在map与sub_map两种地图，对于每一张sub_map，它的地图原点为当前机器人所在的map坐标系下的位姿。那么对于初始化状态时，第一帧雷达数据到来时建立的第一张sub_map的原点是如何确定的？是否是默认的（0，0，0）呢？

根据前面学习的传感器的数据走向可知获取的laser数据会给到LocalTrajectoryBuilder2D::ScanMatch函数进行扫描匹配，展开看一下这个函数：

/**
 * @brief 进行扫描匹配
 * 
 * @param[in] time 点云的时间
 * @param[in] pose_prediction 先验位姿
 * @param[in] filtered_gravity_aligned_point_cloud 匹配用的点云
 * @return std::unique_ptr<transform::Rigid2d> 匹配后的二维位姿
 */
std::unique_ptr<transform::Rigid2d> 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<transform::Rigid2d>(pose_prediction);
  }
  // 使用active_submaps_的第一个子图进行匹配
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();
  // The online correlative scan matcher will refine the initial estimate for
  // the Ceres scan matcher.
  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
  if (options_.use_online_correlative_scan_matching()) {
   
   
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_c