开发者说｜Apollo 6.0 Perception 模块 Fusion 组件（二）：主体算法流程分析

下面是由社区开发者—石鹏提供的文章，本文主要介绍Apollo 6.0 Perception 模块 Fusion 组件（二）：主体算法流程分析。

    ENJOY THE FOLLOWING  

本文将主要从以下几个方面来介绍：

• 前言

•  Fusion 组件成员组成

• 初始化

• 消息回调处理

• 融合

• 总结

Apollo 框架中，每个功能组件都对应一个继承自 Component 类的具体组件类，具体组件类中都包含 Public 的 Init 方法和 Proc 方法，Init 方法实现了组件的初始化，Proc 方法实现了组件的具体算法流程。

Perception 模块 Fusion 组件的具体组件类为 FusionComponent，从上一篇文章《开发者说｜Apollo 6.0 Perception 模块 Fusion 组件（一）：构建与启动流程分析》中已经知道，Fusion 组件的 FusionComponent 类的 Init 方法最终会在 Component 类的 Initialize 方法中被多态调用一次，而 FusionComponent 类的 Proc 方法将成为 Fusion 组件对应 Channel 上的消息回调函数（意即，收到一帧消息触发一次）。

本文将以 Apollo 6.0 源码 master 分支上的 74f7d1a429 提交为基础详细分析 Perception 模块 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> fusion_ 这个数据成员，fusion_ 是个独占智能指针，用于管理 BaseMultiSensorFusion 类型的对象，BaseMultiSensorFusion 是多传感器融合的抽象基类，会被表示障碍物多传感器融合的 ObstacleMultiSensorFusion 类所继承并实现对应纯虚接口方法。fusion_ 中的指针值会在 FusionComponent::InitAlgorithmPlugin 方法中被更新为工厂方法（Factory Method）模式返回的 ObstacleMultiSensorFusion 指针，后文中将对此进行详述。

再来看下 Perception 模块整个 Fusion 组件的 UML 类图：

I 表示由抽象基类实现的接口（Interface）类 ，通常情况下不会对接口类进行实例化；C 表示对接口类的具体实现。

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、内部的初始化接口：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.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<base_class *>());                       \
    }                                                                 \
    static std::vector<base_class *> GetAllInstances() {              \
      std::vector<base_class *> instances;                            \
      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<base_class *>()));       \
      }                                                               \
      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<base_class *>());                       \
    }                                                                 \
    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 主要的初始化动作：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 存在性融合初始化、概率跟踪器参数初始化

具体细节不作展开。

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 的话）。

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 是相同的。

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 方法。

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 方法中的四个主要流程。

2.1 保存当前数据帧

作以下几点说明：

• 判断当前帧数据是否需要参与融合，若不需要，则直接返回；

• ProbabilisticFusion 中有可发布传感器（Publish Sensor）的概念，即 std::vector<std::string> main_sensors_ 成员，被初始化为与 FusionComponent::fusion_main_sensors_ 相同的内容：velodyne128、front_6mm 和 front_12mm；

• ProbabilisticFusion 类内部含有一个布尔类型的数据缓存启动变量 started_，默认为 false，只有接收到过来自可发布传感器的数据才会将 started_ 置 true 以启动数据缓存，若启动了数据缓存，则调用 SensorDataManager::AddSensorMeasurements 方法将当前数据帧缓存到对应传感器的历史数据队列 std::deque<SensorFramePtr> frames_ 中；

• 对于来自可发布传感器以外的消息，只作缓存，不执行后续步骤，意味着只有来自可发布传感器的消息才可以触发融合动作。

2.2 查询各个传感器历史数据中可参与融合的相关数据帧

通过 SensorDataManager::GetLatestFrames 方法分别获取（Sensor::QueryLatestFrame）每个传感器的历史数据中与当前数据帧时间戳最接近的那一帧数据，这样可以得到与传感器类别数量相同的若干帧最新历史数据，再通过 std::sort 对这若干帧最新历史数据进行升序排序，最终得到可参与融合的相关数据帧序列。

2.3 融合所有的相关数据帧

对所有的相关数据帧，循环调用 ProbabilisticFusion::FuseFrame 方法进行融合，这是最终的融合算法入口，后文将会详细阐述。

2.4 从前景航迹和背景航迹中收集可被发布的融合目标

执行完所有相关数据帧的融合动作后，通过在 ProbabilisticFusion::CollectFusedObjects 方法中调用 PbfGatekeeper::AbleToPublish 方法判断相应融合航迹是否满足可发布逻辑，可发布逻辑具体细节不作展开，其主要与以下几点有关：

• PbfGatekeeper 配置参数

• 各传感器与航迹关联过的最新历史量测的可见性

• 航迹融合目标类别

• 航迹融合目标时间戳对应的本地时间

• 与航迹关联过的某种传感器的最新历史量测的距离、速度、置信度、所属的子传感器类别

• 航迹被跟踪上的次数

对于可被发布的航迹，通过 ProbabilisticFusion::CollectObjectsByTrack 方法提取出融合目标信息。

3、最终的融合算法入口：ProbabilisticFusion::FuseFrame 方法

上文中提到 ProbabilisticFusion::FuseFrame 方法是最终的融合算法入口：

  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 与当前航迹关联过的最新历史量测。

3.1 前景航迹融合

ProbabilisticFusion::FuseForegroundTrack 是前景航迹融合方法，其大体流程与背景航迹融合类似，但具体细节内容篇幅巨大，后续会单开一篇文章详细讲解。

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);  // 将新的背景航迹添加到场景的背景航迹列表中
    }
  }
}

背景航迹融合的流程与前景航迹融合类似，都是由下面四个主要步骤组成：

3.2.1 数据关联

航迹与测量的关联是首要步骤，要做的是找出航迹与量测的对应关系。从代码注释中可以看出，背景航迹融合的数据关联策略很简单，只做了背景航迹融合目标与当前帧背景目标的 track id 关联。

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 不变。

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 补充属性
  }
}

最后，若当前航迹至少拥有一个与之关联过且不可见时长未超过阈值的历史量测，则允许该航迹存活。

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 方法将新的背景航迹添加到场景的背景航迹列表中。

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。

本文详细分析了 Apollo 6.0 Perception 模块 Fusion 组件的初始化、消息回调处理以及障碍物融合的主体算法框架。

ProbabilisticFusion::FuseFrame 方法是最终的融合算法入口，对于某一帧相关数据帧，该方法都执行了三个动作：

• 前景航迹融合

• 背景航迹融合

• 移除丢失航迹

文章剖析了“背景航迹融合”和“移除丢失航迹”部分的具体实现，前景航迹融合部分由于篇幅巨大，将在后续的文章中单独讲述。

*《Apollo 6.0 Perception 模块 Fusion 组件（二）：主体算法流程分析》https://zhuanlan.zhihu.com/p/405476381

以上是 "Apollo 6.0 Perception 模块 Fusion 组件（二）：主体算法流程分析" 的全部内容，更多讨论请扫描二维码添加『Apollo小哥哥』为好友，进交流群畅所欲言。欢迎开发者们踊跃投稿，技术交流，共同进步，我们将为热爱技术交流、贡献心得的你送上Apollo专属惊喜礼物，期待与每位开发者一起探索、共赴星海！

©️著作权归作者所有，如需转载，请注明出处，否则将追究法律责任。