Apollo perception源码阅读 | fusion
Apollo perception源码阅读 | fusion
个人辛苦创作，请勿直接搬运!!!
本文为Apollo感知融合源码阅读笔记，建议参照Apollo6.0源码阅读本文，水平有限，有错误的地方希望大佬多加指正!
个人代码注释链接： leox24 / perception_learn
perception代码结构
有必要新增一下关于感知融合模块的代码结构说明。因为有时候不知道哪里是入口，不知道从哪里看起。
Apollo/modules/perception
perception
├── base // 一些基础类
├── BUILD
├── camera //相机检测
├── common // 公用的类
├── data // 一些相机的内外参
├── fusion // 障碍物融合
├── inference //一些接口
├── lib
├── lidar // 激光雷达检测
├── map // 高精度地图
├── onboard // 组件文件结构和组件类的实现
├── Perception_README_3_5.md
├── production // 组件配置文件和启动文件
├── proto // 一些protobuf的消息结构
├── radar // 毫米波雷达
├── README.md
└── tool // 一些工具方法
因为主要是看感知融合的算法实现，也就是camera, lidar, radar, fusion文件夹，另外onboard, production两个文件夹就是入口，从这里出发看代码，流程会比较清楚一点。
顺序应该是：production/*.launch -> production/*.dag -> onboard/component/*componet.cc -> fusion/app 或者 lidar/app等等
- 1 production
Apollo/modules/perception/production
Launch文件定义了模块的启动，dag定义了模块的依赖关系。launch文件中包含了dag文件，每个dag文件中又包含了很多个子组件（component）、相应的动态库和配置文件，每个子组件对应的是相应的类实现。
如launch/perception_all.launch包含了如下：
- dag_streaming_perception.dag
- DetectionComponent //dag文件中包含的组件，对应onboard文件夹中的component文件夹
- RecognitionComponent
- RadarDetectionComponent
- FusionComponent
- V2XFusionComponent
- dag_streaming_perception_camera.dag
- ...
- dag_streaming_perception_trafficlights.dag
- ...
- dag_motion_service.dag
- ...
- 2 onboard
文件夹结构如下：
Apollo/modules/perception/onboard
├── onboard
│ ├── common_flags
│ ├── component //感知融合的组件实现
│ ├── inner_component_messages
│ ├── msg_buffer
│ ├── msg_serializer
│ ├── proto
│ └── transform_wrapper
主要是component文件夹：
Apollo/modules/perception/onboard/component/
├── BUILD //定义了 perception 中所有的 component 如 camera,radar,lidar 等的信息
├── camera_perception_viz_message.cc
├── camera_perception_viz_message.h
├── detection_component.cc // 激光雷达检测
├── detection_component.h
├── fusion_camera_detection_component.cc
├── fusion_camera_detection_component.h
├── fusion_component.cc //融合
├── fusion_component.h
├── lane_detection_component.cc // 车道线
├── lane_detection_component.h
├── lidar_inner_component_messages.h
├── lidar_output_component.cc
├── lidar_output_component.h
├── radar_detection_component.cc // 毫米波雷达
├── radar_detection_component.h
├── recognition_component.cc
├── recognition_component.h
├── segmentation_component.cc
├── segmentation_component.h
├── trafficlights_perception_component.cc // 交通灯
└── trafficlights_perception_component.h
其中对应了每个组件的执行实现，包括初始化配置参数Init()和处理函数Proc()，当然这些组件只是个入口，具体实现还是要看每个组件里面对应的算法类的process函数，后面便会进入每个组件的文件夹，如fusion_component.cc对应的具体实现就在fusion文件夹中的app：
Apollo/modules/perception/fusion
fusion
├── app 入口
├── base 定义的数据类(需清晰了解)
├── common 证据推理，IF，KF等方法
└── lib
那么这些组件的也应该要有一个main函数来实例化对象才能运行是吧，找了一圈，main()函数有很多，而里面有.Proc的地方就只有Apollo/cyber/tools/cyber_recorder/main.cc，对应的启动部分代码应该就是如下了。不过这也是我的猜测，可能也不正确，这些组件都是继承的cyber::Component，而关于Cyber组件我没有看过，具体是如果通过参数传递的就不清楚了。希望懂的大佬多指正一下。
int main(int argc, char** argv) {
...
::apollo::cyber::Init(argv[0]);
Spliter spliter(opt_file_vec[0], opt_output_vec[0], opt_white_channels,
opt_black_channels, opt_begin, opt_end);
bool split_result = spliter.Proc();
return split_result ? 0 : -1;
...
}
反正是一层套一层，封装的比较好，看起来可能会有点麻烦，静下心就行。
fusion模块
主要是融合的框架代码，代码注释量比较大，分成多篇文章来写了
-
关联匹配：Apollo perception fusion感知融合源码分析–Associate关联匹配
-
匈牙利匹配算法：Apollo perception fusion感知融合源码分析–匈牙利匹配
-
卡尔曼滤波：Apollo perception fusion感知融合源码分析–卡尔曼滤波
-
证据推理：Apollo perception fusion感知融合源码分析–证据推理
1 部分参考文章
-
自动驾驶 Apollo 源码分析系列，感知篇(二)：Perception 如何启动？
关于Apollo启动模块的一些内容，更好的了解函数从哪里开始执行的 -
Apollo的感知融合模块解析
这是一篇2.5版本的解析，算法结构是差不多的，流程图比较详尽，没有过多的代码细节，文章比较短，容易理解整个过程。 -
Apollo 5.0源码学习笔记（二）| 感知模块 | 融合模块
讲的很清晰，没有代码细节，不想看源码可以看这篇很详细
2 目录结构
── fusion
├── app 入口
├── base 定义的数据类(需清晰了解)
├── common 证据推理，IF，KF等方法
└── lib
├── data_association
│ └── hm_data_association 关联匹配算法
├── data_fusion
│ ├── existence_fusion
│ │ └── dst_existence_fusion 存在性证据推理更新
│ ├── motion_fusion
│ │ └── kalman_motion_fusion 卡尔曼更新运动属性
│ ├── shape_fusion
│ │ └── pbf_shape_fusion 更新形状属性
│ ├── tracker
│ │ └── pbf_tracker 航迹类
│ └── type_fusion
│ └── dst_type_fusion 类别证据推理更新
├── dummy 虚拟？暂不知道
├── fusion_system
│ └── probabilistic_fusion 概率融合入口
├── gatekeeper
│ └── pbf_gatekeeper 门限
│ └── proto
└── interface
3 FusionComponent类
Apollo/modules/perception/onboard/component/fusion_component.cc
- FusionComponent::Init()
组件类里初始化init函数的配置参数如下：
modules/perception/production/conf/perception/fusion/fusion_component_conf.pb.txt
fusion_method: "ProbabilisticFusion"
fusion_main_sensor: "velodyne128"
object_in_roi_check: true
radius_for_roi_object_check: 120
output_obstacles_channel_name: "/perception/vehicle/obstacles"
output_viz_fused_content_channel_name: "/perception/inner/visualization/FusedObjects"
- FusionComponent::Proc()
fusion_component里的fusion_成员对象是ObstacleMultiSensorFusion类定义的，有fusion_相应的init函数，会传入上述的配置参数
frame是一帧传感器数据，fused_objects是最后融合后的结果
InternalProc()
|
fusion_->Process(frame, &fused_objects);
...
4 ObstacleMultiSensorFusion类
modules/perception/fusion/app/obstacle_multi_sensor_fusion.cc
- ObstacleMultiSensorFusion::Process()
bool ObstacleMultiSensorFusion::Process(const base::FrameConstPtr& frame,
std::vector<base::ObjectPtr>* objects) {
FusionOptions options;
return fusion_->Fuse(options, frame, objects); //ProbabilisticFusion
}
5 ProbabilisticFusion类（最关键的地方）
-
一些类的简单定义：
SensorObject：单个目标
SensorFrame：一帧所有的目标
Sensor：历史十帧数据
SensorDataManager：所有传感器的历史十帧数据
其中有不少接口就不介绍了 -
ProbabilisticFusion::Init()
读取配置参数内容如下：
modules/perception/production/data/perception/fusion/probabilistic_fusion.pt
use_lidar: true
use_radar: true
use_camera: true
tracker_method: "PbfTracker"
data_association_method: "HMAssociation"
gate_keeper_method: "PbfGatekeeper"
prohibition_sensors: "radar_front"
max_lidar_invisible_period: 0.25
max_radar_invisible_period: 0.50
max_camera_invisible_period: 0.75
max_cached_frame_num: 50
- ProbabilisticFusion::Fuse()
核心函数，大概流程图如下：
其实源代码就有简单备注，后文详细展开剖析。
5.1 保存数据
先保存传感器进来的一帧数据，保存最新数据到一个map结构中，map为每个sensor对应的数据队列。
注意每次进来数据后判断是不是主传感器velodyne128，有主传感器数据进来之后(started_=true;)才开始保存数据；之后不是主传感器的数据都直接return掉，只有主传感器的数据进来才触发后续的处理。
也就是说处理周期以velodyne128的周期为准，期间sensor_data_manager会暂存几帧其他传感器的数据，直到velodyne128进来再进行后续处理。
具体传感器的频率是多少还没有找到
// 1. save frame data
{
std::lock_guard<std::mutex> data_lock(data_mutex_);
// 三个if全是false
if (!params_.use_lidar && sensor_data_manager->IsLidar(sensor_frame)) {
return true;
}
if (!params_.use_radar && sensor_data_manager->IsRadar(sensor_frame)) {
return true;
}
if (!params_.use_camera && sensor_data_manager->IsCamera(sensor_frame)) {
return true;
}
// velodyne128
bool is_publish_sensor = this->IsPublishSensor(sensor_frame);
if (is_publish_sensor) {
started_ = true;
}
// 有velodyne128进来才开始save？
if (started_) {
AINFO << "add sensor measurement: " << sensor_frame->sensor_info.name
<< ", obj_cnt : " << sensor_frame->objects.size() << ", "
<< FORMAT_TIMESTAMP(sensor_frame->timestamp);
// 传感器数据管理，保存最新数据到一个map结构中，map为每个sensor对应的数据队列
sensor_data_manager->AddSensorMeasurements(sensor_frame);
}
// 不是主传感器就return
if (!is_publish_sensor) {
return true;
}
}
5.2 查询
查询所有传感器的最新一帧数据，插入到frames中，按时间顺序排序好。
类Sensor里有类SensorFrame的一个队列成员，sensor类有该传感器的最新10帧数据，SensorFrame是其中的一帧数据
// 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);
5.3 融合最新一帧
// 3. perform fusion on related frames
// 融合最新一帧
for (const auto& frame : frames) {
FuseFrame(frame);
}
5.4 融合结果后处理
// 4. collect fused objects
// 规则门限过滤，最新匹配更新过track的obj放入到fused_objects中，并publish
CollectFusedObjects(fusion_time, fused_objects);
6 ProbabilisticFusion::FuseFrame
单帧融合的详细细节：主要是观测和航迹的关联匹配，航迹更新。目录如下：
- FuseFrame
- FuseForegroundTrack
- matcher_->Associate
- UpdateAssignedTracks
- UpdateUnassignedTracks
- CreateNewTracks
- FusebackgroundTrack
- RemoveLostTrack
- FuseForegroundTrack
每一帧融合代码执行结构如下：
- FuseFrame()
void ProbabilisticFusion::FuseFrame(const SensorFramePtr& frame) {
// 融合前景
this->FuseForegroundTrack(frame);
// 融合背景，主要是来自激光雷达的背景数据
this->FusebackgroundTrack(frame);
// 删除未更新的航迹
this->RemoveLostTrack();
}
6.1 FuseForegroundTrack函数
其中四个关键函数
// 关联匹配--HMTrackersObjectsAssociation
AssociationOptions options;
AssociationResult association_result;
matcher_->Associate(options, frame, scenes_, &association_result);
// 更新匹配的航迹
const std::vector<TrackMeasurmentPair>& assignments =
association_result.assignments;
this->UpdateAssignedTracks(frame, assignments);
// 更新未匹配的航迹
const std::vector<size_t>& unassigned_track_inds =
association_result.unassigned_tracks;
this->UpdateUnassignedTracks(frame, unassigned_track_inds);
// 未匹配上的量测新建航迹
const std::vector<size_t>& unassigned_obj_inds =
association_result.unassigned_measurements;
this->CreateNewTracks(frame, unassigned_obj_inds);
6.1.1 HMTrackersObjectsAssociation::Associate函数
关联匹配的代码剖析放在另一篇文章Apollo_perception_match.md
6.1.2 UpdateAssignedTracks
匹配上的结果做更新，使用观测更新tracker，tracker类型是pbf_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
// DstExistenceFusion
// 证据推理（DS theory）更新tracker的存在性
existence_fusion_->UpdateWithMeasurement(measurement, target_timestamp,
options.match_distance);
// KalmanMotionFusion
// 鲁棒卡尔曼滤波更新tracker的运动属性
motion_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
// PbfShapeFusion
// 更新tracker的形状
shape_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
// DstTypeFusion
// 证据推理（DS theory）更新tracker的属性
type_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
track_->UpdateWithSensorObject(measurement);
}
主要是DS theory和Kalman更新tracker的属性。后面文章有介绍。
6.1.3 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);
}
6.1.4 CreateNewTracks
对没有匹配到tracker的观测object，新建航迹tracker，主要是最后的两个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;
// 泛型，radar_front不新建航迹
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;
}
// 新建track，并初始化,添加到scenes_中
TrackPtr track = TrackPool::Instance().Get();
track->Initialize(frame->GetForegroundObjects()[obj_ind]);
scenes_->AddForegroundTrack(track);
// 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);
}
}
}
6.2 FusebackgroundTrack函数
和FuseForegroundTrack函数的过程类似，处理函数也是，同样的四个步骤，关联->更新匹配的航迹->更新未匹配的航迹->新建航迹
6.3 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) {
// track里面所有匹配过的传感器是否存在
// 不存在就删掉，不能直接erase？
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++;
}
}
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++;
}
}
background_tracks.resize(background_track_count);
}