cartographer 代码思想解读（9）- 激光雷达畸变矫正

最新推荐文章于 2023-12-05 18:15:14 发布

jiajiading

最新推荐文章于 2023-12-05 18:15:14 发布

阅读量2.6k

点赞数 4

分类专栏： slam

本文链接：https://blog.csdn.net/jiajiading/article/details/108854097

版权

slam 专栏收录该内容

29 篇文章 89 订阅

订阅专栏

cartographer 代码思想解读（9）- 激光雷达畸变矫正

本节为cartographer前端匹配算法中最后一个核心算法实现内容，下节将会这9节内容串起来构成cartographer中2dslam的前端总体接口local_trajectory_builder_2d类。此节激光矫正具体实现就是在local_trajectory_builder_2d实现的，单独列出来进行分析。是因为至今我使用的SLAM从未进行过矫正考虑，是因为室内机器人速度较低，同时雷达采用性能较好雷达，则可做简单的假设，认为影响较少。但是机器人运行速度较快时，则必须考虑畸变问题。
激光雷达为什么会产生畸变，原因是激光雷达获取的360度或270度的激光光束距离，并非瞬间同步完成的。目前采用的激光雷达大多数是旋转雷达，通过一反光镜高速旋转，向四周进行测距，因此每一帧激光点云中相邻的光束点存在时间间隔measure time。如此一圈360度，若一圈共测量360个点（即一度一个点），则第一个光束测得距离和最后一个获取的时间差将会为 360×measure time。因此当机器人在运动时获得一帧激光点云，第一个点和最后一点实际对应的激光原点坐标将不一致，故导致激光点云畸变。若机器人运行速度越快，则畸变越为严重。

cartographer采用的校准方法是采用前面分析的位姿估计器，可以估计出每时刻的线速度和角速度，因此得出每时刻的位置变换，通过点云中每个点的时间戳，获取其原点位置进行转换校准。

 // 开辟一个新的vector存储所有当前雷达传感器点云每个点对应的位置信息，其位置信息由估计器预测而来
  std::vector<transform::Rigid3f> range_data_poses;
  range_data_poses.reserve(synchronized_data.ranges.size());
  bool warned = false;
  for (const auto& range : synchronized_data.ranges) {
    common::Time time_point = time + common::FromSeconds(range.point_time.time);
    // 遍历每一个点云点的时间戳，理论上应晚于估计器上刻位置时间戳，否则说明传感器采集时间错误
    if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
      if (!warned) {
        LOG(ERROR)
            << "Timestamp of individual range data point jumps backwards from "
            << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
        warned = true;
      }
      //
      time_point = extrapolator_->GetLastExtrapolatedTime();
    }
    // 根据每个点的时间戳估计点云点对应的位置并进行缓存
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

上面代码则是获取点云中每个点的时间戳，并且估计其原点的位置并放入缓存中；

  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
    // 提取每一个点云点的pose（包含时间戳）
    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;
    // 提取此点云对应的原点坐标pose，并进行畸变矫正
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
    // 对此点进行畸变矫正，并转换为pose，不包含时间戳
    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);
    // 计算点到原点距离
    const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
    const float range = delta.norm();
    // 距离满足一定范围内保留，否则丢弃
    if (range >= options_.min_range()) {
      if (range <= options_.max_range()) {
        accumulated_range_data_.returns.push_back(hit_in_local);
      } else {
        // 超出设置最远距离，则放入miss队列中，并且距离全部调整为配置值
        hit_in_local.position =
            origin_in_local +
            options_.missing_data_ray_length() / range * delta;
        accumulated_range_data_.misses.push_back(hit_in_local);
      }
    }
  }

上面代码遍历每个点，计算校准后的点云；

简单总结：
畸变基本步骤：
1.获取每一帧点云每个点的时间戳；
2.代入估计器估计出每个原点时间戳的位置；
3.将每个点对应估计的原点坐标进行转换至世界坐标系；
注意：
在做畸变校准时，需要注意激光雷达旋转方向，考虑时间间隔递增的方向，否则结果会更加不准确；