Apollo 6.0 Perception 模組 Fusion 元件（二）：主體演算法流程分析
本文使用 Zhihu On VSCode 創作併發布
目錄
目錄
0 前言
Apollo 框架中，每個功能元件都對應一個繼承自
Component
類的具體元件類，具體元件類中都包含
public
的
Init
方法和
Proc
方法，
Init
方法實現了元件的初始化，
Proc
方法實現了元件的具體演算法流程。
Perception 模組 Fusion 元件的具體元件類為
FusionComponent
，從上一篇文章
中我們已經知道，Fusion 元件的
FusionComponent
類的
Init
方法最終會在
Component
類的
Initialize
方法中被多型呼叫一次，而
FusionComponent
類的
Proc
方法將成為 Fusion 元件對應 channel 上的訊息回撥函式（意即，收到一幀訊息觸發一次）。
本文將以 Apollo 6。0 原始碼
master
分支上的
74f7d1a429
提交為基礎詳細分析 Perception 模組 Fusion 元件的初始化、訊息回撥處理以及障礙物融合的主體演算法框架，至於前景航跡融合的演算法細節將在後續的文章中單獨講述。
1 Fusion 元件成員組成
首先，開啟
apollo/modules/perception/onboard/component/fusion_component。h
看看
FusionComponent
類中由哪些資料成員和方法成員構成：
class
FusionComponent
：
public
cyber
：：
Component
<
SensorFrameMessage
>
{
public
：
FusionComponent
（）
=
default
；
~
FusionComponent
（）
=
default
；
bool
Init
（）
override
；
// 由 Cyber RT 在啟動元件過程中進行間接呼叫，只會執行一次
bool
Proc
（
const
std
：：
shared_ptr
<
SensorFrameMessage
>&
message
）
override
；
// 由 Cyber RT 在啟動元件過程中註冊為訊息回撥函式
private
：
bool
InitAlgorithmPlugin
（）；
// 元件內部的初始化方法，由 Init 方法進行呼叫
bool
InternalProc
（
const
std
：：
shared_ptr
<
SensorFrameMessage
const
>&
in_message
，
// 元件內部的訊息處理方法，具體演算法流程入口，由 Proc 方法進行呼叫
std
：：
shared_ptr
<
PerceptionObstacles
>
out_message
，
std
：：
shared_ptr
<
SensorFrameMessage
>
viz_message
）；
private
：
static
std
：：
mutex
s_mutex_
；
static
uint32_t
s_seq_num_
；
std
：：
string
fusion_name_
；
// 融合名稱
std
：：
string
fusion_method_
；
// 融合方法
std
：：
vector
<
std
：：
string
>
fusion_main_sensors_
；
// 融合主感測器
bool
object_in_roi_check_
=
false
；
// 是否開啟 HD Map ROI 融合障礙物校驗
double
radius_for_roi_object_check_
=
0
；
// HD Map ROI 融合障礙物校驗半徑
std
：：
unique_ptr
<
fusion
：：
BaseMultiSensorFusion
>
fusion_
；
// 獨佔智慧指標，用於管理多感測器融合抽象基類物件，最為關鍵的資料成員
map
：：
HDMapInput
*
hdmap_input_
=
nullptr
；
// HD Map 輸入
std
：：
shared_ptr
<
apollo
：：
cyber
：：
Writer
<
PerceptionObstacles
>>
writer_
；
// Cyber Writer 物件，用於輸出 protobuf 格式的經 HD Map ROI 校驗過的有效融合障礙物資訊
std
：：
shared_ptr
<
apollo
：：
cyber
：：
Writer
<
SensorFrameMessage
>>
inner_writer_
；
// Cyber Writer 物件，用於輸出視覺化資訊
}；
這裡需要先著重提下
std：：unique_ptr
這個資料成員，
fusion_
是個獨佔智慧指標，用於管理
BaseMultiSensorFusion
型別的物件，
BaseMultiSensorFusion
是多感測器融合的抽象基類，會被表示障礙物多感測器融合的
ObstacleMultiSensorFusion
類所繼承並實現對應純虛介面方法。
fusion_
中的指標值會在
FusionComponent：：InitAlgorithmPlugin
方法中被更新為工廠方法（Factory Method）模式返回的
ObstacleMultiSensorFusion
指標，後文中將對此進行詳述。
再來看下 Perception 模組整個 Fusion 元件的 UML 類圖：
Perception 模組 Fusion 元件 UML 類圖
I
表示由抽象基類實現的介面（Interface）類 ，通常情況下不會對介面類進行例項化；
C
表示對介面類的具體實現。
2 初始化
2。1 對外的初始化介面：FusionComponent：：Init 方法
開啟
apollo/modules/perception/onboard/component/fusion_component。cc
，我們看下
FusionComponent
類
Init
方法的定義：
bool
FusionComponent
：：
Init
（）
{
FusionComponentConfig
comp_config
；
// 例項化配置引數 protobuf 類：fusion_component_config。proto
if
（
！
GetProtoConfig
（
&
comp_config
））
{
// 將 txt 配置檔案中的配置引數讀入 protobuf 類：fusion_component_conf。pb。txt
return
false
；
}
AINFO
<<
“Fusion Component Configs： ”
<<
comp_config
。
DebugString
（）；
// to load component configs
fusion_name_
=
comp_config
。
fusion_name
（）；
// 融合名稱：ObstacleMultiSensorFusion
fusion_method_
=
comp_config
。
fusion_method
（）；
// 融合方法：ProbabilisticFusion
for
（
int
i
=
0
；
i
<
comp_config
。
fusion_main_sensors_size
（）；
++
i
）
{
// 融合主感測器：velodyne128，front_6mm，front_12mm
fusion_main_sensors_
。
push_back
（
comp_config
。
fusion_main_sensors
（
i
））；
}
object_in_roi_check_
=
comp_config
。
object_in_roi_check
（）；
// 是否開啟 HD Map ROI 融合障礙物校驗：true
radius_for_roi_object_check_
=
comp_config
。
radius_for_roi_object_check
（）；
// HD Map ROI 融合障礙物校驗半徑：120m
// init algorithm plugin
ACHECK
（
InitAlgorithmPlugin
（））
<<
“Failed to init algorithm plugin。”
；
// 呼叫私有初始化方法 InitAlgorithmPlugin，執行更詳細的初始化動作
writer_
=
node_
->
CreateWriter
<
PerceptionObstacles
>
（
comp_config
。
output_obstacles_channel_name
（））；
// 用於輸出 protobuf 格式的經 HD Map ROI 校驗過的有效融合障礙物資訊：perception_obstacle。proto
inner_writer_
=
node_
->
CreateWriter
<
SensorFrameMessage
>
（
comp_config
。
output_viz_fused_content_channel_name
（））；
// 用於輸出供視覺化的高精地圖和融合障礙物資訊
return
true
；
}
Fusion 元件的
txt
配置檔案為
apollo/modules/perception/production/conf/perception/fusion/fusion_component_conf。pb。txt
：
fusion_name： “ObstacleMultiSensorFusion”
fusion_method： “ProbabilisticFusion”
fusion_main_sensors： “velodyne128”
fusion_main_sensors： “front_6mm”
fusion_main_sensors： “front_12mm”
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”
從配置引數中可以看出，Fusion 元件會融合來自 128 線 velodyne 鐳射雷達、6mm 焦距前視相機和 12mm 焦距前視相機三個感測器的障礙物訊息。兩個輸出訊息通道分別為：
/perception/vehicle/obstacles
：輸出融合障礙物資訊
/perception/inner/visualization/FusedObjects
：輸出視覺化資訊
FusionComponentConfig
是一個 protobuf 類，用於存放從 Fusion 元件
txt
配置檔案中讀入的配置引數，它對應的 proto 檔案為
apollo/modules/perception/onboard/proto/fusion_component_config。proto
。
2。2 內部的初始化介面：FusionComponent：：InitAlgorithmPlugin 方法
Init
方法中會呼叫
InitAlgorithmPlugin
方法，我們看下它的定義：
bool
FusionComponent
：：
InitAlgorithmPlugin
（）
{
fusion
：：
BaseMultiSensorFusion
*
fusion
=
fusion
：：
BaseMultiSensorFusionRegisterer
：：
GetInstanceByName
（
fusion_name_
）；
// 透過工廠方法模式獲取 ObstacleMultiSensorFusion 類的例項指標
CHECK_NOTNULL
（
fusion
）；
fusion_
。
reset
（
fusion
）；
// 更新 fusion_ 的指標值為 ObstacleMultiSensorFusion 類例項指標
fusion
：：
ObstacleMultiSensorFusionParam
param
；
// 障礙物多感測器融合引數
param
。
main_sensors
=
fusion_main_sensors_
；
// 障礙物多感測器融合引數——主感測器：velodyne128，front_6mm，front_12mm
param
。
fusion_method
=
fusion_method_
；
// 障礙物多感測器融合引數——融合方法：ProbabilisticFusion
ACHECK
（
fusion_
->
Init
（
param
））
<<
“Failed to init ObstacleMultiSensorFusion”
；
// 多型呼叫 ObstacleMultiSensorFusion：：Init 方法，執行主要的初始化動作
if
（
FLAGS_obs_enable_hdmap_input
&&
object_in_roi_check_
）
{
hdmap_input_
=
map
：：
HDMapInput
：：
Instance
（）；
// 獲取 map：：HDMapInput 的唯一例項，這是一個單例類
ACHECK
（
hdmap_input_
->
Init
（））
<<
“Failed to init hdmap input。”
；
// 初始化 HD Map
}
AINFO
<<
“Init algorithm successfully， onboard fusion： ”
<<
fusion_method_
；
return
true
；
}
InitAlgorithmPlugin
方法主要做了三件事：
透過工廠方法模式獲取
ObstacleMultiSensorFusion
類的例項指標
多型呼叫
ObstacleMultiSensorFusion：：Init
方法，執行主要的初始化動作
獲取 HD Map 的唯一例項，並執行初始化
針對前兩點我們會進行詳細討論，HD Map 部分不是本文重點。
2。2。1 透過工廠方法模式獲取 ObstacleMultiSensorFusion 類的例項指標
工廠方法
是一種建立型設計模式，與簡單工廠（Simple Factory）、抽象工廠（Abstract Factory）一併稱為工廠模式，關於三者的區別可以概括為：
簡單工廠：只有一個工廠，將不同產品的生產語句羅列到工廠的一個生產方法中，根據配置引數選擇性生產具體的產品；
工廠方法：每種產品對應一個具體工廠，每個具體工廠負責生產對應的具體產品；
抽象工廠：將不同的產品根據相關性劃分為多個產品族，每個產品族對應一個具體工廠，每個具體工廠中包含多個產品生產方法，用於生產對應產品族中的不同產品。
關於工廠模式具體不作展開，可以查閱相關文章。Apollo 中使用了一種更加泛化和靈活的工廠方法，其泛化性和靈活性透過
Any
類和宏定義實現。
開啟
apollo/modules/perception/fusion/lib/interface/base_multisensor_fusion。h
，看下
BaseMultiSensorFusion
類的定義：
class
BaseMultiSensorFusion
{
public
：
BaseMultiSensorFusion
（）
=
default
；
virtual
~
BaseMultiSensorFusion
（）
=
default
；
virtual
bool
Init
（
const
ObstacleMultiSensorFusionParam
&
param
）
=
0
；
// 初始化介面
virtual
bool
Process
（
const
base
：：
FrameConstPtr
&
frame
，
std
：：
vector
<
base
：：
ObjectPtr
>*
objects
）
=
0
；
// 處理介面
virtual
std
：：
string
Name
（）
const
=
0
；
private
：
DISALLOW_COPY_AND_ASSIGN
（
BaseMultiSensorFusion
）；
// 禁止複製構造與複製賦值
}；
// Class BaseMultiSensorFusion
PERCEPTION_REGISTER_REGISTERER
（
BaseMultiSensorFusion
）；
// 生成 BaseMultiSensorFusion 的客戶端程式碼
#define PERCEPTION_REGISTER_MULTISENSORFUSION（name） \
PERCEPTION_REGISTER_CLASS（BaseMultiSensorFusion， name）
// 生成 name 具體產品的具體工廠類
不難發現，
BaseMultiSensorFusion
是一個抽象基類，其內部含有三個
public
的對外介面：
Init
、
Process
和
Name
，透過使用宏定義
DISALLOW_COPY_AND_ASSIGN
來禁止複製構造與複製賦值，
DISALLOW_COPY_AND_ASSIGN
定義在
apollo/cyber/common/macros。h
中：
#define DISALLOW_COPY_AND_ASSIGN（classname） \
classname（const classname &） = delete； \
classname &operator=（const classname &） = delete；
同時可以發現，
BaseMultiSensorFusion
類定義的外面呼叫了一個宏定義
PERCEPTION_REGISTER_REGISTERER
，並定義了一個新的宏定義
PERCEPTION_REGISTER_MULTISENSORFUSION
來間接呼叫另一個宏定義
PERCEPTION_REGISTER_CLASS
。
PERCEPTION_REGISTER_REGISTERER
用於生成工廠方法的客戶端程式碼，
PERCEPTION_REGISTER_CLASS
用於生成工廠方法的具體工廠類，它們都定義在
apollo/modules/perception/lib/registerer/registerer。h
中：
namespace
apollo
{
namespace
perception
{
namespace
lib
{
// idea from boost any but make it more simple and don‘t use type_info。
// 從 boost 庫借鑑而來的 Any 類實現，可表示任意的抽象產品
class
Any
{
public
：
Any
（）
：
content_
（
NULL
）
{}
template
<
typename
ValueType
>
explicit
Any
（
const
ValueType
&
value
）
：
content_
（
new
Holder
<
ValueType
>
（
value
））
{}
Any
（
const
Any
&
other
）
：
content_
（
other
。
content_
？
other
。
content_
->
Clone
（）
：
nullptr
）
{}
~
Any
（）
{
delete
content_
；
}
template
<
typename
ValueType
>
ValueType
*
AnyCast
（）
{
return
content_
？
&
（
static_cast
<
Holder
<
ValueType
>
*>
（
content_
）
->
held_
）
：
nullptr
；
}
private
：
class
PlaceHolder
{
public
：
virtual
~
PlaceHolder
（）
{}
virtual
PlaceHolder
*
Clone
（）
const
=
0
；
}；
template
<
typename
ValueType
>
class
Holder
：
public
PlaceHolder
{
public
：
explicit
Holder
（
const
ValueType
&
value
）
：
held_
（
value
）
{}
virtual
~
Holder
（）
{}
virtual
PlaceHolder
*
Clone
（）
const
{
return
new
Holder
（
held_
）；
}
ValueType
held_
；
}；
PlaceHolder
*
content_
；
}；
// 可用於生產任意抽象產品的抽象工廠類
class
ObjectFactory
{
public
：
ObjectFactory
（）
{}
virtual
~
ObjectFactory
（）
{}
virtual
Any
NewInstance
（）
{
return
Any
（）；
}
// 產品生產介面
ObjectFactory
（
const
ObjectFactory
&
）
=
delete
；
ObjectFactory
&
operator
=
（
const
ObjectFactory
&
）
=
delete
；
private
：
}；
typedef
std
：：
map
<
std
：：
string
，
ObjectFactory
*>
FactoryMap
；
// 派生類（具體產品）工廠指標對映
typedef
std
：：
map
<
std
：：
string
，
FactoryMap
>
BaseClassMap
；
// 抽象基類（抽象產品）對映
BaseClassMap
&
GlobalFactoryMap
（）；
// 獲取 static 的抽象基類對映
// 結合 BaseClassMap 和 FactoryMap 獲取指定基類對應的所有派生類
bool
GetRegisteredClasses
（
const
std
：：
string
&
base_class_name
，
std
：：
vector
<
std
：：
string
>
*
registered_derived_classes_names
）；
}
// namespace lib
}
// namespace perception
}
// namespace apollo
// 客戶端程式碼
// 為指定的抽象產品類生成一個註冊器類，包含用於生產產品例項的靜態方法
// 宏定義中，## 用於連線兩個記號，# 用於將宏引數轉換為字串
#define PERCEPTION_REGISTER_REGISTERER（base_class） \
class base_class##Registerer { \
typedef ：：apollo：：perception：：lib：：Any Any； \
typedef ：：apollo：：perception：：lib：：FactoryMap FactoryMap； \
\
public： \
static base_class *GetInstanceByName（const ：：std：：string &name） { \
FactoryMap &map = \
：：apollo：：perception：：lib：：GlobalFactoryMap（）［#base_class］； \
FactoryMap：：iterator iter = map。find（name）； \
if （iter == map。end（）） { \
for （auto c ： map） { \
AERROR << “Instance：” << c。first； \
} \
AERROR << “Get instance ” << name << “ failed。”； \
return nullptr； \
} \
Any object = iter->second->NewInstance（）； \
return *（object。AnyCast
} \
static std：：vector
std：：vector
FactoryMap &map = \
：：apollo：：perception：：lib：：GlobalFactoryMap（）［#base_class］； \
instances。reserve（map。size（））； \
for （auto item ： map） { \
Any object = item。second->NewInstance（）； \
instances。push_back（*（object。AnyCast
} \
return instances； \
} \
static const ：：std：：string GetUniqInstanceName（） { \
FactoryMap &map = \
：：apollo：：perception：：lib：：GlobalFactoryMap（）［#base_class］； \
CHECK_EQ（map。size（）， 1U） << map。size（）； \
return map。begin（）->first； \
} \
static base_class *GetUniqInstance（） { \
FactoryMap &map = \
：：apollo：：perception：：lib：：GlobalFactoryMap（）［#base_class］； \
CHECK_EQ（map。size（）， 1U） << map。size（）； \
Any object = map。begin（）->second->NewInstance（）； \
return *（object。AnyCast
} \
static bool IsValid（const ：：std：：string &name） { \
FactoryMap &map = \
：：apollo：：perception：：lib：：GlobalFactoryMap（）［#base_class］； \
return map。find（name） ！= map。end（）； \
} \
}；
// 具體工廠類
// 在 Perception 功能模組 。so 動態庫檔案載入期間，被 __attribute__（（constructor））
// 修飾的函式會優先執行，建立具體產品類對應的具體工廠指標，並將其對映到相應的 FactoryMap 中
#define PERCEPTION_REGISTER_CLASS（clazz， name） \
namespace { \
class ObjectFactory##name ： public apollo：：perception：：lib：：ObjectFactory { \
public： \
virtual ~ObjectFactory##name（） {} \
virtual ：：apollo：：perception：：lib：：Any NewInstance（） { \
return ：：apollo：：perception：：lib：：Any（new name（））； \
} \
}； \
__attribute__（（constructor）） void RegisterFactory##name（） { \
：：apollo：：perception：：lib：：FactoryMap &map = \
：：apollo：：perception：：lib：：GlobalFactoryMap（）［#clazz］； \
if （map。find（#name） == map。end（）） map［#name］ = new ObjectFactory##name（）； \
} \
}
// namespace
在感知子功能模組
。so
動態庫檔案載入期間，下面這條語句
PERCEPTION_REGISTER_REGISTERER
（
BaseMultiSensorFusion
）；
將為
BaseMultiSensorFusion
類建立客戶端類
BaseMultiSensorFusionRegisterer
，透過呼叫
BaseMultiSensorFusionRegisterer：：GetInstanceByName
可以獲得指定具體產品類
ObstacleMultiSensorFusion
的例項指標，問題在於，
ObstacleMultiSensorFusion
的具體工廠是什麼時候建立的？
開啟
apollo/modules/perception/fusion/app/obstacle_multi_sensor_fusion。cc
，最後有這樣一條語句：
PERCEPTION_REGISTER_MULTISENSORFUSION
（
ObstacleMultiSensorFusion
）；
結合上面的分析，該語句會被替換為：
PERCEPTION_REGISTER_CLASS
（
BaseMultiSensorFusion
，
ObstacleMultiSensorFusion
）
將上面這個宏定義進行展開：
namespace
{
class
ObjectFactoryObstacleMultiSensorFusion
：
public
apollo
：：
perception
：：
lib
：：
ObjectFactory
{
public
：
virtual
~
ObjectFactoryObstacleMultiSensorFusion
（）
{}
virtual
：：
apollo
：：
perception
：：
lib
：：
Any
NewInstance
（）
{
// 產品生產方法
return
：：
apollo
：：
perception
：：
lib
：：
Any
（
new
ObstacleMultiSensorFusion
（））；
// 生產 ObstacleMultiSensorFusion 具體產品
}
}；
__attribute__
（（
constructor
））
void
RegisterFactoryObstacleMultiSensorFusion
（）
{
：：
apollo
：：
perception
：：
lib
：：
FactoryMap
&
map
=
：：
apollo
：：
perception
：：
lib
：：
GlobalFactoryMap
（）［
“BaseMultiSensorFusion”
］；
if
（
map
。
find
（
“ObstacleMultiSensorFusion”
）
==
map
。
end
（））
map
［
“ObstacleMultiSensorFusion”
］
=
new
ObjectFactoryObstacleMultiSensorFusion
（）；
// 建立具體工廠指標，並將其多型地對映到相應的 FactoryMap 中
}
}
// namespace
ObjectFactoryObstacleMultiSensorFusion
即為
ObstacleMultiSensorFusion
對應的具體工廠類，
RegisterFactoryObstacleMultiSensorFusion
函式由於被
__attribute__（（constructor））
屬性修飾，因而會在 Perception 功能模組
。so
動態庫檔案載入期間被執行，完成
ObstacleMultiSensorFusion
類具體工廠指標的建立（相應的
new
語句），並將其多型地對映到相應的 FactoryMap 中。最後，透過呼叫客戶端方法
BaseMultiSensorFusionRegisterer：：GetInstanceByName
即可獲得
ObstacleMultiSensorFusion
類的例項指標。
2。2。2 主要的初始化動作：ObstacleMultiSensorFusion：：Init 方法
FusionComponent：：fusion_
所管理物件的靜態型別雖然是
BaseMultiSensorFusion
，但由於其指標被繫結到工廠方法返回的
ObstacleMultiSensorFusion
型別指標上，故
InitAlgorithmPlugin
方法中的語句
ACHECK
（
fusion_
->
Init
（
param
））
<<
“Failed to init ObstacleMultiSensorFusion”
；
會多型呼叫
ObstacleMultiSensorFusion：：Init
，開啟
apollo/modules/perception/fusion/app/obstacle_multi_sensor_fusion。cc
看下其定義：
bool
ObstacleMultiSensorFusion
：：
Init
（
const
ObstacleMultiSensorFusionParam
&
param
）
{
if
（
fusion_
！=
nullptr
）
{
AINFO
<<
“Already inited”
；
return
true
；
}
BaseFusionSystem
*
fusion
=
BaseFusionSystemRegisterer
：：
GetInstanceByName
（
param
。
fusion_method
）；
// 透過工廠方法模式獲取 ProbabilisticFusion 類的例項指標
fusion_
。
reset
（
fusion
）；
FusionInitOptions
init_options
；
// 融合初始化可選項
init_options
。
main_sensors
=
param
。
main_sensors
；
// 融合初始化可選項——主感測器：velodyne128，front_6mm，front_12mm
if
（
fusion_
==
nullptr
||
！
fusion_
->
Init
（
init_options
））
{
// 多型呼叫 ProbabilisticFusion：：Init 方法
AINFO
<<
“Failed to Get Instance or Initialize ”
<<
param
。
fusion_method
；
return
false
；
}
return
true
；
}
ObstacleMultiSensorFusion：：fusion_
也是個獨佔智慧指標，用於管理
BaseFusionSystem
型別的物件，
BaseFusionSystem
是融合方法的抽象基類，會被表示機率融合的
ProbabilisticFusion
類所繼承並實現對應純虛介面方法。類似上文的分析，這裡
fusion_
中的指標值會被更新為工廠方法模式返回的
ProbabilisticFusion
指標，具體不再贅述。同時，下面的語句
if
（
fusion_
==
nullptr
||
！
fusion_
->
Init
（
init_options
））
{
會多型呼叫
ProbabilisticFusion：：Init
，開啟
apollo/modules/perception/fusion/lib/fusion_system/probabilistic_fusion/probabilistic_fusion。cc
看下其定義：
bool
ProbabilisticFusion
：：
Init
（
const
FusionInitOptions
&
init_options
）
{
main_sensors_
=
init_options
。
main_sensors
；
// 主感測器：velodyne128，front_6mm，front_12mm
BaseInitOptions
options
；
if
（
！
GetFusionInitOptions
（
“ProbabilisticFusion”
，
&
options
））
{
// 讀取 probabilistic_fusion。config 中 pt 配置檔案的相對路徑（root_dir）與名稱（conf_file）
return
false
；
}
std
：：
string
work_root_config
=
GetAbsolutePath
（
// 獲取 pt 配置檔案父路徑的絕對路徑
lib
：：
ConfigManager
：：
Instance
（）
->
work_root
（），
options
。
root_dir
）；
std
：：
string
config
=
GetAbsolutePath
（
work_root_config
，
options
。
conf_file
）；
// 獲取 pt 配置檔案的絕對路徑
ProbabilisticFusionConfig
params
；
// 例項化機率融合配置引數 protobuf 類：probabilistic_fusion_config。proto
if
（
！
cyber
：：
common
：：
GetProtoFromFile
（
config
，
&
params
））
{
// 將 pt 配置檔案中的配置引數讀入 protobuf 類：probabilistic_fusion。pt
AERROR
<<
“Read config failed： ”
<<
config
；
return
false
；
}
params_
。
use_lidar
=
params
。
use_lidar
（）；
// 是否使用 lidar：true
params_
。
use_radar
=
params
。
use_radar
（）；
// 是否使用 radar：true
params_
。
use_camera
=
params
。
use_camera
（）；
// 是否使用 camera：true
params_
。
tracker_method
=
params
。
tracker_method
（）；
// 跟蹤方法：PbfTracker
params_
。
data_association_method
=
params
。
data_association_method
（）；
// 資料關聯方法：HMAssociation
params_
。
gate_keeper_method
=
params
。
gate_keeper_method
（）；
// 門限保持方法：PbfGatekeeper
for
（
int
i
=
0
；
i
<
params
。
prohibition_sensors_size
（）；
++
i
）
{
// 被禁止用於建立新航跡的感測器：radar_front
params_
。
prohibition_sensors
。
push_back
（
params
。
prohibition_sensors
（
i
））；
}
// static member initialization from PB config
Track
：：
SetMaxLidarInvisiblePeriod
（
params
。
max_lidar_invisible_period
（））；
// Lidar 歷史量測最大不可見時長：0。25s
Track
：：
SetMaxRadarInvisiblePeriod
（
params
。
max_radar_invisible_period
（））；
// Radar 歷史量測最大不可見時長：0。50s
Track
：：
SetMaxCameraInvisiblePeriod
（
params
。
max_camera_invisible_period
（））；
// Camera 歷史量測最大不可見時長：0。75s
Sensor
：：
SetMaxCachedFrameNumber
（
params
。
max_cached_frame_num
（））；
// 快取的最大幀數：50
scenes_
。
reset
（
new
Scene
（））；
// 初始化用於管理場景的共享智慧指標，場景中包含所有的前景航跡與背景航跡
if
（
params_
。
data_association_method
==
“HMAssociation”
）
{
// 如果使用 HMAssociation 資料關聯方法
matcher_
。
reset
（
new
HMTrackersObjectsAssociation
（））；
// 多型地初始化用於管理資料關聯的獨佔智慧指標
}
else
{
AERROR
<<
“Unknown association method： ”
<<
params_
。
data_association_method
；
return
false
；
}
if
（
！
matcher_
->
Init
（））
{
// 多型呼叫 HMTrackersObjectsAssociation：：Init
AERROR
<<
“Failed to init matcher。”
；
return
false
；
}
if
（
params_
。
gate_keeper_method
==
“PbfGatekeeper”
）
{
// 如果使用 PbfGatekeeper 門限保持方法
gate_keeper_
。
reset
（
new
PbfGatekeeper
（））；
// 多型地初始化用於管理門限保持的獨佔智慧指標
}
else
{
AERROR
<<
“Unknown gate keeper method： ”
<<
params_
。
gate_keeper_method
；
return
false
；
}
if
（
！
gate_keeper_
->
Init
（））
{
// 多型呼叫 PbfGatekeeper：：Init
AERROR
<<
“Failed to init gatekeeper。”
；
return
false
；
}
bool
state
=
DstTypeFusion
：：
Init
（）
&&
DstExistenceFusion
：：
Init
（）
&&
PbfTracker
：：
InitParams
（）；
// DST 型別融合初始化、DST 存在性融合初始化、機率跟蹤器引數初始化
return
state
；
}
ProbabilisticFusion：：Init
主要做了三方面的初始化工作：
ProbabilisticFusion
引數初始化
ProbabilisticFusion
成員初始化
其它初始化：DST 型別融合初始化、DST 存在性融合初始化、機率跟蹤器引數初始化
具體細節不作展開。
3 訊息回撥處理
3。1 對外的訊息處理介面：FusionComponent：：Proc 方法
如前文所述，
FusionComponent
類的
Proc
方法由 Fusion 元件對應 channel 上的訊息進行回撥觸發，我們開啟
apollo/modules/perception/onboard/component/fusion_component。cc
看下
Proc
的定義：
bool
FusionComponent
：：
Proc
（
const
std
：：
shared_ptr
<
SensorFrameMessage
>&
message
）
{
if
（
message
->
process_stage_
==
ProcessStage
：：
SENSOR_FUSION
）
{
return
true
；
}
std
：：
shared_ptr
<
PerceptionObstacles
>
out_message
（
new
（
std
：：
nothrow
）
// 管理融合障礙物訊息的共享智慧指標
PerceptionObstacles
）；
std
：：
shared_ptr
<
SensorFrameMessage
>
viz_message
（
new
（
std
：：
nothrow
）
// 管理視覺化訊息的共享智慧指標
SensorFrameMessage
）；
// TODO（convert sensor id）
const
auto
&
itr
=
std
：：
find
（
fusion_main_sensors_
。
begin
（），
fusion_main_sensors_
。
end
（），
message
->
sensor_id_
）；
if
（
itr
==
fusion_main_sensors_
。
end
（））
{
// 對於來自融合主感測器以外的訊息，不執行後續步驟
AINFO
<<
“Fusion receives message from ”
<<
message
->
sensor_id_
<<
“ which is not in main sensors。 Skip sending。”
；
return
true
；
}
bool
status
=
InternalProc
（
message
，
out_message
，
viz_message
）；
// 呼叫私有訊息處理方法 InternalProc
if
（
status
）
{
writer_
->
Write
（
out_message
）；
// 傳送融合障礙物訊息
AINFO
<<
“Send fusion processing output message。”
；
// send msg for visualization
if
（
FLAGS_obs_enable_visualization
）
{
inner_writer_
->
Write
（
viz_message
）；
// 傳送視覺化訊息
}
}
return
status
；
}
Proc
內部對接收到的感測器訊息進行判斷，若訊息來自融合主感測器以外的其它感測器，則不作處理，否則呼叫私有訊息處理方法
FusionComponent：：InternalProc
對訊息進行融合處理，並視處理結果傳送融合障礙物訊息和視覺化訊息（若使能視覺化 flag 得話）。
3。2 內部的訊息處理介面：FusionComponent：：InternalProc 方法
我們看下
FusionComponent：：InternalProc
的定義：
bool
FusionComponent
：：
InternalProc
（
const
std
：：
shared_ptr
<
SensorFrameMessage
const
>&
in_message
，
std
：：
shared_ptr
<
PerceptionObstacles
>
out_message
，
std
：：
shared_ptr
<
SensorFrameMessage
>
viz_message
）
{
{
std
：：
unique_lock
<
std
：：
mutex
>
lock
（
s_mutex_
）；
s_seq_num_
++
；
}
PERF_BLOCK_START
（）；
const
double
timestamp
=
in_message
->
timestamp_
；
const
uint64_t
lidar_timestamp
=
in_message
->
lidar_timestamp_
；
std
：：
vector
<
base
：：
ObjectPtr
>
valid_objects
；
// 有效融合障礙物
if
（
in_message
->
error_code_
！=
apollo
：：
common
：：
ErrorCode
：：
OK
）
{
// 接收的感測器訊息存在異常
if
（
！
MsgSerializer
：：
SerializeMsg
（
timestamp
，
lidar_timestamp
，
in_message
->
seq_num_
，
valid_objects
，
in_message
->
error_code_
，
out_message
。
get
（）））
{
AERROR
<<
“Failed to gen PerceptionObstacles object。”
；
return
false
；
}
if
（
FLAGS_obs_enable_visualization
）
{
viz_message
->
process_stage_
=
ProcessStage
：：
SENSOR_FUSION
；
viz_message
->
error_code_
=
in_message
->
error_code_
；
}
AERROR
<<
“Fusion receive message with error code， skip it。”
；
return
true
；
}
base
：：
FramePtr
frame
=
in_message
->
frame_
；
// 感測器原始障礙物資訊
frame
->
timestamp
=
in_message
->
timestamp_
；
std
：：
vector
<
base
：：
ObjectPtr
>
fused_objects
；
// 融合障礙物資訊
if
（
！
fusion_
->
Process
（
frame
，
&
fused_objects
））
{
// 多型呼叫 ObstacleMultiSensorFusion：：Process，處理感測器原始障礙物資訊，生成融合障礙物資訊
AERROR
<<
“Failed to call fusion plugin。”
；
return
false
；
}
PERF_BLOCK_END_WITH_INDICATOR
（
“fusion_process”
，
in_message
->
sensor_id_
）；
Eigen
：：
Matrix4d
sensor2world_pose
=
in_message
->
frame_
->
sensor2world_pose
。
matrix
（）；
if
（
object_in_roi_check_
&&
FLAGS_obs_enable_hdmap_input
）
{
// 若開啟了 HD Map ROI 融合障礙物校驗且使能了 HD Map 輸入
// get hdmap
base
：：
HdmapStructPtr
hdmap
（
new
base
：：
HdmapStruct
（））；
if
（
hdmap_input_
）
{
// HD Map 載入成功
base
：：
PointD
position
；
// 感測器在世界座標系中的位置
position
。
x
=
sensor2world_pose
（
0
，
3
）；
position
。
y
=
sensor2world_pose
（
1
，
3
）；
position
。
z
=
sensor2world_pose
（
2
，
3
）；
hdmap_input_
->
GetRoiHDMapStruct
（
position
，
radius_for_roi_object_check_
，
hdmap
）；
// 獲取感測器指定半徑範圍（120m）內的 HD Map
// TODO（use check）
// ObjectInRoiSlackCheck（hdmap， fused_objects， &valid_objects）； // HD Map ROI 融合障礙物校驗
valid_objects
。
assign
（
fused_objects
。
begin
（），
fused_objects
。
end
（））；
// 複製經 HD Map ROI 校驗過的有效融合障礙物到 valid_objects
}
else
{
// HD Map 載入失敗，直接複製融合障礙物到 valid_objects
valid_objects
。
assign
（
fused_objects
。
begin
（），
fused_objects
。
end
（））；
}
}
else
{
// 若未開啟 HD Map ROI 融合障礙物校驗或未使能 HD Map 輸入，直接複製融合障礙物到 valid_objects
valid_objects
。
assign
（
fused_objects
。
begin
（），
fused_objects
。
end
（））；
}
PERF_BLOCK_END_WITH_INDICATOR
（
“fusion_roi_check”
，
in_message
->
sensor_id_
）；
// produce visualization msg // 生成視覺化訊息
if
（
FLAGS_obs_enable_visualization
）
{
viz_message
->
timestamp_
=
in_message
->
timestamp_
；
viz_message
->
seq_num_
=
in_message
->
seq_num_
；
viz_message
->
frame_
=
base
：：
FramePool
：：
Instance
（）。
Get
（）；
viz_message
->
frame_
->
sensor2world_pose
=
in_message
->
frame_
->
sensor2world_pose
；
viz_message
->
sensor_id_
=
in_message
->
sensor_id_
；
viz_message
->
hdmap_
=
in_message
->
hdmap_
；
// 視覺化 HD Map 資訊
viz_message
->
process_stage_
=
ProcessStage
：：
SENSOR_FUSION
；
viz_message
->
error_code_
=
in_message
->
error_code_
；
viz_message
->
frame_
->
objects
=
fused_objects
；
// 視覺化融合障礙物資訊
}
// produce pb output msg // 生成 protobuf 格式的輸出訊息
apollo
：：
common
：：
ErrorCode
error_code
=
apollo
：：
common
：：
ErrorCode
：：
OK
；
if
（
！
MsgSerializer
：：
SerializeMsg
（
timestamp
，
lidar_timestamp
，
// 序列化有效融合障礙物資訊 valid_objects 到輸出訊息
in_message
->
seq_num_
，
valid_objects
，
error_code
，
out_message
。
get
（）））
{
AERROR
<<
“Failed to gen PerceptionObstacles object。”
；
return
false
；
}
PERF_BLOCK_END_WITH_INDICATOR
（
“fusion_serialize_message”
，
in_message
->
sensor_id_
）；
const
double
cur_time
=
：：
apollo
：：
cyber
：：
Clock
：：
NowInSeconds
（）；
const
double
latency
=
（
cur_time
-
timestamp
）
*
1e3
；
// 演算法時延
AINFO
<<
std
：：
setprecision
（
16
）
<<
“FRAME_STATISTICS：Obstacle：End：msg_time［”
<<
timestamp
<<
“］：cur_time［”
<<
cur_time
<<
“］：cur_latency［”
<<
latency
<<
“］：obj_cnt［”
<<
valid_objects
。
size
（）
<<
“］”
；
AINFO
<<
“publish_number： ”
<<
valid_objects
。
size
（）
<<
“ obj”
；
return
true
；
}
從程式碼中我們不難看出，
FusionComponent：：InternalProc
方法主要做了四件事：
呼叫
ObstacleMultiSensorFusion：：Process
方法，處理輸入資訊，生成融合障礙物資訊
fused_objects
使用 HD Map ROI 校驗融合障礙物有效性，生成有效融合障礙物資訊
valid_objects
生成視覺化訊息
viz_message
序列化有效融合障礙物資訊
valid_objects
，生成 protobuf 格式的輸出訊息
out_message
需要指出的是，HD Map ROI 融合障礙物校驗方法
ObjectInRoiSlackCheck
在 Apollo 中尚未實現，相應的校驗步驟在
FusionComponent：：InternalProc
中也被註釋掉了，所以最終的有效融合障礙物資訊
valid_objects
與融合障礙物資訊
fused_objects
是相同的。
4 融合
4。1 ObstacleMultiSensorFusion：：Process 方法
FusionComponent：：InternalProc
方法呼叫的
ObstacleMultiSensorFusion：：Process
方法定義在
apollo/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：：Fuse
}
參照前文分析，
ObstacleMultiSensorFusion：：fusion_
所管理物件的靜態型別雖然是
BaseFusionSystem
，但由於其指標被繫結到工廠方法返回的
ProbabilisticFusion
型別指標上，故
ObstacleMultiSensorFusion：：Process
方法中的語句
return
fusion_
->
Fuse
（
options
，
frame
，
objects
）；
會多型呼叫
ProbabilisticFusion：：Fuse
方法。
4。2 ProbabilisticFusion：：Fuse 方法
ProbabilisticFusion：：Fuse
方法定義在
apollo/modules/perception/fusion/lib/fusion_system/probabilistic_fusion/probabilistic_fusion。cc
中：
bool
ProbabilisticFusion
：：
Fuse
（
const
FusionOptions
&
options
，
const
base
：：
FrameConstPtr
&
sensor_frame
，
std
：：
vector
<
base
：：
ObjectPtr
>*
fused_objects
）
{
if
（
fused_objects
==
nullptr
）
{
AERROR
<<
“fusion error： fused_objects is nullptr”
；
return
false
；
}
// 感測器資料管理單例類的唯一例項指標，管理各個感測器的歷史 10 幀資料
auto
*
sensor_data_manager
=
SensorDataManager
：：
Instance
（）；
// 1。 save frame data // 儲存當前資料幀
{
std
：：
lock_guard
<
std
：：
mutex
>
data_lock
（
data_mutex_
）；
// 資料鎖
if
（
！
params_
。
use_lidar
&&
sensor_data_manager
->
IsLidar
（
sensor_frame
））
{
return
true
；
// 若不融合 Lidar 資料且當前幀來自 Lidar，則直接返回
}
if
（
！
params_
。
use_radar
&&
sensor_data_manager
->
IsRadar
（
sensor_frame
））
{
return
true
；
// 若不融合 Radar 資料且當前幀來自 Radar，則直接返回
}
if
（
！
params_
。
use_camera
&&
sensor_data_manager
->
IsCamera
（
sensor_frame
））
{
return
true
；
// 若不融合 Camera 資料且當前幀來自 Camera，則直接返回
}
// 當前幀是否來自可釋出感測器（與主感測器相同：velodyne128，front_6mm，front_12mm）
bool
is_publish_sensor
=
this
->
IsPublishSensor
（
sensor_frame
）；
if
（
is_publish_sensor
）
{
// 若當前幀來自可釋出感測器
started_
=
true
；
// 使能資料快取啟動變數（預設為 false）
}
if
（
started_
）
{
// 若啟動資料快取
AINFO
<<
“add sensor measurement： ”
<<
sensor_frame
->
sensor_info
。
name
<<
“， obj_cnt ： ”
<<
sensor_frame
->
objects
。
size
（）
<<
“， ”
<<
FORMAT_TIMESTAMP
（
sensor_frame
->
timestamp
）；
sensor_data_manager
->
AddSensorMeasurements
（
sensor_frame
）；
// 快取當前資料幀到對應感測器的歷史資料序列中
}
// 對於來自可釋出感測器以外的訊息，不執行後續步驟，意味著只有來自可釋出感測器的訊息才可以觸發融合動作
if
（
！
is_publish_sensor
）
{
return
true
；
}
}
// 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
）；
// 獲取每個感測器歷史資料中與當前融合時間最接近（≤）的資料幀組成的序列
AINFO
<<
“Get ”
<<
frames
。
size
（）
<<
“ related frames for fusion”
；
// 3。 perform fusion on related frames // 融合所有的相關資料幀
for
（
const
auto
&
frame
：
frames
）
{
// 對每一幀資料
FuseFrame
（
frame
）；
// 融合單幀資料，最終的融合演算法入口
}
// 4。 collect fused objects // 從前景航跡和背景航跡中收集可被髮布的融合目標
CollectFusedObjects
（
fusion_time
，
fused_objects
）；
return
true
；
}
這裡我們需要先簡單介紹下四個至關重要的類：
SensorDataManager
：感測器資料管理單例類，透過一個無序對映
std：：unordered_map
管理所有感測器的資料，每個感測器的資料型別是
Sensor
，
SensorDataManager
內部含有一個關鍵方法
AddSensorMeasurements
用於快取感測器資料；
Sensor
：感測器資料類，透過一個雙端佇列
std：：deque
維護感測器 10 幀的歷史資料，每一幀的型別是
SensorFrame
，
Sensor
內部含有一個關鍵方法
AddFrame
，
AddFrame
將
Frame
型別的新的資料幀轉換為
SensorFrame
型別的資料，並新增到歷史資料佇列中。
Frame
資料到
SensorFrame
資料轉換的過程中完成了資料中目標的前景與背景分類；
SensorFrame
：感測器資料幀類，透過
std：：vector
維護每一幀的前景目標列表
foreground_objects_
和背景目標列表
background_objects_
，每個目標的型別是
SensorObject
；
SensorObject
：感測器目標類。
下圖展示了四個關鍵類之間的關係：
感測器的資料管理
下面我們依次展開
ProbabilisticFusion：：Fuse
方法中的四個主要流程。
4。2。1 儲存當前資料幀
作以下幾點說明：
判斷當前幀資料是否需要參與融合，若不需要，則直接返回；
ProbabilisticFusion
中有
可釋出感測器
（Publish Sensor）的概念，即
std：：vector
成員，被初始化為與
FusionComponent：：fusion_main_sensors_
相同的內容：velodyne128、front_6mm 和 front_12mm；
ProbabilisticFusion
類內部含有一個布林型別的資料快取啟動變數
started_
，預設為
false
，只有接收到過來自可釋出感測器的資料才會將
started_
置
true
以啟動資料快取，若啟動了資料快取，則呼叫
SensorDataManager：：AddSensorMeasurements
方法將當前資料幀快取到對應感測器的歷史資料佇列
std：：deque
中；
對於來自可釋出感測器以外的訊息，只作快取，不執行後續步驟，意味著只有來自可釋出感測器的訊息才可以觸發融合動作。
4。2。2 查詢各個感測器歷史資料中可參與融合的相關資料幀
透過
SensorDataManager：：GetLatestFrames
方法分別獲取（
Sensor：：QueryLatestFrame
）每個感測器的歷史資料中與當前資料幀時間戳最接近的那一幀資料，這樣可以得到與感測器類別數量相同的若干幀最新歷史資料，再透過
std：：sort
對這若干幀最新歷史資料進行升序排序，最終得到可參與融合的相關資料幀序列。
4。2。3 融合所有的相關資料幀
對所有的相關資料幀，迴圈呼叫
ProbabilisticFusion：：FuseFrame
方法進行融合，這是最終的融合演算法入口，後文將會詳細闡述。
4。2。4 從前景航跡和背景航跡中收集可被髮布的融合目標
執行完所有相關資料幀的融合動作後，透過在
ProbabilisticFusion：：CollectFusedObjects
方法中呼叫
PbfGatekeeper：：AbleToPublish
方法判斷相應融合航跡是否滿足可釋出邏輯，可釋出邏輯具體細節不作展開，其主要與以下幾點有關：
PbfGatekeeper
配置引數
各感測器與航跡關聯過的最新歷史量測的可見性
航跡融合目標類別
航跡融合目標時間戳對應的本地時間
與航跡關聯過的某種感測器的最新歷史量測的距離、速度、置信度、所屬的子感測器類別
航跡被跟蹤上的次數
對於可被髮布的航跡，透過
ProbabilisticFusion：：CollectObjectsByTrack
方法提取出融合目標資訊。
4。3 最終的融合演算法入口：ProbabilisticFusion：：FuseFrame 方法
上文中提到
ProbabilisticFusion：：FuseFrame
方法是最終的融合演算法入口：
void
ProbabilisticFusion
：：
FuseFrame
（
const
SensorFramePtr
&
frame
）
{
AINFO
<<
“Fusing frame： ”
<<
frame
->
GetSensorId
（）
<<
“， foreground_object_number： ”
<<
frame
->
GetForegroundObjects
（）。
size
（）
<<
“， background_object_number： ”
<<
frame
->
GetBackgroundObjects
（）。
size
（）
<<
“， timestamp： ”
<<
FORMAT_TIMESTAMP
（
frame
->
GetTimestamp
（））；
this
->
FuseForegroundTrack
（
frame
）；
// 前景航跡融合
this
->
FusebackgroundTrack
（
frame
）；
// 背景航跡融合
this
->
RemoveLostTrack
（）；
// 移除丟失航跡
}
可以看到，對於某一幀相關資料幀，執行了三個動作：
前景航跡融合
背景航跡融合
移除丟失航跡
在談航跡融合前，需要先看下定義在
apollo/modules/perception/fusion/base/track。h
中的航跡類
Track
都包含哪些資料成員：
class
Track
{
//
// ······
// 此處省略了方法成員
// ······
//
protected
：
SensorId2ObjectMap
lidar_objects_
；
// 所有的 Lidar 各自與當前航跡關聯過的最新歷史量測
SensorId2ObjectMap
radar_objects_
；
// 所有的 Radar 各自與當前航跡關聯過的最新歷史量測
SensorId2ObjectMap
camera_objects_
；
// 所有的 Camera 各自與當前航跡關聯過的最新歷史量測
FusedObjectPtr
fused_object_
=
nullptr
；
// 管理當前航跡融合目標的共享智慧指標
double
tracking_period_
=
0。0
；
// 當前航跡的跟蹤時長
double
existence_prob_
=
0。0
；
// 當前航跡的存在機率
double
toic_prob_
=
0。0
；
bool
is_background_
=
false
；
// 當前航跡是否是背景航跡
bool
is_alive_
=
true
；
// 當前航跡是否存活
size_t
tracked_times_
=
0
；
// 當前航跡的跟蹤次數
private
：
FRIEND_TEST
（
TrackTest
，
test
）；
static
size_t
s_track_idx_
；
// track id
static
double
s_max_lidar_invisible_period_
；
// Lidar 歷史量測最大不可見時長：0。25s
static
double
s_max_radar_invisible_period_
；
// Radar 歷史量測最大不可見時長：0。50s
static
double
s_max_camera_invisible_period_
；
// Camera 歷史量測最大不可見時長：0。75s
}；
lidar_objects_
、
radar_objects_
和
camera_objects_
分別存放了某種型別感測器中每種子型別感測器與當前航跡關聯過的最新歷史量測，以
camera_objects_
為例，它存放了每種 Camera 與當前航跡關聯過的最新歷史量測。
4。3。1 前景航跡融合
ProbabilisticFusion：：FuseForegroundTrack
是前景航跡融合方法，其大體流程與背景航跡融合類似，但具體細節內容篇幅巨大，後續會單開一篇文章詳細講解。
4。3。2 背景航跡融合
void
ProbabilisticFusion
：：
FusebackgroundTrack
（
const
SensorFramePtr
&
frame
）
{
// 1。 association // 資料關聯
size_t
track_size
=
scenes_
->
GetBackgroundTracks
（）。
size
（）；
// 背景航跡數量
size_t
obj_size
=
frame
->
GetBackgroundObjects
（）。
size
（）；
// 當前幀的背景目標數量
std
：：
map
<
int
，
size_t
>
local_id_2_track_ind_map
；
// 背景航跡融合目標 track id 與航跡索引間的對映關係
std
：：
vector
<
bool
>
track_tag
（
track_size
，
false
）；
// 航跡匹配結果標籤列表：0 - 不存在與航跡匹配的量測，1 - 存在與航跡匹配的量測
std
：：
vector
<
bool
>
object_tag
（
obj_size
，
false
）；
// 量測匹配結果標籤列表：0 - 不存在與量測匹配的航跡，1 - 存在與量測匹配的航跡
std
：：
vector
<
TrackMeasurmentPair
>
assignments
；
// 航跡與量測的配對關係列表
std
：：
vector
<
TrackPtr
>&
background_tracks
=
scenes_
->
GetBackgroundTracks
（）；
// 所有的背景航跡
for
（
size_t
i
=
0
；
i
<
track_size
；
++
i
）
{
// 對每個背景航跡
const
FusedObjectPtr
&
obj
=
background_tracks
［
i
］
->
GetFusedObject
（）；
// 背景航跡中的融合目標
int
local_id
=
obj
->
GetBaseObject
（）
->
track_id
；
// 背景航跡中融合目標的 track id
local_id_2_track_ind_map
［
local_id
］
=
i
；
// 建立背景航跡融合目標 track id 與航跡索引間的對映關係
}
std
：：
vector
<
SensorObjectPtr
>&
frame_objs
=
frame
->
GetBackgroundObjects
（）；
// 當前幀的所有背景目標
for
（
size_t
i
=
0
；
i
<
obj_size
；
++
i
）
{
// 對每個背景目標
int
local_id
=
frame_objs
［
i
］
->
GetBaseObject
（）
->
track_id
；
// 背景目標的 track id
const
auto
&
it
=
local_id_2_track_ind_map
。
find
（
local_id
）；
// 查詢是否存在與當前背景目標 track id 相同的背景航跡融合目標
if
（
it
！=
local_id_2_track_ind_map
。
end
（））
{
// 存在與當前背景目標 track id 相同的背景航跡融合目標
size_t
track_ind
=
it
->
second
；
// 與當前背景目標對應的背景航跡索引
assignments
。
push_back
（
std
：：
make_pair
（
track_ind
，
i
））；
// 構造航跡與量測的配對關係
track_tag
［
track_ind
］
=
true
；
// 將航跡匹配結果標籤列表中的對應元素置 true
object_tag
［
i
］
=
true
；
// 將量測匹配結果標籤列表中的對應元素置 true
continue
；
}
}
// 2。 update assigned track // 更新匹配上的航跡
for
（
size_t
i
=
0
；
i
<
assignments
。
size
（）；
++
i
）
{
// 對航跡與量測配對關係列表中的每一組配對關係
int
track_ind
=
static_cast
<
int
>
（
assignments
［
i
］。
first
）；
// 背景航跡索引
int
obj_ind
=
static_cast
<
int
>
（
assignments
［
i
］。
second
）；
// 與背景航跡關聯上的當前幀背景目標索引
background_tracks
［
track_ind
］
->
UpdateWithSensorObject
（
frame_objs
［
obj_ind
］）；
// 使用背景目標更新與其關聯上的背景航跡
}
// 3。 update unassigned track // 更新未被匹配上的航跡
std
：：
string
sensor_id
=
frame
->
GetSensorId
（）；
// 當前量測幀所屬的感測器 id
for
（
size_t
i
=
0
；
i
<
track_tag
。
size
（）；
++
i
）
{
// 對航跡匹配結果標籤列表中的每個元素
if
（
！
track_tag
［
i
］）
{
// 如果背景航跡不存在與之匹配的量測
background_tracks
［
i
］
->
UpdateWithoutSensorObject
（
sensor_id
，
// 更新該未被匹配上的背景航跡
frame
->
GetTimestamp
（））；
}
}
// 4。 create new track // 建立新航跡
for
（
size_t
i
=
0
；
i
<
object_tag
。
size
（）；
++
i
）
{
// 對量測匹配結果標籤列表中的每個元素
if
（
！
object_tag
［
i
］）
{
// 如果量測不存在與之匹配的背景航跡
TrackPtr
track
=
TrackPool
：：
Instance
（）。
Get
（）；
// 從航跡池中獲取一個未經初始化的航跡
track
->
Initialize
（
frame
->
GetBackgroundObjects
（）［
i
］，
true
）；
// 使用未被匹配上的量測目標初始化背景航跡
scenes_
->
AddBackgroundTrack
（
track
）；
// 將新的背景航跡新增到場景的背景航跡列表中
}
}
}
背景航跡融合的流程與前景航跡融合類似，都是由下面四個主要步驟組成：
4。3。2。1 資料關聯
航跡與量測的關聯是首要步驟，要做的是找出航跡與量測的對應關係。從程式碼註釋中可以看出，背景航跡融合的資料關聯策略很簡單，只做了背景航跡融合目標與當前幀背景目標的 track id 關聯。
4。3。2。2 更新匹配上的航跡
完成航跡與量測的關聯後，透過
Track：：UpdateWithSensorObject
方法更新與量測匹配上的背景航跡：
void
Track
：：
UpdateWithSensorObject
（
const
SensorObjectPtr
&
obj
）
{
std
：：
string
sensor_id
=
obj
->
GetSensorId
（）；
// 量測目標所屬的感測器 id
SensorId2ObjectMap
*
objects
=
nullptr
；
if
（
IsLidar
（
obj
））
{
// 量測目標來自 Lidar
objects
=
&
lidar_objects_
；
// 獲取所有的 Lidar 各自與當前航跡關聯過的最新歷史量測
}
else
if
（
IsRadar
（
obj
））
{
// 目標來自 Radar
objects
=
&
radar_objects_
；
// 獲取所有的 Radar 各自與當前航跡關聯過的最新歷史量測
}
else
if
（
IsCamera
（
obj
））
{
// 目標來自 Camera
objects
=
&
camera_objects_
；
// 獲取所有的 Camera 各自與當前航跡關聯過的最新歷史量測
}
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_
）；
// 刪除不可見時長超過閾值的 Lidar 歷史量測
UpdateSensorObjectWithMeasurement
（
&
radar_objects_
，
sensor_id
，
obj
->
GetTimestamp
（），
s_max_radar_invisible_period_
）；
// 刪除不可見時長超過閾值的 Radar 歷史量測
UpdateSensorObjectWithMeasurement
（
&
camera_objects_
，
sensor_id
，
obj
->
GetTimestamp
（），
s_max_camera_invisible_period_
）；
// 刪除不可見時長超過閾值的 Camera 歷史量測
if
（
is_background_
）
{
// 如果當前航跡是背景航跡
return
UpdateWithSensorObjectForBackground
（
obj
）；
// 更新背景航跡融合目標並返回
}
// 下面的程式碼只適用於前景航跡融合，暫不做分析
fused_object_
->
GetBaseObject
（）
->
latest_tracked_time
=
obj
->
GetTimestamp
（）；
UpdateSupplementState
（
obj
）；
UpdateUnfusedState
（
obj
）；
is_alive_
=
true
；
}
首先，透過
IsLidar
方法、
IsRadar
方法和
IsCamera
方法判斷量測目標所屬的大的感測器類別：Lidar、Radar 或 Camera。
然後，透過
Track：：UpdateSensorObject
方法新建或更新量測目標所屬感測器與當前航跡關聯過的最新歷史量測：
void
Track
：：
UpdateSensorObject
（
SensorId2ObjectMap
*
objects
，
const
SensorObjectPtr
&
obj
）
{
std
：：
string
sensor_id
=
obj
->
GetSensorId
（）；
// 量測目標所屬的感測器 id
auto
it
=
objects
->
find
（
sensor_id
）；
// 查詢來自量測目標所屬感測器的資料是否與當前航跡關聯過
if
（
it
==
objects
->
end
（））
{
// 來自量測目標所屬感測器的資料未與當前航跡關聯過
（
*
objects
）［
sensor_id
］
=
obj
；
// 新增與當前航跡關聯過的 ［感測器-最新歷史量測］ 對映關係
}
else
{
// 來自量測目標所屬感測器的資料與當前航跡關聯過
it
->
second
=
obj
；
// 更新對應感測器與當前航跡關聯過的最新歷史量測
}
}
其次，透過
Track：：UpdateSensorObjectWithMeasurement
方法刪除 Lidar、Radar 和 Camera 中不可見時長超過閾值的歷史量測：
void
Track
：：
UpdateSensorObjectWithMeasurement
（
SensorId2ObjectMap
*
objects
，
const
std
：：
string
&
sensor_id
，
double
measurement_timestamp
，
double
max_invisible_period
）
{
for
（
auto
it
=
objects
->
begin
（）；
it
！=
objects
->
end
（）；）
{
// 對於某種型別感測器（Lidar，Radar，Camera）下的各種子型別感測器
if
（
it
->
first
！=
sensor_id
）
{
// 如果量測目標不是來自當前子型別感測器
double
period
=
measurement_timestamp
-
it
->
second
->
GetTimestamp
（）；
// 量測目標和當前子型別感測器與航跡關聯過的最新歷史量測間的時間間隔
if
（
period
>
max_invisible_period
）
{
// 如果時間間隔超過對應的最大不可見時長
it
->
second
=
nullptr
；
// 清空當前子型別感測器與航跡關聯過的最新歷史量測
it
=
objects
->
erase
（
it
）；
// 擦除當前子型別感測器的歷史量測記錄
}
else
{
// 時間間隔小於對應的最大不可見時長
++
it
；
// 處理下一個子型別感測器
}
}
else
{
// 量測目標來自當前子型別感測器
++
it
；
// 處理下一個子型別感測器
}
}
}
最後，透過
Track：：UpdateWithSensorObjectForBackground
方法更新背景航跡的融合目標：
void
Track
：：
UpdateWithSensorObjectForBackground
（
const
SensorObjectPtr
&
obj
）
{
std
：：
shared_ptr
<
base
：：
Object
>
fused_base_object
=
fused_object_
->
GetBaseObject
（）；
// 背景航跡融合目標
std
：：
shared_ptr
<
const
base
：：
Object
>
measurement_base_object
=
obj
->
GetBaseObject
（）；
// 量測目標
int
track_id
=
fused_base_object
->
track_id
；
// 暫存背景航跡融合目標的 track id
*
fused_base_object
=
*
measurement_base_object
；
// 使用量測目標直接替換背景航跡融合目標
fused_base_object
->
track_id
=
track_id
；
// 維持背景航跡融合目標 track id 不變
}
可以看出，背景航跡融合目標的更新過程比較簡單，只是單純地使用量測目標替換了融合目標，並維持融合目標 track id 不變。
4。3。2。3 更新未被匹配上的航跡
透過
Track：：UpdateWithoutSensorObject
方法更新未與量測匹配上的背景航跡：
void
Track
：：
UpdateWithoutSensorObject
（
const
std
：：
string
&
sensor_id
，
double
measurement_timestamp
）
{
UpdateSensorObjectWithoutMeasurement
（
&
lidar_objects_
，
sensor_id
，
measurement_timestamp
，
s_max_lidar_invisible_period_
）；
// 設定與航跡關聯過的 Lidar 最新歷史量測的不可見時長，並刪除不可見時長超過閾值的 Lidar 歷史量測
UpdateSensorObjectWithoutMeasurement
（
&
radar_objects_
，
sensor_id
，
measurement_timestamp
，
s_max_radar_invisible_period_
）；
// 設定與航跡關聯過的 Radar 最新歷史量測的不可見時長，並刪除不可見時長超過閾值的 Radar 歷史量測
UpdateSensorObjectWithoutMeasurement
（
&
camera_objects_
，
sensor_id
，
measurement_timestamp
，
s_max_camera_invisible_period_
）；
// 設定與航跡關聯過的 Camera 最新歷史量測的不可見時長，並刪除不可見時長超過閾值的 Camera 歷史量測
UpdateSupplementState
（）；
// 更新航跡融合目標的補充屬性狀態（形參預設為 nullptr）
is_alive_
=
（
！
lidar_objects_
。
empty
（））
||
（
！
radar_objects_
。
empty
（））
||
（
！
camera_objects_
。
empty
（））；
// 當前航跡至少擁有一個與之關聯過且不可見時長未超過閾值的歷史量測才允許存活
}
首先，透過
Track：：UpdateSensorObjectWithoutMeasurement
方法設定與航跡關聯過的某種感測器的最新歷史量測的不可見時長，並刪除不可見時長超過閾值的歷史量測：
void
Track
：：
UpdateSensorObjectWithoutMeasurement
（
SensorId2ObjectMap
*
objects
，
const
std
：：
string
&
sensor_id
，
double
measurement_timestamp
，
double
max_invisible_period
）
{
for
（
auto
it
=
objects
->
begin
（）；
it
！=
objects
->
end
（）；）
{
// 對於某種型別感測器（Lidar，Radar，Camera）下的各種子型別感測器
double
period
=
measurement_timestamp
-
it
->
second
->
GetTimestamp
（）；
// 量測幀和當前子型別感測器與航跡關聯過的最新歷史量測間的時間間隔
if
（
it
->
first
==
sensor_id
）
{
// 如果量測幀來自當前子型別感測器
it
->
second
->
SetInvisiblePeriod
（
period
）；
// 設定當前子型別感測器與航跡關聯過的最新歷史量測的不可見時長
}
else
if
（
it
->
second
->
GetInvisiblePeriod
（）
>
0。0
）
{
// 如果量測幀不是來自當前子型別感測器，且當前子型別感測器與航跡關聯過的最新歷史量測的不可見時長大於 0
it
->
second
->
SetInvisiblePeriod
（
period
）；
// 設定當前子型別感測器與航跡關聯過的最新歷史量測的不可見時長
}
if
（
period
>
max_invisible_period
）
{
// 如果時間間隔超過對應的最大不可見時長
it
->
second
=
nullptr
；
// 清空當前子型別感測器與航跡關聯過的最新歷史量測
it
=
objects
->
erase
（
it
）；
// 擦除當前子型別感測器的歷史量測記錄
}
else
{
// 時間間隔小於對應的最大不可見時長
++
it
；
// 處理下一個子型別感測器
}
}
}
然後，透過
Track：：UpdateSupplementState
方法更新航跡融合目標的補充屬性狀態（形參預設為
nullptr
）：
void
Track
：：
UpdateSupplementState
（
const
SensorObjectPtr
&
src_object
）
{
std
：：
shared_ptr
<
base
：：
Object
>
dst_obj
=
fused_object_
->
GetBaseObject
（）；
// 航跡融合目標
if
（
src_object
！=
nullptr
）
{
// 如果輸入的量測目標形參不為空
std
：：
shared_ptr
<
const
base
：：
Object
>
src_obj
=
src_object
->
GetBaseObject
（）；
// 量測目標
if
（
IsLidar
（
src_object
））
{
// 量測目標來自 Lidar
dst_obj
->
lidar_supplement
=
src_obj
->
lidar_supplement
；
// 使用量測目標的 Lidar 補充屬性更新航跡融合目標的 Lidar 補充屬性
}
else
if
（
IsRadar
（
src_object
））
{
// 量測目標來自 Radar
dst_obj
->
radar_supplement
=
src_obj
->
radar_supplement
；
// 使用量測目標的 Radar 補充屬性更新航跡融合目標的 Radar 補充屬性
}
else
if
（
IsCamera
（
src_object
））
{
// 量測目標來自 Camera
dst_obj
->
camera_supplement
=
src_obj
->
camera_supplement
；
// 使用量測目標的 Camera 補充屬性更新航跡融合目標的 Camera 補充屬性
}
}
if
（
lidar_objects_
。
empty
（））
{
// 不存在有效的與當前航跡關聯過的 Lidar 歷史量測
dst_obj
->
lidar_supplement
。
Reset
（）；
// 重置航跡融合目標的 Lidar 補充屬性
}
if
（
radar_objects_
。
empty
（））
{
// 不存在有效的與當前航跡關聯過的 Radar 歷史量測
dst_obj
->
radar_supplement
。
Reset
（）；
// 重置航跡融合目標的 Radar 補充屬性
}
if
（
camera_objects_
。
empty
（））
{
// 不存在有效的與當前航跡關聯過的 Camera 歷史量測
dst_obj
->
camera_supplement
。
Reset
（）；
// 重置航跡融合目標的 Camera 補充屬性
}
}
最後，若當前航跡至少擁有一個與之關聯過且不可見時長未超過閾值的歷史量測，則允許該航跡存活。
4。3。2。4 建立新航跡
對於未與背景航跡匹配上的量測，需要為其建立新的背景航跡。首先，從航跡池中獲取一個未經初始化的航跡。
然後，透過
Track：：Initialize
方法使用未被匹配上的量測目標初始化背景航跡：
bool
Track
：：
Initialize
（
SensorObjectPtr
obj
，
bool
is_background
）
{
Reset
（）；
// 重置航跡的各個屬性
int
track_id
=
static_cast
<
int
>
（
GenerateNewTrackId
（））；
// 生成新的 track id
is_background_
=
is_background
；
// 是否是背景航跡
std
：：
shared_ptr
<
base
：：
Object
>
fused_base_obj
=
fused_object_
->
GetBaseObject
（）；
// 航跡融合目標
std
：：
shared_ptr
<
const
base
：：
Object
>
sensor_base_obj
=
obj
->
GetBaseObject
（）；
// 量測目標
*
fused_base_obj
=
*
sensor_base_obj
；
// 將量測目標複製到航跡融合目標
fused_base_obj
->
track_id
=
track_id
；
// 更新航跡融合目標 track id
UpdateWithSensorObject
（
obj
）；
// 使用量測目標更新該航跡
return
true
；
}
Track：：Initialize
方法內部也會呼叫
Track：：UpdateWithSensorObject
方法，參考前文，此處不再贅述。
最後，透過
Scene：：AddBackgroundTrack
方法將新的背景航跡新增到場景的背景航跡列表中。
4。3。3 移除丟失航跡
如前文所述，
Track：：is_alive_
成員表徵了航跡是否存活，若航跡至少擁有一個與之關聯過且不可見時長未超過閾值的歷史量測，則允許該航跡存活。對於已經失活的前景航跡和背景航跡，透過
ProbabilisticFusion：：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
）
{
// 對於每一個前景航跡
if
（
foreground_tracks
［
i
］
->
IsAlive
（））
{
// 當前前景航跡存活
if
（
i
！=
foreground_track_count
）
{
// 當前存活的前景航跡之前存在失活航跡
foreground_tracks
［
foreground_track_count
］
=
foreground_tracks
［
i
］；
// 將當前存活的前景航跡移動到前景航跡列表新的位置
trackers_
［
foreground_track_count
］
=
trackers_
［
i
］；
// 將當前存活的前景航跡對應的 tracker 移動到 tracker 列表新的位置
}
foreground_track_count
++
；
// 存活的前景航跡計數加 1
}
}
AINFO
<<
“Remove ”
<<
foreground_tracks
。
size
（）
-
foreground_track_count
<<
“ foreground tracks。 ”
<<
foreground_track_count
<<
“ tracks left。”
；
foreground_tracks
。
resize
（
foreground_track_count
）；
// 析構前景航跡列表尾部多餘的元素
trackers_
。
resize
（
foreground_track_count
）；
// 析構 tracker 列表尾部多餘的元素
// 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
++
；
// 存活的背景航跡計數加 1
}
}
AINFO
<<
“Remove ”
<<
background_tracks
。
size
（）
-
background_track_count
<<
“ background tracks”
；
background_tracks
。
resize
（
background_track_count
）；
// 析構背景航跡列表尾部多餘的元素
}
需要指出的是，每個前景航跡都有一個與之對應的 tracker，在移除失活前景航跡的同時，需要同時移除相應的 tracker。
5 總結
本文詳細分析了 Apollo 6。0 Perception 模組 Fusion 元件的初始化、訊息回撥處理以及障礙物融合的主體演算法框架。
ProbabilisticFusion：：FuseFrame
方法是最終的融合演算法入口，對於某一幀相關資料幀，該方法都執行了三個動作：
前景航跡融合
背景航跡融合
移除丟失航跡
文章剖析了“背景航跡融合”和“移除丟失航跡”部分的具體實現，前景航跡融合部分由於篇幅巨大，我們將在後續的文章中單獨講述。
參考
自動駕駛 Apollo 原始碼分析系列，感知篇（八）：感知融合程式碼的基本流程
Apollo perception 原始碼閱讀 | fusion
如何新增新的 fusion 融合系統
How exactly does __attribute__（（constructor）） work？
Apollo 原始碼分析：視覺感知 （v5。5）
Abstract Factory in C++
Factory Method in C++
抽象工廠模式和工廠模式的區別？
如果覺得文章對你有所幫助的話，歡迎點贊/評論/收藏/關注，相互交流，共同進步~
上一篇:天大漢碩考研經驗
下一篇：年近40，考MBA有希望嗎？
