Apollo | perception | fusion

/**
 * @description: 对所有的相关数据帧,循环调用 ProbabilisticFusion::FuseFrame 方法进行融合，
 * 这是最终的融合算法入口。可以看到，对于某一帧相关数据帧，执行三个动作：
 * 
 * 1.前景航迹融合 FuseForegroundTrack
 *    matcher_->Associate
 *    UpdateAssignedTracks
 *    UpdateUnassignedTracks
 *    CreateNewTracks
 * 2.背景航迹融合 FusebackgroundTrack
 * 3.移除丢失航迹 RemoveLostTrack
 * 
 */
void ProbabilisticFusion::FuseFrame(const SensorFramePtr& frame) {
  AINFO << "Fusing frame: " << frame->GetSensorId()
        << ", foreground_object_number: "
        << frame->GetForegroundObjects().size()
        << ", background_object_number: "
        << frame->GetBackgroundObjects().size()
        << ", timestamp: " << FORMAT_TIMESTAMP(frame->GetTimestamp());
  this->FuseForegroundTrack(frame);  // 前景航迹融合
  this->FusebackgroundTrack(frame);  // 背景航迹融合
  this->RemoveLostTrack();           // 移除丢失航迹
}

void ProbabilisticFusion::FuseForegroundTrack(const SensorFramePtr& frame) {
  PERF_BLOCK_START();
  std::string indicator = "fusion_" + frame->GetSensorId();
  // 关联匹配--HMTrackersObjectsAssociation
  AssociationOptions options;
  AssociationResult association_result;
  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");
}