Apollo Fusion Module感知融合模块

组件和类定义

融合跟其他部分一样，也是一个独立的组件，位于

onboard/component/fusion_component.*

fusion_component里的fusion_成员对象是ObstacleMultiSensorFusion类定义的，有fusion_相应的init函数，会传入上述的配置参数，frame是一帧传感器数据，fused_objects是最后融合后的结果

fusion_->Process(frame, &fused_objects);

类的定义如下：

class FusionComponent : public cyber::Component<SensorFrameMessage> {
 public:
  FusionComponent() = default;
  ~FusionComponent() = default;
  // 初始化函数
  bool Init() override;
  // 融合处理函数
  bool Proc(const std::shared_ptr<SensorFrameMessage>& message) override; // 注册为消息回调函数

 private:
  bool InitAlgorithmPlugin(); // 组件内部的初始化方法，由 Init 方法进行调用
  bool InternalProc(const std::shared_ptr<SensorFrameMessage const>& in_message, // 组件内部的消息处理方法，具体算法流程入口，由 Proc 方法进行调用
                    std::shared_ptr<PerceptionObstacles> out_message,
                    std::shared_ptr<SensorFrameMessage> viz_message);

 private:
  static std::mutex s_mutex_;
  static uint32_t s_seq_num_;

  std::string fusion_name_;     // 融合名称
  std::string fusion_method_;   // 融合方法
  std::string fusion_main_sensor_;  // 融合主传感器
  bool object_in_roi_check_ = false;    // 是否开启 HD Map ROI 融合障碍物校验
  double radius_for_roi_object_check_ = 0;  // HD Map ROI 融合障碍物校验半径

  std::unique_ptr<fusion::BaseMultiSensorFusion> fusion_; // 独占智能指针，用于管理多传感器融合抽象基类对象，最为关键的数据成员
  map::HDMapInput* hdmap_input_ = nullptr;
  std::shared_ptr<apollo::cyber::Writer<PerceptionObstacles>> writer_; // Cyber Writer 对象，用于输出 protobuf 格式的经 HD Map ROI 校验过的有效融合障碍物信息
  std::shared_ptr<apollo::cyber::Writer<SensorFrameMessage>> inner_writer_; // Cyber Writer 对象，用于输出可视化信息
};

其中主要为Init函数和Proc函数

初始化

初始化函数如下：

bool FusionComponent::Init() {
  FusionComponentConfig comp_config;
  if (!GetProtoConfig(&comp_config)) {
    return false;
  }
  AINFO << "Fusion Component Configs: " << comp_config.DebugString();

  // to load component configs
  fusion_name_ = comp_config.fusion_name(); // 融合名称：ObstacleMultiSensorFusion
  fusion_method_ = comp_config.fusion_method(); // 融合方法：ProbabilisticFusion
  fusion_main_sensor_ = comp_config.fusion_main_sensor();   // 融合主传感器：velodyne128，front_6mm，front_12mm
  object_in_roi_check_ = comp_config.object_in_roi_check(); // 是否开启 HD Map ROI 融合障碍物校验：true
  radius_for_roi_object_check_ = comp_config.radius_for_roi_object_check(); // HD Map ROI 融合障碍物校验半径：120m

  // init algorithm plugin
  ACHECK(InitAlgorithmPlugin()) << "Failed to init algorithm plugin.";  // 调用私有初始化方法 InitAlgorithmPlugin，执行更详细的初始化动作
  // 创建消息发布器
  // 用于输出 protobuf 格式的经 HD Map ROI 校验过的有效融合障碍物信息：perception_obstacle.proto
  writer_ = node_->CreateWriter<PerceptionObstacles>(
      comp_config.output_obstacles_channel_name());
  // 用于输出供可视化的高精地图和融合障碍物信息
  inner_writer_ = node_->CreateWriter<SensorFrameMessage>(
      comp_config.output_viz_fused_content_channel_name());
  return true;
}

初始化函数主要调用InitAlgorithmPlugin函数进行融合部分的初始化，主要做了如下三件事：

通过工厂方法模式获取 ObstacleMultiSensorFusion 类的实例指针

多态调用 ObstacleMultiSensorFusion::Init 方法，执行主要的初始化动作

获取 HD Map 的唯一实例，并执行初始化

bool FusionComponent::InitAlgorithmPlugin() {
  fusion::BaseMultiSensorFusion* fusion =
    fusion::BaseMultiSensorFusionRegisterer::GetInstanceByName(fusion_name_); // 通过工厂方法模式获取 ObstacleMultiSensorFusion 类的实例指针
  CHECK_NOTNULL(fusion);
  fusion_.reset(fusion); // 更新 fusion_ 的指针值为 ObstacleMultiSensorFusion 类实例指针
  fusion::ObstacleMultiSensorFusionParam param; // 障碍物多传感器融合参数
  param.main_sensor = fusion_main_sensor_;  // 障碍物多传感器融合参数——主传感器：velodyne128，front_6mm，front_12mm
  param.fusion_method = fusion_method_; // 障碍物多传感器融合参数——融合方法：ProbabilisticFusion
  // 融合的初始化
  ACHECK(fusion_->Init(param)) << "Failed to init ObstacleMultiSensorFusion"; // 多态调用 ObstacleMultiSensorFusion::Init 方法，执行主要的初始化动作

  if (FLAGS_obs_enable_hdmap_input && object_in_roi_check_) {
    hdmap_input_ = map::HDMapInput::Instance(); // 获取 map::HDMapInput 的唯一实例，这是一个单例类
    ACHECK(hdmap_input_->Init()) << "Failed to init hdmap input.";  // 初始化 HD Map
  }
  AINFO << "Init algorithm successfully, onboard fusion: " << fusion_method_;
  return true;
}

上述Init函数多台调用ObstacleMultiSensorFusion::Init方法，最终会调用ProbabilisticFusion::Init方法，执行必要的初始化动作

fusion/lib/fusion_system/probabilistic_fusion.*

中的Init函数

bool ProbabilisticFusion::Init(const FusionInitOptions& init_options) {
  main_sensor_ = init_options.main_sensor; // 主传感器：velodyne128，front_6mm，front_12mm

  BaseInitOptions options;
  if (!GetFusionInitOptions("ProbabilisticFusion", &options)) { // 读取 probabilistic_fusion.config 中 pt 配置文件的相对路径（root_dir）与名称（conf_file）
    return false;
  }

  std::string work_root_config = GetAbsolutePath(
      lib::ConfigManager::Instance()->work_root(), options.root_dir); // 获取 pt 配置文件父路径的绝对路径

  std::string config = GetAbsolutePath(work_root_config, options.conf_file); // 获取 pt 配置文件的绝对路径
  ProbabilisticFusionConfig params; // 实例化概率融合配置参数 protobuf 类：probabilistic_fusion_config.proto

  if (!cyber::common::GetProtoFromFile(config, &params)) { // 将 pt 配置文件中的配置参数读入 protobuf 类：probabilistic_fusion.pt
    AERROR << "Read config failed: " << config;
    return false;
  }
  params_.use_lidar = params.use_lidar();   // 是否使用 lidar：true
  params_.use_radar = params.use_radar();   // 是否使用 radar：true
  params_.use_camera = params.use_camera(); // 是否使用 camera：true
  params_.tracker_method = params.tracker_method(); // 跟踪方法：PbfTracker
  params_.data_association_method = params.data_association_method();   // 数据关联方法：HMAssociation
  params_.gate_keeper_method = params.gate_keeper_method();     // 门限保持方法：PbfGatekeeper
  for (int i = 0; i < params.prohibition_sensors_size(); ++i) { // 被禁止用于创建新航迹的传感器：radar_front
    params_.prohibition_sensors.push_back(params.prohibition_sensors(i));
  }

  // static member initialization from PB config
  Track::SetMaxLidarInvisiblePeriod(params.max_lidar_invisible_period());   // Lidar 历史量测最大不可见时长：0.25s
  Track::SetMaxRadarInvisiblePeriod(params.max_radar_invisible_period());   // Radar 历史量测最大不可见时长：0.50s
  Track::SetMaxCameraInvisiblePeriod(params.max_camera_invisible_period()); // Camera 历史量测最大不可见时长：0.75s
  Sensor::SetMaxCachedFrameNumber(params.max_cached_frame_num());   // 缓存的最大帧数：50

  scenes_.reset(new Scene());   // 初始化用于管理场景的共享智能指针，场景中包含所有的前景航迹与背景航迹
  if (params_.data_association_method == "HMAssociation") {  // 如果使用 HMAssociation 数据关联方法
    matcher_.reset(new HMTrackersObjectsAssociation()); // 多态地初始化用于管理数据关联的独占智能指针
  } else {
    AERROR << "Unknown association method: " << params_.data_association_method;
    return false;
  }
  if (!matcher_->Init()) {  // 多态调用 HMTrackersObjectsAssociation::Init
    AERROR << "Failed to init matcher.";
    return false;
  }

  if (params_.gate_keeper_method == "PbfGatekeeper") {  // 如果使用 PbfGatekeeper 门限保持方法
    gate_keeper_.reset(new PbfGatekeeper());    // 多态地初始化用于管理门限保持的独占智能指针
  } else {
    AERROR << "Unknown gate keeper method: " << params_.gate_keeper_method;
    return false;
  }
  if (!gate_keeper_->Init()) {  // 多态调用 PbfGatekeeper::Init
    AERROR << "Failed to init gatekeeper.";
    return false;
  }
  // DST 类型融合初始化、DST 存在性融合初始化、概率跟踪器参数初始化
  bool state = DstTypeFusion::Init() && DstExistenceFusion::Init() &&
               PbfTracker::InitParams();

  return state;
}

消息回调处理

bool FusionComponent::Proc(const std::shared_ptr<SensorFrameMessage>& message) {
  if (message->process_stage_ == ProcessStage::SENSOR_FUSION) {
    return true;
  }
  std::shared_ptr<PerceptionObstacles> out_message(new (std::nothrow)
                                                       PerceptionObstacles);
  std::shared_ptr<SensorFrameMessage> viz_message(new (std::nothrow)
                                                      SensorFrameMessage);
  bool status = InternalProc(message, out_message, viz_message);    // 调用私有消息处理方法 InternalProc
  if (status) {
    // TODO(conver sensor id)
    if (message->sensor_id_ != fusion_main_sensor_) {
      AINFO << "Fusion receive from " << message->sensor_id_ << "not from "
            << fusion_main_sensor_ << ". Skip send.";
    } else {
      // Send("/apollo/perception/obstacles", out_message);
      writer_->Write(out_message);  // 发送融合障碍物消息
      AINFO << "Send fusion processing output message.";
      // send msg for visualization
      if (FLAGS_obs_enable_visualization) {
        // Send("/apollo/perception/inner/PrefusedObjects", viz_message);
        inner_writer_->Write(viz_message);  // 发送可视化消息
      }
    }
  }
  return status;
}

内部消息处理

FusionComponent::InternalProc 方法主要做了四件事：

调用 ObstacleMultiSensorFusion::Process 方法，处理输入信息，生成融合障碍物信息 fused_objects

使用 HD Map ROI 校验融合障碍物有效性，生成有效融合障碍物信息 valid_objects

生成可视化消息 viz_message

序列化有效融合障碍物信息 valid_objects，生成 protobuf 格式的输出消息 out_message

bool FusionComponent::InternalProc(
    const std::shared_ptr<SensorFrameMessage const>& in_message,
    std::shared_ptr<PerceptionObstacles> out_message,
    std::shared_ptr<SensorFrameMessage> viz_message) {
  {
    std::unique_lock<std::mutex> lock(s_mutex_);
    s_seq_num_++;
  }

  PERF_BLOCK_START();
  const double timestamp = in_message->timestamp_;
  const uint64_t lidar_timestamp = in_message->lidar_timestamp_;
  std::vector<base::ObjectPtr> valid_objects;   // 有效融合障碍物
  if (in_message->error_code_ != apollo::common::ErrorCode::OK) {
    if (!MsgSerializer::SerializeMsg(
            timestamp, lidar_timestamp, in_message->seq_num_, valid_objects,
            in_message->error_code_, out_message.get())) {
      AERROR << "Failed to gen PerceptionObstacles object.";
      return false;
    }
    if (FLAGS_obs_enable_visualization) {
      viz_message->process_stage_ = ProcessStage::SENSOR_FUSION;
      viz_message->error_code_ = in_message->error_code_;
    }
    AERROR << "Fusion receive message with error code, skip it.";
    return true;
  }
  base::FramePtr frame = in_message->frame_;    // 传感器原始障碍物信息
  frame->timestamp = in_message->timestamp_;

  std::vector<base::ObjectPtr> fused_objects;   // 融合障碍物信息
  if (!fusion_->Process(frame, &fused_objects)) {   // 多态调用 ObstacleMultiSensorFusion::Process，处理传感器原始障碍物信息，生成融合障碍物信息
    AERROR << "Failed to call fusion plugin.";
    return false;
  }
  PERF_BLOCK_END_WITH_INDICATOR("fusion_process", in_message->sensor_id_);

  if (in_message->sensor_id_ != fusion_main_sensor_) {
    return true;
  }

  Eigen::Matrix4d sensor2world_pose =
      in_message->frame_->sensor2world_pose.matrix();
  if (object_in_roi_check_ && FLAGS_obs_enable_hdmap_input) {   // 若开启了 HD Map ROI 融合障碍物校验且使能了 HD Map 输入
    // get hdmap
    base::HdmapStructPtr hdmap(new base::HdmapStruct());
    if (hdmap_input_) { // HD Map 加载成功
      base::PointD position;    // 传感器在世界坐标系中的位置
      position.x = sensor2world_pose(0, 3);
      position.y = sensor2world_pose(1, 3);
      position.z = sensor2world_pose(2, 3);
      hdmap_input_->GetRoiHDMapStruct(position, radius_for_roi_object_check_,
                                      hdmap); // 获取传感器指定半径范围（120m）内的 HD Map
      // TODO(use check)
      // ObjectInRoiSlackCheck(hdmap, fused_objects, &valid_objects);   // HD Map ROI 融合障碍物校验
      valid_objects.assign(fused_objects.begin(), fused_objects.end()); // 拷贝经 HD Map ROI 校验过的有效融合障碍物到 valid_objects
    } else { // HD Map 加载失败，直接拷贝融合障碍物到 valid_objects
      valid_objects.assign(fused_objects.begin(), fused_objects.end());
    }
  } else {  // 若未开启 HD Map ROI 融合障碍物校验或未使能 HD Map 输入，直接拷贝融合障碍物到 valid_objects
    valid_objects.assign(fused_objects.begin(), fused_objects.end());
  }
  PERF_BLOCK_END_WITH_INDICATOR("fusion_roi_check", in_message->sensor_id_);

  // produce visualization msg  // 生成可视化消息
  if (FLAGS_obs_enable_visualization) {
    viz_message->timestamp_ = in_message->timestamp_;
    viz_message->seq_num_ = in_message->seq_num_;
    viz_message->frame_ = base::FramePool::Instance().Get();
    viz_message->frame_->sensor2world_pose =
        in_message->frame_->sensor2world_pose;
    viz_message->sensor_id_ = in_message->sensor_id_;
    viz_message->hdmap_ = in_message->hdmap_;   // 可视化 HD Map 信息
    viz_message->process_stage_ = ProcessStage::SENSOR_FUSION;
    viz_message->error_code_ = in_message->error_code_;
    viz_message->frame_->objects = fused_objects;   // 可视化融合障碍物信息
  }
  // produce pb output msg
  apollo::common::ErrorCode error_code = apollo::common::ErrorCode::OK;
  if (!MsgSerializer::SerializeMsg(timestamp, lidar_timestamp,  // 序列化有效融合障碍物信息 valid_objects 到输出消息
                                   in_message->seq_num_, valid_objects,
                                   error_code, out_message.get())) {
    AERROR << "Failed to gen PerceptionObstacles object.";
    return false;
  }
  PERF_BLOCK_END_WITH_INDICATOR("fusion_serialize_message",
                                in_message->sensor_id_);

  const double cur_time = ::apollo::cyber::Clock::NowInSeconds();
  const double latency = (cur_time - timestamp) * 1e3;  // 算法时延
  AINFO << std::setprecision(16) << "FRAME_STATISTICS:Obstacle:End:msg_time["
        << timestamp << "]:cur_time[" << cur_time << "]:cur_latency[" << latency
        << "]:obj_cnt[" << valid_objects.size() << "]";
  AINFO << "publish_number: " << valid_objects.size() << " obj";
  return true;
}

需要指出的是，HD Map ROI 融合障碍物校验方法 ObjectInRoiSlackCheck 在 Apollo 中尚未实现，相应的校验步骤在 FusionComponent::InternalProc 中也被注释掉了，所以最终的有效融合障碍物信息 valid_objects 与融合障碍物信息 fused_objects 是相同的。

融合

融合过程是调用ProbabilisticFusion::Fuse函数

bool ProbabilisticFusion::Fuse(const FusionOptions& options,
                               const base::FrameConstPtr& sensor_frame,
                               std::vector<base::ObjectPtr>* fused_objects) {
  if (fused_objects == nullptr) {
    AERROR << "fusion error: fused_objects is nullptr";
    return false;
  }
  // 传感器数据管理单例类的唯一实例指针，管理各个传感器的历史 10 帧数据
  auto* sensor_data_manager = SensorDataManager::Instance();
  // 1. save frame data // 保存当前数据帧
  {
    std::lock_guard<std::mutex> data_lock(data_mutex_);
    if (!params_.use_lidar && sensor_data_manager->IsLidar(sensor_frame)) {
      return true;  // 若不融合 Lidar 数据且当前帧来自 Lidar，则直接返回
    }
    if (!params_.use_radar && sensor_data_manager->IsRadar(sensor_frame)) {
      return true;  // 若不融合 Radar 数据且当前帧来自 Radar，则直接返回
    }
    if (!params_.use_camera && sensor_data_manager->IsCamera(sensor_frame)) {
      return true;  // 若不融合 Camera 数据且当前帧来自 Camera，则直接返回
    }
    // 当前帧是否来自可发布传感器（与主传感器相同：velodyne128，front_6mm，front_12mm）
    bool is_publish_sensor = this->IsPublishSensor(sensor_frame);

    // 若当前帧来自可发布传感器
    // 缓存当前数据帧到对应传感器的历史数据序列中
    AINFO << "add sensor measurement: " << sensor_frame->sensor_info.name
          << ", obj_cnt : " << sensor_frame->objects.size() << ", "
          << FORMAT_TIMESTAMP(sensor_frame->timestamp);
    sensor_data_manager->AddSensorMeasurements(sensor_frame);

    // 对于来自可发布传感器以外的消息，不执行后续步骤，意味着只有来自可发布传感器的消息才可以触发融合动作
    if (!is_publish_sensor) {
      return true;
    }
  }

  // 2. query related sensor_frames for fusion // 查询每个传感器历史数据中的相关数据帧
  std::lock_guard<std::mutex> fuse_lock(fuse_mutex_);
  double fusion_time = sensor_frame->timestamp; // 当前融合时间
  std::vector<SensorFramePtr> frames;   // 当前融合时间下每个传感器历史数据中的相关数据帧组成的序列
  sensor_data_manager->GetLatestFrames(fusion_time, &frames); // 获取每个传感器历史数据中与当前融合时间最接近（≤）的数据帧组成的序列
  AINFO << "Get " << frames.size() << " related frames for fusion";

  // 3. perform fusion on related frames // 融合所有的相关数据帧
  for (const auto& frame : frames) {
    FuseFrame(frame);   // 融合单帧数据，最终的融合算法入口
  }

  // 4. collect fused objects
  // 从前景航迹和背景航迹中收集可被发布的融合目标
  CollectFusedObjects(fusion_time, fused_objects);
  return true;
}

数据结构

SensorFrameMessage

class SensorFrameMessage {
 public:
  SensorFrameMessage() { type_name_ = "SensorFrameMessage"; }
  ~SensorFrameMessage() = default;
  std::string GetTypeName() { return type_name_; }
  SensorFrameMessage* New() const { return new SensorFrameMessage; }

 public:
  apollo::common::ErrorCode error_code_ = apollo::common::ErrorCode::OK;

  std::string sensor_id_;
  double timestamp_ = 0.0;
  uint64_t lidar_timestamp_ = 0;
  uint32_t seq_num_ = 0;
  std::string type_name_;
  base::HdmapStructConstPtr hdmap_;

  // 融合输入帧结构
  base::FramePtr frame_;

  ProcessStage process_stage_ = ProcessStage::UNKNOWN_STAGE;
};

Frame

struct alignas(16) Frame {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Frame() { sensor2world_pose.setIdentity(); }

  void Reset() {
    timestamp = 0.0;
    objects.clear();
    sensor2world_pose.setIdentity();
    sensor_info.Reset();
    lidar_frame_supplement.Reset();
    radar_frame_supplement.Reset();
    camera_frame_supplement.Reset();
  }
  // @brief sensor information
  SensorInfo sensor_info;

  double timestamp = 0.0;
  // Frame中包含的感知检测的目标
  std::vector<std::shared_ptr<Object>> objects;
  Eigen::Affine3d sensor2world_pose;

  // sensor-specific frame supplements
  LidarFrameSupplement lidar_frame_supplement;
  RadarFrameSupplement radar_frame_supplement;
  CameraFrameSupplement camera_frame_supplement;
  UltrasonicFrameSupplement ultrasonic_frame_supplement;
};

Object

struct alignas(16) Object {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Object();
  std::string ToString() const;
  void Reset();

  // @brief object id per frame, required
  int id = -1;

  // @brief convex hull of the object, required
  PointCloud<PointD> polygon;

  // oriented boundingbox information
  // @brief main direction of the object, required
  Eigen::Vector3f direction = Eigen::Vector3f(1, 0, 0);
  /*@brief the yaw angle, theta = 0.0 <=> direction(1, 0, 0),
    currently roll and pitch are not considered,
    make sure direction and theta are consistent, required
  */
  float theta = 0.0f;
  // @brief theta variance, required
  float theta_variance = 0.0f;
  // @brief center of the boundingbox (cx, cy, cz), required
  Eigen::Vector3d center = Eigen::Vector3d(0, 0, 0);
  // @brief covariance matrix of the center uncertainty, required
  Eigen::Matrix3f center_uncertainty;
  /* @brief size = [length, width, height] of boundingbox
     length is the size of the main direction, required
  */
  Eigen::Vector3f size = Eigen::Vector3f(0, 0, 0);
  // @brief size variance, required
  Eigen::Vector3f size_variance = Eigen::Vector3f(0, 0, 0);
  // @brief anchor point, required
  Eigen::Vector3d anchor_point = Eigen::Vector3d(0, 0, 0);

  // @brief object type, required
  ObjectType type = ObjectType::UNKNOWN;
  // @brief probability for each type, required
  std::vector<float> type_probs;

  // @brief object sub-type, optional
  ObjectSubType sub_type = ObjectSubType::UNKNOWN;
  // @brief probability for each sub-type, optional
  std::vector<float> sub_type_probs;

  // @brief existence confidence, required
  float confidence = 1.0f;

  // tracking information
  // @brief track id, required
  int track_id = -1;
  // @brief velocity of the object, required
  Eigen::Vector3f velocity = Eigen::Vector3f(0, 0, 0);
  // @brief covariance matrix of the velocity uncertainty, required
  Eigen::Matrix3f velocity_uncertainty;
  // @brief if the velocity estimation is converged, true by default
  bool velocity_converged = true;
  // @brief velocity confidence, required
  float velocity_confidence = 1.0f;
  // @brief acceleration of the object, required
  Eigen::Vector3f acceleration = Eigen::Vector3f(0, 0, 0);
  // @brief covariance matrix of the acceleration uncertainty, required
  Eigen::Matrix3f acceleration_uncertainty;

  // @brief age of the tracked object, required
  double tracking_time = 0.0;
  // @brief timestamp of latest measurement, required
  double latest_tracked_time = 0.0;

  // @brief motion state of the tracked object, required
  MotionState motion_state = MotionState::UNKNOWN;
  // // Tailgating (trajectory of objects)
  std::array<Eigen::Vector3d, 100> drops;
  std::size_t drop_num = 0;
  // // CIPV
  bool b_cipv = false;
  // @brief brake light, left-turn light and right-turn light score, optional
  CarLight car_light;
  // @brief sensor-specific object supplements, optional
  LidarObjectSupplement lidar_supplement;
  RadarObjectSupplement radar_supplement;
  CameraObjectSupplement camera_supplement;
  FusionObjectSupplement fusion_supplement;

  // @debug feature to be used for semantic mapping
//  std::shared_ptr<apollo::prediction::Feature> feature;
};

融合基础类

这些基础类主要定义在fusion/base文件夹下

SensorObject类：某单一传感器的一个目标，其实就是包含了某个传感器的相关属性定义和一个base::Object成员变量

SensorFrame类：某传感器的一帧所有目标数据，定义了std::vector成员变量

Sensor类：某传感器历史多帧信息，定义有std::deque类型成员变量，并提供获取最新数据帧的一些接口函数

SensorDataManager类：最终在ProbabilisticFusion类中作为成员变量用来存储<sensor_id, SensorPtr>键值对，即存储多个传感器的多帧历史信息的类，作为ProbabilisticFusion类的数据输入缓存

Track类：单个跟踪目标，在该类中完成大部分跟踪目标状态更新工作

Scene类：前景和背景跟踪目标的管理，定义有std::vector forground_tracks_和std::vector background_tracks_

PbfTracker类：单个跟踪目标，也是主要的融合算法发生过程所在，定义了不同的融合算法类

融合函数

void ProbabilisticFusion::FuseFrame(const SensorFramePtr& frame) {
  AINFO << "Fusing frame: " << frame->GetSensorId()
        << ", foreground_object_number: "
        << frame->GetForegroundObjects().size()
        << ", background_object_number: "
        << frame->GetBackgroundObjects().size()
        << ", timestamp: " << GLOG_TIMESTAMP(frame->GetTimestamp());
  // 融合前景航迹
  this->FuseForegroundTrack(frame);
  // 融合背景航迹
  this->FusebackgroundTrack(frame);
  // 移除丢失的航迹
  this->RemoveLostTrack();
}

融合流程

Image

融合的全流程

Image

前景融合

FuseForegroundTrack

void ProbabilisticFusion::FuseForegroundTrack(const SensorFramePtr& frame) {
  PERF_BLOCK_START();
  std::string indicator = "fusion_" + frame->GetSensorId();

  AssociationOptions options;
  AssociationResult association_result;
  // 关联匹配--HMTrackersObjectsAssociation
  matcher_->Associate(options, frame, scenes_, &association_result);
  PERF_BLOCK_END_WITH_INDICATOR(indicator, "association");

  const std::vector<TrackMeasurmentPair>& assignments =
      association_result.assignments;
  // 更新匹配的航迹
  this->UpdateAssignedTracks(frame, assignments);
  PERF_BLOCK_END_WITH_INDICATOR(indicator, "update_assigned_track");

  const std::vector<size_t>& unassigned_track_inds =
      association_result.unassigned_tracks;
  // 更新未匹配的航迹
  this->UpdateUnassignedTracks(frame, unassigned_track_inds);
  PERF_BLOCK_END_WITH_INDICATOR(indicator, "update_unassigned_track");

  const std::vector<size_t>& unassigned_obj_inds =
      association_result.unassigned_measurements;
  // 未匹配上的量测新建航迹
  this->CreateNewTracks(frame, unassigned_obj_inds);
  PERF_BLOCK_END_WITH_INDICATOR(indicator, "create_track");
}

UpdateAssignedTracks

匹配上的结果做更新，使用观测更新tracker，tracker类型是pbf_tracker

void ProbabilisticFusion::UpdateAssignedTracks(
    const SensorFramePtr& frame,
    const std::vector<TrackMeasurmentPair>& assignments) {
  // Attention: match_distance should be used
  // in ExistenceFusion to calculate existence score.
  // We set match_distance to zero if track and object are matched,
  // which only has a small difference compared with actural match_distance
  TrackerOptions options;
  options.match_distance = 0;
  for (size_t i = 0; i < assignments.size(); ++i) {
    size_t track_ind = assignments[i].first;
    size_t obj_ind = assignments[i].second;
    trackers_[track_ind]->UpdateWithMeasurement(
        options, frame->GetForegroundObjects()[obj_ind], frame->GetTimestamp());
  }
}

tracker更新的函数中会更新四个部分，existence、motion、shape、type和tracker的信息，前四个fusion的配置参数在modules/perception/proto/pbf_tracker_config.proto

void PbfTracker::UpdateWithMeasurement(const TrackerOptions& options,
                                       const SensorObjectPtr measurement,
                                       double target_timestamp) {
  std::string sensor_id = measurement->GetSensorId();
  ADEBUG << "fusion_updating..." << track_->GetTrackId() << " with "
         << sensor_id << "..." << measurement->GetBaseObject()->track_id << "@"
         << FORMAT_TIMESTAMP(measurement->GetTimestamp());
  existence_fusion_->UpdateWithMeasurement(measurement, target_timestamp,
                                           options.match_distance);
  motion_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
  shape_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
  type_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
  track_->UpdateWithSensorObject(measurement);
}

UpdateUnassignedTracks

同上UpdateAssignedTracks一样，对没有匹配到观测的tracker，更新同样的参数

void ProbabilisticFusion::UpdateUnassignedTracks(
    const SensorFramePtr& frame,
    const std::vector<size_t>& unassigned_track_inds) {
  // Attention: match_distance(min_match_distance) should be used
  // in ExistenceFusion to calculate toic score.
  // Due to it hasn't been used(mainly for front radar object pub in
  // gatekeeper),
  // we do not set match_distance temporarily.
  TrackerOptions options;
  options.match_distance = 0;
  std::string sensor_id = frame->GetSensorId();
  for (size_t i = 0; i < unassigned_track_inds.size(); ++i) {
    size_t track_ind = unassigned_track_inds[i];
    trackers_[track_ind]->UpdateWithoutMeasurement(
        options, sensor_id, frame->GetTimestamp(), frame->GetTimestamp());
  }
}

void PbfTracker::UpdateWithoutMeasurement(const TrackerOptions& options,
                                          const std::string& sensor_id,
                                          double measurement_timestamp,
                                          double target_timestamp) {
  existence_fusion_->UpdateWithoutMeasurement(sensor_id, measurement_timestamp,
                                              target_timestamp,
                                              options.match_distance);
  motion_fusion_->UpdateWithoutMeasurement(sensor_id, measurement_timestamp,
                                           target_timestamp);
  shape_fusion_->UpdateWithoutMeasurement(sensor_id, measurement_timestamp,
                                          target_timestamp);
  type_fusion_->UpdateWithoutMeasurement(sensor_id, measurement_timestamp,
                                         target_timestamp,
                                         options.match_distance);
  track_->UpdateWithoutSensorObject(sensor_id, measurement_timestamp);
}

CreateNewTracks

对没有匹配到tracker的观测object，新建航迹tracker

void ProbabilisticFusion::CreateNewTracks(
    const SensorFramePtr& frame,
    const std::vector<size_t>& unassigned_obj_inds) {
  for (size_t i = 0; i < unassigned_obj_inds.size(); ++i) {
    size_t obj_ind = unassigned_obj_inds[i];

    bool prohibition_sensor_flag = false;
    std::for_each(params_.prohibition_sensors.begin(),
                  params_.prohibition_sensors.end(),
                  [&](std::string sensor_name) {
                    if (sensor_name == frame->GetSensorId())
                      prohibition_sensor_flag = true;
                  });
    if (prohibition_sensor_flag) {
      continue;
    }
    TrackPtr track = TrackPool::Instance().Get();
    track->Initialize(frame->GetForegroundObjects()[obj_ind]);
    scenes_->AddForegroundTrack(track);

    ADEBUG << "object id: "
           << frame->GetForegroundObjects()[obj_ind]->GetBaseObject()->track_id
           << ", create new track: " << track->GetTrackId();

    if (params_.tracker_method == "PbfTracker") {
      std::shared_ptr<BaseTracker> tracker;
      tracker.reset(new PbfTracker());
      tracker->Init(track, frame->GetForegroundObjects()[obj_ind]);
      trackers_.emplace_back(tracker);
    }
  }
}

背景融合

FusebackgroundTrack

和FuseForegroundTrack函数的过程类似，处理函数也是，同样的四个步骤，关联->更新匹配的航迹->更新未匹配的航迹->新建航迹

void ProbabilisticFusion::FusebackgroundTrack(const SensorFramePtr& frame) {
  // 1. association // 数据关联
  size_t track_size = scenes_->GetBackgroundTracks().size(); // 背景航迹数量
  size_t obj_size = frame->GetBackgroundObjects().size();   // 当前帧的背景目标数量
  std::map<int, size_t> local_id_2_track_ind_map;           // 背景航迹融合目标 track id 与航迹索引间的映射关系
  std::vector<bool> track_tag(track_size, false);           // 航迹匹配结果标签列表：0 - 不存在与航迹匹配的量测，1 - 存在与航迹匹配的量测
  std::vector<bool> object_tag(obj_size, false);            // 量测匹配结果标签列表：0 - 不存在与量测匹配的航迹，1 - 存在与量测匹配的航迹
  std::vector<TrackMeasurmentPair> assignments;             // 航迹与量测的配对关系列表

  std::vector<TrackPtr>& background_tracks = scenes_->GetBackgroundTracks();    // 所有的背景航迹
  for (size_t i = 0; i < track_size; ++i) { // 对每个背景航迹
    const FusedObjectPtr& obj = background_tracks[i]->GetFusedObject(); // 背景航迹中的融合目标
    int local_id = obj->GetBaseObject()->track_id;  // 背景航迹中融合目标的 track id
    local_id_2_track_ind_map[local_id] = i;     // 建立背景航迹融合目标 track id 与航迹索引间的映射关系
  }

  std::vector<SensorObjectPtr>& frame_objs = frame->GetBackgroundObjects(); // 当前帧的所有背景目标
  for (size_t i = 0; i < obj_size; ++i) {   // 对每个背景目标
    int local_id = frame_objs[i]->GetBaseObject()->track_id;    // 背景目标的 track id
    const auto& it = local_id_2_track_ind_map.find(local_id);   // 查找是否存在与当前背景目标 track id 相同的背景航迹融合目标
    if (it != local_id_2_track_ind_map.end()) { // 存在与当前背景目标 track id 相同的背景航迹融合目标
      size_t track_ind = it->second;    // 与当前背景目标对应的背景航迹索引
      assignments.push_back(std::make_pair(track_ind, i));  // 构造航迹与量测的配对关系
      track_tag[track_ind] = true;      // 将航迹匹配结果标签列表中的对应元素置 true
      object_tag[i] = true;             // 将量测匹配结果标签列表中的对应元素置 true
      continue;
    }
  }

  // 2. update assigned track   // 更新匹配上的航迹
  for (size_t i = 0; i < assignments.size(); ++i) { // 对航迹与量测配对关系列表中的每一组配对关系
    int track_ind = static_cast<int>(assignments[i].first); // 背景航迹索引
    int obj_ind = static_cast<int>(assignments[i].second);  // 与背景航迹关联上的当前帧背景目标索引
    background_tracks[track_ind]->UpdateWithSensorObject(frame_objs[obj_ind]);  // 使用背景目标更新与其关联上的背景航迹
  }

  // 3. update unassigned track // 更新未被匹配上的航迹
  std::string sensor_id = frame->GetSensorId(); // 当前量测帧所属的传感器 id
  for (size_t i = 0; i < track_tag.size(); ++i) {   // 对航迹匹配结果标签列表中的每个元素
    if (!track_tag[i]) {    // 如果背景航迹不存在与之匹配的量测
      background_tracks[i]->UpdateWithoutSensorObject(sensor_id,    // 更新该未被匹配上的背景航迹
                                                      frame->GetTimestamp());
    }
  }

  // 4. create new track     // 创建新航迹
  for (size_t i = 0; i < object_tag.size(); ++i) {  // 对量测匹配结果标签列表中的每个元素
    if (!object_tag[i]) {   // 如果量测不存在与之匹配的背景航迹
      TrackPtr track = TrackPool::Instance().Get(); // 从航迹池中获取一个未经初始化的航迹
      track->Initialize(frame->GetBackgroundObjects()[i], true);    // 使用未被匹配上的量测目标初始化背景航迹
      scenes_->AddBackgroundTrack(track);   // 将新的背景航迹添加到场景的背景航迹列表中
    }
  }
}

RemoveLostTrack

void ProbabilisticFusion::RemoveLostTrack() {
  // need to remove tracker at the same time
  size_t foreground_track_count = 0;
  std::vector<TrackPtr>& foreground_tracks = scenes_->GetForegroundTracks();
  for (size_t i = 0; i < foreground_tracks.size(); ++i) {
    if (foreground_tracks[i]->IsAlive()) {
      if (i != foreground_track_count) {
        foreground_tracks[foreground_track_count] = foreground_tracks[i];
        trackers_[foreground_track_count] = trackers_[i];
      }
      foreground_track_count++;
    }
  }
  AINFO << "Remove " << foreground_tracks.size() - foreground_track_count
        << " foreground tracks. " << foreground_track_count << " tracks left.";
  foreground_tracks.resize(foreground_track_count);
  trackers_.resize(foreground_track_count);

  // only need to remove frame track
  size_t background_track_count = 0;
  std::vector<TrackPtr>& background_tracks = scenes_->GetBackgroundTracks();
  for (size_t i = 0; i < background_tracks.size(); ++i) {
    if (background_tracks[i]->IsAlive()) {
      if (i != background_track_count) {
        background_tracks[background_track_count] = background_tracks[i];
      }
      background_track_count++;
    }
  }
  AINFO << "Remove " << background_tracks.size() - background_track_count
        << " background tracks";
  background_tracks.resize(background_track_count);
}

融合结果后处理

执行完所有相关数据帧的融合动作后，通过在 ProbabilisticFusion::CollectFusedObjects 方法中调用 PbfGatekeeper::AbleToPublish 方法判断相应融合航迹是否满足可发布逻辑:

PbfGatekeeper 配置参数

各传感器与航迹关联过的最新历史量测的可见性

航迹融合目标类别

航迹融合目标时间戳对应的本地时间

与航迹关联过的某种传感器的最新历史量测的距离、速度、置信度、所属的子传感器类别

航迹被跟踪上的次数

void ProbabilisticFusion::CollectFusedObjects(
    double timestamp, std::vector<base::ObjectPtr>* fused_objects) {
  fused_objects->clear();

  size_t fg_obj_num = 0;
  const std::vector<TrackPtr>& foreground_tracks =
      scenes_->GetForegroundTracks();
  for (size_t i = 0; i < foreground_tracks.size(); ++i) {
    if (gate_keeper_->AbleToPublish(foreground_tracks[i])) {
      this->CollectObjectsByTrack(timestamp, foreground_tracks[i],
                                  fused_objects);
      ++fg_obj_num;
    }
  }

  size_t bg_obj_num = 0;
  const std::vector<TrackPtr>& background_tracks =
      scenes_->GetBackgroundTracks();
  for (size_t i = 0; i < background_tracks.size(); ++i) {
    if (gate_keeper_->AbleToPublish(background_tracks[i])) {
      this->CollectObjectsByTrack(timestamp, background_tracks[i],
                                  fused_objects);
      ++bg_obj_num;
    }
  }

  AINFO << "collect objects : fg_obj_cnt = " << fg_obj_num
        << ", bg_obj_cnt = " << bg_obj_num
        << ", timestamp = " << FORMAT_TIMESTAMP(timestamp);
}

关联配算法

位于FuseForegroundTrack函数内部

  // 关联匹配--HMTrackersObjectsAssociation
  AssociationOptions options;
  AssociationResult association_result;
  matcher_->Associate(options, frame, scenes_, &association_result);

关联匹配的实现在Apollo/modules/perception/fusion/lib/data_association文件夹下

其中最主要是HMTrackersObjectsAssociation类的实现：

IdAssign，首先直接匹配航迹中之前就关联过的观测

计算关联矩阵，关联值取的是最小欧氏距离

最小化匹配，先计算连通子图，再对连通子图计算匈牙利匹配

PostIdAssign，Apollo特性，再做一遍IdAssign

生成最终的未匹配的航迹和未匹配的观测

保存航迹track和观测meas的关联值（最小距离）