欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识！
您现在的位置是： 首页

Apollo perception fusion感知融合源码分析--证据推理

程序员文章站 2022-07-12 09:47:05
...

Apollo perception源码分析

Apollo6.0 感知融合

建议参照源码阅读本文，水平有限，有错误的地方希望多加指正

代码注释 leox24 / perception_learn

1 证据推理算法原理

1.1 算法教案

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

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

D-S证据理论

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

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

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

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

  • 对于给定的假设 h ∈ H h\in H h∈H，， μ h \mu_h μh​是观测空间 O O 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 ) P(H) P(H)表示 H H H的所有子集构成的集类（称为 H H H的幂集），映射 m : P ( H ) → [ 0 , 1 ] m:P(H)\rightarrow [0,1] m:P(H)→[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 ) P(H) P(H)表示 H H H的所有子集构成的集类（称为 H H H的幂集），映射 B e l : P ( H ) → [ 0 , 1 ] Bel:P(H)\rightarrow [0,1] Bel:P(H)→[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 ) P(H) P(H)表示 H H H的所有子集构成的集类（称为 H H H的幂集），映射 P l : P ( H ) → [ 0 , 1 ] Pl:P(H)\rightarrow [0,1] Pl:P(H)→[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)

  • B e l ( A ) ≥ P l ( A ) Bel(A) \geq Pl(A) Bel(A)≥Pl(A)

  • 设 H H H是有限集合， B e l Bel Bel和 P l Pl Pl分别是定义在 P ( H ) P(H) P(H)上的信度函数和似真度函数，对于任意 A ∈ P ( H ) A \in P(H) A∈P(H)，其信度区间定义为
    [ B e l ( A ) , P e l ( A ) ] ⊆ [ 0 , 1 ] [Bel(A), Pel(A)] \subseteq [0,1] [Bel(A),Pel(A)]⊆[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 H 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 H H上的两个mass函数，则
    m ( ∅ ) = 0 m(\emptyset)=0 m(∅)=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-证据推理

主要是融合的证据推理更新部分，对existence和type使用证据推理进行更新。承接文章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
  // 证据推理（DS theory）更新tracker的存在性
  existence_fusion_->UpdateWithMeasurement(measurement, target_timestamp,
                                           options.match_distance);
  // DstTypeFusion
  // 证据推理（DS theory）更新tracker的属性
  type_fusion_->UpdateWithMeasurement(measurement, target_timestamp);
...
}

2.1 dst_evidence（证据推理的具体实现）

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

2.1.1 DSTEvidenceTest-测试入口

主要是设定假设空间，构造函数初始化，和run函数，run函数相当于整个证据推理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管理假设空间H
  DSTEvidenceTest()
      : 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的证据推理计算。

主要是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
44

subset_relations_：子集为 =
0:0
1:1
2:2
3:0 1 3
4:0 1 2 3 4

inter_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里所需要的概率值，相当于证据推理最后的结果。

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];
    }
  }
}

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

2.2 existence_fusion（存在概率融合）

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

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，证据推理合成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.3
				UpdateShape(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技巧。就这样了，接着看感知代码了，有缘更新。

相关标签： Apollo perception 算法 人工智能 自动驾驶

上一篇： 数据结构-查找-二分查找-插值查找-斐波那契查找

下一篇： manacher算法及其应用

推荐阅读