多传感器融合——后融合

多传感器融合的方案可以分成前融合（Early Fusion）方案和后融合（Late Fusion）方案。

前融合也叫特征级融合，不同传感器的数据会在特征级别进行合并，也就是说，不同模态的数据经过处理和合并后会得到一个特征集合。一般来说，每个模态数据的特征会被分别提取，然后被提取到的特征会被合并为一个新的特征向量，这个特征向量可以表示多个模态数据合并后的信息，然后合并后的特征会被用于模型训练。前融合能够保证特征在融合的过程中不丢失，并且部署非常简单。但是，多模态的融合会导致特征维度过高，进而导致模型泛化性能下降。此外，特征融合后，多个模态之间不能解耦，也就是不能将这个系统做成可插拔的，整个框架的灵活性会下降。

后融合也叫决策层融合，不同输入类型的数据，被喂到不同的模型里。一般来说，后融合分别训练针对不同模态数据的模型，然后分别使用这些模型对相应输入进行推理，整合得到的推理结果得到最终结果。后融合是模块化的，因此更换某个模型也不会影响整个融合，无非就是对齐一下输出。系统设计也比较简单，用 KF 之类的跟踪算法进行匹配即可。

前融合

网上前融合的开源项目不少，但是在 Github 仓库中往往不会出现“Early Fusion”的关键词。

Frustum PointNets

使用 2D Object Det 在图像中得到 BBox 并得到类别，借助相机内参将 2D BBox 提升为 3D frustum（视锥）
基于上述视锥，用改进的 PointNet 进行 3D obj instance seg
基于分割后的目标点云，使用另一个 PointNet 进行 amodal 3D BBox reg

论文中有一个概念“Amodal”。modal perception 指的是分析直接可见的区域，amodal perception 指的是非模态感知，是从心理学中引申出来的概念，比如被栅栏挡住的狗，虽然我们看到的是“一条断断续续的狗”，但是我们仍然会认为它是“一条连续的狗”。

Amodal Detection of 3D Objects: Inferring 3D Bounding Boxes from 2D Ones in RGB-Depth Images, not only find object localizations in the 3D world, but also estimate their physical sizes and poses, even if only parts of them are visible in the RGB-D image[4]

意思就是，在感知算法中，amodal perception 指从相机的 2D 图像中获取 3D BBox，包括目标的物理尺寸和位姿，即使目标在 2D 图像中部分可见（被遮挡）。

PointPainting

一个图像实例分割网络，得到像素级的分割结果（Score）
LiDAR 点云被投影到分割后的 MASK 中，使用分割得到的 Score 标注
使用 LiDAR 的检测网络进行推理，得到 3D BBox

BevFusion

使用 BEVFromer 或者 LSS，从多个视角（6 个）RGB 相机中提取并生成 BEV 特征，在这里作者提出了很多高效的方法
使用 3D 卷积，从 LiDAR 点云数据中提取 BEV 特征，与相机特征 concat
将 concat 后的 BEV 特征送到 BEV Encoder 中，得到融合 BEV 特征地图
最后使用不同的 Heads 完成不同 3D 任务

和其他算法的区别是，BEVFusion 将相机和 LiDAR 的特征整合到同一个 BEV 空间，而不是把一个传感器的特征投影到另一个传感器特征上。

前融合的难点

时间同步：多模态数据的时间戳可能出现不对齐的情况
空间同步：多模态数据的内外参标定可能不准确，和时间同步的问题类似，可能导致 PC 和 RGB 数据时空不对齐，影响融合效果
数据增强：多模态数据增强比单模态数据的增强困难很多
模块耦合：如果更换了传感器或者模型，整个模型架构都需要重新设计

后融合

后融合先获取 Camera 和 LiDAR 的检测结果，然后进行匹配（匈牙利或 KM 算法）和状态更新（KF 类算法），因此后融合是目标级的融合。后融合中，根据不同的具体方法，Camera 和 LiDAR 的输出结果可能是 2D 和 3D。后融合的目的一般是获取更为鲁棒的 BBox 框边界估计和位置估计。

搜索“Late fusion”能够获取到的信息很少，这是因为前&后融合是学术或工业界人为作出的分类，更多出现在 Survey 或 Review 中，而大部分作者在设计模型的时候并不会强调自己是前&后融合。

此外，后融合更广泛地用于工业界中，学术界更喜欢在前融合上做创新。比较知名的使用了后融合的开源框架是 Apollo 和 Autoware，随着版本迭代，这两个框架中也加入了很多前融合的模型。

在后融合中，KF 用于融合观测方程，将 Camera 和 LiDAR 的输出结果在观测层面融合，然后迭代状态方程[11]。

CLOCs

来自论文《CLOCs: Camera-LiDAR Object Candidates Fusion for 3D Object Detection》。方法是将相机的 2D BBox 和 LiDAR 的 3D BBox 转换为联合检测候选，表现为稀疏的 Tensor，即下图的蓝色框，这一步的目的是把 2D & 3D 检测候选变成一组一致的联合检测候选。然后使用一系列的 2D 卷积来处理输入稀疏 Tensor 中的非空元素，最终，被处理完的 Tensor 通过 max pooling 映射到被学习的图像。

通过构建一个 k \times n \times 4 的 Tensor，维度 4 代表 T_{i,j} = [IoU_{i,j}, s_i^{\text{2D}}, s_j^{\text{3D}}, d_j] 这 4 个通道分别代表 2D 和 3D BBox 的 IoU，2D 检测的 Score，3D 检测结果的 Score，LiDAR 检测的 BBox 和 LiDAR xyxy 平面的归一化距离。当 IoU 为 0 时，整个 Tensor 会被清除。

CLOCs 确实是在 Object-Level 或 Decision-Level 进行融合，但是并没有使用 KF 一类的跟踪方法进行后融合，仍然是依赖 CNN 输出融合结果。

Matlab 中的后融合

Matlab 也提供了一个 Object-Level 后融合案例，使用 LiDAR 的 3D BBox（PointPillars）和 Camera 的 2D BBox（Yolov4）检测结果，最后使用 joint integrated probabilistic data association (JIPDA) tracker 实现融合。

代码分析

比较特别的是，Matlab 通过读取标注信息作为 Camera 和 LiDAR 的检测结果。

[ptCld,lidarBboxes] = helperExtractLidarData(dataLog);
[img,cameraBBoxes] = helperExtractCameraData(dataLog,dataFolder,camIdx);

接下来初始化 Filter 的参数，其中 initLidarCameraFusionFilter 参数会让函数返回一个 EKF tracker。不过这几天 Matlab 官网崩了，没办法通过源码看到 @helperInitLidarCameraFusionFilter 这个函数内部的细节。

% Setup the tracker 
tracker = trackerJPDA( ...
    TrackLogic="Integrated",...
    FilterInitializationFcn=@helperInitLidarCameraFusionFilter,...
    AssignmentThreshold=[20 200],...
    MaxNumTracks=500,...
    DetectionProbability=0.95,...
    MaxNumEvents=50,...
    ClutterDensity=1e-7,...
    NewTargetDensity=1e-7,...
    ConfirmationThreshold=0.99,...
    DeletionThreshold=0.2,...
    DeathRate=0.5);

循环处理 80 帧数据，每帧都加载一次传感器数据，同时更新自车位置、朝向和速度。

然后从 LiDAR 和 Camera 数据中提取 BBox，外参和时间戳，这两个传感器的数据会被转换到车体坐标系。

在第 1 帧仅使用 LiDAR 数据初始化跟踪器，后面的帧使用 LiDAR 和 Camera 的融合数据，然后将融合数据喂到 tracker 中。

% Set the number of frames to process from the dataset
numFrames = 80;

for frame = 1:numFrames   
    % Load the data
    fileName = fullfile(dataPath,strcat(num2str(frame,"%03d"),".mat"));
    load(fileName,'dataLog');

    % Find current time
    time = dataLog.LidarData.Timestamp - tOffset;

    % Update ego pose using GPS data to track in global coordinate frame
    [pos, orient, vel] = egoTrajectory.lookupPose(time);
    egoPose.Position = pos;
    egoPose.Orientation = eulerd(orient,"ZYX","frame");
    egoPose.Velocity = vel;

    % Assemble lidar detections into objectDetection format
    [~, lidarBoxes, lidarPose] = helperExtractLidarData(dataLog);
    lidarDetections = helperAssembleLidarDetections(lidarBoxes,lidarPose,time,1,egoPose);

    % Assemble camera detections into objectDetection format
    cameraDetections = cell(0,1);
    for k = 1:1:numel(dataLog.CameraData)
        [img, camBBox,cameraPose] = helperExtractCameraData(dataLog, dataFolder,k);
        cameraBoxes{k} = camBBox; %#ok<SAGROW>
        thisCameraDetections = helperAssembleCameraDetections(cameraBoxes{k},cameraPose,time,k+1,egoPose);
        cameraDetections = [cameraDetections;thisCameraDetections]; %#ok<AGROW> 
    end

    % Concatenate detections
    if frame == 1
        detections = lidarDetections;
    else
        detections = [lidarDetections;cameraDetections];
    end
    % Run the tracker
    tracks = tracker(detections, time);

    % Visualize the results
     display(dataFolder,dataLog, egoPose, lidarDetections, cameraDetections, tracks);
end

Apollo 中的后融合

以 Apollo 6.0 为例。

感知模块位于：

Path
----
C:\Users\FunnyWii\Downloads\apollo-6.0.0\apollo-6.0.0\modules\perception

感知模块输入包括：

128 线 LiDAR 输入
16 线 LiDAR 输入
RADAR 数据
图像数据
RADAR 外参
相机内参&外参
自车速度和角速度

输出包括：

3D 目标 tracks，包含朝向，速度，类别
交通灯检测结果

接下来看一下文件夹结构：

perception
├── base      // 感知模块公共的基础类定义
├── camera  // 相机检测（子模块）
├── common  // 感知模块公共基础操作定义,比如图像和点云的一些预处理
├── data    // 相机内外参
├── fusion  // 多模态数据的融合
├── inference // 深度学习推理
├── lib        // 基础库，包括线程、时间等
├── lidar   // 激光雷达检测（子模块）
├── map     // 高精地图
├── onboard // 子模块入口
├── production  // 感知模块入口，模型权重
├── proto       // protobuf 的消息结构
├── radar       // 毫米波雷达处理（子模块）
└── tool        // 离线测试工具

感知模块入口在 production 文件夹中，代码从这里开始看。通过 production 中的 launch 文件加载对应 dag，启动感知模块。

launch 文件用来启动，dag 文件描述了整个系统的拓扑关系，也定义了每个 Component 需要订阅的话题。

onboard 是各个子模块的入口，不同模态的数据在这里进行处理。

inference 深度学习模型部署后的推理模块，权重文件存放在 production/data 中。

camera/lidar/radar 三种传感器的识别和跟踪等功能。

fusion 多模态数据融合，感知模块终点。