Apollo perception源码分析--fusion--融合框架
程序员文章站
2022-07-12 11:55:01
...
Apollo perception代码
Apollo6.0 感知融合
代码注释 leox24 / perception_learn
fusion模块源码剖析
主要是融合的框架代码
匈牙利匹配算法的代码细节在Apollo perception源码分析–fusion–匈牙利匹配
关联匹配的细节放在TODO
证据推理和卡尔曼滤波的代码细节放在TODO
部分参考文献
自动驾驶 Apollo 源码分析系列，感知篇(二)：Perception 如何启动？
关于Apollo启动模块的一些内容，更好的了解函数从哪里开始执行的
Apollo的感知融合模块解析
这是一篇2.5版本的解析，算法结构是差不多的，流程图比较详尽，没有过多的代码细节，文章比较短，容易理解整个过程。
Apollo 5.0源码学习笔记（二）| 感知模块 | 融合模块
讲的很清晰，没有代码细节，不想看源码可以看这篇很详细
目录结构
── 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
配置参数
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"
入口
注意处理类的初始化init函数，主要是注意配置参数
fusion_component里的fusion_成员对象是ObstacleMultiSensorFusion类定义的，有fusion_相应的init函数，会传入上述的配置参数
frame是一帧传感器数据，fused_objects是最后融合后的结果
modules/perception/onboard/component/fusion_component.cc
fusion_->Process(frame, &fused_objects);
ObstacleMultiSensorFusion类
modules/perception/fusion/app/obstacle_multi_sensor_fusion.cc
bool ObstacleMultiSensorFusion::Process(const base::FrameConstPtr& frame,
std::vector<base::ObjectPtr>* objects) {
FusionOptions options;
return fusion_->Fuse(options, frame, objects); //ProbabilisticFusion
}
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
- 核心函数：Fuse函数
bool ProbabilisticFusion::Fuse(const FusionOptions& options,
const base::FrameConstPtr& sensor_frame,
std::vector<base::ObjectPtr>* fused_objects)
其实源代码就有简单备注，后文详细展开剖析。
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;
}
}
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);
3.融合最新一帧
// 3. perform fusion on related frames
// 融合最新一帧
for (const auto& frame : frames) {
FuseFrame(frame);
}
每一帧融合代码执行结构如下：
FuseFrame()函数
void ProbabilisticFusion::FuseFrame(const SensorFramePtr& frame) {
// 融合前景
this->FuseForegroundTrack(frame);
// 融合背景，主要是来自激光雷达的背景数据
this->FusebackgroundTrack(frame);
// 删除未更新的航迹
this->RemoveLostTrack();
}
3.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);
3.1.1 HMTrackersObjectsAssociation::Associate函数
匈牙利匹配的代码剖析放在另一篇文章TODO
3.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的属性，再往里写文章就太长了，具体的细节后续更新文章TODO
3.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);
}
3.1.4 CreateNewTracks
对没有匹配到tracker的观测object，新建航迹tracker，主要是最后的两个Init函数。可以详细看下Track和BaseTracker两个类
track<tracker<trackers
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);
}
}
}
3.2 FusebackgroundTrack函数
和FuseForegroundTrack函数的过程类似，处理函数也是，同样的四个步骤，关联->更新匹配的航迹->更新未匹配的航迹->新建航迹
3.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);
}
4.融合结果后处理
// 4. collect fused objects
// 规则门限过滤，最新匹配更新过track的obj放入到fused_objects中，并publish
CollectFusedObjects(fusion_time, fused_objects);
上一篇： apollo spring @value 源码分析
下一篇： idea和gradle编译源码