接上一篇，Node::StartTrajectoryWithDefaultTopics实际就一个Node::AddTrajectory，它的流程图:

const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
    expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
const int trajectory_id = 
    map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, topics, trajectory_id);
is_active_trajectory_[trajectory_id] = true;
for (const auto& sensor_id : expected_sensor_ids)
{  // unordered_set
  subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;

Node::ComputeExpectedSensorIds

函数返回的是std::set<TrajectoryBuilderInterface::SensorId> expected_topics;类型：

struct SensorId
{
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };
    SensorType type;
    std::string id;
}

它记录的是所有传感器的类型和对应id，id是一个字符串，记录了传感器所对应的ROS主题名称。 如果有多个雷达，id就是scan1,scan2……

至于为什么这样命名，由下面的函数规定，字符串在cartographer_ros\node_constants.h中赋值

cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
  cartographer_ros_msgs::SensorTopics topics;
  topics.laser_scan_topic = kLaserScanTopic;
  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
  topics.point_cloud2_topic = kPointCloud2Topic;
  topics.imu_topic = kImuTopic;
  topics.odometry_topic = kOdometryTopic;
  topics.nav_sat_fix_topic = kNavSatFixTopic;
  topics.landmark_topic = kLandmarkTopic;
  return topics;
}

map_builder_bridge_.AddTrajectory

两个参数是const std::set<TrajectoryBuilderInterface::SensorId>& expected_sensor_ids 和 const TrajectoryOptions& trajectory_options

expected_sensor_ids记录了用于建图的所有传感器名称和类型

MapBuilderBridge的构造函数就是加载配置和map_builder

// 添加一个轨迹跟踪器，同时将构建成功的索引返回
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
    expected_sensor_ids,  trajectory_options.trajectory_builder_options,
    // 这个参数相当于注册了一个回调函数 OnLocalSlamResult ,  用于响应map_builder_完成
    // 一个局部SLAM 或者 说是成功构建了一个子图的事件
    ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
                ::std::placeholders::_1, ::std::placeholders::_2,
                ::std::placeholders::_3, ::std::placeholders::_4,
                ::std::placeholders::_5) );

LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

// std::unordered_map<int, std::unique_ptr<SensorBridge>>  sensor_bridges_;
// 检查 trajectory_id， 确保之前没有使用过
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
// SensorBridge将ROS的消息转换成 Cartographer 中的传感器数据类型
// 前面都是参数，最后的参数是Cartographer的一个核心对象，TrajectoryBuilderInterface指针
// 通过sensor_bridge对象转换后的数据都是通过它喂给Cartographer的
sensor_bridges_[trajectory_id] = 
    cartographer::common::make_unique<SensorBridge>(
        trajectory_options.num_subdivisions_per_laser_scan,
        trajectory_options.tracking_frame,
        node_options_.lookup_transform_timeout_sec, tf_buffer_,
        // 实际类型是 CollatedTrajectoryBuilder
        map_builder_->GetTrajectoryBuilder(trajectory_id)  );
// 轨迹相关的配置保存到容器对象trajectory_options_中并检查后，返回刚刚生成的索引trajectory_id
auto emplace_result = trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;

MapBuilder::AddTrajectoryBuilder 和 GetTrajectoryBuilder到后面继续看

回调函数 OnLocalSlamResult

void MapBuilderBridge::OnLocalSlamResult(
    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> )
{
  // LocalSlamData是定义在 MapBuilderBridge 内部的结构体，用于记录局部SLAM反馈的状态
  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_);

  /* std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>
      trajectory_state_data_  GUARDED_BY(mutex_);*/
  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

最后赋值给了trajectory_state_data_，唯一应用的地方在MapBuilderBridge::GetTrajectoryStates()

可以对此函数做修改，添加日志等功能

Node::AddExtrapolator

constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
// 省略一行：把lua文件的 imu_gravity_time_constant 参数赋给变量 gravity_time_constant
extrapolators_.emplace(
  std::piecewise_construct, std::forward_as_tuple(trajectory_id),
  std::forward_as_tuple(
      ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
      gravity_time_constant)  );

添加用于位姿插值的对象extrapolators_， 类型std::map<int, PoseExtrapolator> extrapolators_;

Node::AddSensorSamplers

sensor_samplers_.emplace(
  std::piecewise_construct, std::forward_as_tuple(trajectory_id),
  std::forward_as_tuple(
      options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
      options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
      options.landmarks_sampling_ratio)  );

类型std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;

对cartographer中的采样器(fixed_ratio_sampler.cc)的封装，用一个计数器来按照一个指定的频率对原始的数据进行降采样，采样频率可以通过轨迹参数文件来配置

LaunchSubscribers

完成传感器消息的订阅,在传感器数据的作用下驱动系统运转，进而完成位姿估计和建图的任务。详细分析见第四篇