Apollo perception源码阅读 | fusion

Apollo perception 专栏收录该内容
9 篇文章 26 订阅

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<T>类,而关于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模块

主要是融合的框架代码,代码注释量比较大,分成多篇文章来写了

1 部分参考文章

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()
    核心函数,大概流程图如下:
    fusion流程图

其实源代码就有简单备注,后文详细展开剖析。

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()
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 theoryKalman更新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函数。可以详细看下TrackBaseTracker两个类。

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);
}
©️2022 CSDN 皮肤主题:大白 设计师:CSDN官方博客 返回首页

打赏作者

GoodluckTian

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值