自动驾驶Apollo6.0源码阅读－感知篇：感知融合前景融合

本文链接：https://blog.csdn.net/ControlLearner/article/details/122973738

本文深入解析Apollo6.0自动驾驶系统的感知融合过程，重点介绍了ProbabilisticFusion模块的数据关联、已匹配和未匹配航迹的更新，以及新航迹创建的细节。涉及到的关键技术包括证据推理（DS理论）、卡尔曼滤波以及匈牙利匹配算法在数据融合和跟踪中的应用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

自动驾驶Apollo6.0源码阅读－感知篇：感知融合前景融合

ProbabilisticFusion::FuseForegroundTrack
参考资料

ProbabilisticFusion::FuseForegroundTrack

在这里插入图片描述

matcher_->Associate

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

this->UpdateAssignedTracks

匹配上的结果做更新，使用探测更新tracker，tracker类是pdf_tracker

void ProbabilisticFusion::UpdateAssignedTracks(
    const SensorFramePtr& frame,
    const std::vector<TrackMeasurmentPair>& assignments) {
  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;
    //pbf_tracker,观测更新tracker
    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，就是init()中的默认值。

// 观测更新tracker
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());
  // options.match_distance = 0
  // @liuxinyu: DstExitenceFusion 
  // 证据推理（DS theory）更新Tracker的存在性
  existence_fusion_->UpdateWithMeasurement(measurement,target_timestamp,options.match_distance);
  // NOTE(@liuxinyu):KalmanMotionFusion
  // 鲁棒卡尔曼滤波更新tracker的运动属性（非标准CA运动模型）
  motion_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
  // NOTE(@liuxinyu): PbfShapeFusion
  // 更新tracker的形状
  shape_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
   // @liuxinyu: DstTypeFusion
  // 证据推理（DS theory）更新Tracker的属性
  type_fusion_->UpdateWithMeasurement(measurement, target_timestamp);

  track_->UpdateWithSensorObject(measurement);
}

shape_fusion_：

// @liuxinyu: 
// 1.优先使用Lidar的形状，最近的历史关联的观测中若有lidar,radar和camera观测都不会更新
// 2.其次优先camera的形状，间隔更新时间比较小的话，radar观测仅仅更新中心，形状用历史camera更新
// 3.最后最近的历史观测中lidar和camera都没有的话，才使用radar的观测更新

主要是DS theory和更新tracker的属性。后续展开。

this->UpdateUnassignedTracks

同上UpdateAssignedTracks一样，对未匹配探测的航迹tracker，更新同样的参数：

// 没有匹配探测时更新tracker
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);
}

this->CreateNewTracks

对没有匹配到tracker的探测object，新建航迹track，主要是最后的两个Init函数。可以详细看下Track和BaseTracker两个类。

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;
    // @liuxinyu: 如果是prohibition_sensor，则跳过
    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;
    }
    // NOTE(@liuxinyu): 新建track,并初始化，添加到scenes_中
    TrackPtr track = TrackPool::Instance().Get();
    track->Initialize(frame->GetForegroundObjects()[obj_ind]);
    scenes_->AddForegroundTrack(track);

    // NOTE(@liuxinyu): pbfTracker：新建tracker，track初始化tracker，tracker插入到航迹集合trackers_中
    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);
    }
  }
}