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

Posted 2023-03-31 江南才尽，年少无知！

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

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

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)：

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

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

  // Records an event that will contribute to the computed rate.
  // 对数据队列进行更新
  void Pulse(common::Time time) 
    // 将传入的时间放入队列中
    events_.push_back(Eventtime, 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秒的数据个数。

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

$(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(Eventtime, 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%，那么说明系统能够处理更多的数据。

 
 
 

cartographer源码解析

cartographer为Google提供的激光SLAM开源库，通常通过其提供的ROS平台封装进行使用，该库结构清晰，模块完整，值得深入研究。

项目官网：https://google-cartographer.readthedocs.io/en/latest/

项目Github：https://github.com/cartographer-project/cartographer

环境安装与配置具体参考：https://www.cnblogs.com/lvchaoshun/p/9824528.html

安装过程中可能会碰到一些版本冲突之类的问题，耐心上网寻求解决方法。

温馨提示：本系列主要介绍源码内容，不介绍环境搭建，仅作为自己学习过程记录，可能会比较乱，有疑问欢迎留言讨论。

下面进入正题，下图是cartographer官网提供的软件架构图，本系列将通过cartographer_ros封装库中调用顺序，对程序主线进行介绍，暂未使用到的或次要的信息以后有空再分析。

 

---

 首先我们找到cartographer_ros中程序入口，即cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc，其中的main函数为cartographer_ros节点启动的程序入口。

 其他都是ros平台初始化相关的函数，只用看98行Run方法。

可以看到该函数内主要是些初始化工作，创建map_builder传给node进行初始化、读取pbstream地图文件，并调用StartTrajectoryWithDefaultTopics函数，该函数定义在node.cc中。

 我们会发现AddTrajectory是核心功能入口。××该函数需重点关注，之后我们还会回来一个个分析其中调用的关键函数。

 

 该函数功能比较多，我们一步步看。首先363行创建一个set保存传感器话题的编号，用于之后订阅器订阅。366行调用map_builder_bridge_实例的AddTrajectory函数，在cartographer_ros中类名包含“bridge”的类基本上都是作为“桥梁”调用cartographer源码的类。我们可以看到map_builder_bridge_为map_builder_bridge.cc中定义的MapBuilderBridge的实例，此时关注map_builder_bridge.cc中定义的AddTrajectory函数内容。

 根据cartographer全局配置参数和最初Run方法中定义的map_builder实例类型，我们了解到map_builder_为cartographer/mapping/map_builder.cc中定义的MapBuilder类的实例。上图函数内容分为两块，一块调用map_builder_的AddTrajectoryBuilder函数，创建trajectory_builder并设置结果回调，返回一个路径id作为当前轨迹的编号，详情下篇再介绍；另一块主要初始化一个SensorBridge实例，并放入sensor_birdges_这个map中，以及保存该路径id对应的配置参数。

接下来我们回到node.cc中核心功能入口函数。368与369行分别创建与当前路径编号对应的位姿预测器(PoseExtrapolator)与传感器采样器(TrajectorySensorSamplers)，根据配置参数创建实例没啥好说的。371行启动ros话题订阅器，并设置回调函数。这之后的创建定时器和记录话题编号也很简单。我们主要关注话题订阅的回调函数。

上图截取了LaunchSubsribers函数的部分代码，通过其回调函数，我们发现最终会调用sensor_bridge.cc中定义的HandleLaserScan函数。此处仅仅举个例子，该函数仅处理激光雷达数据(普通LaserScan类型消息、多回波、点云等)，其他传感器数据(如IMU、里程计等)会调用不同的回调函数。

我们观察HandleLaserScan函数实现。

 首先对点云细分，默认num_subdivisions_per_laser_scan_=10。points对象的类型为cartographer_ros自定义类型，其中保存了每个点的光强、位置与时间增量。time_to_subdivision_end保存了最后一个点的时间增量，即一块细分点云间时间间隔(第一个点之前的时刻到最后一个点之后的时刻之间的时间间隔)。之后做一些时间戳的运算，或许是诸如(..., -0.06, -0.04, -0.02, 0)的一组时间戳。最后调用HandleRangefinder将数据添加到全局路径构造器，如下图所示。

此处trajectory_builder_会是GlobalTrajectoryBuilder类的实例，是由于map_builder_的AddTrajectoryBuilder函数中调用了对应的构造函数，此处已涉及cartographer源码。

 本节主要介绍了cartographer_ros封装是如何跳转到cartographer源码中的，主要包括ros节点、map_builder与trajectory_builder的创建，pose_extrapolator与trajectory_sensor_samplers的初始化以及传感器话题订阅与处理。从下一节开始，将根据调用顺序介绍cartographer源码。