Cartographer源码阅读2D&3D-前端根据时间戳滤波-RangeDataCollator

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.根据要保留的数据的起点终点，拷贝数据到resultif (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]_beforepoint.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;
}