cartographer 点云同步处理

1.点云同步处理的类  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()) {}
     
      sensor::TimedPointCloudOriginData AddRangeData(
          const std::string& sensor_id,
          const 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.
      std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
      common::Time current_start_ = common::Time::min();
      common::Time current_end_ = common::Time::min();
    };

其中有四个成员函数：

生成点云的所有传感器的 sensor_ids

      const std::set<std::string> expected_sensor_ids_;

修剪合并点云时的所有点云  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();

2.AddRangeData

    sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
        const std::string& sensor_id,
        const sensor::TimedPointCloudData& timed_point_cloud_data)

1.首先检测该点云数据 sensor_id 是否在期望的sensor_ids里面，否则程序停止

  CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);

2.当该sensor_id 已经在 id_to_pending_data_ 中

当我们有两个相同传感器的消息时，移除两个中旧的数据，但不发送电流

1>跟新current_start,current_end 时间

    current_start_ = current_end_;
    current_end_ = id_to_pending_data_.at(sensor_id).time;

2>修剪合并在  id_to_pending_data_ 中的数据  CropAndMerge

    auto result = CropAndMerge();

3>将当前传感器数据放入 id_to_pending_data_ 中，并且返回result

        id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
        return result;

3.当sensor_id 不在 id_to_pending_data_ 中时

1>首先当前数据加到 id_to_pending_data_ 中，

  id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);

2>使 id_to_pending_data_ 中 包含期望的所有数据  

      if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
        return {};
      }

3>跟新时间

      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;

4>修剪合并  CropAndMerge
3.CropAndMerge

sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge()

1.定义返回对象

  sensor::TimedPointCloudOriginData result{current_end_, {}, {}};

2.遍历 id_to_pending_data_ 中所有传感器的数据，其对于传感器数据处理一样

      for (auto it = id_to_pending_data_.begin();
           it != id_to_pending_data_.end();) {
        sensor::TimedPointCloudData& data = it->second;
        sensor::TimedPointCloud& ranges = it->second.ranges;

2>将在[current_start_,current_end_]区间内的传感器数据  下表确定出来

        auto overlap_begin = ranges.begin();
        while (overlap_begin < ranges.end() &&
               data.time + common::FromSeconds((*overlap_begin)[3]) <
                   current_start_) {
          ++overlap_begin;
        }
        auto overlap_end = overlap_begin;
        while (overlap_end < ranges.end() &&
               data.time + common::FromSeconds((*overlap_end)[3]) <= current_end_) {
          ++overlap_end;
        }

3>复制重叠范围。

        if (overlap_begin < overlap_end) {
          std::size_t origin_index = result.origins.size();
          result.origins.push_back(data.origin);
          double time_correction = common::ToSeconds(data.time - current_end_);
          for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
               ++overlap_it) {
            sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it,
                                                                      origin_index};
            // current_end_ + point_time[3]_after == in_timestamp +
            // point_time[3]_before
            point.point_time[3] += time_correction;
            result.ranges.push_back(point);
          }
        }

涉及到：   处理了前一时刻的点云，然后将时间跟新

4>将缓冲点丢弃，直到overlap_end。

        if (overlap_end == ranges.end()) {
          it = id_to_pending_data_.erase(it);
        } else if (overlap_end == ranges.begin()) {
          ++it;
        } else {
          data = sensor::TimedPointCloudData{
              data.time, data.origin,
              sensor::TimedPointCloud(overlap_end, ranges.end())};
          ++it;
        }

3.按照时间排序，并且返回结果

      std::sort(result.ranges.begin(), result.ranges.end(),
                [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
                   const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
                  return a.point_time[3] < b.point_time[3];
                });
      return result;

感觉cartographer处理点云数据有一帧延时

原文地址：https://blog.csdn.net/xiaoma_bk/article/details/85261422