cartographer 模块篇1 激光数据处理

1、激光数据处理

1.1 原始点云处理

1.1.1 HandleLaserScanMessage()

原始点云的处理是在 cartographer_ros中完成的，任务是将接收到的传感器消息转换为后续算法所需的数据结构，这个过程是在ros消息的回调函数中完成的， 我们知道传感器消息的回调函数统一定义在 SensorBridge 类(详细参考cartographer模块分析0: ROS入口代码解析)中(/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc)，一般我们使用的激光雷达发送的激光消息类型是sensor_msgs::LaserScan，对应的回调函数如下：

// 处理 sensor_msgs::LaserScan message 
void SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  // 对数据进行转换
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

其中执行的 ToPointCloudWithIntensities()，位于msg_conversion.cc中，而在此函数中，只是调用了：
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg),
最后返回 std::make_tuple(point_cloud, timestamp)，
timestamp: 为该帧最后一个点的绝对时间
point_cloud: 是 PointCloudWithIntensities类型的数据：

struct PointCloudWithIntensities {
  // 包含时间的点云容器
  TimedPointCloud points;
  std::vector<float> intensities;
};

points 保存每个点的xy坐标以及相对于最后一个点的时间戳。

1.1.2 HandleLaserScan()

1.1.3 HandleRangefinder()

1.2 点云去畸变

首先, 获取该帧第一个激光点的时间戳,

// 得到第一个点的时间戳
  const common::Time time_first_point =
      time +                                   // 第一帧时间  
      common::FromSeconds(
          synchronized_data.ranges.front()
              .point_time.time);               // 这里是得到 该帧第一个激光点 相对时间
  // 检查插值器   插值器时间应该要晚于第一个激光点    为啥?????????????????????????
  if (time_first_point < extrapolator_->GetLastPoseTime()) 
  {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

然后根据时间戳通过外插器, 计算出每个激光点对应的雷达原点pose:

// 通过外插器 得到每一个激光点对应的雷达位姿  
  std::vector<transform::Rigid3f> range_data_poses;                                 // 保存每个激光点时刻雷达的pose 
  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);    // double 转 Duration 类型 
    // 如果该点的时间晚于插值器上一刻时间戳  说明时间错误 
    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>());
  }

最关键就是 上面最后一行代码, 通过 extrapolator_->ExtrapolatePose(time_point).cast() 预测出激光点的pose.

疑问
extrapolator_->GetLastExtrapolatedTime() 是啥 ???
然后,知道每个激光点对应的激光雷达在submap下的局部pose, 就可以求出每个激光点在submap下的真实位置, 也就完成了去畸变的过程.

/************************************畸变去除*************************************/
  // Drop any returns below the minimum range and convert returns beyond the
  // maximum range into misses.
  // 处理好的数据放在 accumulated_range_data_
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
    // 时间 + 位姿
    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;
    // 
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);     // ???????????????????????
    // 获得激光点在submap坐标系? 下的位置 
    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);      // Tso * po = ps
    // 激光点的距离向量  
    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);
      }
    }
  }

疑问: range_data_poses[i] 得到的pose到底是相对什么坐标系的?
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index) 该怎么理解??

1.3 滤波

前端里程计中，进行匹配之前会进行滤波操作，主要的函数有：
sensor::CropRangeData()
sensor::VoxelFilter()
sensor::AdaptiveVoxelFilter()

其中比较复杂并且有意思的是体素滤波, 下面总结一下体素滤波的实现思路.
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution)
resolution: 体素的size point_cloud: 处理的点云
step1:
最关键的一步, 筛选出每个体素内部保留的一个点, 也就完成了滤波的过程.

const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
      point_cloud.points(), resolution,
      [](const RangefinderPoint& point) { return point.position; });

RandomizedVoxelFilterIndices()的核心思想是:

1. 用一个hash_map 管理所有的voxel

// hash_map  即 unordered_map
  absl::flat_hash_map<VoxelKeyType, std::pair<int, int>>
      voxel_count_and_point_index;

VoxelKeyType为voxel的序号, std::pair<int, int> 的第一个元素保存该voxel内部的点的个数, 第二个元素保存该voxel中选择的那一个点的序号
2. 遍历当前所有点, 并设置voxel内的数据

for (size_t i = 0; i < point_cloud.size(); i++) {
    // 索引 该voxel  point_function 返回该点的position
    auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex(point_function(point_cloud[i]), resolution)];
    // 该voxel first 保存点的数量  
    voxel.first++;
    if (voxel.first == 1) {
      voxel.second = i;
    } else {
      std::uniform_int_distribution<> distribution(1, voxel.first);       // 随机数生成   1-voxel.first
      if (distribution(generator) == voxel.first) {
        voxel.second = i;
      }
    }
  }

GetVoxelCellIndex(point_function(point_cloud[i]), resolution) 根据point的position以及voxel的size, 得到该点处于的voxel的序号, 然后 通过hash_map 找到对应的voxel, 然后将voxel内部的点的数量++,
当点的数量为1时, 那么保留的就自然是该点,因此 voxel.second = i, 当点的数量逐渐增多时,该保留哪个点, 就取决与当前生成的随机数是否等于 voxel.first, 如果相等, 那么就保留当前点.
有个疑问这样做的话, 越到后续的点被选取的可能性就越小, 不能做到均匀采样啊??

另外使用的体素滤波还有自适应体素滤波 AdaptivelyVoxelFiltered()
AdaptivelyVoxelFiltered主要解决的问题是, 由于voxel的size选取不当,导致滤波后得到的点云中点的数量过于少,自适应voxel滤波, 可以自动调节voxel的size, 使得最终输出的点云数量达到最小的阈值.
AdaptivelyVoxelFiltered()的输入参数中没有voxel size的值,只需要输入待滤波的点云即可.
AdaptivelyVoxelFiltered()的精华如下所示:

for (float high_length = options.max_length(); high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
    // 体素的大小减小一半 
    float low_length = high_length / 2.f;
    // 再次体素滤波 
    result = VoxelFilter(point_cloud, low_length);
    // 如果数量达到要求, 则通过二分搜索  逐步增大voxel , 一直到临界点
    if (result.size() >= options.min_num_points()) {
      // Binary search to find the right amount of filtering. 'low_length' gave
      // a sufficiently dense 'result', 'high_length' did not. We stop when the
      // edge length is at most 10% off.
      // 控制
      while ((high_length - low_length) / low_length > 1e-1f) {
        const float mid_length = (low_length + high_length) / 2.f;
        const PointCloud candidate = VoxelFilter(point_cloud, mid_length);
        // 说明 size 还可以增加 
        if (candidate.size() >= options.min_num_points()) {
          low_length = mid_length;
          result = candidate;
        } else {
          high_length = mid_length;
        }
      }
      return result;
    }
  }

核心思想是: 通过二分查找逐步扩大voxel的size, 当high_length与low_length的长度足够小时, 停止查找!!