(02)Cartographer源码无死角解析-(23) 传感器数据类型自动推断与数据利用率计算

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

在前面的博客中，还有很多细节的东西没有讲解，如下：

CollatedTrajectoryBuilder::AddSensorData() 函数中的sensor::MakeDispatchable() 函数
CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) 函数的参数sensor::Data类
CollatedTrajectoryBuilder::HandleCollatedSensorData() 中 rate_timers_ 变量的使用

那么接下来就会对他们进行一个纤细的分析。

二、Dispatchable

首先来看 src/cartographer/cartographer/sensor/data.h 文件中的 class Data，这里就不啰嗦了，其就是一个接口类，该接口定义了两个纯虚函数，其只有一个成员 const std::string sensor_id_，该为 topic name。

该类的一个派生类位于 src/cartographer/cartographer/sensor/internal/dispatchable.h 中，值得注意的是该类为一个模板类，模板参数为 DataType。

template <typename DataType> //模板类,模板参数
class Dispatchable : public Data {
    
    //继承于类 data
 public:
  //构造函数，同时调用会调用父类的构造函数。对参数data赋值给成员变量data_
  Dispatchable(const std::string &sensor_id, const DataType &data)
      : Data(sensor_id), data_(data) {
    
    }

  //重写父类函数，直接返回 data_.time 即可(表示DataType类型的数据,必须还有成员变量.time)
  common::Time GetTime() const override {
    
     return data_.time; }

  // 重写父类函数,调用传入的trajectory_builder的AddSensorData()
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    
    
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }
  //返回成员变量data_的一个引用
  const DataType &data() const {
    
     return data_; }

 private:
  const DataType data_;//构造函数初始化列表进行赋值，之后便不可再进行更改
};

另外，在 dispatchable.h 文件的下面，还可以看到如下代码：

// c++11: template <typename DataType> 
// 函数模板的调用使用 实参推演 来进行
// 类模板 模板形参的类型必须在类名后的尖括号中明确指定, 不能使用实参推演 
// 在类外声明一个 函数模板, 使用 实参推演 的方式来使得 类模板可以自动适应不同的数据类型


// 根据传入的data的数据类型,自动推断DataType, 实现一个函数处理不同类型的传感器数据
template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
    const std::string &sensor_id, const DataType &data) {
    
    
  return absl::make_unique<Dispatchable<DataType>>(sensor_id, data);
}

简单的说，其就是创建一个 Dispatchable类型的智能指针，该类型的模板参数为DataType。在创建时需要传入两个参数，分别为const std::string &sensor_id, 与 const DataType &data。

那么为什么要在 Dispatchable 的外面写这样一个函数呢？而不把他写在 class Dispatchable 之中呢？主要时应为类成员模板函数不具备自动推导模板参数的能力，而非成员函数，也就是普通函数时具备这个能力的。如CollatedTrajectoryBuilder.h 中如下一段代码：

  // 处理雷达点云数据
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    
    
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }

其调用了 sensor::MakeDispatchable(sensor_id, timed_point_cloud_data) 函数，其会根据 timed_point_cloud_data 的类型，自动推导 sensor::MakeDispatchable 的模板参数 DataType= sensor::TimedPointCloudData。但是类别成员函数时不具备这个能力的。

sensor::TimedPointCloudData 的定义如下：

// 时间同步前的点云
struct TimedPointCloudData {
    
    
  common::Time time;        // 点云最后一个点的时间
  Eigen::Vector3f origin;   // 以tracking_frame_到雷达坐标系的坐标变换为原点
  TimedPointCloud ranges;   // 数据点的集合, 每个数据点包含xyz与time, time是负的
  // 'intensities' has to be same size as 'ranges', or empty.
  std::vector<float> intensities; // 空的
};

与前面的推断一致，该结构体时存在成员变量 time 的。struct ImuData 同样包含成员变量time。

三、RateTimer逻辑分析

在 CollatedTrajectoryBuilder 中存在成员变量 rate_timers_，定义如下：

  std::map<std::string, common::RateTimer<>> rate_timers_;

其是在回调函数 CollatedTrajectoryBuilder::HandleCollatedSensorData() 中被使用，该函数实现代码如下：

```cpp
/**
 * @brief 处理 按照时间顺序分发出来的传感器数据
 * 
 * @param[in] sensor_id 传感器的topic的名字
 * @param[in] data 需要处理的数据(Data是个类模板,可处理多种不同数据类型的数据)
 */
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
    const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
    
    
  auto it = rate_timers_.find(sensor_id);
  // 找不到就新建一个
  if (it == rate_timers_.end()) {
    
    
    // map::emplace()返回一个pair
    // emplace().first表示新插入元素或者原始位置的迭代器
    // emplace().second表示插入成功,只有在key在map中不存在时才插入成功
    it = rate_timers_
             .emplace(
                 std::piecewise_construct, 
                 std::forward_as_tuple(sensor_id),
                 std::forward_as_tuple(
                     common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
             .first;
  }
  
  // 对数据队列进行更新
  it->second.Pulse(data->GetTime());

  if (std::chrono::steady_clock::now() - last_logging_time_ >
      common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
    
    
    for (const auto& pair : rate_timers_) {
    
    
      LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
    }
    last_logging_time_ = std::chrono::steady_clock::now();
  }

  // 也就是跑carto时候的消息：
  // [ INFO]: collated_trajectory_builder.cc:72] imu rate: 10.00 Hz 1.00e-01 s +/- 4.35e-05 s (pulsed at 100.44% real time)
  // [ INFO]: collated_trajectory_builder.cc:72] scan rate: 19.83 Hz 5.04e-02 s +/- 4.27e-05 s (pulsed at 99.82% real time)

  // 将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用
  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}

其上的代码主要分为如下个部分(RateTimer位于src/cartographer/cartographer/common/internal/rate_timer.h)：

$\color{blue}(1):$ 其首先呢，会根据 sensor_id 在 rate_timers_ 这个字典中查找一下，是否已经创建了与 sensor_id 对应的 RateTimer 对象，如果没有则会实例化一个 RateTimer 对象，其传给构造函数的参数为kSensorDataRatesLoggingPeriodSeconds=15秒转换成标准时间的数据，然后赋值给成员变量RateTimer::window_duration_。

$\color{blue}(2):$ 与当前 sensor_id 对应的 RateTimer 实例对象调用其成员函数 Pulse()。该函数如下所示：

  // Records an event that will contribute to the computed rate.
  // 对数据队列进行更新
  void Pulse(common::Time time) {
    
    
    // 将传入的时间放入队列中
    events_.push_back(Event{
    
    time, ClockType::now()});
    // 删除队列头部数据,直到队列中最后与最前间的时间间隔小于window_duration_
    while (events_.size() > 2 &&
           (events_.back().wall_time - events_.front().wall_time) >
               window_duration_) {
    
    
      events_.pop_front();
    }
  }

从上面可以看到Event 实例存在两个成员变量，第一个是传入的形参 time，其为订阅话题消息数据中的时间戳。第二个参数为调用该数据的时间。该函数的目录，就是计算在 window_duration_ 时间内，其共调用了多少(events_.size())数据。为了方便理解，下面举两个例子，因为HandleCollatedSensorData是回调函数，所以可能同时有多个线程执行到 Pulse(common::Time time)：

①→假设为100个线程执行同时执行到Pulse，那么此时 events_ 中的这100个时间点，可得 events_.back().wall_time-events_.front().wall_time <window_duration_(15秒)，不满足循环，此时 events_.size()=100也等于15秒的数据个数。

②→假设为20个线程执行到Pulse，且 events_.back().wall_time - events_.front().wall_time > window_duration_= 15秒。也就是说，接受数据比较快，但是调用数据时比较慢，此时就会把循环 events_ 第一个元素抛掉，直到events_.back().wall_time - events_.front().wall_time < window_duration_。总的来说，events_包含的还是15秒内的.wall 时间点。其 events_.size()等于15秒的数据个数。

$\color{red}注意:$ 最终 events_.back().wall_time - events_.front().wall_time 表示15秒内调用数据的首尾差。

$\color{blue}(3):$ 执行信息的打印，主要涉及到 RateTimer中的 DebugString() 函数：

  // Returns a debug string representation.
  std::string DebugString() const {
    
    
    if (events_.size() < 2) {
    
    
      return "unknown";
    }

    // c++11: std::fixed 与 std::setprecision(2) 一起使用, 表示输出2位小数点的数据

    std::ostringstream out;
    out << std::fixed << std::setprecision(2) << ComputeRate() << " Hz "
        << DeltasDebugString() << " (pulsed at "
        << ComputeWallTimeRateRatio() * 100. << "% real time)";
    return out.str();
  }

其打印的格式类似如下：

	// 也就是跑carto时候的消息：
 [ INFO]: collated_trajectory_builder.cc:72] imu rate: 10.00 Hz 1.00e-01 s +/- 4.35e-05 s (pulsed at 100.44% real time)
 [ INFO]: collated_trajectory_builder.cc:72] scan rate: 19.83 Hz 5.04e-02 s +/- 4.27e-05 s (pulsed at 99.82% real time)

DebugString() 函数通过带哦用 ComputeRate()，DeltasDebugString()，ComputeWallTimeRateRatio() 分别计算频率，数据时间间隔的均值与标准差，以及数据生成与数据调用的比例。总体代码注释如下。

四、RateTimer代码注释

class RateTimer {
    
    
 public:
  // Computes the rate at which pulses come in over 'window_duration' in wall
  // time.
  //explicit禁止隐式类型转换
  explicit RateTimer(const common::Duration window_duration)
      : window_duration_(window_duration) {
    
    }
  ~RateTimer() {
    
    }

  RateTimer(const RateTimer&) = delete;//禁用默认拷贝构造函数
  RateTimer& operator=(const RateTimer&) = delete;//禁用默认赋值=号赋值函数

  // Returns the pulse rate in Hz.
  // 计算平均频率,数据的个数 - 1 除以调用该批数据开始与结束的时间间隔
  double ComputeRate() const {
    
    
    if (events_.empty()) {
    
    
      return 0.;
    }
    return static_cast<double>(events_.size() - 1) /
           common::ToSeconds((events_.back().time - events_.front().time));
  }

  // Returns the ratio of the pulse rate (with supplied times) to the wall time
  // rate. For example, if a sensor produces pulses at 10 Hz, but we call Pulse
  // at 20 Hz wall time, this will return 2.

  //这里的events_表示window_duration_(默认15秒)内所有数据的
  //生成时间.time(订阅话题msg的时间戳)与调用时间.wall_time

  double ComputeWallTimeRateRatio() const {
    
    
    if (events_.empty()) {
    
    
      return 0.;
    }
            //计算生产该批数据消耗的时间
    return common::ToSeconds((events_.back().time - events_.front().time)) /
           //除以该批数据被调用消耗的时间
           common::ToSeconds(events_.back().wall_time -
                             events_.front().wall_time);
  }

  // Records an event that will contribute to the computed rate.
  // 对数据队列进行更新
  void Pulse(common::Time time) {
    
    
    // 将传入的时间放入队列中
    events_.push_back(Event{
    
    time, ClockType::now()});
    // 删除队列头部数据,直到队列中最后与最前间的时间间隔小于window_duration_
    while (events_.size() > 2 &&
           (events_.back().wall_time - events_.front().wall_time) >
               window_duration_) {
    
    
      events_.pop_front();
    }
  }

  // Returns a debug string representation.
  std::string DebugString() const {
    
    
    if (events_.size() < 2) {
    
    
      return "unknown";
    }

    // c++11: std::fixed 与 std::setprecision(2) 一起使用, 表示输出2位小数点的数据

    std::ostringstream out;
    out << std::fixed << std::setprecision(2) << ComputeRate() << " Hz "
        << DeltasDebugString() << " (pulsed at "
        << ComputeWallTimeRateRatio() * 100. << "% real time)";
    return out.str();
  }

 private:
  struct Event {
    
    
    common::Time time;
    typename ClockType::time_point wall_time;
  };

  // Computes all differences in seconds between consecutive pulses.
  // 返回每2个数据间的时间间隔
  std::vector<double> ComputeDeltasInSeconds() const {
    
    
    CHECK_GT(events_.size(), 1);
    const size_t count = events_.size() - 1;
    std::vector<double> result;
    result.reserve(count);
    for (size_t i = 0; i != count; ++i) {
    
    
      result.push_back(
          common::ToSeconds(events_[i + 1].time - events_[i].time));
    }
    return result;
  }

  // Returns the average and standard deviation of the deltas.
  // 计算数据时间间隔的均值与标准差
  std::string DeltasDebugString() const {
    
    
    const auto deltas = ComputeDeltasInSeconds();
    const double sum = std::accumulate(deltas.begin(), deltas.end(), 0.);
    // 计算均值
    const double mean = sum / deltas.size();

    double squared_sum = 0.;
    for (const double x : deltas) {
    
    
      squared_sum += common::Pow2(x - mean);
    }
    // 计算标准差
    const double sigma = std::sqrt(squared_sum / (deltas.size() - 1));

    std::ostringstream out;
    out << std::scientific << std::setprecision(2) << mean << " s +/- " << sigma
        << " s";
    return out.str();
  }

  std::deque<Event> events_;
  const common::Duration window_duration_;
};

五、结语

结合上篇博客的结论→初始注册的回调函数，整理数据之后，最终都会调用到 sensor::Collator::AddTrajectory()，把数据传送给了 GlobalTrajectoryBuilder。那么该篇博客的重要代码：

double ComputeWallTimeRateRatio() const{
    
    ......}

表示的，就是传感器数据的利用率，如下打印的 100.44%：

[ INFO]: collated_trajectory_builder.cc:72] imu rate: 10.00 Hz 1.00e-01 s +/- 4.35e-05 s (pulsed at 100.44% real time)

所以这里在打印的信息中，该信息是及为重要的，如果超过100%，那么说明系统能够处理更多的数据。