cartographer随记---程序脉络

我之前的一篇blog中介绍过cartographer中，传感器数据进来以后是如何被储存以及加入到位姿图中的。

https://blog.csdn.net/weixin_44684139/article/details/106107610

这一篇我想真正梳理一下cartographer的前端是如何实现的。

包括传感器信息是如何被前端获取，又如何进行处理的。

从node.cc开始

// 非常重要的函数，用于创建轨迹。
// 参数1：轨迹建立参数
// 参数2：各个传感器消息的话题名称
int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics) {
  
  // 得到携带的传感器的SensorId——包含传感器类型和传感器话题
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
      
  // map_builder_bridge_.AddTrajectory——新建一条轨迹相关的各类bridge桥和各类处理函数
  /*总结其工作：
   * (1)map_builder_->AddTrajectoryBuilder
   * 	（1.1）通过local_trajectory_builder创建global_trajectory_builder，进而创建真正的trajectory_builder，即CollatedTrajectoryBuilder类型
   * 	（1.2）向pose_graph_中加入修剪器trimmer和轨迹的初始位姿
   * (2)创建sensor_bridge，这个传感器桥在后面的LaunchSubscribers很关键，因为传感器消息订阅者的一系列回调函数均是由sensor_bridge完成的
   */
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  
  // 创建cartographer::mapping::PoseExtrapolator类型，用于前端的位姿推断
  AddExtrapolator(trajectory_id, options);
  
  // 创建TrajectorySensorSamplers，即传感器采样器，规定了传感器采样频率
  AddSensorSamplers(trajectory_id, options);  // 传感器数据采集相关
  
  // 启动一些订阅者，定义回调函数，是传感器信息的入口
  // 所以如果想按照数据进来的时间顺序看程序的话，从这里进入看程序
  LaunchSubscribers(options, topics, trajectory_id);  // 重点关注这个
  
  // 将轨迹设置为激活状态
  is_active_trajectory_[trajectory_id] = true;
  
  // subscribed_topics_的元素表示了订阅哪些传感器话题，sensor_id.id为传感器对应的话题名称
  // 2d里面仅有echoes和imu，其中echoes为：<remap from="echoes" to="horizontal_laser_2d" />
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
//     std::cout<<sensor_id.id<<std::endl;
  }
  return trajectory_id;
}

AddTrajectory函数真正打开了cartographer的大门，实现方式包括：
（1）map_builder_bridge_.AddTrajectory
（2）AddExtrapolator
（3）AddSensorSamplers
（4）LaunchSubscribers

map_builder_bridge_.AddTrajectory函数中，将ros与carto链接起来的代码为：

  const int trajectory_id = map_builder_->AddTrajectoryBuilder(  // 这已经是cartographer项目中的代码了
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,  // OnLocalSlamResult函数
                  ::std::placeholders::_1, ::std::placeholders::_2,
                  ::std::placeholders::_3, ::std::placeholders::_4,
                  ::std::placeholders::_5));

会发现传入了一个函数参量，即MapBuilderBridge::OnLocalSlamResult，这是一个用来回调的函数，先看看它的代码。

数据回调初露端倪

void MapBuilderBridge::OnLocalSlamResult(  // 在addtrajectory中被调用
    const int trajectory_id, const ::cartographer::common::Time time,
    const Rigid3d local_pose,
    ::cartographer::sensor::RangeData range_data_in_local,
    const std::unique_ptr<const ::cartographer::mapping::
                              TrajectoryBuilderInterface::InsertionResult>) {
  std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
      std::make_shared<TrajectoryState::LocalSlamData>(
          TrajectoryState::LocalSlamData{time, local_pose,
                                         std::move(range_data_in_local)});
  cartographer::common::MutexLocker lock(&mutex_);
  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

可以发现其作用为将传感器数据加入到trajectory_state_data_容器中，按照轨迹索引进行储存。trajectory_state_data_容器是使用提供传感器数据进行前端位姿估计后将数据存储于此的。这是为了便于发布。

那么现在会有一个疑问，什么时候产生回调机制呢？我们首先看看这个作为参数被传入map_builder_->AddTrajectoryBuilder()中以后，最终去了哪里，被谁收留了呢？

继续往下走

深入map_builder

MapBuilder::AddTrajectoryBuilder函数位于map_builder.cc中，

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {

最后一个参数local_slam_result_callback即为传入的回调函数。这个函数在我的另一篇blog中已有涉及。

接下来观察他去哪里了：

    trajectory_builders_.push_back(    //真正地创建一个TrajectoryBuilder并推进容器，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
        common::make_unique<CollatedTrajectoryBuilder>(  // CollatedTrajectoryBuilder这里面我写了回调的总体思路（但与local_slam_result_callback无关）
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder2D( 
                std::move(local_trajectory_builder), trajectory_id,		// GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
                static_cast<PoseGraph2D*>(pose_graph_.get()),
                local_slam_result_callback)));  // 需要传入回调local_slam_result_callback

CreateGlobalTrajectoryBuilder2D函数需要回调函数作为参数传入。此函数的作用是创建一个GlobalTrajectoryBuilder类型，这个类型又用来构造CollatedTrajectoryBuilder类型。

先看CreateGlobalTrajectoryBuilder2D，其位于global_trajectory_builder.cc中

std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
    const TrajectoryBuilderInterface::LocalSlamResultCallback&
        local_slam_result_callback) {
  return common::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback);
}

最终构造了一个GlobalTrajectoryBuilder类型，这个类型携带着这个回调函数

然后你会发现，还是这个文件global_trajectory_builder.cc中的AddSensorData函数调用了这个回调函数。

  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
      ........
          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));
    }
  }

回顾一下这个回调函数的作用：

作用为将传感器数据加入到trajectory_state_data_容器中，按照轨迹索引进行储存。trajectory_state_data_容器是使用提供传感器数据进行前端位姿估计后将数据存储于此的。这是为了便于发布。

现在我们只需要明确何时调用global_trajectory_builder类型的AddSensorData函数，我们就可以顺水推舟知道传感器的消息是如何经过回调提供给前端的。

整理思路，一锤定音

传感器信息进来（以点云数据为例）的流动方向：

node.cc的HandlePointCloud2Message -> sensor_bridge.cc的HandlePointCloud2Message -> HandleRangefinder -> collated_trajectory_builder.h（注意是.h）的AddSensorData

停一下，你会发现collated_trajectory_builder.cc的AddSensorData函数调用AddData函数的时候使用了sensor::MakeDispatchable函数，将传感器数据类型重载为dipatchable类型（定义于dispatchable.h）
这是很重要的！！！！！！！

继续：collated_trajectory_builder.cc的AddData -> collator.cc的AddSensorData -> odered_multi_queue的Add -> Dispatch()产生回调（注意此回调非上面所说的OnLocalSlamResult，而是在传感器数据进来之前odered_multi_queue被构造的时候定义的回调，传感器数据进来先回调它，是一个“第一回调”），回调的时候输入的传感器数据不要忘了是dipatchable类型

回调函数定义于collated_trajectory_builder.cc中的HandleCollatedSensorData

其中关键语句为：

data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());

这里的data别忘了，之前说了，是dipatchable类型.
此外，wrapped_trajectory_builder_是global_trajectory_builder类型。

找到dipatchable.h：

// dipatchable.h
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

trajectory_builder是global_trajectory_builder类型，因此要找到global_trajectory_builder.cc中的AddSensorData

串起来了！

这个“第二回调”函数调用了最开始的回调函数。
回顾一下这个回调函数的作用：

作用为将传感器数据加入到trajectory_state_data_容器中，按照轨迹索引进行储存。trajectory_state_data_容器是使用提供传感器数据进行前端位姿估计后将数据存储于此的。这是为了便于发布。

终于，trajectory_state_data_容器被填充了传感器数据。接下来看前端如何运用数据进行位姿推断，并将相关信息加入位姿图中。

AddSensorData

我们知道，传感器数据进来以后，在第一层回调中调用global_trajectory_builder.cc的AddSensorData

找到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(  // AddRangeData的作用是添加RangeData，返回匹配结果
            sensor_id, timed_point_cloud_data);
    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 = common::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())});
    }
    // 回调函数为map_builder_bridge.cc的OnLocalSlamResult，即将局部slam的结果放到容器中，便于发布
    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_->AddRangeData完成了局部slam的位姿推断工作，然后用pose_graph_->AddNode将节点加入位姿图中。

函数local_slam_result_callback_即为回调函数map_builder_bridge.cc的OnLocalSlamResult，即将局部slam的结果放到之前所说的trajectory_state_data_容器中，便于发布。

发布机制为node.cc的定时器：

  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.submap_publish_period_sec),
      &Node::PublishSubmapList, this));
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.pose_publish_period_sec),
      &Node::PublishTrajectoryStates, this));
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
      &Node::PublishTrajectoryNodeList, this));
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
      &Node::PublishLandmarkPosesList, this));
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kConstraintPublishPeriodSec),
      &Node::PublishConstraintList, this));

即每隔一段时间进入回调函数进行处理。这个函数PublishTrajectoryStates 便是将trajectory_state_data_容器中的数据进行发布。