Cartographer源码阅读-2D前端根据时间戳滤波-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.
// 存放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();
};
接口:
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) {
// 判断该sensor_id必须存在
CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
// 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_赋值当前帧的时间
current_end_ = id_to_pending_data_.at(sensor_id).time;
// 根据时间戳处理数据
auto result = CropAndMerge();
// 处理后的数据放入id_to_pending_data_中,当前帧取代上一帧
id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
return result;
}
// 如果没有数据,直接赋值
id_to_pending_data_.emplace(sensor_id, 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();
}
根据时间滤点:
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;
sensor::TimedPointCloud& ranges = it->second.ranges;
// 根据时间戳计算要删除的点的终点,也是要保留数据的起点
auto overlap_begin = ranges.begin();
while (overlap_begin < ranges.end() &&
data.time + common::FromSeconds((*overlap_begin)[3]) <
current_start_) {
++overlap_begin;
}
// 计算要保留的数据的终点,一般情况下,终点即为ranges.end()
auto overlap_end = overlap_begin;
while (overlap_end < ranges.end() &&
data.time + common::FromSeconds((*overlap_end)[3]) <= 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);
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);
}
}
// 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 {
data = sensor::TimedPointCloudData{
data.time, data.origin,
sensor::TimedPointCloud(overlap_end, ranges.end())};
++it;
}
}
// 所有数据重新按照时间戳排序
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;
}