[Cartographer]AddTrajectory

AddTrajectory

从Node::StartTrajectoryWithDefaultTopics()进来后，开始使用函数Node::AddTrajectory添加轨迹

int Node::AddTrajectory(const TrajectoryOptions& options) {

  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);

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

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

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

  // 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);

  // 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
  // 检查设置的topic名字是否在ros中存在, 不存在则报错
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));

  // 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }

  return trajectory_id;
}

在该函数中，重要的是调用map_builder_bridge的AddTrajectory，该函数的功能主要是添加一条轨迹，构建一条轨迹的话，需要SLAM的前端和后端，因此需要提前把这两部分配置完成，来到MapBuilderBridge::AddTrajectory中，主要的流程如下：（如图所示）

1. 主要是调用函数MapBuilder::AddTrajectoryBuilder构建SLAM 前端LocalTrajectoryBuilder2D，接着和位姿图PoseGraph2D打包组合成SLAM后端GlobalTrajectoryBuilder，然后再进一步打包构成CollatedTrajectoryBuilder，最后将其放到MapBuilder::trajectory_builders_中，返回轨迹的idtrajectory_id。
2. 为这个新的轨迹添加一个SensorBridge，刚刚构建的该轨迹的CollatedTrajectoryBuilder也会保存到里面。
   

组成成分

1.MapBuilder

• 说明：Cartographer中的地图构建器，用于地图的构建。

成员变量	作用
options_	记录运行配置
thread_pool_	线程池
pose_graph_	位姿图
sensor_collator	数据的收集，统一分发器
trajectory_builders_	所有轨迹构建器
all_trajectory_builder_options_	所有轨迹构建器的配置

2.CollatedTrajectoryBuilder

• 说明：包括了SLAM的前端、后端、数据分发器。

3.LocalTrajectoryBuilder2D

• 说明：2DSLAM的前端，用于scan-to-map的匹配，生成submap并估计位姿

4.pose_graph_

• 说明：后端优化，进行闭环检测、node-node，node-submap等约束的位姿优化

5.SensorBridge

• 说明：将ROS的话题数据转换成Cartographer内部的数据格式，然后给到sensor_collator

6.sensor_collator

• 说明：收集所有的数据，然后进行数据统一的分发（时间同步处理）