点云时间同步-cartographer
Kong Liangqian Lv6
 2021-11-06 18:38:29  

对雷达的同步的函数文件位于cartographer/mapping/internal/range_data_collator.cc文件中

同步雷达时间的类为RangeDataCollator

首先查看一下头文件，

构造函数

对expectedsensor_ids 进行赋值，注意expectedsensor_ids他是一个集合

1
2
3
4
5
6
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()) {}

成员变量

1
2
3
4
const std::set<std::string> expected_sensor_ids_;
std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_; // 待处理的数据
common::Time current_start_ = common::Time::min();
common::Time current_end_ = common::Time::min();

成员函数

此头文件只有两个函数，一个是AddRangeData，另外一个是CropAndMerge

1
2
3
4
5
6
7
8
9
 // 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();

AddRangeData

AddRangeData 的功能为多个雷达时间同步，记录当前同步数据的开始时间和结束时间

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
/**
 * @brief 多个雷达数据的时间同步
 * 
 * @param[in] sensor_id 雷达数据的话题
 * @param[in] timed_point_cloud_data 雷达数据
 * @return sensor::TimedPointCloudOriginData 根据时间处理之后的数据
 */
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
    const std::string& sensor_id,
    sensor::TimedPointCloudData timed_point_cloud_data) { // 第一次拷贝
  CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);

  // 从sensor_bridge传过来的数据的intensities为空
  timed_point_cloud_data.intensities.resize(
      timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);

  // TODO(gaschler): These two cases can probably be one.
  // 如果同话题的点云, 还有没处理的, 就先处同步没处理的点云, 将当前点云保存
  if (id_to_pending_data_.count(sensor_id) != 0) {
    // current_end_为上一次时间同步的结束时间
    // current_start_为本次时间同步的开始时间
    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_ = id_to_pending_data_.at(sensor_id).time;
    auto result = CropAndMerge();
    // 保存当前点云
    id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
    return result;
  }

  // 先将当前点云添加到 等待时间同步的map中
  id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));

  // 等到range数据的话题都到来之后再进行处理
  if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
    return {};
  }

  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_是本次时间同步的结束时间
  // 是待时间同步map中的 所有点云中最早的时间戳
  current_end_ = oldest_timestamp;
  return CropAndMerge();
}

返回雷达的数据结构

1
2
3
4
5
6
7
8
9
10
11
// 时间同步后的点云
struct TimedPointCloudOriginData {
  struct RangeMeasurement {
    TimedRangefinderPoint point_time;   // 带时间戳的单个数据点的坐标 xyz
    float intensity;                    // 强度值
    size_t origin_index;                // 属于第几个origins的点
  };
  common::Time time;                    // 点云的时间
  std::vector<Eigen::Vector3f> origins; // 点云是由几个点云组成, 每个点云的原点
  std::vector<RangeMeasurement> ranges; // 数据点的集合
};

代码解析

点云传进来是没有强度的，所以忽略

1
2
3
// 从sensor_bridge传过来的数据的intensities为空
timed_point_cloud_data.intensities.resize(
    timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);

如果同话题的点云, 还有没处理的, 就先处同步没处理的点云, 然后将当前点云保存。

这里的currentend 获取的是idto_pending_data 中点云的time属性，这就是一个点云最后一个点的时间

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// TODO(gaschler): These two cases can probably be one.
// 如果同话题的点云, 还有没处理的, 就先处同步没处理的点云, 将当前点云保存
if (id_to_pending_data_.count(sensor_id) != 0) {
  // current_end_为上一次时间同步的结束时间
  // current_start_为本次时间同步的开始时间
  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_ = id_to_pending_data_.at(sensor_id).time;
  auto result = CropAndMerge();
  // 保存当前点云
  id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
  return result;
}

如果没有没有处理过的点云，则直接添加

1
2
3
4
5
6
// 先将当前点云添加到 等待时间同步的map中
id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
// 等到range数据的话题都到来之后再进行处理
if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
  return {};
}

添加完之后，设置同步点云的开始时间和结束时间

1
2
3
4
5
6
7
8
9
10
11
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_是本次时间同步的结束时间
// 是待时间同步map中的 所有点云中最早的时间戳
current_end_ = oldest_timestamp;
return CropAndMerge();

CropAndMerge与解析

对时间段内的数据进行截取与合并, 返回时间同步后的点云。

重点：雷达时间的同步即，把一段时间之内的雷达信息按照时间顺序再重新排一遍

把AddRangeData 之中已经确定了 这一段时间的 开始和结束时间，current_begin 和 current_end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
// 对时间段内的数据进行截取与合并, 返回时间同步后的点云
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;

    // 找到点云中 最后一个时间戳小于current_start_的点的索引
    auto overlap_begin = ranges.begin();
    while (overlap_begin < ranges.end() &&
           data.time + common::FromSeconds((*overlap_begin).time) <
               current_start_) {
      ++overlap_begin;
    }

    // 找到点云中 最后一个时间戳小于等于current_end_的点的索引
    auto overlap_end = overlap_begin;
    while (overlap_end < ranges.end() &&
           data.time + common::FromSeconds((*overlap_end).time) <=
               current_end_) {
      ++overlap_end;
    }

    // 丢弃点云中时间比起始时间早的点, 每执行一下CropAndMerge()打印一次log
    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.
    if (overlap_begin < overlap_end) {
      // 获取下个点云的index, 即当前vector的个数
      std::size_t origin_index = result.origins.size();
      result.origins.push_back(data.origin);  // 插入原点坐标

      // 获取此传感器时间与集合时间戳的误差, 比如下面图中scan_2到current_end_的时间
      const float time_correction =
          static_cast<float>(common::ToSeconds(data.time - current_end_));

      auto intensities_overlap_it =
          intensities.begin() + (overlap_begin - ranges.begin());
      // reserve() 在预留空间改变时, 会将之前的数据拷贝到新的内存中
      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
        // 针对每个点时间戳进行修正, 让最后一个点的时间为0
        point.point_time.time += time_correction;  
        result.ranges.push_back(point);
      } // end for
    } // end if

    // 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;
    }
  } // end for

  // 对各传感器的点云 按照每个点的时间从小到大进行排序
  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;
}

可以看下面一段图，scan_1首先来一段数据，scan_2紧随其后，则

  • current_end为第一段的最后一个点的时间
  • current_begin 是一个很小的值

那么会把scan_1所有的数据先放入result，然后把scan_2的部分数据放入result。

最后按照每一个点的时间进行从小到大的排序进行返回

第二次时间同步，需要从AddRangeData中再一次确定curren_end.

  • current_end 为scan_2的最后一个点的值
  • current_begin 为第一次的current_end的值

直接把scan_2的第二段返回

image-20211107232236079

 Comments