cartographer 源码解析 (四)

女爷i 2022-09-01 09:27 263阅读 0赞

## 相关链接： ##

#### [cartographer 源码解析(一)][cartographer] ####

#### [cartographer 源码解析(二)][cartographer 1] ####

#### [cartographer 源码解析(三)][cartographer 2] ####

#### [cartographer 源码解析(四)][cartographer 3] ####

#### [cartographer 源码解析(五)][cartographer 4] ####

#### [cartographer 源码解析(六)][cartographer 5] ####

因为第四节用文字表达起来有些难以直观的去理解，所以我录制了讲解的视频：[https://www.bilibili.com/video/BV1go4y1D792][https_www.bilibili.com_video_BV1go4y1D792]  
这一章主要内容是多激光的数据同步  
首先看一下全局的轨迹构建器里面`global_trajectory_builder.cc`里面的添加传感器数据`AddSensorData()`  
废话不多数，在这是，上代码

void AddSensorData(
          const std::string& sensor_id,
          const sensor::TimedPointCloudData& timed_point_cloud_data) override {
        
        CHECK(local_trajectory_builder_)
            << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
        std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
            matching_result = local_trajectory_builder_->AddRangeData(
                sensor_id, timed_point_cloud_data); // 第一步往前端里面送，做scan-match
        if (matching_result == nullptr) {
        
          // The range data has not been fully accumulated yet.
          return;
        }
        kLocalSlamMatchingResults->Increment();
        std::unique_ptr<InsertionResult> insertion_result;
        if (matching_result->insertion_result != nullptr) {
        
          kLocalSlamInsertionResults->Increment();
          auto node_id = pose_graph_->AddNode(
              matching_result->insertion_result->constant_data, trajectory_id_,
              matching_result->insertion_result->insertion_submaps);
          CHECK_EQ(node_id.trajectory_id, trajectory_id_);
          insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
        
              node_id, matching_result->insertion_result->constant_data,
              std::vector<std::shared_ptr<const Submap>>(
                  matching_result->insertion_result->insertion_submaps.begin(),
                  matching_result->insertion_result->insertion_submaps.end())});
        }
        if (local_slam_result_callback_) {
        
          local_slam_result_callback_(
              trajectory_id_, matching_result->time, matching_result->local_pose,
              std::move(matching_result->range_data_in_local),
              std::move(insertion_result));
        }
      }

我们这里就先只分析激光了。  
**local\_trajectory\_builder\_2d.cc**

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
    LocalTrajectoryBuilder2D::AddRangeData(
        const std::string& sensor_id,
        const sensor::TimedPointCloudData& unsynchronized_data) {
        
      auto synchronized_data =
          range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
      if (synchronized_data.ranges.empty()) {
        
        LOG(INFO) << "Range data collator filling buffer.";
        return nullptr;
      }
    
      const common::Time& time = synchronized_data.time;
      // Initialize extrapolator now if we do not ever use an IMU.
      if (!options_.use_imu_data()) {
        
        InitializeExtrapolator(time);
      }
    
      if (extrapolator_ == nullptr) {
        
        // Until we've initialized the extrapolator with our first IMU message, we
        // cannot compute the orientation of the rangefinder.
        LOG(INFO) << "Extrapolator not yet initialized.";
        return nullptr;
      }
    
      CHECK(!synchronized_data.ranges.empty());
      // TODO(gaschler): Check if this can strictly be 0.
      CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
      const common::Time time_first_point =
          time +
          common::FromSeconds(synchronized_data.ranges.front().point_time.time);
      if (time_first_point < extrapolator_->GetLastPoseTime()) {
        
        LOG(INFO) << "Extrapolator is still initializing.";
        return nullptr;
      }
    
      std::vector<transform::Rigid3f> range_data_poses;
      range_data_poses.reserve(synchronized_data.ranges.size());
      bool warned = false;
      for (const auto& range : synchronized_data.ranges) {
        
        common::Time time_point = time + common::FromSeconds(range.point_time.time);
        if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
        
          if (!warned) {
        
            LOG(ERROR)
                << "Timestamp of individual range data point jumps backwards from "
                << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
            warned = true;
          }
          time_point = extrapolator_->GetLastExtrapolatedTime();
        }
        range_data_poses.push_back(
            extrapolator_->ExtrapolatePose(time_point).cast<float>());
      }
    
      if (num_accumulated_ == 0) {
        
        // 'accumulated_range_data_.origin' is uninitialized until the last
        // accumulation.
        accumulated_range_data_ = sensor::RangeData{
        {
        }, {
        }, {
        }};
      }
    
      // Drop any returns below the minimum range and convert returns beyond the
      // maximum range into misses.
      for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
        
        const sensor::TimedRangefinderPoint& hit =
            synchronized_data.ranges[i].point_time;
        const Eigen::Vector3f origin_in_local =
            range_data_poses[i] *
            synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
        sensor::RangefinderPoint hit_in_local =
            range_data_poses[i] * sensor::ToRangefinderPoint(hit);
        const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
        const float range = delta.norm();
        if (range >= options_.min_range()) {
        
          if (range <= options_.max_range()) {
        
            accumulated_range_data_.returns.push_back(hit_in_local);
          } else {
        
            hit_in_local.position =
                origin_in_local +
                options_.missing_data_ray_length() / range * delta;
            accumulated_range_data_.misses.push_back(hit_in_local);
          }
        }
      }
      ++num_accumulated_;
    
      if (num_accumulated_ >= options_.num_accumulated_range_data()) {
        
        const common::Time current_sensor_time = synchronized_data.time;
        absl::optional<common::Duration> sensor_duration;
        if (last_sensor_time_.has_value()) {
        
          sensor_duration = current_sensor_time - last_sensor_time_.value();
        }
        last_sensor_time_ = current_sensor_time;
        num_accumulated_ = 0;
        const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
            extrapolator_->EstimateGravityOrientation(time));
        // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
        // 'time'.
        accumulated_range_data_.origin = range_data_poses.back().translation();
        return AddAccumulatedRangeData(
            time,
            TransformToGravityAlignedFrameAndFilter(
                gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
                accumulated_range_data_),
            gravity_alignment, sensor_duration);
      }
      return nullptr;
    }

逐行解析，我们发现，通过`sensor_bridge`转换成的激光点云的类型的形参是`TimedPointCloudData`类型。形参的名字叫`unsynchronized_data`。没有同步的数据。  
这个数据进入到前端里面，需要进行同步，我们看一下是什么类型的。

auto synchronized_data =
          range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);

**range\_data\_collator\_是RangeDataCollator**类型

RangeDataCollator range_data_collator_;

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()) {
        }
    
      // 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();
    
      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();
    
      constexpr static float kDefaultIntensityValue = 0.f;
    };

我们之前的章节里有讲过`Collated_trajectory_builder`里面有一个`sensor_Collator`，他使用来搜集所有的需要的传感器的数据。他是等着集齐了7颗龙珠（也就是集齐了需要的所有的传感器数据）才会给到各自的回调函数中。然后分发出去，一直传到`global_trajectory_builder`里面。  
那么这个`loacl_trajectory_trajectory_builder`里面有一个`RangeDataCollator`，那么他是用来做什么的呢？  
从名字中可以看出是测距相关的数据收集器。  
然后我们看看他添加数据的时候都做了什么？

sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
        const std::string& sensor_id,
        sensor::TimedPointCloudData timed_point_cloud_data) {
        
      CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
      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_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;
      }
      id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
      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_ = oldest_timestamp;
      return CropAndMerge();
    }

可以猜到传入一个不同步的数据，出来之后是同步的数据。

auto synchronized_data =
          range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);

由此可以猜到是做了一个数据同步。

首先是做了一个检查

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

首先是这个range\_data\_collator里面也有一个容器，这个容器里存放了跟测距有关的很多传感器的数据。看sensor\_id是否是0.如果是，说明我根本就不想去订阅你。

if (id_to_pending_data_.count(sensor_id) != 0) {
        
     ...
     }

然后是有一个`id_to_pending_data_`。那么这个`id_to_pending_data_`是什么呢？

std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;

可以看到是一个`key`为`string`, `value`是`sensor::TimedPointCloudData`类型的`map`。一个传感器的名字以及其所对应的数据。

if (id_to_pending_data_.count(sensor_id) != 0) {
        
     ...
     }

假设我们是第一帧的激光数据进来了，`id_to_pending_data_`应该是空的。所以不能执行if语句。所以直接emplace。

id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));

然后我们继续往下走，

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

这是什么意思呢？就是存储的传感器的数据，跟我所需要的传感器的数据是否都有了呢？如果都有了，我们就不通过`if`就继续往下执行。

current_start_ = current_end_;

追根溯源

common::Time current_start_ = common::Time::min();
      common::Time current_end_ = common::Time::min();

发现`current_start_`是一个时间。可以发现，刚开始`current_start_` `current_end_`都是时间最小值。

common::Time oldest_timestamp = common::Time::max();

我们发现oldest\_timestamp是最晚的时间戳。然后把oldest\_timestamp设置为一个极大值，下面做一个比较。一步一步的比，越来越小。

for (const auto& pair : id_to_pending_data_) {
        
        oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
      }

找到这个最小值。越早就越小，怎么找呢？从存储的数据中，一个个翻，如果比当前的小，就更新。

我们回想一下，我们一帧点云，经过sensor\_bridge之后呢？最后的时间是该帧点云的最后一个点的时间戳。如果最后一个点的时间戳是0，那么前面一个点就是-1。再往前就是-2，反正最后一个点相对时间戳就是0。  
然后再看代码中涉及到的`CropAndMerge()`函数。我们还是假设刚开始只有一帧激光进来

current_end_ = oldest_timestamp;

可以看到`current_end_`就是`oldest_timestamp`。  
**range\_data\_collator.cc**

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;
    
        auto overlap_begin = ranges.begin();
        while (overlap_begin < ranges.end() &&
               data.time + common::FromSeconds((*overlap_begin).time) <
                   current_start_) {
        
          ++overlap_begin;
        }
        auto overlap_end = overlap_begin;
        while (overlap_end < ranges.end() &&
               data.time + common::FromSeconds((*overlap_end).time) <=
                   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.
        if (overlap_begin < overlap_end) {
        
          std::size_t origin_index = result.origins.size();
          result.origins.push_back(data.origin);
          const float time_correction =
              static_cast<float>(common::ToSeconds(data.time - current_end_));
          auto intensities_overlap_it =
              intensities.begin() + (overlap_begin - ranges.begin());
          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
            point.point_time.time += 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 {
        
          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;
        }
      }
    
      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;
    }

逐行解析，首先用result 可以将值给返回。就是将处理完的结果最后放到这里面。然后return出去，暂时用不到。

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

然后是for循环，查看传感器数据中的每一条数据，现在是只有一条。我们就查看当前帧

for (auto it = id_to_pending_data_.begin();

`for`循环中

sensor::TimedPointCloudData& data = it->second;
        const sensor::TimedPointCloud& ranges = it->second.ranges;
        const std::vector<float>& intensities = it->second.intensities;

这代码不说自明，就是仿照着`pointCloud`的格式进行赋值。`ranges`是测距值，`intensities`是强度值。

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

`overlap_begin`是一个迭代器，从所有的测距值的开头开始。while循环的第一个条件是从头便利到尾，

overlap_begin < ranges.end()

其中`overlap_begin`就是`ranges.begin()`

`data.time`是最后一个点的时间戳，也就是绝对时间戳，就是当前帧的时间戳。

data.time + common::FromSeconds((*overlap_begin).time)

这个就是当前点的绝对时间戳加上相对时间戳。就是每个点的绝对时间戳。每个点的绝对时间戳＜ `current_start_` 。最后一个点的绝对时间戳加每一个点的相对时间戳等于每一个点的绝对时间戳。

我们可以知道current\_start\_一开始就非常小，所以while 一开始就不满足。为什么呢？我们可以看看第一个while 结束的条件，当`overlap_begin == ranges.end()`的时候，条件才会结束，而此时，我们再看赋值语句。

auto overlap_end = overlap_begin;

也就是说`overlap_end == overlap_begin == ranges.end()`，此时，我们再往下看`while`的条件`overlap_end < ranges.end()`。 所以说这个 `while` 条件一开始就不会被满足。

while (overlap_end < ranges.end() &&
               data.time + common::FromSeconds((*overlap_end).time) <=
                   current_end_) {
        
          ++overlap_end;
        }

目前，代码执行到这一步，`overlap_end`和`overlap_begin`是相等的。`current_end_`是最后一个点的时间戳。

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center]

如果当前需要的只有一种传感器数据，比如说只需要scan1, 同时scan1 来了多帧数据，那么我们下一帧数据与上一帧数据重叠的部分去掉。保存不重叠的部分。  
如果我们需要多种传感器数据，比如说需要不同话题的scan 数据，那么我们先把几种不同的传感器数据都保存下来，再按照时间戳进行排序。

[cartographer]: https://dqwang.blog.csdn.net/article/details/118539690
[cartographer 1]: https://dqwang.blog.csdn.net/article/details/118881867
[cartographer 2]: https://dqwang.blog.csdn.net/article/details/119023628
[cartographer 3]: https://dqwang.blog.csdn.net/article/details/119154163
[cartographer 4]: https://dqwang.blog.csdn.net/article/details/119845028
[cartographer 5]: https://dqwang.blog.csdn.net/article/details/120352115
[https_www.bilibili.com_video_BV1go4y1D792]: https://www.bilibili.com/video/BV1go4y1D792
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70_pic_center]: /images/20220829/6943f9c0c5214843a1564e4b63f84777.png