cartographer源码阅读----2D前端根据时间戳滤波(RangeDataCollator)

cartographer源码阅读----2D前端根据时间戳滤波(RangeDataCollator)

基本原理：
根据当前激光帧的时间戳和上一帧激光帧的时间戳，找到当前激光帧和上一帧激光帧时间重叠的激光点，滤除时间重复的点。

在Local_Trajectory_builder.cc中,AddRangeData的对象的类型是RangeDataCollator,作用是将时间上未对齐的雷达数据,进行对齐,滤掉重叠的点

class RangeDataCollator {
 public:
  explicit RangeDataCollator(
      const std::vector<std::string>& expected_range_sensor_ids)
      : expected_sensor_ids_(expected_range_sensor_ids.begin(),
                             expected_range_sensor_ids.end()) {}

  // If timed_point_cloud_data has incomplete intensity data, we will fill the
  // missing intensities with kDefaultIntensityValue.
  // 添加未对齐的雷达数据
  sensor::TimedPointCloudOriginData AddRangeData(
      const std::string& sensor_id,
      sensor::TimedPointCloudData timed_point_cloud_data);

 private:
  sensor::TimedPointCloudOriginData CropAndMerge();

  const std::set<std::string> expected_sensor_ids_;
  // Store at most one message for each sensor.
  // 存放sensor_id和数据
  std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
  common::Time current_start_ = common::Time::min();
  common::Time current_end_ = common::Time::min();

  constexpr static float kDefaultIntensityValue = 0.f;
};

}  // namespace mapping
}  // namespace cartographer

接口函数Local_Trajectory_builder.cc

  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }

点击AddRangeData

进入range_data_collator.cc

sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
    const std::string& sensor_id,
    sensor::TimedPointCloudData timed_point_cloud_data) {
  // 检查，是否是期望的sensor_id
  CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
  timed_point_cloud_data.intensities.resize(
      timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
  // TODO(gaschler): These two cases can probably be one.
  // id_to_pending_data_是什么?查看，map类型
  // 如果之前有数据
  if (id_to_pending_data_.count(sensor_id) != 0) {
    current_start_ = current_end_;
    // When we have two messages of the same sensor, move forward the older of
    // the two (do not send out current).
    // 当前帧的current_end_赋值当前帧的时间
    current_end_ = id_to_pending_data_.at(sensor_id).time;
    auto result = CropAndMerge();
    // 处理后的数据放入id_to_pending_data_中，当前帧取代上一帧
    id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
    return result;
  }
  // 如果没有数据，直接赋值
  id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
  if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
    return {};
  }
  // 并计算上一帧的时间戳，赋值current_end_
  current_start_ = current_end_;
  // We have messages from all sensors, move forward to oldest.
  common::Time oldest_timestamp = common::Time::max();
  for (const auto& pair : id_to_pending_data_) {
    oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
  }
  // 最后一个点的绝对时间戳
  current_end_ = oldest_timestamp;
  return CropAndMerge();
}

点击CropAndMerge()函数

根据时间点滤波-处理函数CropAndMerge()

sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
  sensor::TimedPointCloudOriginData result{current_end_, {}, {}};
  bool warned_for_dropped_points = false;
  // 遍历所有的点
  for (auto it = id_to_pending_data_.begin();
       it != id_to_pending_data_.end();) {
    // 获取数据
    sensor::TimedPointCloudData& data = it->second; 
    const sensor::TimedPointCloud& ranges = it->second.ranges;
    const std::vector<float>& intensities = it->second.intensities;

    // 根据时间戳计算要删除的点的终点，也是要保留数据的起点
    auto overlap_begin = ranges.begin();
    while (overlap_begin < ranges.end() &&
           data.time + common::FromSeconds((*overlap_begin).time) <
               current_start_) {
      ++overlap_begin;
    }
    // 计算要保留的数据的终点，一般情况下，终点即为ranges.end()
    auto overlap_end = overlap_begin;
    while (overlap_end < ranges.end() &&
           data.time + common::FromSeconds((*overlap_end).time) <=
               current_end_) {
      ++overlap_end;
    }
    if (ranges.begin() < overlap_begin && !warned_for_dropped_points) {
      LOG(WARNING) << "Dropped " << std::distance(ranges.begin(), overlap_begin)
                   << " earlier points.";
      warned_for_dropped_points = true;
    }

    // Copy overlapping range.根据要保留的数据的起点终点，拷贝数据到result
    if (overlap_begin < overlap_end) {
      std::size_t origin_index = result.origins.size();
      result.origins.push_back(data.origin);
      const float time_correction =
          static_cast<float>(common::ToSeconds(data.time - current_end_));
      auto intensities_overlap_it =
          intensities.begin() + (overlap_begin - ranges.begin());
      result.ranges.reserve(result.ranges.size() +
                            std::distance(overlap_begin, overlap_end));
      for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
           ++overlap_it, ++intensities_overlap_it) {
        sensor::TimedPointCloudOriginData::RangeMeasurement point{
            *overlap_it, *intensities_overlap_it, origin_index};
        // current_end_ + point_time[3]_after == in_timestamp +
        // point_time[3]_before
        point.point_time.time += time_correction;
        result.ranges.push_back(point);
      }
    }

    // Drop buffered points until overlap_end.
    if (overlap_end == ranges.end()) {
      it = id_to_pending_data_.erase(it);
    } else if (overlap_end == ranges.begin()) {
      ++it;
    } else {
      const auto intensities_overlap_end =
          intensities.begin() + (overlap_end - ranges.begin());
      data = sensor::TimedPointCloudData{
          data.time, data.origin,
          sensor::TimedPointCloud(overlap_end, ranges.end()),
          std::vector<float>(intensities_overlap_end, intensities.end())};
      ++it;
    }
  }

  // 所有数据重新按照时间戳排序
  std::sort(result.ranges.begin(), result.ranges.end(),
            [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
               const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
              return a.point_time.time < b.point_time.time;
            });
  return result;
}

参考链接: https://blog.csdn.net/yeluohanchan/article/details/108674859.