(02)Cartographer源码无死角解析-(13) Node::AddTrajectory()→位姿估算器与采样器

讲解关于slam一系列文章汇总链接:

史上最全slam从零开始

，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:

(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

末

正

下

方

中

心

提

供

了

本

人

联

系

方

式

，

点

击

本

人

照

片

即

可

显

示

→

官

方

认

证

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

$文末正下方中心提供了本人联系方式，点击本人照片即可显示 W X \to 官方认证$

一、前言

通过上一篇博客，找到代码分析的突破口，其是位于 node_main.cc 中调用的如下代码：

  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

对 node.StartTrajectoryWithDefaultTopics 函数追踪下去，后续到了 node.cc 文件中的 Node::AddTrajectory() 函数，那么下面就对其进行一个具体的分析。其主要包含如下几个重要的函数：

//根据传感器的类型与数量重定义话题的名称
ComputeExpectedSensorIds(options); 

map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

// 新增一个位姿估计器
AddExtrapolator(trajectory_id, options);
// 新生成一个传感器数据采样器
AddSensorSamplers(trajectory_id, options);
// 订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);

在 Node::AddTrajectory() 函数中，其首先调用的是 ComputeExpectedSensorIds() 函数，该函数的主要目的也比较简单：首先，一个机器人是可以安装多类传感器的，并且每类传感器可以安装多个→即多类多量。如自动驾驶车量就会有一个GPS，一个IMU，多个雷达，多个相机传感器，他们都有自己本身的轨迹预测，多轨迹融合是后面需要讲解的内容。

二、ComputeExpectedSensorIds()

首先面临的问题是，这么多个传感器，都有自己的轨迹预测，应该如何去命名，以便能够轻易进行区分，源码中是这样实现的，首先定义一个结构体 struct SensorId，其中包含了一个枚举 enum class SensorType，如下所示：

  //代码位于src/cartographer/cartographer/mapping/trajectory_builder_interface.h
  struct SensorId {
    // c++11: 限域枚举 enum class 
    enum class SensorType {
      RANGE = 0, //范围传感器，如回声波雷达，点云雷达
      IMU, //惯性传感器
      ODOMETRY, //里程计
      FIXED_FRAME_POSE, //GPS
      LANDMARK,//路标点
      LOCAL_SLAM_RESULT //前端slam的结果也作为一种传感器
    };

    SensorType type;  // 传感器类型
    std::string id;   // topic的名字
    ......
  };

也就是说 Cartographer_ros 支持的传感器只有 SensorType 中的种类，其他的暂时是不支持的。ComputeExpectedSensorIds() 返回也是 struct SensorId 类型的集合，该函数对逻辑也就比较简单的。

1、范围传感器

如关于单线雷达默认的 kLaserScanTopic=“scan”，但是根据配置参数 options.num_laser_scans 其有可能配置多个单线雷达，那么命名方式就会发生改变，变成→”scan_1″，“scan_2”，“scan_3”，

……

$. . . . . .$

当然，也有可能只存在一个单线雷达，那么直接命名为默认值 “scan” 即可，代码部分列举如下：

 // 如果只有一个传感器, 那订阅的topic就是topic
 // 如果是多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推
 for (const std::string& topic :
      ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
   expected_topics.insert(SensorId{SensorType::RANGE, topic});
 }

该代码还是比较简单的，首先调用 ComputeRepeatedTopicNames() 函数，根据 options.num_laser_scans(单线雷达数目) 获得 topics name，然后以 SensorId 类型插入到 expected_topics 之中。

ComputeRepeatedTopicNames() 函数的实现，也是比较简单的，就是根据传入的 topic 以及 num_topics 两个参数，生成类似 “scan_1”，“scan_2”，“scan_3” 的 topic 统一返回，注意如果 num_topics==1，则直接返回，如 “scan”，代码注释如下：

/**
 * @brief 如果只有一个传感器, 那订阅的topic就是topic, 
 *        如果是多个传感器, 那订阅的topic就是topic_1,topic_2
 * 
 * @param[in] topic 订阅话题的名字
 * @param[in] num_topics 传感器的个数
 * @return std::vector<std::string> 订阅话题的名字的集合
 */
std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
                                                   const int num_topics) {              
  CHECK_GE(num_topics, 0); //该类传感器的数目需要大于等于0
  if (num_topics == 1) { //如果该类传感器的数目为1,则直接返回
    return {topic}; 
  }
  std::vector<std::string> topics; //创建一个存储topic的容器
  topics.reserve(num_topics);//提前扩充容器空间，起到加速作用
  for (int i = 0; i < num_topics; ++i) {
    topics.emplace_back(topic + "_" + std::to_string(i + 1)); //在初始topic的后面加下划线以及编号
  }
  // num_topics要是0就返回空的vector
  return topics;
}

如果 num_topics==0，则返回的 topics 为空。然后再回到 Node::ComputeExpectedSensorIds() 函数，其经过重定义的 topic 都以结构体 SensorId 类型存储于 expected_topics(集合set) 变量中。从代码上可以看到 kLaserScanTopic 单线雷达、kMultiEchoLaserScanTopic 回声雷达、kPointCloud2Topic 多线雷达、其都属于 SensorType::RANGE 范围传感器。

2、IMU传感器

在配置文件.lua中，一般我们需要配置如下参数：

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.use_trajectory_builder_2d = true

其默认在 src/cartographer/configuration_files/map_builder.lua 中配置，都是为 false，实际应用过程中，需要至少选择一个设置为 true，即确定使用 3D 轨迹还是 2D轨迹。再回到ComputeExpectedSensorIds() ，可以看到如下代码：

  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.
  // 3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
  }

其是对数据参数进行一个验证，满足下面条件中的任意一个条件，都会添加一个 IMU 话题：

)

\color{blue}{(1):}

$(1) :$

使用3D轨迹(3D轨迹默认必须使用IMU的)

)

\color{blue}{(2):}

$(2) :$

使用2D轨迹同时还启用IMU数据，即参数 TRAJECTORY_BUILDER_2D.use_imu_data 设置为 true。

3、里程计

  // Odometry is optional.
  // 里程计可有可无, topic的个数只能有一个
  if (options.use_odometry) {
    expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
  }

4、GPS

  // NavSatFix is optional.
  // gps可有可无, topic的个数只能有一个
  if (options.use_nav_sat) {
    expected_topics.insert(
        SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
  }

5、Landmark

  // Landmark is optional.
  // Landmark可有可无, topic的个数只能有一个
  if (options.use_landmarks) {
    expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
  }

三、AddExtrapolator()

Node::AddTrajectory() 函数调用 ComputeRepeatedTopicNames() 函数之后，就可以获得这些传感器相关的topic 信息，即 ComputeRepeatedTopicNames() 函数的返回结果 expected_sensor_ids，本人如下所示：

在这里插入图片描述

从这里可以获得的信息是：共两个传感器，一个多线雷达，一个IMU。该结果 expected_sensor_ids 回被传递给：

  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

对于 map_builder_bridge_.AddTrajectory() 函数先略过，主要关注其返回值 trajectory_id 为一个常量整形即可，先来看看其随后被调用的 AddExtrapolator() 函数。AddExtrapolator() 实际就是 odome(里程计) 和 IMU 的轨迹的融合，即新增的一个位姿估算器，对位姿做预测，用于前端匹配之前的先验。AddExtrapolator() 的代码流程如下：

)

\color{blue}{(1):}

$(1) :$

在Node类中，即Node.h文件，可以看到如下定义：

  std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;

其创建了一个map对象，key 为 int 整形，value 为 PoseExtrapolator 类型。这里的 key 就是 trajectory_id。Node::AddExtrapolator() 首先调用：

CHECK(extrapolators_.count(trajectory_id) == 0);

函数，检测新生成的轨迹的 id 是否在 extrapolators_中，如果是则报错。

)

\color{blue}{(2):}

$(2) :$

根据是否使用3D轨迹，获得配置文件中3D轨迹或者2D轨迹设定的 imu_gravity_time_constant 重力时间常数，默认配置都是10

)

\color{blue}{(3):}

$(3) :$

利用 map::emplace() 直接往 extrapolators_ 容器中插入实例元素，std::piecewise_construct 与 std::forward_as_tuple() 理解为固定搭配。

如果觉得(3)不好理解，先看代码：

/**
 * @brief 新增一个位姿估计器
 * 
 * @param[in] trajectory_id 轨迹id
 * @param[in] options 参数配置
 */
void Node::AddExtrapolator(const int trajectory_id,
                           const TrajectoryOptions& options) {
  //位姿估计器估算时间间隔   
  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms

  // 新生成的轨迹的id 不应该在extrapolators_中
  CHECK(extrapolators_.count(trajectory_id) == 0);

  // imu_gravity_time_constant在2d, 3d中都是10
  const double gravity_time_constant =
      node_options_.map_builder_options.use_trajectory_builder_3d()
          ? options.trajectory_builder_options.trajectory_builder_3d_options()
                .imu_gravity_time_constant()
          : options.trajectory_builder_options.trajectory_builder_2d_options()
                .imu_gravity_time_constant();

  // c++11: map::emplace() 用于通过在容器中插入新元素来扩展map容器
  // 元素是直接构建的（既不复制也不移动）.仅当键不存在时才进行插入
  // c++11: std::forward_as_tuple tuple的完美转发
  // 该 tuple 在以右值为参数时拥有右值引用数据成员, 否则拥有左值引用数据成员
  // c++11: std::piecewise_construct 分次生成tuple的标志常量
  // 在map::emplace()中使用forward_as_tuple时必须要加piecewise_construct,不加就报错
  // https://www.cnblogs.com/guxuanqing/p/11396511.html

  // 以1ms, 以及重力常数10, 作为参数构造PoseExtrapolator
  extrapolators_.emplace(
      std::piecewise_construct, 
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));
}

其上的 extrapolators_.emplace() 表示添加一对键值对，key 为 trajectory_id，value 为一个 ::cartographer::mapping::PoseExtrapolator 的实例，创建该实例需要传入了 ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec) 与 gravity_time_constant 两个参数。

四、AddSensorSamplers()

在新增一个位姿估算器之后，其还会带调用 AddSensorSamplers() 新生成一个传感器数据采样器，存储于 Node 类成员 std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_ 之中。代码的实现与前面的比较相似，还是利用利用 map::emplace() 直接往 sensor_samplers_ 容器中插入实例元素，结合 std::piecewise_construct 与 std::forward_as_tuple() 搭配使用，即往成员变量 sensor_samplers_ 中插入的 key 为 trajectory_id，value 为 TrajectorySensorSamplers 的类对象。

构建 TrajectorySensorSamplers 类对象构造传入如下参数：

options.rangefinder_sampling_ratio,  //范围传感器(如单,多线雷达)采样频率
options.odometry_sampling_ratio, //里程计采样频率
options.fixed_frame_pose_sampling_ratio, //GPS采样频率
options.imu_sampling_ratio, //IMU采样频率
options.landmarks_sampling_ratio //landmarks采样频率

查看配置文件，可以看到上述参数传入的都是1，代表全采样(如果为0.5则表示每两个样本数据采集一次，设置为0则表示丢弃所有数据)。另外，要注意的是上诉采样频率都被保存于 TrajectorySensorSamplers 之中，如 TrajectorySensorSamplers 的构造函数：

  // 控制各个传感器数据的采样频率
  struct TrajectorySensorSamplers {
    TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
                             const double odometry_sampling_ratio,
                             const double fixed_frame_pose_sampling_ratio,
                             const double imu_sampling_ratio,
                             const double landmark_sampling_ratio)
        : rangefinder_sampler(rangefinder_sampling_ratio),
          odometry_sampler(odometry_sampling_ratio),
          fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
          imu_sampler(imu_sampling_ratio),
          landmark_sampler(landmark_sampling_ratio) {}

    ::cartographer::common::FixedRatioSampler rangefinder_sampler;
    ::cartographer::common::FixedRatioSampler odometry_sampler;
    ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
    ::cartographer::common::FixedRatioSampler imu_sampler;
    ::cartographer::common::FixedRatioSampler landmark_sampler;
  };

在 FixedRatioSampler 的构造函数中，可以看到，如果采样频率设置为0，其会打印警告信息。FixedRatioSampler 中有实现 Pulse() 函数，调用该函数，可以判断当前采集的样本是否使用。其实现的函数比较巧妙，如下：

// 在比例小于ratio_时返回true, ratio_设置为1时都返回true, 也就是说使用所有的数据
bool FixedRatioSampler::Pulse() {
  ++num_pulses_; 
  if (static_cast<double>(num_samples_) / num_pulses_ < ratio_) {
    ++num_samples_;
    return true;
  }
  // 返回false时代表数据可以不用,可以跳过计算
  return false;
}

假设 ratio_ 为0.7，根据下面表格来分析下他的运行过程：

0.0

0.50

0.67

0.75

0.6

0.67

0.71

0.63

0.56

0.60

返

回

结

果

\begin{array}{c} num\_samples\_ & 0& 1& 2& 3& 3& 4& 5& 5&5&6\\ num\_pulses\_ & 1& 2& 3& 4& 5& 6& 7& 8& 9&10\\ result\_ratio & 0.0& 0.50& 0.67& 0.75& 0.6& 0.67& 0.71& 0.63& 0.56& 0.60\\ 返回结果 & true &true &true & false &true &true & false & false &true &true \end{array}

$n u m_s a m p l e s_n u m_p u l s e s_r e s u l t_r a t i o 返回结果 01 0.0 t r u e 12 0.50 t r u e 23 0.67 t r u e 34 0.75 f a l s e 35 0.6 t r u e 46 0.67 t r u e 57 0.71 f a l s e 58 0.63 f a l s e 59 0.56 t r u e 610 0.60 t r u e$

可以看到10个数据中，有个为true，概率分布刚刚好为0.7，有兴趣的朋友可以继续算下去，做个验证。另外 FixedRatioSampler 中，还实现了一个DebugString()函数，主要就是为了调试打印使用的。

五、结语

通过该篇博客，已经讲解了 Node::AddTrajectory() 中的三个重要函数：

  //根据传感器的类型与数量重定义话题的名称
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);

  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);

  // 新生成一个传感器数据采样器
  AddSensorSamplers(trajectory_id, options);

还剩下一个函数 LaunchSubscribers() 还没有讲解，不过没有关系，下篇博客会对其进行详细的分析。

原文链接：https://blog.csdn.net/weixin_43013761/article/details/127638039