Apollo感知融合（部分）源码解读

原创

已于 2025-01-24 10:43:25 修改 · 1k 阅读

30 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #开源

于 2025-01-24 10:41:02 首次发布

文章合集地址：Github - BlogY

1 感知模块架构

激光雷达（lidar）、毫米波雷达（radar）、相机（camera）均有各自的检测、过滤和跟踪模块。

各传感器处理结果统一交给融合模块（multi_sensor_fusion）处理，再进行后续预测和规划。

请添加图片描述

2 融合子模块架构

融合的主流程：

GetLatestFrames：获取某传感器所有待融合数据，按时间排序。
FuseFrame：逐帧融合。分为前景轨迹融合、背景轨迹融合。两种轨迹融合的流程均为：
- 数据关联
- 关联轨迹更新
- 未关联上的轨迹更新
- 轨迹初始化
丢失轨迹删除，结果整理等

请添加图片描述

3 入口

文件：modules\perception\multi_sensor_fusion\multi_sensor_fusion_component.cc

类：MultiSensorFusionComponent

Apollo 框架中，每个功能组件都对应一个继承自 Component 类的具体组件类，具体组件类中都包含 public 的 Init 方法和 Proc 方法，Init 方法实现了组件的初始化，Proc 方法实现了组件的具体算法流程。

对外接口：Init、Proc

对内接口：InitAlgorithmPlugin、InternalProc**（*算法入口）**

4 数据结构

Object (1个目标) → SensorObject (指定传感器) → Track (目标跟踪，加时间维度) → Scene (所有Track集合)

SensorObject → Frame (一帧目标的集合) → SensorFrame (指定传感器) → Sensor (传感器，管理SensorFrame)→ SensorDataManager (管理Sensor)

Object的属性

id
ORB_info（theta，center，size，anchor_point，type，confidence）：ORB信息
Tracking_info（track_id，velocity，acceleration，age，latest_timestamp）：跟踪信息
CIPV（light）: 路径最近车辆

Frame的属性

sensor_info：传感器信息
timestamp：时间戳
objects：目标集合
sensor2world_pose：位姿

Track的属性


is_background_：是否背景
lidar_objects_, ...：sensor_id和目标的map
fused_object_：融合目标，保留最后融合进去的目标信息
is_alive_: 是否活跃
tracked_times...：跟踪计数等

5 MultiSensorFusionComponent::InternalProc

1 融合

base::FramePtr frame = in_message->frame_;
frame->timestamp = in_message->timestamp_;

std::vector<base::ObjectPtr> fused_objects;
if (!fusion_->Fuse(frame, &fused_objects)) {
   
    // 融合核心函数
  AERROR << "Failed to call fusion plugin.";
  return false;
}

2 高精地图校验

3 结果发布

6 ProbabilisticFusion::Fuse

6.1 保存frame data

sensor_data_manager->AddSensorMeasurements(sensor_frame);

SensorDataManager::AddSensorMeasurements

sensor_ptr->AddFrame(frame_ptr);

Sensor::AddFrame

SensorFramePtr frame(new SensorFrame(frame_ptr));

SensorFrame::Initialize

// 根据lidar_supplement提供的信息分成前景信息和背景信息
for (const auto& base_obj : base_objects) {
   
   
  SensorObjectPtr obj(new SensorObject(base_obj, header_));
  if (base_obj->lidar_supplement.is_background) {
   
   
    background_objects_.emplace_back(obj);
  } else {
   
   
    foreground_objects_.emplace_back(obj);
  }
}

6.2 索引相关数据帧

不是每收到一帧数据就融合，而是攒一批数据每隔一段时间融合。

sensor_data_manager->GetLatestFrames(fusion_time, &frames);

SensorDataManager::GetLatestFrame

// 获取各传感器未融合的数据帧
// sensors_：class SensorDataManager
// std::unordered_map<std::string, SensorPtr> sensors_;
for (auto it = sensors_.begin(); it != sensors_.end(); ++it) {
   
   
    SensorFramePtr frame = it->second->QueryLatestFrame(timestamp);
    if (frame != nullptr) {
   
   
      frames->push_back(frame);
    }
  }
...
std::sort(frames->begin(), frames->end(),
          [](const SensorFramePtr& p1, const SensorFramePtr& p2) {
   
   
            return p1->GetTimestamp() < p2->GetTimestamp();
          });

Sensor::QueryLatestFrames


// 上一次融合的时间戳和当前帧时间戳之间的数据帧
for (size_t i = 0; i < frames_.size(); ++i) {
   
   
  if (frames_[i]->GetTimestamp() > latest_query_timestamp_ &&
      frames_[i]->GetTimestamp() <= timestamp) {
   
   
    frames->push_back(frames_[i]);
  }
}

6.3 执行融合

// 融合排好序的单帧数据
for (const auto& frame : frames) {
   
   
  FuseFrame(frame);
}

ProbabilisticFusion::FuseFrame

  FuseForegroundTrack(frame); // 前景数据融合
  FusebackgroundTrack(frame); // 背景数据融合
  RemoveLostTrack(); // 移除丢失轨迹

6.3.1 背景融合

ProbabilisticFusion::FusebackgroundTrack

6.3.1.1 关联

简单地根据track_id是否相等判断关联，等式两边分别为：

scenes_->GetBackgroundTracks()[i]->GetFusedObject()->GetBaseObject()->track_id

frame_objs[i]->GetBaseObject()->track_id

猜想：背景目标只从激光雷达中获取，因此track_id是一致的，都是激光雷达跟踪的id。

6.3.1.2 更新匹配轨迹

向轨迹中添加最近观测，并删除长期不可见轨迹。

background_tracks[track_ind]->UpdateWithSensorObject(frame_objs[obj_ind]);

Track::UpdateWithSensorObject

void Track::UpdateWithSensorObject(const SensorObjectPtr& obj) {
   
   
  // objects：与当前航迹匹配过的、和当前观测传感器类型一致的历史观测
  // typedef std::map<std::string, SensorObjectPtr> SensorId2ObjectMap;
  std::string sensor_id = obj->GetSensorId();
  SensorId2ObjectMap* objects = nullptr;
  if (IsLidar(obj)) {
   
   
    objects = &lidar_objects_;
  } else if (IsRadar(obj)) {
   
   
    objects = &radar_objects_;
  } else if (IsCamera(obj)) {
   
   
    objects = &camera_objects_;
  } else {
   
   
    return;
  }
  // 更新对应传感器类型的最新历史观测为本轮观测
  UpdateSensorObject(objects, obj);
  double time_diff = obj->GetTimestamp() - fused_object_->GetTimestamp();
  tracking_period_ += time_diff;
	
	// 删除各传感器超过最大不可见时长的全部历史观测
  UpdateSensorObjectWithMeasurement(&lidar_objects_, sensor_id,
                                    obj->GetTimestamp(),
                                    s_max_lidar_invisible_period_);
  UpdateSensorObjectWithMeasurement(&radar_objects_, sensor_id,
                                    obj->GetTimestamp(),
                                    s_max_radar_invisible_period_);
  UpdateSensorObjectWithMeasurement(&camera_objects_, sensor_id,
                                    obj->GetTimestamp(),
                                    s_max_camera_invisible_period_);

	// 更新融合对象
  if (is_background_) {
   
   
    return UpdateWithSensorObjectForBackground(obj);
  }

  fused_object_->GetBaseObject()->latest_tracked_time = obj->GetTimestamp();
  UpdateSupplementState(obj);
  UpdateUnfusedState(obj);
  is_alive_ = true;
}

Track::UpdateSensorObject

void Track::UpdateSensorObject(SensorId2ObjectMap* objects,
                               const SensorObjectPtr& obj) {
   
   
  std::string sensor_id = obj->GetSensorId();
  auto it = objects->find(sensor_id);
  if (it == objects->end()) {
   
   
    (*objects)[sensor_id] = obj; // 没关联过的传感器首次赋值
  } else {
   
   
    it->second = obj; // 关联过的传感器，修改value值
  }
}

Track::UpdateSensorObjectWithMeasurement

void Track::UpdateSensorObjectWithMeasurement(SensorId2ObjectMap* objects,
                                              const std::string& sensor_id,
                                              double measurement_timestamp,
                                              double max_invisible_period) {
   
   
  for (auto it = objects->begin(); it != objects->