Apollo 5.0源碼學習筆記（二）| 感知模塊 | 融合模塊

本系列博客旨在記錄自己在學習百度無人駕駛開源框架Apollo的心得和體會，歡迎大家閱讀和點贊，並提出寶貴意見，大家相互學習，如需轉載，請註明出處，謝謝！

這篇文章主要介紹Apollo 5.0感知模塊中的融合模塊。由於不同傳感器各有優缺點，傳感器融合的必要性不言而喻，Apollo融合模塊做的是目標級融合，即首先不同傳感器自己先進行目標檢測檢測和目標跟蹤，得到目標序列後作爲融合模塊的輸入，進行進一步的目標推理，從而得到更加準確可靠的感知結果。

一、融合入口類

在上一篇文章Apollo 5.0源碼學習筆記（一）| 感知模塊 | 感知框架總覽我提到了感知模塊所有子模塊的入口類都定義在modules/perception/onboard/component文件夾下對應源文件中，如融合模塊的入口類就是定義在modules/perception/onboard/component/fusion_component.h中的FusionComponent類。
輸入： SensorFrameMessage類型消息；
輸出： PerceptionObstacles類型消息；
FusionComponent類的參數配置protobuf文件爲：modules/perception/onboard/proto/fusion_component_config.proto，具體參數實現定義在文件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: "/apollo/perception/obstacles"
output_viz_fused_content_channel_name: "/perception/inner/visualization/FusedObjects"

從配置參數可以看出，融合的主傳感器設置爲velodyne128，融合方法使用的是ProbabilisticFusion，發佈融合後目標障礙物消息的話題名爲/apollo/perception/obstacles。

FusionComponent類主要負責接收融合所需要的來自不同傳感器的目標序列消息，然後利用ProbabilisticFusion融合類完成實際融合工作，得到融合後感知結果，最後把融合結果在FusionComponent類中發佈，供其他模塊使用。

二、fusion下基礎類含義說明

在利用不同傳感器感知結果來進行目標級融合時，會涉及到對很多單個目標、數據幀所有目標以及多幀目標等數據的存儲問題，因此會定義很多數據結構來完成這個工作，這給閱讀源碼帶來了很多困惑，因此此處首先對這些基礎自定義數據結構進行一個簡單總結，方便後面的解釋。

這些基礎類主要定義在fusion/base文件夾下：
SensorObject類——某單一傳感器的一個目標，其實就是包含了某個傳感器的相關屬性定義和一個base::Object成員變量；
SensorFrame類——某傳感器的一幀所有目標數據，定義了std::vector<SensorObject>成員變量；
Sensor類——某傳感器歷史多幀信息，定義有std::deque<SensorFramePtr>類型成員變量，並提供獲取最新數據幀的一些接口函數；
SensorDataManager類——最終在ProbabilisticFusion類中作爲成員變量用來存儲<sensor_id, SensorPtr>鍵值對，即存儲多個傳感器的多幀歷史信息的類，定義有std::unorderd_map<std::string, SensorPtr> sensors_成員變量，作爲ProbabilisticFusion類的數據輸入緩存；
Track類——單個跟蹤目標，在該類中完成大部分跟蹤目標狀態更新工作；
Scene類——前景和背景跟蹤目標的管理，定義有std::vector<TrackPtr> forground_tracks_和std::vector<TrackPtr> background_tracks_；
PbfTracker類——單個跟蹤目標，也是主要的融合算法發生過程所在，定義了不同的融合算法類。

二、融合類ProbabilisticFusion解析

融合主要過程是通過ProbabilisticFusion類完成的，該類定義在modules/perception/fusion/lib/fusion_system/probabilistic_fusion/probabilistic_fusion.h中，該類繼承自BaseFusionSystem類。下面按照一些主要函數進行分別解析。

ProbabilisticFusion::Init函數

---

主要是對該類進行一些參數配置，並根據這些配置的參數，來實例化具體的算法類。
參數配置protobuf文件定義在modules/perception/proto/probabilistic_fusion_config.proto，具體實例化的參數定義在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

可以看出實際融合的傳感器包括lidar、radar和camera，以及實際使用的跟蹤算法、數據關聯算法、門限保持方法的具體類名，另外還定義了其他一些參數。

ProbabilisticFusion::CreateNewTracks函數

---

目的：創建新跟蹤目標
prohibition_sensors: "radar_front"在modules/perception/production/data/perception/fusion/probabilistic_fusion.pt文件中定義；
首先判斷當前檢測幀是否是prohibition_sensors中一種，如果是，則不用來創建跟蹤目標，可以看出，不用radar來創建跟蹤目標。
如果真實需要創建新的跟蹤目標，首先從TrackPool中獲取共享指針track，然後利用當前檢測結果Initialize該Track跟蹤目標之後添加到scenes_中；
然後創建一個PbfTracker，Init該track，添加到成員變量trackers_中，即trackers_保存PbfTracker序列，PbfTracker中存儲的是TrackPtr；

由於track一直是同一個指針，其實scenes_和trackers_存儲的是同一跟蹤目標序列，分開在不同的類中對跟蹤目標進行更新。

PbfTracker::Init函數通過調用InitMethods函數，從而利用track指針構造了每個算法類，例如motion_fusion_等，這樣就直接能夠在motion_fusion_`中通過指針更新track的內容了。

新版本的融合類沒有把所有有關跟蹤目標更新操作都放在PbfTracker類中了，而是在PbfTracker類中保存一個Track指針，由各個motion_fusion_、shape_fusion_等類共同保存該指針（即是同一個跟蹤目標），因此把更新操作分擔給了其他各個功能類完成，在Track中只是進行匹配上傳感器的信息更新和管理。

主要保存目標屬性的成員變量：Track::fused_object_是FusedObjectPtr類型，其主要保存的變量是ObjectPtr。

ProbabilisticFusion::UpdateAssignedTracks函數

---

目的：更新匹配對跟蹤目標
利用匹配上的檢測目標，更新跟蹤目標。

調用的是PbfTracker::UpdateWithMeasurement函數，依次完成存在性、運動、形狀和類型的融合；

最後調用track_->UpdateWithSensorObject，在該函數內部，主要是利用當前匹配的檢測目標，來更新跟蹤目標中存儲的當前檢測目標map內容，一共存儲了lidar_objects_,radar_objects_和camera_objects_三種；

函數內部依次調用：
1、DstExistanceFusion::UpdateWithMeasurement:
利用DS理論完成對跟蹤目標存在性的判定，設置Track中的toic_p_、existance_prob_屬性；

2、KalmanMotionFusion::UpdateWithMeasurement：
利用Kalman濾波，完成對目標狀態濾波，設置Track中的
anchor_point、center、velocity、acceleration、center_uncertainty、velocity_uncertainty、acceleration_uncertainty屬性；

3、PbfShapeFusion::UpdateWithMeasurement：
根據匹配的測量量，完成對目標的形狀推理，設置Track中的size、direction、theta、polygon屬性，同時也更新了Track的center、anchor_point屬性；

4、DstTypeFusion::UpdateWithMeasurement：
利用DS理論，完成對跟蹤目標的type、sub_type、type_probs屬性更新