自动驾驶Apollo6.0源码阅读－感知篇：感知融合-数据关联

入口

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

  AssociationOptions options;
  AssociationResult association_result;
  // @liuxinyu: 目標之間數據關聯 - 經典的算法有NN，JPDA，HM等
  // Apollo6.0用的HM（匈牙利算法）
  // modules/perception/fusion/lib/data_association/hm_data_association/hm_tracks_objects_match.cc
  matcher_->Associate(options, frame, scenes_, &association_result);

HMTrackersObjectsAssociation::Associate

该类中最主要的函数，具体步骤：
1. IdAssign
2. compute Association Distance Mat
3. Minimize Assignment
4. PostIdAssign
5. generate Unassigned Data
6. Compute Distance

"Talk is cheap, show me the code"

// @liuxinyu: 数据关联最核心的问题其实是距离的计算，合适的距离决定了数据关联的质量。
// 距离不单指物理上的距离，也可以包括用量化的数值对一个目标在类别、外形、颜色的差异化表达。
bool HMTrackersObjectsAssociation::Associate(
    const AssociationOptions& options, SensorFramePtr sensor_measurements,
    ScenePtr scene, AssociationResult* association_result) {
   
   
  const std::vector<SensorObjectPtr>& sensor_objects =
      sensor_measurements->GetForegroundObjects();
  const std::vector<TrackPtr>& fusion_tracks = scene->GetForegroundTracks();
  std::vector<std::vector<double>> association_mat;
  // @liuxinyu: 判斷FusionTracks（航迹）或者SensorObjects（观测）是否为空;
  // 如果是则返回true，重置未匹配到的tracks数量，重置未匹配到的sensor_obj数量
  if (fusion_tracks.empty() || sensor_objects.empty()) {
   
   
    association_result->unassigned_tracks.resize(fusion_tracks.size());
    association_result->unassigned_measurements.resize(sensor_objects.size());
    // 初始化
    std::iota(association_result->unassigned_tracks.begin(),
              association_result->unassigned_tracks.end(), 0);
    std::iota(association_result->unassigned_measurements.begin(),
              association_result->unassigned_measurements.end(), 0);
    return true;
  }
  // sensor ID
  std::string measurement_sensor_id = sensor_objects[0]->GetSensorId();
  // sensor 时间戳
  double measurement_timestamp = sensor_objects[0]->GetTimestamp();
  // 重置之前的计算距离
  track_object_distance_.ResetProjectionCache(measurement_sensor_id,
                                              measurement_timestamp);
  // 前雷达不进行IdAssign
  bool do_nothing = (sensor_objects[0]->GetSensorId() == "radar_front");
  // @liuxinyu: 1.ID匹配
  // 通过ID进行匹配，track之前匹配过的障碍物ID，得到association_result三个结果
  IdAssign(fusion_tracks, sensor_objects, &association_result->assignments,
           &association_result->unassigned_tracks,
           &association_result->unassigned_measurements, do_nothing, false);
  //  4*4 齐次矩阵变换
  Eigen::Affine3d pose;
  // sensor2world的转换
  sensor_measurements->GetPose(&pose);
  // 平移向量
  Eigen::Vector3d ref_point = pose.translation();

  ADEBUG << "association_measurement_timestamp@" << measurement_timestamp;
  // @liuxinyu: 2.计算距离关联矩阵association_mat[num_track,num_meas],取最小欧式距离
  ComputeAssociationDistanceMat(fusion_tracks, sensor_objects, ref_point,
                                association_result->unassigned_tracks,
                                association_result->unassigned_measurements,
                                &association_mat);

  int num_track = static_cast<int>(fusion_tracks.size());
  int num_measurement = static_cast<int>(sensor_objects.size());
  // 初始化置0,航迹track到观测measurement的距离
  association_result->track2measurements_dist.assign(num_track, 0);
  // 初始化置0,观测measurement到航迹track的距离
  association_result->measurement2track_dist.assign(num_measurement, 0);

  // g:global 1:local 两个转换便于查询观测和航迹的index
  // track_ind_l2g[i] =  track_index, track_ind_g2l[track_index] = i;
  std::vector<int> track_ind_g2l;
  track_ind_g2l.resize(num_track, -1);
  for (size_t i = 0; i < association_result->unassigned_tracks.size(); i++) {
   
   
    track_ind_g2l[association_result->unassigned_tracks[i]] =
        static_cast<int>(i);
  }

  // measurement_ind_l2g[i] = measurement_index, measurement_ind_g2l[measurement_index]=i
  std::vector<int> measurement_ind_g2l;
  measurement_ind_g2l.resize(num_measurement, -1);
  // measurement_ind_l2g
  std::vector<size_t> measurement_ind_l2g =
      association_result->unassigned_measurements;
  for (size_t i = 0; i < association_result->unassigned_measurements.size();
       i++) {
   
   
    measurement_ind_g2l[association_result->unassigned_measurements[i]] =
        static_cast<int>(i);
  }
  //  track_ind_l2g
  std::vector<size_t> track_ind_l2g = association_result->unassigned_tracks;

  // 校验，未分配航迹或未分配观测为空？则结束。
  if (association_result->unassigned_tracks.empty() ||
      association_result->unassigned_measurements.empty()) {
   
   
    return true;
  }
  // @liuxinyu: 3.最小化匹配（匈牙利分配），先计算连通子图，再对连通子图计算匈牙利匹配
  // 关联矩阵对应的是i,通过l2g转换为index,插入到对应的三个vector中
  bool state = MinimizeAssignment(
      association_mat, track_ind_l2g, measurement_ind_l2g,
      &association_result->assignments, &association_result->unassigned_tracks,
      &association_result->unassigned_measurements);

  // start do post assign
  std::vector<TrackMeasurmentPair> post_assignments;
  // @liuxinyu: 4.后期优化ID匹配关系
  // 再做一遍IdAssign,未分配的航迹（仅有相机观测生成的）和未分配的观测
  PostIdAssign(fusion_tracks, sensor_objects,
               association_result->unassigned_tracks,
               association_result->unassigned_measurements, &post_assignments);
  association_result->assignments.insert(association_result->assignments.end(),
                                         post_assignments.begin(),
                                         post_assignments.end());
  // @liuxinyu: 5.排除已匹配的，生成未匹配的tracks数据和未匹配的measuremnets
  GenerateUnassignedData(fusion_tracks.size(), sensor_objects.size(),
                         association_result->assignments,
                         &association_result->unassigned_tracks,