cartographer 重点问题1：传感器数据是如何传递到算法内部的

cartographer_ros将各种传感器数据处理成特定的数据类型后传入后续底层核心算法中。
cartographer_ros 处理的流程。
首先入口文件是 /cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc：

一、开始

int main(int argc, char** argv) {
  // glog相关
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";
   
  ::ros::init(argc, argv, "cartographer_node");
  ::ros::start();
   
  cartographer_ros::ScopedRosLogSink ros_log_sink;
  cartographer_ros::Run();             // 主要流程 ！！！！！！！！！！！！！
  ::ros::shutdown();
}

主要的执行过程在 cartographer_ros::Run() 中， 主要流程如下：

可以看到,最主要就是构造了Node类对象，然后调用了Node类的函数：
StartTrajectoryWithDefaultTopics(trajectory_options)

Node类对象的构造中传入了一个MapBuilder对象，而这个MapBuilder对象里面包装的就是SLAM算法本体！
该MapBuilder对象用于构造Node类中的MapBuilderBridge类对象，在Node类中，map_builder_bridge_.AddTrajectory()将被用于创建SLAM算法。

MapBuilder对象即SLAM算法的构建与初始化在下一篇中分析。
本篇中先看看数据是如何传递到SLAM算法内部的。

二、数据如何传入算法层

2.1 MapBuilderBridge::AddTrajectory()

该函数中首先调用MapBuilder类的：
map_builder_->AddTrajectoryBuilder(…)。
然后构造 构造 SensorBridge 对象 。

2.1.1 MapBuilder::AddTrajectoryBuilder

该函数中根据建图使用的激光雷达采用不同的局部轨迹生成器，
2D激光：
std::unique_ptr< LocalTrajectoryBuilder2D > local_trajectory_builder;
3D激光：
std::unique_ptr< LocalTrajectoryBuilder3D > local_trajectory_builder;
创建好局部轨迹生成器后，接着构造CollatedTrajectoryBuilder对象，并将其放入到trajectory_builders_中。

//压入trajectory_builders_容器
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
    trajectory_options, sensor_collator_.get(), trajectory_id,
    expected_sensor_ids,
    // 创建一个 GlobalTrajectoryBuilder 对象     
    CreateGlobalTrajectoryBuilder2D(     // 返回指向基类的指针 std::unique_ptr<TrajectoryBuilderInterface>
        std::move(local_trajectory_builder), trajectory_id,
        static_cast<PoseGraph2D*>(pose_graph_.get()),
        local_slam_result_callback)));

构造CollatedTrajectoryBuilder对象首先要通过CreateGlobalTrajectoryBuilder2D() 构造出GlobalTrajectoryBuilder对象，GlobalTrajectoryBuilder类是一个模板类
template <typename LocalTrajectoryBuilder, typename PoseGraph>
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface
需要将局部轨迹生成器local_trajectory_builder与位姿优化器pose_graph_通过模板参数传入。

// 构建2D地图的重载  
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) {
  // 返回一个 GlobalTrajectoryBuilder 对象指针  
  return absl::make_unique<
      // 模板类  
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback);
}

2.1.2 SensorBridge对象构造

传感器数据就是通过SensorBridge对象传入到算法内部的！
Node类的传感器回调函数中，会将传感器数据传入到Node类的 MapBuilderBridge对象的SensorBridge对象中，
并调用 SensorBridge 的Handle函数。
如下是Node类的odometry回调函数，可以看到最后将数据传送给了sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);

void Node::HandleOdometryMessage(const int trajectory_id,
                                 const std::string& sensor_id,
                                 const nav_msgs::Odometry::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  // 频率控制 
  if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
    return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
  // extrapolators_使用里程计数据进行位姿预测
  if (odometry_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
  }
  // 将数据传入到 SensorBridge 的Handle函数中
  sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}

然后在SensorBridge 的Handle函数中，会将数据传送给
::cartographer::mapping::TrajectoryBuilderInterface* const trajectory_builder_;的AddSensorData()函数中。
如下，为SensorBridge::HandleOdometryMessage()

// 调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleOdometryMessage(
    const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
  // 数据类型与数据坐标系的转换
  std::unique_ptr<carto::sensor::OdometryData> odometry_data =
      ToOdometryData(msg);
  if (odometry_data != nullptr) {
    // tag: 这个trajectory_builder_是谁, 讲map_builder时候再揭晓
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
  }
}

那么，trajectory_builder_ 又是什么呢？ 我们从SensorBridge的构造函数中寻找答案…

// 初始化 sensor_bridges_   
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
    trajectory_options.num_subdivisions_per_laser_scan,
    trajectory_options.tracking_frame,
    node_options_.lookup_transform_timeout_sec, tf_buffer_,
    map_builder_->GetTrajectoryBuilder(trajectory_id));             // 获取 map_builder_ 中创建的 TrajectoryBuilder  

可以看到，trajectory_builder_用map_builder_->GetTrajectoryBuilder(trajectory_id) 进行了赋值，也就是获取了 trajectory_builders_ 中的 TrajectoryBuilderInterface指针。

  // 返回指向CollatedTrajectoryBuilder的指针
  mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
      int trajectory_id) const override {
    return trajectory_builders_.at(trajectory_id).get();
  }

而map_builder_的trajectory_builders_的元素就是在之前的MapBuilder::AddTrajectoryBuilder()函数中创建的，

// CollatedTrajectoryBuilder初始化
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
    trajectory_options, sensor_collator_.get(), trajectory_id,
    expected_sensor_ids,
    // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
    // cartographer/cartographer/mapping/internal/global_trajectory_builder.h
    // 该函数构造出GlobalTrajectoryBuilder 对象
    CreateGlobalTrajectoryBuilder2D(
        std::move(local_trajectory_builder), trajectory_id,
        static_cast<PoseGraph2D*>(pose_graph_.get()),
        local_slam_result_callback, pose_graph_odometry_motion_filter)));

trajectory_builders_里面装的也就是CollatedTrajectoryBuilder类对象。

那么根据多态，trajectory_builder_->AddSensorData() 调用的应该是 CollatedTrajectoryBuilder类 的 AddSensorData()

void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }

疑问这里貌似并没有传入底层算法的操作？？？？？？？？？ 先把问题放着里，以后再仔细看看