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