#! https://zhuanlan.zhihu.com/p/389572333

Apollo perception源码阅读 | fusion之D-S证据理论

本文为Apollo感知融合源码阅读笔记，建议参照Apollo6.0源码阅读本文，水平有限，有错误的地方希望大佬多加指正!
个人代码注释链接： leox24 / perception_learn

1 D-S证据理论算法原理

1.1 算法教案

原理和例题建议理解下面两篇文章，看完这两篇应该就能差不多就会计算D-S证据理论公式了，个人感觉理论核心其实就是对交集的乘积求和…

• D-S证据理论__浙大课件PPT

• D-S证据理论

1.2 mass函数、信度函数、似真度函数

以下为《多源信息融合（第二版）韩崇昭》 关于D-S证据理论的定义和定理，和上面两篇差不多

• 设$H$表示某个有限集合，称为假设空间，这是一个完备的、元素间相互不交的空间。

• 假定$O$表示观测空间，或称为实验结果集合。

• 对于给定的假设$h\in H$，， μ h \mu_h μh​是观测空间$O$上的概率

• 证据空间定义为 ϵ = { H , O , μ h 1 , μ h 2 , . . . , μ h n } \epsilon = \{H, O, \mu_{h1}, \mu_{h2},..., \mu_{hn} \} ϵ={H,O,μh1​,μh2​,...,μhn​}

• 假定$P(H)$表示$H$的所有子集构成的集类（称为$H$的幂集），映射$m:P(H)\to [0,1]$称为一个基本概率赋值（basic propability assignment，BPA）或mass函数，实际是对各种假设的评价权值。mass函数/BPA不是概率，不满足可列可加性。 m ( ∅ ) = 0 ; ∑ A ⊆ H m ( A ) = 1 m(\emptyset)=0; \displaystyle \sum_{A \subseteq H}{m(A)} = 1 m(∅)=0;A⊆H∑​m(A)=1

• 假定$P(H)$表示$H$的所有子集构成的集类（称为$H$的幂集），映射$Bel:P(H)\to [0,1]$称为一个信度函数（belief function）， B e l ( A ) = ∑ D ⊆ A m ( D ) Bel(A) = \displaystyle \sum_{D \subseteq A}{m(D)} Bel(A)=D⊆A∑​m(D)

• 假定$P(H)$表示$H$的所有子集构成的集类（称为$H$的幂集），映射$Pl:P(H)\to [0,1]$称为一个似真度函数（plausibility function）, P l ( A ) = ∑ D ∩ A ≠ ∅ m ( D ) Pl(A) = \displaystyle \sum_{D \cap A \neq \emptyset}{m(D)} Pl(A)=D∩A=∅∑​m(D)

• $Pl(A)\ge Bel(A)$

• 设$H$是有限集合，$Bel$和$Pl$分别是定义在$P(H)$上的信度函数和似真度函数，对于任意$A\in P(H)$，其信度区间定义为
  $$[Bel(A),Pl(A)]\subseteq [0,1]$$

• 设 z 1 , z 2 , . . . , z l ∈ O z_1,z_2,...,zl \in O z1​,z2​,...,zl∈O为L个互斥且完备的观测，即 μ ( z i ) \mu(z_i) μ(zi​)表示第i个观测发生的概率，满足 z i ∩ z j = ∅ , ∀ ≠ j z_i \cap z_j = \emptyset, \forall \neq j zi​∩zj​=∅,∀=j且 ∑ i = 1 l μ ( z i ) = 1 \displaystyle \sum^l_{i=1}\mu(z_i)=1 i=1∑l​μ(zi​)=1，对于每个 z i ∈ O z_i \in O zi​∈O，当 m ( ∗ ∣ z i ) , B e l ( ∗ ∣ z i ) , P l ( ∗ ∣ z i ) m(* | z_i),Bel(* | z_i),Pl(* | z_i) m(∗∣zi​),Bel(∗∣zi​),Pl(∗∣zi​)分别是$H$上的mass函数，信度函数和似真度函数时，则：
  m ( A ) = ∑ i = 1 l m ( A ∣ z i ) μ ( z i ) m(A) = \displaystyle \sum^l_{i=1}{m(A | z_i)\mu(z_i)} m(A)=i=1∑l​m(A∣zi​)μ(zi​)
  B e l ( A ) = ∑ i = 1 l B e l ( A ∣ z i ) μ ( z i ) Bel(A) = \displaystyle \sum^l_{i=1}{Bel(A | z_i)\mu(z_i)} Bel(A)=i=1∑l​Bel(A∣zi​)μ(zi​)
  P l ( A ) = ∑ i = 1 l P l ( A ∣ z i ) μ ( z i ) Pl(A) = \displaystyle \sum^l_{i=1}{Pl(A | z_i)\mu(z_i)} Pl(A)=i=1∑l​Pl(A∣zi​)μ(zi​)

1.3 Dempster-Shafer合成公式

• 设 m 1 , m 2 m_1,m_2 m1​,m2​是$H$上的两个mass函数，则
  $$m(\varnothing )=0$$
  m ( A ) = 1 N ∑ E ∩ F = A m 1 ( E ) m 2 ( F ) , A ≠ ∅ m(A) = \frac1N \displaystyle \sum_{E \cap F = A}{m_1(E)m_2(F), A \neq \emptyset} m(A)=N1​E∩F=A∑​m1​(E)m2​(F),A=∅
  是mass函数，其中 N = ∑ E ∩ F ≠ ∅ m 1 ( E ) m 2 ( F > 0 N = \displaystyle \sum_{E \cap F \neq \emptyset}{m_1(E)m_2(F} > 0 N=E∩F=∅∑​m1​(E)m2​(F>0
  为归一化系数。
  可以记为： m = m 1 ⊕ m 2 m = m_1 \oplus m_2 m=m1​⊕m2​，合成公式满足结合律和交换律。

（个人理解：Dempster-Shafer合成公式可以理解为单个交集的乘积除以所有交集的乘积，Bel(A)找A的子集mass相加，Pl(A)找A的交集mass相加）

后面代码的注释里有用到以上的一些名词，建议理解以上的理论和文章再继续阅读

2 fusion-D-S证据理论

主要是融合的D-S证据理论更新部分，对existence和type使用D-S证据理论进行更新。承接文章Apollo_perception_fusion中融合的更新航迹细节。

Apollo/modules/perception/fusion/lib/data_fusion/tracker/pbf_tracker/pbf_tracker.h

// 观测更新tracker
void PbfTracker::UpdateWithMeasurement(const TrackerOptions& options,const SensorObjectPtr measurement,double target_timestamp) {
...// options.match_distance = 0// DstExistenceFusion// D-S证据理论（DS theory）更新tracker的存在性existence_fusion_->UpdateWithMeasurement(measurement, target_timestamp,options.match_distance);// DstTypeFusion// D-S证据理论（DS theory）更新tracker的属性type_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
...
}

2.1 dst_evidence（D-S证据理论的具体实现）

主要三个类的实现：Dst、DstManager、DSTEvidenceTest
文件地址

Apollo/modules/perception/fusion/common/dst_evidence.h
Apollo/modules/perception/fusion/common/dst_evidence.cc
Apollo/modules/perception/fusion/common/dst_evidence_test.cc

Dst_CMake版本
单独把D-S证据理论类的实现代码单独拿出来了，加了测试案例，可以简单调试用，便于理解
（说实话有点看不懂里面的一些单词缩写，部分是自己根据原理猜想的，欢迎指正）
existence和type的融合都是基于Dst类，Dst类也是D-S证据理论的核心实现。
Dst类的实现说实话看了很久，建议先看dst_evidence_test.cc，DSTEvidenceTest类基于谷歌测试框架写的，单独拎出来测试的时候删掉了该基类。DSTEvidenceTest类中有四个测试案例，放到main函数里进行测试了

2.1.1 DSTEvidenceTest-测试入口

主要是设定假设空间，构造函数初始化，和run函数，run函数相当于整个D-S证据理论Dst的流程，从这里进去看Dst类的实现就应该一清二楚了。

// 假设空间H = {F111=1，FA18=2，P3C=4，FAST=3(F111+FA18)，UNKOWN=7(F111+FA18+P3C)}
static const std::vector<uint64_t> fod_subsets = {F111, FA18, P3C, FAST,UNKOWN};// 初始化两个观测空间O：sensor1,sensor2;以及融合结果// 使用dstManager管理假设空间HDSTEvidenceTest(): sensor1_dst_("test"), sensor2_dst_("test"), fused_dst_("test") {DstManager *dst_manager = DstManager::Instance();std::vector<std::string> fod_subset_names = {"F111", "FA18", "P3C", "FAST","UNKOWN"};dst_manager->AddApp("test", fod_subsets, fod_subset_names);// 计算值和真值是否相等，测试用vec_equal_ = [](const std::vector<double> &vec,const std::vector<double> &gt) {CHECK_EQ(vec.size(), gt.size());for (size_t i = 0; i < vec.size(); ++i) {EXPECT_NEAR(vec[i], gt[i], 1e-6);}};}// Dst流程void run(const std::vector<double> &sensor1_data,const std::vector<double> &sensor2_data) {// 根据观测数据设置BPA/mass函数(Bba？？？)，其实就是直接赋值// 这里是两组数据计算融合后的结果，在融合程序里应该是观测和航迹ASSERT_TRUE(sensor1_dst_.SetBbaVec(sensor1_data));ASSERT_TRUE(sensor2_dst_.SetBbaVec(sensor2_data));// “+”重载：即是Dempster-Shafer合成公式// 这里所有假设的mass函数已经计算完了fused_dst_ = sensor1_dst_ + sensor2_dst_;// 计算：Spt信度函数 Pls似真度函数 Utc信度空间大小fused_dst_.ComputeSptPlsUct();// 计算：所有假设的概率值fused_dst_.ComputeProbability();// mass函数fused_dst_vec_ = fused_dst_.GetBbaVec();// 信度函数fused_spt_vec_ = fused_dst_.GetSupportVec();// 似真度函数fused_pls_vec_ = fused_dst_.GetPlausibilityVec();// 信度空间大小fused_uct_vec_ = fused_dst_.GetUncertaintyVec();// 融合后的概率值fused_prob_vec_ = fused_dst_.GetProbabilityVec();}

2.1.2 DstManager

构造函数这里主要是对DstManager类进行实例化，并添加app。Dempster-Shafer合成公式很快计算完mass函数是因为DstManager类把假设空间的元素关系计算好了，这里DstManager主要是对假设空间的元素做处理，方便后面Dst的D-S证据理论计算。

主要是DstManager类的实现,计算处假设空间内元素之间的交集和子集。子集和交集的计算是根据二进制的按位与和按位或来计算出来的，假设空间的元素基数也是按位与计算出二进制有多少个1，这里不做赘述了，可以看我的注释代码，子集跑一下单独拎出来的cmake测试代码。

// DstManager主要是计算假设空间的关系
bool DstManager::AddApp(const std::string &app_name,const std::vector<uint64_t> &fod_subsets,const std::vector<std::string> &fod_subset_names) {// string和DstCommonData map图if (dst_common_data_.find(app_name) != dst_common_data_.end()) {AWARN << boost::format("Dst %s was added!") % app_name;}// data的假设空间初始化赋值DstCommonData dst_data;dst_data.fod_subsets_ = fod_subsets;// 假设空间的每项对应0,1,2...index// 测试案例的subsets_ind_map_就是{1-0,2-1,4-2,3-3,7-4}BuildSubsetsIndMap(&dst_data);// 校验：假设空间有重复值if (dst_data.subsets_ind_map_.size() != dst_data.fod_subsets_.size()) {AERROR << boost::format("Dst %s: The input fod subsets"" have repetitive elements.") %app_name;return false;}// 检查subsets_ind_map_有没有{假设空间元素最大值，假设空间大小}FodCheck(&dst_data);// 计算假设空间的元素基数，区分单元素和混合元素// 测试案例的元素基数就是(1,1,1,2,3)ComputeCardinalities(&dst_data);// 计算关系subset_relations_、inter_relations_、combination_relations_// subset_relations_是该元素的子集// inter_relations_是该元素的交集// combination_relations_是包含该元素的一对交集，交集对的前后可以对调if (!ComputeRelations(&dst_data)) {return false;}dst_data.init_ = true;// fod_subset_names赋值给dst_data的成员变量BuildNamesMap(fod_subset_names, &dst_data);std::lock_guard<std::mutex> lock(map_mutex_);dst_common_data_[app_name] = dst_data;return true;
}

像测试用例中的假设空间，对应的关系值如下：

fod_subset_：假设空间H = {F111=1，FA18=2，P3C=4，FAST=3(F111+FA18)，UNKOWN=7(F111+FA18+P3C)}fod_subset_cardinalities_：元素基数 = {1,1,1,2,3}combination_relations_：关系对为 = 
00 03 04 30 40
11 13 14 31 41
22 24 42 
33 34 43
44subset_relations_：子集为 =
0:0
1:1
2:2
3:0 1 3
4:0 1 2 3 4inter_relations_：交集为 = 
0:0 3 4
1:1 3 4
2:2 4
3:0 1 3 4
4:0 1 2 3 4

2.1.3 Dst

Dst主要计算bba_vec_（mass函数），support_vec_（Bel信度函数）和plausibility_vec_（Pl似真度函数），以及probability_vec_（概率）和uncertainty_vec_（信度区间大小/不确定性值），这两个在书上并没有找到相应的名字，这里直译了。

主要是加号重载，ComputeSptPlsUct()和ComputeProbability()三个函数的代码。

Dempster-Shafer合成

合成公式： m = m 1 ⊕ m 2 m = m_1 \oplus m_2 m=m1​⊕m2​，对Dst类重载加号，进来两个观测，计算假设空间的每个元素的融合后的mass函数：对该元素关系对（交集关系）的mass函数的乘积求和。

Dst operator+(const Dst &lhs, const Dst &rhs) {CHECK_EQ(lhs.app_name_, rhs.app_name_)<< boost::format("lhs Dst(%s) is not equal to rhs Dst(%s)") %lhs.app_name_ % rhs.app_name_;lhs.SelfCheck();rhs.SelfCheck();Dst res(lhs.app_name_);std::vector<double> &resbba_vec_ = res.bba_vec_;const auto &combination_relations = lhs.dst_data_ptr_->combination_relations_;// 计算假设空间的每个元素的mass函数for (size_t i = 0; i < resbba_vec_.size(); ++i) {const auto &combination_pairs = combination_relations[i];// AINFO << "pairs size: " << combination_pairs.size();double &belief_mass = resbba_vec_[i];belief_mass = 0.0;// 该元素的关系对（交集关系）的mass函数的乘积求和for (auto combination_pair : combination_pairs) {// AINFO << boost::format("(%d %d)") % combination_pair.first//     % combination_pair.second;belief_mass += lhs.GetIndBfmass(combination_pair.first) *rhs.GetIndBfmass(combination_pair.second);}// AINFO << boost::format("belief_mass: %lf") % belief_mass;}// 除以归一化系数res.Normalize();return res;
}

对于测试用例中的假设空间

mass(0) = m1(0) * m2(0) + m1(0) * m2(3) + m1(0) * m2(4) + m1(3) * m2(0) + m1(4) * m(0)
mass(1) = m1(1) * m2(1) + m1(1) * m2(3) + m1(1) * m2(4) + m1(3) * m2(1) + m1(4) * m(1)
mass(2) = m1(2) * m2(2) + m1(2) * m2(4) + m1(4) * m2(2)
mass(3) = m1(3) * m2(3) + m1(3) * m2(4) + m1(4) * m2(3)
mass(4) = m1(4) * m2(4)然后对每个mass函数归一化（除以总和），m1(i)，m2(i)输入对应的观测量即可

ComputeSptPlsUct

计算support_vec_（Bel信度函数），plausibility_vec_（Pl似真度函数）和uncertainty_vec_（信度区间大小/不确定性值）
Bel信度函数是假设空间内元素子集的mass函数和
Pl信度函数是假设空间内元素交集的mass函数和
信度区间[Bel,Pl]，uncertainty_vec_ = Pl - Bel

void Dst::ComputeSptPlsUct() const {
...for (size_t i = 0; i < size; ++i) {double &spt = support_vec_[i];double &pls = plausibility_vec_[i];double &uct = uncertainty_vec_[i];const auto &subset_inds = subset_relations[i];const auto &inter_inds = inter_relations[i];// AINFO << boost::format("inter_size: (%d %d)") % i % inter_inds.size();// 子集的mass函数和for (auto subset_ind : subset_inds) {spt += bba_vec_[subset_ind];}// 交集的mass函数和for (auto inter_ind : inter_inds) {pls += bba_vec_[inter_ind];}// AINFO << boost::format("pls: (%d %lf)") % i % pls;// 信度空间的大小uct = pls - spt;}
}

ComputeProbability

计算probability_vec_（概率），这里的公式没有在找到的资料里看到，不过公式是和Dempster-Shafer合成几乎一样的，只不过加了个系数（元素i基数/元素j基数），和混合项的元素的mass函数相乘时，系数比较小，得到的概率值准确。公式应该是：

m = ( c a r d 1 / c a r d 2 ) ∗ ( m 1 ⊕ m 2 ) m = (card_1 / card_2) *( m_1 \oplus m_2) m=(card1​/card2​)∗(m1​⊕m2​)
c a r d i card_i cardi​为第i个假设空间内的元素基数。
这里的概率值也是fusion里所需要的概率值，相当于D-S证据理论最后的结果。

void Dst::ComputeProbability() const {SelfCheck();probability_vec_.clear();probability_vec_.resize(bba_vec_.size(), 0.0);const auto &combination_relations = dst_data_ptr_->combination_relations_;const std::vector<size_t> &fod_subset_cardinalities =dst_data_ptr_->fod_subset_cardinalities_;// 双重for循环for (size_t i = 0; i < combination_relations.size(); ++i) {const auto &combination_pairs = combination_relations[i];// 元素i基数double intersection_card = static_cast<double>(fod_subset_cardinalities[i]);for (auto combination_pair : combination_pairs) {size_t a_ind = combination_pair.first;size_t b_ind = combination_pair.second;// 和Dempster-Shafer合成一样，只不过加了个系数（元素i基数/元素j基数）probability_vec_[a_ind] +=intersection_card /static_cast<double>(fod_subset_cardinalities[b_ind]) *bba_vec_[b_ind];}}
}

（个人觉得D-S证据理论其实是比较简单的算法，但是Apollo把算法实现抽象的比较好，可以计算不同的假设空间，以前接触的D-S证据理论代码都是一套代码只能针对一种假设空间，代码思路很重要，值得学习）

2.2 existence_fusion（存在概率融合）

其他的地方就不描述了，主要还是UpdateWithMeasurement函数，该函数前面只要是一些前处理，计算出当前的存在概率值,接着对观测和航迹的存在概率值进行D-S证据理论融合，得到融合后的存在概率值，

2.2.1 量测更新存在概率

当前概率值的一些计算就不阐述了，根据不同传感器，选取距离和速度项来计算。

假设空间其实很简单，设置好假设空间后，传入相应的mass函数，直接对Dst对象使用重载后的加号运算，就可以得到合成后的mass函数。然后对该融合后的Dst对象求取概率值即可。其他的一些细节应该就是简单修正了，实际工程中应该都是不一样的，这里就不阐述了（懒得看，没必要看，不想看）

void DstExistenceFusion::UpdateWithMeasurement(const SensorObjectPtr measurement, double target_timestamp,double match_dist)
...// 存在概率计算公式double obj_exist_prob = exist_factor * decay;Dst existence_evidence(fused_existence_.Name());existence_evidence.SetBba({{ExistenceDstMaps::EXIST, obj_exist_prob},{ExistenceDstMaps::EXISTUNKOWN, 1 - obj_exist_prob}});const double exist_fused_w = 1.0;//后两项=1，D-S证据理论合成mass函数fused_existence_ =fused_existence_ + existence_evidence * exist_fused_w * association_prob;
...

2.3 type_fusion（类型融合）

还是主要介绍UpdateWithMeasurement函数，和上面一样，假设空间和观测的mass函数初始化后，直接合成。

2.3.1 量测更新存在概率

假设空间有7个元素，照搬

  std::map<uint64_t, size_t> hyp_to_typ_map_ = {{PEDESTRIAN, static_cast<size_t>(base::ObjectType::PEDESTRIAN)},{BICYCLE, static_cast<size_t>(base::ObjectType::BICYCLE)},{VEHICLE, static_cast<size_t>(base::ObjectType::VEHICLE)},{OTHERS_MOVABLE, static_cast<size_t>(base::ObjectType::UNKNOWN_MOVABLE)},{OTHERS_UNMOVABLE,static_cast<size_t>(base::ObjectType::UNKNOWN_UNMOVABLE)},{OTHERS, static_cast<size_t>(base::ObjectType::UNKNOWN)},{UNKNOWN, static_cast<size_t>(base::ObjectType::UNKNOWN)}};

void DstTypeFusion::UpdateWithMeasurement(const SensorObjectPtr measurement,double target_timestamp) {Dst measurement_dst(name_);// 初始化观测Dst，对观测mass函数赋值measurement_dst = TypeProbsToDst(measurement->GetBaseObject()->type_probs);ADEBUG << "type_probs: "<< vector2string<float>(measurement->GetBaseObject()->type_probs);// mass函数合成fused_dst_ =fused_dst_ + measurement_dst * GetReliability(measurement->GetSensorId());ADEBUG << "reliability: " << GetReliability(measurement->GetSensorId());// update subtype// 子类别？很相信相机检测的类别if (IsCamera(measurement)) {track_ref_->GetFusedObject()->GetBaseObject()->sub_type =measurement->GetBaseObject()->sub_type;}// 更新类别和类别的概率UpdateTypeState();
}

2.4 fusion-形状更新

融合里对形状更新的部分，很简单，附带加上了

// 观测更新tracker
void PbfTracker::UpdateWithMeasurement(const TrackerOptions& options,const SensorObjectPtr measurement,double target_timestamp) {std::string sensor_id = measurement->GetSensorId();
...// PbfShapeFusion// 更新tracker的形状shape_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
...

2.4.1 shape_fusion

逻辑应该是：

1.优先使用lidar的形状，最近的历史关联的观测中有lidar的话，radar和camera观测都不会做更新

2.其次优先camera的形状，间隔更新时间比较小的话，radar观测仅仅更新中心，形状用历史camera更新

3.最后最近的历史观测中lidar和camera都没有的话，才使用radar的观测更新

void PbfShapeFusion::UpdateWithMeasurement(const SensorObjectPtr measurement,double target_timestamp) {仅一处用历史观测做更新，其余全是用measurement做更新。
UpdateState = UpdateShape + UpdateCenter观测是lidar：UpdateState(measurement)观测是radar：if 最新的lidar历史观测为空：if 最新的camera历史观测不为空：if 观测-camera 时间戳<0.3UpdateShape(camera)UpdateCenter(measurement)else 最新的camera历史观测为空：UpdateState(measurement)else不更新观测是camera：if 最新的lidar历史观测为空：UpdateState(measurement)else不更新

更新形状

直接透传

void PbfShapeFusion::UpdateShape(const SensorObjectConstPtr& measurement) {base::ObjectPtr dst_obj = track_ref_->GetFusedObject()->GetBaseObject();base::ObjectConstPtr src_obj = measurement->GetBaseObject();dst_obj->size = src_obj->size;dst_obj->direction = src_obj->direction;dst_obj->theta = src_obj->theta;dst_obj->polygon = src_obj->polygon;
}

更新中心点

也是直接透传

void PbfShapeFusion::UpdateCenter(const SensorObjectConstPtr& measurement) {base::ObjectPtr dst_obj = track_ref_->GetFusedObject()->GetBaseObject();base::ObjectConstPtr src_obj = measurement->GetBaseObject();dst_obj->center = src_obj->center;dst_obj->anchor_point = src_obj->anchor_point;
}

3 结语

有点烂尾了，拖了很久，一直没看这部分，因为没啥算法含量，抽点下班时间赶紧写完了，就这样吧。花了近一个月零散时间，Apollo融合部分的大部分代码终于看完了，看一点写一点，写的比较乱。当然有些细节肯定是不大懂的，毕竟没有实际调试过每个部分，但是也大概理清了Apollo融合部分的思路，也学到了些coding技巧。就这样了，接着看感知代码了，有缘更新。

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！