cartographer（3）轨迹添加

1.在文件/cartographer_ros/cartographer_ros/map_builder_bridge.cc进入MapBuilderBridge::AddTrajectory 再进入map_builder_->AddTrajectoryBuilder

在文件/cartographer/cartographer/mapping/map_builder.cc

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,    //传感器id
    const proto::TrajectoryBuilderOptions& trajectory_options,//轨迹配置
    LocalSlamResultCallback local_slam_result_callback) {//局部slam结果回调

  const int trajectory_id = trajectory_builders_.size();  //获取轨迹id

// 运动过滤器, 运动太小没必要进行更新
  absl::optional<MotionFilter> pose_graph_odometry_motion_filter;

  if (trajectory_options.has_pose_graph_odometry_motion_filter()) {
    LOG(INFO) << "Using a motion filter for adding odometry to the pose graph.";
    pose_graph_odometry_motion_filter.emplace(
        MotionFilter(trajectory_options.pose_graph_odometry_motion_filter()));
  }

  if (options_.use_trajectory_builder_3d()) { //3d轨迹构建
    std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_3d_options()) {
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder3D>(
          trajectory_options.trajectory_builder_3d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }

    DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));

    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        CreateGlobalTrajectoryBuilder3D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph3D*>(pose_graph_.get()),
            local_slam_result_callback)));
  } 

//2d轨迹构建
else {
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;//局部构建器初始化
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),  //加载配置
          SelectRangeSensorIds(expected_sensor_ids));  //选择传感器id
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));//创建后端优化

// CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback)));
  } // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder

  MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                  pose_graph_.get()); // 是否是纯定位模式, 如果是则只保存最近3个submap//定时获取后端优化

  if (trajectory_options.has_initial_trajectory_pose()) {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();//获取初始位置
    pose_graph_->SetInitialTrajectoryPose( //设置初始化位置
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));
  }

// 保存轨迹的配置文件
  proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
  for (const auto& sensor_id : expected_sensor_ids) {
    *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
  }

  *options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
      trajectory_options;
  all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
  CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());//最后检查一下轨迹跟踪器对象及其配置的数量
  return trajectory_id;
}

 2.MapBuilderBridge::AddTrajectory内

//用于将ROS的消息转换成Cartographer中的传感器数据类型。

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_,//tf查询超时时间

      map_builder_->GetTrajectoryBuilder(trajectory_id));//轨迹id

3.局部slam回调--记录下轨迹状态

//记录下轨迹状态
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)});  //用于记录局部SLAM反馈的状态
  absl::MutexLock lock(&mutex_);
  local_slam_data_[trajectory_id] = std::move(local_slam_data);  //写入容器trajectory_state_data
}