C++ Cartographer源码中关于MapBuilder的声明与构造

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

map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)

  // MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);//在map_builder.cc中实现，工厂函数
                                                                                //在这里,实例化一个MapBuilder, 而MapBuilder是MapBuilderInterface的子类
                                                                                //MapBuilder的AddTrajectoryBuilder实例化了CollatedTrajectoryBuilder 

// 工厂函数,生成接口API
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) 
{
    // Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
    const int trajectory_id = map_builder_->AddTrajectoryBuilder(
    expected_sensor_ids, trajectory_options.trajectory_builder_options,
    [this]() {
    OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
    });
    // Step: 2 为这个新轨迹 添加一个SensorBridge
    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)); // CollatedTrajectoryBuilder
    ......
    return trajectory_id;
}

  struct SensorId {
    // c++11: 限域枚举 enum class 
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };
    SensorType type;  // 传感器类型
    std::string id;   // topic的名字
    bool operator==(const SensorId& other) const {
      return std::forward_as_tuple(type, id) ==
             std::forward_as_tuple(other.type, other.id);
    }
    bool operator<(const SensorId& other) const {
      return std::forward_as_tuple(type, id) <
             std::forward_as_tuple(other.type, other.id);
    }
  };

  // Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      // lambda表达式 local_slam_result_callback_
      [this](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>) {
        // 保存local slam 的结果数据 5个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });

  // Step: 2 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>( //sensor_bridges_是一个unorderedmap
      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是MapBuilder的实例化,  在map_builder.h中实现,
                                                          // 返回当前轨迹的CollatedTrajectoryBuilder的指针.
                                                          // CollatedTrajectoryBuilder就是前端后端绑在一起的,传给了SensorBridge

/**
 * @brief 创建一个新的 TrajectoryBuilder 并返回它的 trajectory_id
 * 
 * @param[in] expected_sensor_ids 所有需要的topic的名字的集合
 * @param[in] trajectory_options 轨迹的参数配置
 * @param[in] local_slam_result_callback 需要传入的回调函数
 *        实际上是map_builder_bridge.cc 中的 OnLocalSlamResult() 函数
 * @return int 新生成的轨迹的id
 */
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) 
{
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>( //建立一个local_trajectory_builder
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    // CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( //NOTE:MapBuilder::AddTrajectoryBuilder使用的是CollatedTrajectoryBuilder
        trajectory_options, sensor_collator_.get(), trajectory_id, 
        expected_sensor_ids,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(  //全局轨迹构建器
                                          //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
                                          //继承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一个父类
            std::move(local_trajectory_builder), //前端构建器
            trajectory_id, //
            static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿图
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
}

  // A callback which is called after local SLAM processes an accumulated
  // 'sensor::RangeData'. If the data was inserted into a submap, reports the
  // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
  using LocalSlamResultCallback =
      std::function<void(int /* trajectory ID */, common::Time, //返回值为空, 5个参数列表
                         transform::Rigid3d /* local pose estimate */,
                         sensor::RangeData /* in local frame */,
                         std::unique_ptr<const InsertionResult>)>; //InsertionResult保存了地图的具体数据,栅格值

/**
 * @brief local_slam_data中存储所有雷达点云的数据结构
 * 
 * @param origin  点云的原点在local坐标系下的坐标
 * @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit
 * @param misses  是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点
 */
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses; // local坐标系下的坐标
};

  struct InsertionResult {
    NodeId node_id;
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
  };

  struct Data {
    common::Time time;
    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment;
    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;
    // Used for loop closure in 3D.
    sensor::PointCloud high_resolution_point_cloud;
    sensor::PointCloud low_resolution_point_cloud;
    Eigen::VectorXf rotational_scan_matcher_histogram;
    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose;
  };

      // lambda表达式 local_slam_result_callback_
      [this](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>) 
        // 保存local slam 的结果数据 5个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      }

/**
 * @brief 保存local slam 的结果
 * 
 * @param[in] trajectory_id 当前轨迹id
 * @param[in] time 扫描匹配的时间
 * @param[in] local_pose 扫描匹配计算出的在local坐标系下的位姿
 * @param[in] range_data_in_local 扫描匹配使用的雷达数据
 */
void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id, const ::cartographer::common::Time time,
    const Rigid3d local_pose,
    ::cartographer::sensor::RangeData range_data_in_local) {
  std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
      std::make_shared<LocalTrajectoryData::LocalSlamData>(
          LocalTrajectoryData::LocalSlamData{time, local_pose,
                                             std::move(range_data_in_local)});
  // 保存结果数据
  absl::MutexLock lock(&mutex_);
  local_slam_data_[trajectory_id] = std::move(local_slam_data);
  // todo: local_slam_data_[trajectory_id].loc
}

    // LocalSlamData中包含了local slam的一些数据, 包含当前时间, 当前估计的位姿, 以及累计的所有雷达数据
    struct LocalSlamData {
      ::cartographer::common::Time time;
      ::cartographer::transform::Rigid3d local_pose;
      ::cartographer::sensor::RangeData range_data_in_local;
    };

    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>( //建立一个local_trajectory_builder
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    // CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( //NOTE:MapBuilder::AddTrajectoryBuilder使用的是CollatedTrajectoryBuilder
        trajectory_options, sensor_collator_.get(), trajectory_id, 
        expected_sensor_ids,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(  //全局轨迹构建器
                                          //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
                                          //继承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一个父类
            std::move(local_trajectory_builder), //前端构建器
            trajectory_id, //
            static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿图
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

// 只返回传感器类型是RANGE的topic的集合
std::vector<std::string> SelectRangeSensorIds(
    const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
  std::vector<std::string> range_sensor_ids;
  for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
    if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
      range_sensor_ids.push_back(sensor_id.id);
    }
  }
  return range_sensor_ids;
}

// 2d的完整的slam
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,
    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
  return absl::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback, pose_graph_odometry_motion_filter);
}

// Collates sensor data using a sensor::CollatorInterface, then passes it on to
// a mapping::TrajectoryBuilderInterface which is common for 2D and 3D.
// 使用 sensor::CollatorInterface 整理传感器数据, 
// 然后将其传递到2D和3D通用的 mapping::TrajectoryBuilderInterface
// 处理传感器数据, 使其按照时间排列, 然后传入GlobalTrajectoryBuilder