【Python】串口通信库pyserial2
6.8 多传感器融合：YOLO与激光雷达/雷达数据的深度结合
6.8.1 引言：为什么需要非视觉传感器——以激光雷达为例
摄像头因其丰富的信息（颜色、纹理、形状）而成为自动驾驶、智能监控等视觉感知系统的核心。但其固有的局限性不容忽视：
- 深度信息缺失：单目摄像头难以直接获取目标的精确三维位置和距离，需要复杂的几何或深度学习方法进行估算。
- 光照依赖：在强光、弱光、逆光或夜晚环境下，图像质量急剧下降，导致目标检测性能严重衰退。
- 恶劣天气影响：雾、雨、雪等天气会严重散射光线，使图像模糊，降低可见性。
- 遮挡敏感：如果目标被完全遮挡，摄像头将无法感知到。
**激光雷达（LiDAR）**作为一种主动式测距传感器，通过发射激光束并测量反射回来的时间来精确计算物体距离。它能生成高密度的三维点云数据，直接提供目标的精确三维位置信息。
激光雷达的优势：
- 高精度三维定位：直接提供目标的X、Y、Z坐标，不受目标纹理影响。
- 不受光照影响：主动发射激光，不受环境光照条件影响，可在夜间或复杂光照下工作。
- 穿透能力：对于稀疏的雨雪、雾气具有一定穿透能力（相比摄像头）。
- 直接获取地形信息：能精确测绘周围环境的三维结构。
激光雷达的局限性：
- 信息稀疏：相较于图像，点云数据不包含纹理、颜色等语义信息，难以直接识别目标种类。
- 成本较高：高性能多线激光雷达设备价格昂贵。
- 受雨雪大颗粒影响：极端恶劣天气下，激光会被严重散射，性能下降。
显然，摄像头擅长“识别是什么”，激光雷达擅长“识别在哪里，有多远”。将两者融合，能够实现优势互补，构建出更强大、更鲁棒的环境感知系统。
6.8.2 传感器融合的层次与策略
多传感器融合可以发生在不同的数据处理层次，从原始数据到最终决策：
-
早期融合（Early Fusion / Input-level Fusion）：
- 概念：在原始数据层面进行融合。例如，将摄像头的图像数据和激光雷达的点云数据直接组合成一个多通道输入，然后送入一个统一的神经网络进行处理。
- 优势：最大限度地保留了原始信息，理论上可以学习到最丰富的跨模态特征。
- 挑战：数据类型异构（图像是2D像素阵列，点云是3D稀疏点集），数据配准困难，对融合网络的结构设计要求极高，通常需要专门设计和训练多模态深度学习模型。
-
中期融合（Mid-level Fusion / Feature-level Fusion）：
- 概念：将来自不同传感器的原始数据分别送入各自的特征提取网络，然后在网络的中间层（例如，YOLO的Backbone或Neck部分）将提取到的特征进行融合（如拼接、逐元素相加等），再送入后续网络层进行目标检测或识别。
- 优势：在特征层面进行融合，兼顾了原始信息的丰富性和特征的抽象性，比早期融合更容易实现。
- 挑战：需要设计合适的特征融合模块，并且仍然需要对融合模型进行端到端训练。
-
后期融合（Late Fusion / Decision-level Fusion）：
- 概念：每个传感器独立地进行感知（例如，YOLO从图像中检测目标，另一个网络从点云中检测目标），然后将各自独立的检测结果（如边界框、类别、置信度）在最终决策层面进行融合。这是我们目前在多摄像头系统中采用的策略，也是本节将继续深入扩展的重点。
- 优势：
- 模块化：各个传感器模态的感知模块可以独立开发、测试和维护，降低了系统复杂性。
- 灵活性：可以方便地增删传感器，无需重新训练整个融合模型。
- 易于实现：不需要重新训练复杂的融合网络，只需设计好融合逻辑。
- 挑战：在决策层面进行融合可能会丢失一些原始数据和底层特征的互补信息。
在本节中，我们将继续采用后期融合的策略，将YOLO在图像中检测到的2D目标与激光雷达在点云中检测到的3D目标进行融合。这种方法在工程实践中最为常见和灵活。
6.8.3 坐标系转换与时间同步：多传感器融合的关键
在融合不同传感器数据之前，最关键的前提是将它们统一到同一个全局坐标系中，并确保它们在时间上对齐。
-
坐标系转换 (Coordinate Transformation)：
- 问题：每个传感器都有自己的本地坐标系（例如，摄像头图像的像素坐标系，激光雷达的传感器坐标系）。为了融合，我们需要将它们都转换到一个共同的参考坐标系，通常是车辆坐标系（对于自动驾驶）或全局地图坐标系（对于固定监控）。
- 内参标定 (Intrinsic Calibration)：获取传感器自身的内部几何参数。
- 摄像头：焦距、主点、畸变系数。
- 激光雷达：通常厂家提供已标定好的内部参数。
- 外参标定 (Extrinsic Calibration)：获取传感器相对于参考坐标系（或另一个传感器）的旋转矩阵（Rotation Matrix,
R）和平移向量（Translation Vector,t）。- 这组
R和t定义了传感器在三维空间中的位置和姿态。例如，从激光雷达坐标系到车辆坐标系的变换。 - 对于一个三维点
P_sensor在传感器坐标系中，其在参考坐标系P_ref中的位置为：P_ref = R * P_sensor + t。 - 如何获取R和t：通常通过专门的标定方法（如棋盘格、点云配准等）在离线阶段完成。这是系统部署前必不可少的工作。
- 这组
- 我们将采用的简化：在后期融合中，YOLO检测结果首先被映射到我们之前定义的2D BEV世界坐标系（通过单应性矩阵）。激光雷达通常会输出3D目标边界框，这些3D边界框可以相对容易地投影到我们的2D BEV平面上。因此，这里的挑战主要在于将LiDAR的3D坐标映射到我们期望的BEV 2D坐标，并与摄像头的BEV坐标对齐。
-
时间同步 (Time Synchronization)：
- 问题：不同传感器的数据采集频率和时间戳可能不一致。如果不对齐，融合不同时间点的数据会导致信息错位，甚至产生错误的感知结果（例如，摄像头看到的目标是当前位置，而激光雷达检测到的是它0.1秒前的位置）。
- 硬件同步：最理想的方式是通过硬件触发机制（如GPS脉冲PPS信号、同步时钟）强制所有传感器在同一时刻采集数据。
- 软件同步：当硬件同步不可行时，可以在软件层面根据时间戳对数据进行近似对齐。例如，对不同传感器流设置一个时间窗，只融合在同一时间窗内的数据；或者对时间戳较早的数据进行预测，以匹配最新时间戳的数据。
- 我们系统中的时间同步：目前，我们的
FusionAndTrackingWorker在每次处理时，会收集来自所有摄像头的“最新”检测结果。在引入LiDAR时，我们将假定LiDAR数据也带有时间戳，并尽量在相近时间窗内进行处理。
6.8.4 YOLO与LiDAR数据的后期融合实践
我们将增强现有的FusionAndTrackingWorker，使其能够接收并融合来自LiDAR的检测结果。
6.8.4.1 模拟LiDAR检测流
由于我们无法直接连接真实LiDAR硬件，我们将模拟一个LiDARDetectorWorker线程，它会像YOLO一样，输出经过处理的“LiDAR检测结果”。这些结果通常是3D边界框，例如由激光雷达目标检测算法（如PointPillars、CenterPoint）输出的。
一个LiDAR检测结果通常包含：
- 3D边界框信息：
[x, y, z, dx, dy, dz, yaw](中心点坐标X,Y,Z，尺寸DX,DY,DZ，偏航角)。 - 类别信息：LiDAR目标检测通常也能识别出目标类别（如“car”, “pedestrian”）。
- 置信度。
我们将把这个3D信息转换到我们的2D BEV世界坐标系中，以便与摄像头的检测结果进行融合。在2D BEV中，我们通常只关心X、Y位置和目标的长度、宽度，以及可能的高度（如果BEV包含高度信息）。
LiDAR 3D Box到2D BEV的转换：
对于一个LiDAR 3D边界框[x, y, z, dx, dy, dz, yaw]：
- 它的中心点
[x, y]通常可以直接作为BEV中的中心点。 - 它的长度
dx和宽度dy可以直接作为BEV中的宽度和高度（或者在考虑偏航角yaw时，将其投影到BEV的轴对齐矩形）。 z轴信息和dz（高度）信息可以用于额外的过滤（例如，只关心地面以上一定高度的目标），或者在某些高级BEV表示中被可视化。
这里我们假设LiDAR已经提供了轴对齐的2D BEV边界框，或者我们可以简单地将其3D框的XY中心作为BEV中心，XY尺寸作为BEV尺寸。
# lidar_detector_worker.py (模拟LiDAR检测的线程)
import threading
import queue
import time
import numpy as np
import logging
logger = logging.getLogger(__name__)
class LiDARDetectorWorker(threading.Thread):
def __init__(self, output_lidar_queue, update_interval=0.1):
"""
模拟激光雷达检测器工作者线程。
它不从真实LiDAR读取，而是生成模拟的LiDAR检测结果。
:param output_lidar_queue: 用于存储模拟LiDAR检测结果的线程安全队列。
:param update_interval: 模拟LiDAR数据生成的时间间隔（秒）。
"""
super().__init__()
self.output_lidar_queue = output_lidar_queue # 存储输出队列
self.update_interval = update_interval # 存储更新间隔
self.stop_event = threading.Event() # 用于通知线程停止的事件
self.name = "LiDARDetectorWorker" # 设置线程名称
logger.info(f"模拟LiDAR检测器 '{
self.name}' 已初始化。") # 记录初始化信息
self._current_sim_obj_id = 0 # 模拟对象的ID计数器
self._simulated_objects = {
} # 存储模拟对象的当前状态
def run(self):
logger.info(f"模拟LiDAR检测器 '{
self.name}' 正在启动...") # 记录启动信息
try:
while not self.stop_event.is_set(): # 只要停止事件未被设置，就继续循环
start_time = time.time() # 记录循环开始时间
# 模拟生成或更新LiDAR检测结果
# 在这里，我们可以模拟一些物体在BEV地图上的运动
# 每次迭代，更新这些模拟物体的位置
simulated_detections = self._generate_simulated_detections() # 生成模拟检测
if simulated_detections: # 如果有模拟检测结果
output_data = {
'timestamp': time.time(), # 当前时间戳
'detections_3d': simulated_detections # 模拟的3D检测结果列表
}
try:
self.output_lidar_queue.put(output_data, timeout=1) # 将结果放入队列
logger.debug(f"模拟LiDAR: 已发送 {
len(simulated_detections)} 个检测。") # 记录调试信息
except queue.Full: # 如果队列已满
logger.warning("模拟LiDAR检测队列已满，丢弃当前结果。") # 记录警告
# 控制模拟数据生成速率
elapsed_time = time.time() - start_time # 计算经过时间
sleep_time = self.update_interval - elapsed_time # 计算需要休眠的时间
if sleep_time > 0: # 如果需要休眠
time.sleep(sleep_time) # 休眠
except Exception as e: # 捕获其他未知异常
logger.critical(f"模拟LiDAR检测器 '{
self.name}' 发生致命错误: {
e}", exc_info=True) # 记录详细错误
finally:
logger.info(f"模拟LiDAR检测器 '{
self.name}' 已退出。") # 记录线程退出
def stop(self):
"""
通知线程停止。
"""
logger.info(f"正在停止模拟LiDAR检测器 '{
self.name}'...") # 记录停止请求
self.stop_event.set() # 设置停止事件
def _generate_simulated_detections(self):
"""
内部方法：生成模拟的LiDAR 3D边界框检测。
模拟几个物体在BEV地图上移动。
LiDAR检测框格式: [cx, cy, cz, w, h, l, yaw, class_id, score]
我们将简化为 [cx, cy, w, h, class_id, score] for 2D BEV projection.
这里我们假设BEV地图是1000x1000，代表10x10米区域。
"""
# 定义一些初始模拟对象
if not self._simulated_objects:
self._simulated_objects = {
0: {
'bbox_3d': [500, 100, 1.0, 1.8, 1.0, 4.0, 0, 0, 0.95], 'vx': 5, 'vy': 0, 'class_id': 0, 'score': 0.95}, # ID0: 汽车，从上往下移动
1: {
'bbox_3d': [200, 800, 1.0, 0.8, 0.8, 1.8, 0, 1, 0.90], 'vx': 0, 'vy': -3, 'class_id': 1, 'score': 0.90}, # ID1: 行人，从下往上移动
}
self._current_sim_obj_id = 2
detections = [] # 存储当前帧的模拟检测结果
for obj_id, obj_state in self._simulated_objects.items(): # 遍历模拟对象
bbox = obj_state['bbox_3d'] # 获取当前边界框状态
vx, vy = obj_state['vx'], obj_state['vy'] # 获取速度
# 更新位置 (cx, cy)
# 这里的vx, vy是BEV坐标系下的像素/秒速度
bbox[0] += vx * self.update_interval # 更新x坐标
bbox[1] += vy * self.update_interval # 更新y坐标
# 模拟边界反弹或重新生成
if not (0 < bbox[0] < 1000 and 0 < bbox[1] < 1000): # 如果超出BEV地图范围 (假设1000x1000)
# 简单处理：重新生成位置
bbox[0] = np.random.randint(100, 900) # 随机x
bbox[1] = np.random.randint(100, 900) # 随机y
obj_state['vx'] = np.random.randint(-10, 10) # 随机x速度
obj_state['vy'] = np.random.randint(-10, 10) # 随机y速度
logger.info(f"模拟LiDAR: 对象 {
obj_id} 超出边界，重新生成位置。") # 记录日志
# 提取2D BEV相关的部分 (cx, cy, w, h)
# LiDar Bbox: [cx, cy, cz, w, h, l, yaw, class_id, score]
# 我们只需要 [cx, cy, w, l] 作为BEV的 [cx, cy, w, h] (高度是l)
# 因为w是横向尺寸，l是纵向尺寸，在BEV中，我们通常将它们视为2D框的w和h
cx, cy = bbox[0], bbox[1] # 获取中心x, y
w_3d, h_3d, l_3d = bbox[3], bbox[4], bbox[5] # 获取3D宽度、高度、长度
# 在BEV中，我们将3D框的宽度和长度映射到2D框的宽度和高度
# 同时假设BEV地图的单位是像素，并且与实际米数有对应关系
# 例如，如果1000像素代表10米，那么1像素 = 0.01米
# 那么3D框的尺寸 (米) * 100 得到像素尺寸
# 为了简化，这里直接使用模拟的像素尺寸
# 或者，如果你知道LiDAR输出的尺寸是米，可以乘以一个米到像素的比例因子
# 例如，模拟一个车辆在BEV中是100x200像素
# 1.8, 1.0, 4.0 (w, h, l)
# 在BEV中通常是 (l, w) -> (height, width) or (width, length)
# 假设 BEV 边界框是 [cx, cy, width_on_map, height_on_map]
# 这里我们用 bbox[3] 作为宽度，bbox[5] 作为高度
bev_w = bbox[3] * 100 # 将米转换为像素，假设1米=100像素
bev_h = bbox[5] * 100 # 将米转换为像素
det_info = {
'bbox_bev': [cx, cy, bev_w, bev_h], # BEV坐标系下的 [cx, cy, w, h]
'class_id': obj_state['class_id'], # 类别ID
'score': obj_state['score'], # 置信度
'raw_3d_bbox': bbox # 原始3D边界框，用于调试或更复杂的处理
}
detections.append(det_info) # 添加到检测列表
# 模拟随机添加新对象
if np.random.rand() < 0.02: # 2%的概率添加新对象
new_cx = np.random.randint(100, 900)
new_cy = np.random.randint(100, 900)
new_class = np.random.choice([0, 1]) # 随机选择汽车或行人
new_w = 1.8 if new_class == 0 else 0.8
new_h = 1.0 if new_class == 0 else 0.8
new_l = 4.0 if new_class == 0 else 1.8
new_vx = np.random.randint(-10, 10)
new_vy = np.random.randint(-10, 10)
self._simulated_objects[self._current_sim_obj_id] = {
'bbox_3d': [new_cx, new_cy, 1.0, new_w, new_h, new_l, 0, new_class, 0.95],
'vx': new_vx, 'vy': new_vy, 'class_id': new_class, 'score': 0.95
}
logger.info(f"模拟LiDAR: 新对象 {
self._current_sim_obj_id} 已生成。")
self._current_sim_obj_id += 1
# 模拟随机移除对象
if np.random.rand() < 0.01 and self._simulated_objects: # 1%的概率移除对象
obj_to_remove = np.random.choice(list(self._simulated_objects.keys()))
del self._simulated_objects[obj_to_remove]
logger.info(f"模拟LiDAR: 对象 {
obj_to_remove} 已移除。")
return detections # 返回模拟检测结果
- 代码深度解析与LiDARDetectorWorker亮点：
- 模拟数据生成:
_generate_simulated_detections()方法是核心，它模拟了LiDAR传感器检测到目标并输出其3D边界框的过程。 - LiDAR 3D Box表示: 模拟的
bbox_3d使用了[cx, cy, cz, w, h, l, yaw]的格式，这是3D目标检测中常见的表示。cx, cy, cz: 3D边界框的中心点坐标。w, h, l: 3D边界框的宽度、高度、长度。yaw: 目标的偏航角（旋转）。
- 动态模拟:
_simulated_objects字典维护了当前场景中模拟对象的运动状态。它会根据时间步长更新对象的位置，并模拟对象在边界反弹、随机生成和移除，以模拟动态场景。 - 3D到2D BEV投影:
det_info['bbox_bev'] = [cx, cy, bev_w, bev_h]这行代码将模拟的3D LiDAR检测结果转换成了2D BEV平面上的边界框[cx, cy, w, h]。- LiDAR的
cx, cy可以直接作为BEV的中心点。 w_3d和l_3d（3D边界框的宽度和长度）被用作BEV的宽度和高度。这里乘以了一个100的比例因子，假设我们的BEV地图是按1像素=0.01米绘制的，所以1米在BEV上是100像素。这个比例因子需要根据你的实际BEV地图的尺寸和单位进行调整。- 注意：这里为了简化，我们没有考虑
yaw角将3D旋转框投影到2D轴对齐框的复杂性。在实际中，如果LiDAR输出的是有角度的3D框，需要将其投影到BEV的轴对齐矩形，或者在追踪器中处理旋转边界框。
- LiDAR的
- 模拟数据生成:
6.8.4.2 增强FusionAndTrackingWorker以融合LiDAR数据
现在，我们将修改FusionAndTrackingWorker，使其能够接收LiDAR检测结果，并将其与YOLO的视觉检测结果进行融合。融合的关键在于数据关联：如何判断一个YOLO检测和一个LiDAR检测是否对应同一个物理目标。
融合策略（Late Fusion）：
-
独立感知，统一关联:
- YOLO（从摄像头）输出其2D检测。
- LiDAR（模拟）输出其2D BEV检测（经过转换）。
- 将所有来自YOLO和LiDAR的检测，在BEV坐标系下统一起来。
- 在追踪器中，将这些统一的检测结果作为一个整体，与已有的追踪轨迹进行匹配。
-
增强检测结果:
- 当一个YOLO检测和一个LiDAR检测被关联到同一个物理目标时，我们可以利用LiDAR提供的更精确的深度信息来修正YOLO检测的2D BEV位置和尺寸估计。
- 同时，YOLO提供的丰富语义信息（类别、Re-ID特征）可以弥补LiDAR在识别目标种类上的不足。
为了简化，我们将采取一种直接的融合策略：
- 将所有来自YOLO和LiDAR的BEV检测结果合并为一个统一的检测列表。
- DeepSORT将对这个合并后的列表进行追踪。
- 在合并时，我们需要为每个检测添加一个
source字段（例如'camera','lidar'），以便在后续处理中区分其来源。 - 当多个传感器检测到同一个目标时，我们需要一种机制来选择最好的信息。例如，可以基于置信度选择，或者对位置进行加权平均。
# fusion_and_tracking_worker.py (修改后的版本，处理LiDAR数据)
import threading
import queue
import time
import logging
import numpy as np
import cv2
import yaml
from deep_sort_tracker import DeepSortTracker
from feature_extractor import BaseFeatureExtractor, cosine_distance
logger = logging.getLogger(__name__)
class FusionAndTrackingWorker(threading.Thread):
def __init__(self, input_result_queues, output_global_tracks_queue,
homography_matrices_paths, min_conf_threshold=0.5,
reid_model_path=None, reid_device='cpu',
sort_max_age=5, sort_min_hits=3,
sort_iou_threshold=0.3, sort_max_cosine_distance=0.2,
input_lidar_queue=None, # 新增LiDAR输入队列
fusion_iou_threshold=0.5 # 融合YOLO和LiDAR检测的IoU阈值
):
super().__init__()
self.input_result_queues = input_result_queues
self.output_global_tracks_queue = output_global_tracks_queue
self.input_lidar_queue = input_lidar_queue # 存储LiDAR输入队列
self.homography_matrices = {
}
self._load_homography_matrices(homography_matrices_paths)
self.min_conf_threshold = min_conf_threshold
self.fusion_iou_threshold = fusion_iou_threshold # 存储融合IoU阈值
self.sort_tracker = DeepSortTracker(
feature_extractor_model_path=reid_model_path,
reid_device=reid_device,
max_age=sort_max_age,
n_init=sort_min_hits,
iou_threshold=sort_iou_threshold,
max_cosine_distance=sort_max_cosine_distance
)
self.stop_event = threading.Event()
self.name = "FusionAndTrackingWorker"
logger.info(f"融合与追踪工作者 '{
self.name}' 已初始化。")
def _load_homography_matrices(self, paths_dict):
# ... (与之前相同，不变)
for cam_id, path in paths_dict.items():
try:
with open(path, 'r') as f:
data = yaml.safe_load(f)
matrix_list = data.get('homography_matrix')
if matrix_list is not None:
self.homography_matrices[cam_id] = np.array(matrix_list, dtype=np.float32)
logger.info(f"成功为摄像头 {
cam_id} 加载单应性矩阵: {
path}")
else:
logger.error(f"文件 '{
path}' 中未找到 'homography_matrix' 键。")
except FileNotFoundError:
logger.error(f"单应性矩阵文件未找到: {
path} for camera {
cam_id}")
raise FileNotFoundError(f"Missing homography matrix for camera {
cam_id} at {
path}")
except Exception as e:
logger.error(f"加载摄像头 {
cam_id} 单应性矩阵文件 '{
path}' 时出错: {
e}", exc_info=True)
raise
def _convert_pixel_to_world(self, bbox_pixel, cam_id):
# ... (与之前相同，不变)
# 注意：这里的 bev_scale_factor_w 和 bev_scale_factor_h 仍然是简化假设，
# 实际使用中需要根据你的BEV地图单位和标定参数精确推导。
# 这里为了使LiDAR和YOLO的BEV检测能大致对齐，我们假设LiDAR模拟也使用这个比例。
H = self.homography_matrices.get(cam_id)
if H is None:
logger.warning(f"摄像头 {
cam_id} 没有单应性矩阵，无法进行坐标转换。")
return None
x1, y1, x2, y2 = bbox_pixel
bottom_center_pixel = np.array([[ (x1 + x2) / 2, y2 ]], dtype=np.float32)
bottom_center_pixel_homog = np.array([bottom_center_pixel[0][0], bottom_center_pixel[0][1], 1], dtype=np.float32).reshape(3, 1)
world_point_homog = np.dot(H, bottom_center_pixel_homog)
world_x = world_point_homog[0, 0] / world_point_homog[2, 0]
world_y = world_point_homog[1, 0] / world_point_homog[2, 0]
original_w_pixel = x2 - x1
original_h_pixel = y2 - y1
# 与LiDAR模拟中的比例因子保持一致，确保坐标系对齐
bev_scale_factor_w = 0.01 # 假设1像素的宽度在BEV中对应0.01米
bev_scale_factor_h = 0.01 # 假设1像素的高度在BEV中对应0.01米
world_w = original_w_pixel * bev_scale_factor_w
world_h = original_h_pixel * bev_scale_factor_h
return np.array([world_x, world_y, world_w, world_h])
# 辅助函数：IoU计算 (与SortTracker中的_calculate_iou相同，用于传感器间融合)
def _calculate_iou_for_fusion(self, bbox1, bbox2):
"""
计算两个边界框的IoU (Intersection over Union)。
边界框格式: [cx, cy, w, h] (中心点, 宽度, 高度)
"""
x1_1, y1_1, x2_1, y2_1 = bbox1[0] - bbox1[2]/2, bbox1[1] - bbox1[3]/2, \
bbox1[0] + bbox1[2]/2, bbox1[1] + bbox1[3]/2
x1_2, y1_2, x2_2, y2_2 = bbox2[0] - bbox2[2]/2, bbox2[1] - bbox2[3]/2, \
bbox2[0] + bbox2[2]/2, bbox2[1] + bbox2[3]/2
inter_x1 = max(x1_1, x1_2)
inter_y1 = max(y1_1, y1_2)
inter_x2 = min(x2_1, x2_2)
inter_y2 = min(y2_1, y2_2)
inter_w = max(0, inter_x2 - inter_x1)
inter_h = max(0, inter_y2 - inter_y1)
inter_area = inter_w * inter_h
area1 = bbox1[2] * bbox1[3]
area2 = bbox2[2] * bbox2[3]
union_area = area1 + area2 - inter_area
if union_area == 0:
return 0.0
return inter_area / union_area
def run(self):
logger.info(f"融合与追踪工作者 '{
self.name}' 正在启动...")
try:
while not self.stop_event.is_set():
# 记录本次融合循环的起始时间，用于时间戳对齐
current_fusion_loop_time = time.time()
all_camera_detections_bev = [] # 存储所有摄像头转换到BEV的检测
all_lidar_detections_bev = [] # 存储所有LiDAR转换到BEV的检测 (LiDAR已经模拟为BEV)
# 1. 从所有摄像头的队列中收集最新一批检测结果
for cam_id, q in self.input_result_queues.items():
latest_cam_result = None
while not q.empty():
try:
latest_cam_result = q.get_nowait()
except queue.Empty:
break
if latest_cam_result:
for det_idx, det in enumerate(latest_cam_result['detections']):
if det['confidence'] >= self.min_conf_threshold:
world_bbox = self._convert_pixel_to_world(det['box'], cam_id)
if world_bbox is not None:
camera_detection = {
'bbox_bev': world_bbox, # BEV坐标系下的 [cx, cy, w, h]
'score': det['confidence'],
'class_id': det['class_id'],
'source': 'camera', # 标记来源
'camera_id': cam_id,
'image_crop': latest_cam_result['image_crops'][det_idx] # 用于Re-ID
}
all_camera_detections_bev.append(camera_detection)
# 2. 从LiDAR队列中收集最新一批检测结果
if self.input_lidar_queue: # 如果LiDAR队列存在
latest_lidar_result = None
while not self.input_lidar_queue.empty():
try:
latest_lidar_result = self.input_lidar_queue.get_nowait()
except queue.Empty:
break
if latest_lidar_result:
for det in latest_lidar_result['detections_3d']: # 遍历LiDAR检测
# LiDAR模拟的det已经是BEV的 [cx, cy, w, h]，我们直接使用
# 在实际中，这里会有一个从3D LiDAR Box到2D BEV Box的转换
lidar_detection = {
'bbox_bev': np.array(det['bbox_bev'], dtype=np.float32), # 确保是NumPy数组
'score': det['score'],
'class_id': det['class_id'],
'source': 'lidar', # 标记来源
'raw_3d_bbox': det['raw_3d_bbox'] # 原始3D信息
}
all_lidar_detections_bev.append(lidar_detection)
# 3. 传感器级后期融合：合并YOLO和LiDAR检测
# 这一步的核心是解决重复检测和信息增强。
final_detections_for_sort = [] # 存储最终送给SORT的检测
matched_lidar_indices = set() # 记录已与YOLO匹配的LiDAR检测索引
# 优先处理摄像头检测（因为它们有Re-ID特征）
for cam_det in all_camera_detections_bev:
best_match_iou = 0.0 # 最佳匹配IoU
best_lidar_match_idx = -1 # 最佳LiDAR匹配索引
# 尝试与LiDAR检测进行匹配
for lidar_idx, lidar_det in enumerate(all_lidar_detections_bev):
if lidar_idx in matched_lidar_indices: # 如果该LiDAR检测已被匹配，跳过
continue
# 计算BEV下的IoU
iou = self._calculate_iou_for_fusion(cam_det['bbox_bev'], lidar_det['bbox_bev'])
if iou > best_match_iou and iou >= self.fusion_iou_threshold: # 如果IoU超过阈值且是最佳
best_match_iou = iou
best_lidar_match_idx = lidar_idx
if best_lidar_match_idx != -1: # 如果找到匹配的LiDAR检测
matched_lidar_indices.add(best_lidar_match_idx) # 标记为已匹配
# 融合操作：可以进行位置加权平均，或者选择高置信度一方
# 这里我们选择LiDAR的位置作为主导（假设LiDAR位置更精确），YOLO的类别和置信度
# 并将Re-ID特征带入
fused_bbox_bev = lidar_det['bbox_bev'] # 融合后使用LiDAR的位置
fused_class_id = cam_det['class_id'] # 使用摄像头的类别ID
fused_score = max(cam_det['score'], lidar_det['score']) # 选择置信度较高的
fused_detection = {
'bbox': fused_bbox_bev, # 注意：DeepSORT期望键为'bbox'，值为[cx, cy, w, h]
'score': fused_score,
'class_id': fused_class_id,
'source': 'fused_camera_lidar',
'image_crop': cam_det['image_crop'] # 包含原始图像裁剪以提取Re-ID特征
}
final_detections_for_sort.append(fused_detection)
logger.debug(f"融合: 摄像头检测 {
cam_det['camera_id']} 与LiDAR检测 {
best_lidar_match_idx} 融合。")
else: # 如果没有匹配的LiDAR检测，只使用摄像头检测
# 注意：DeepSORT期望键为'bbox'
cam_det_for_sort = {
'bbox': cam_det['bbox_bev'],
'score': cam_det['score'],
'class_id': cam_det['class_id'],
'source': 'camera',
'image_crop': cam_det['image_crop']
}
final_detections_for_sort.append(cam_det_for_sort)
logger.debug(f"融合: 摄像头检测 {
cam_det['camera_id']} 未找到LiDAR匹配。")
# 处理未匹配的LiDAR检测
for lidar_idx, lidar_det in enumerate(all_lidar_detections_bev):
if lidar_idx not in matched_lidar_indices: # 如果LiDAR检测未被匹配
# 注意：DeepSORT期望键为'bbox'
lidar_det_for_sort = {
'bbox': lidar_det['bbox_bev'],
'score': lidar_det['score'],
'class_id': lidar_det['class_id'],
'source': 'lidar',
'image_crop': None # LiDAR检测没有图像裁剪用于Re-ID
}
final_detections_for_sort.append(lidar_det_for_sort)
logger.debug(f"融合: 未匹配的LiDAR检测 {
lidar_idx} 已加入。")
# 4. 将所有融合后的检测结果送入DeepSORT追踪器
# 提取所有的image_crops用于Re-ID特征提取
all_crops_for_reid = [d['image_crop'] for d in final_detections_for_sort]
global_tracks = self.sort_tracker.update(final_detections_for_sort, all_crops_for_reid)
# 5. 将追踪结果放入输出队列 (不变)
if global_tracks:
output_data = {
'frame_count': self.sort_tracker.frame_count,
'timestamp': time.time(),
'tracked_objects': []
}
for track in global_tracks:
bbox_world = track.get_state_as_bbox()
output_data['tracked_objects'].append({
'track_id': track.track_id,
'bbox_world': bbox_world.tolist(),
'score': track.score,
'class_id': track.class_id,
'hits': track.hits,
'age': track.age,
'source': track.source if hasattr(track, 'source') else 'unknown' # 可以将来源信息加入轨迹
})
try:
self.output_global_tracks_queue.put(output_data, timeout=1)
logger.debug(f"已输出帧 {
output_data['frame_count']} 的 {
len(global_tracks)} 个全局追踪结果。")
except queue.Full:
logger.warning("全局追踪结果队列已满，丢弃当前结果。")
# 简单的时间同步，确保循环在合理的时间间隔内运行
elapsed_loop_time = time.time() - current_fusion_loop_time
# 假设我们希望融合频率与YOLO推理频率大致相同，例如20ms
desired_loop_interval = 0.02 # 50Hz
sleep_time = desired_loop_interval - elapsed_loop_time
if sleep_time > 0:
time.sleep(sleep_time)
except Exception as e:
logger.critical(f"融合与追踪工作者 '{
self.name}' 发生致命错误: {
e}", exc_info=True)
finally:
logger.info(f"融合与追踪工作者 '{
self.name}' 已退出。")
def stop(self):
logger.info(f"正在停止融合与追踪工作者 '{
self.name}'...")
self.stop_event.set()
- 代码深度解析与FusionAndTrackingWorker增强亮点：
- 新增LiDAR输入队列:
__init__方法现在接受一个input_lidar_queue参数，用于从LiDARDetectorWorker接收数据。 - 融合IoU阈值:
fusion_iou_threshold参数用于控制YOLO检测和LiDAR检测之间进行融合的重叠度要求。 - 统一检测结构: 为了方便后续处理，所有传入
FusionAndTrackingWorker的检测（无论是来自YOLO还是LiDAR）都被转换为一个统一的字典格式，其中包含'bbox_bev'（BEV坐标系下的框）、'score'、'class_id'以及关键的'source'字段（'camera'或'lidar'）。 - 图像裁剪传递:
camera_detection字典现在包含了'image_crop'字段，将YOLO检测对应的原始图像裁剪传递下去，这对于DeepSORT中的Re-ID特征提取至关重要。LiDAR检测则没有图像裁剪，因此其'image_crop'为None。 - 后期融合逻辑:
- 收集: 遍历所有摄像头队列和LiDAR队列，收集最新的检测结果。
- YOLO与LiDAR匹配: 对于每个YOLO检测，遍历所有LiDAR检测，计算它们在BEV坐标系下的IoU。
- 最佳匹配选择: 如果一个YOLO检测与多个LiDAR检测重叠，选择IoU最高且超过
fusion_iou_threshold的LiDAR检测进行匹配。 - 信息融合: 如果YOLO检测与LiDAR检测成功匹配：
- 位置信息：这里我们选择LiDAR检测的位置作为融合后的最终位置 (
fused_bbox_bev = lidar_det['bbox_bev'])，因为LiDAR通常提供更精确的深度和位置信息。 - 类别/置信度：选择摄像头检测的类别和置信度 (
fused_class_id = cam_det['class_id'],fused_score = max(cam_det['score'], lidar_det['score']))，因为摄像头在语义识别上更具优势。 - Re-ID特征：关键是将摄像头的图像裁剪带入融合后的检测 (
'image_crop': cam_det['image_crop'])，这样DeepSORT仍然可以从这个裁剪中提取外观特征，即使融合后的位置由LiDAR主导。
- 位置信息：这里我们选择LiDAR检测的位置作为融合后的最终位置 (
- 未匹配处理:
- 未匹配的YOLO检测（没有对应LiDAR匹配的）仍然会被添加到
final_detections_for_sort。 - 未匹配的LiDAR检测（没有对应YOLO匹配的）也会被添加到
final_detections_for_sort。
- 未匹配的YOLO检测（没有对应LiDAR匹配的）仍然会被添加到
- 送入DeepSORT:
self.sort_tracker.update(final_detections_for_sort, all_crops_for_reid)。final_detections_for_sort包含了来自所有传感器（以及融合后）的所有检测，all_crops_for_reid则对应这些检测的图像裁剪（如果来源是摄像头）。
- 轨迹来源标记: 在输出给主程序时，可以在每个追踪对象中增加
'source'字段，表明该轨迹主要来源于哪个传感器（或融合）。
- 新增LiDAR输入队列:
6.8.5.1 更新主应用程序(main_integrated_system.py)
现在，我们必须更新main_integrated_system.py，使其能够启动模拟的LiDAR工作者，并将其输出传递给FusionAndTrackingWorker。
# main_integrated_system.py (最终版本，包含LiDAR融合)
import cv2
import numpy as np
import time
import queue
import threading
import logging
import os
import torch
# 导入自定义模块
from multi_camera_capture import CameraCaptureThread
from yolo_inference_worker import YOLOInferenceWorker
from fusion_and_tracking_worker import FusionAndTrackingWorker
from lidar_detector_worker import LiDARDetectorWorker # 导入LiDAR模拟器
# ... (日志配置和CAMERA_IDS, CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FPS, YOLO_MODEL_PATH等参数不变)
# LiDAR模拟配置
LIDAR_UPDATE_INTERVAL = 0.05 # 模拟LiDAR数据生成间隔 (20Hz)
LIDAR_OUTPUT_QUEUE_MAX_SIZE = 5 # 模拟LiDAR检测结果队列容量
# 融合与追踪配置 (DeepSORT参数，新增LiDAR融合IoU阈值)
# ... (REID_MODEL_PATH, REID_DEVICE, SORT_MAX_AGE, SORT_N_INIT, SORT_IOU_THRESHOLD, SORT_MAX_COSINE_DISTANCE 不变)
FUSION_IOU_THRESHOLD = 0.2 # 融合YOLO和LiDAR检测的IoU阈值 (调低以允许更多融合)
# --- 全局队列字典，用于模块间通信 ---
g_frame_queues = {
cid: queue.Queue(maxsize=FRAME_QUEUE_MAX_SIZE) for cid in CAMERA_IDS}
g_result_queues = {
cid: queue.Queue(maxsize=RESULT_QUEUE_MAX_SIZE) for cid in CAMERA_IDS}
g_lidar_queue = queue.Queue(maxsize=LIDAR_OUTPUT_QUEUE_MAX_SIZE) # 新增LiDAR检测队列
g_global_tracks_queue = queue.Queue(maxsize=GLOBAL_TRACKS_QUEUE_MAX_SIZE)
# ... (辅助函数 draw_detections_on_frame, draw_tracks_on_map 不变)
def main_integrated_system():
logger.info("主程序: 启动集成系统 (多传感器融合 DeepSORT Enabled)...")
# ... (1. 启动摄像头捕获线程 不变)
capture_threads = []
for cam_id in CAMERA_IDS:
if cam_id not in HOMOGRAPHY_MATRIX_PATHS or not os.path.exists(HOMOGRAPHY_MATRIX_PATHS[cam_id]):
logger.error(f"摄像头 {
cam_id} 的单应性矩阵文件 '{
HOMOGRAPHY_MATRIX_PATHS.get(cam_id, '未配置')}' 不存在。请先运行 'homography_calibrator.py' 进行标定！")
return
thread = CameraCaptureThread(
camera_id=cam_id, frame_queue=g_frame_queues[cam_id],
width=CAMERA_WIDTH, height=CAMERA_HEIGHT, fps=CAMERA_FPS
)
capture_threads.append(thread)
thread.start()
logger.info(f"主程序: 摄像头 {
cam_id} 捕获线程已启动。")
# --- 2. 启动YOLO推理工作者线程 ---
inference_worker = YOLOInferenceWorker(
model_path=YOLO_MODEL_PATH, input_queues=g_frame_queues,
output_queues=g_result_queues, batch_size=YOLO_BATCH_SIZE,
wait_timeout=YOLO_INFERENCE_WAIT_TIMEOUT, device=YOLO_DEVICE
)
inference_worker.start()
logger.info("主程序: YOLO推理工作者线程已启动。")
# --- 3. 启动模拟LiDAR检测器线程 ---
lidar_detector = LiDARDetectorWorker(
output_lidar_queue=g_lidar_queue, # LiDAR输出队列
update_interval=LIDAR_UPDATE_INTERVAL
)
lidar_detector.start()
logger.info("主程序: 模拟LiDAR检测器线程已启动。")
# --- 4. 启动融合与追踪工作者线程 (DeepSORT + LiDAR Fusion) ---
fusion_worker = FusionAndTrackingWorker(
input_result_queues=g_result_queues,
output_global_tracks_queue=g_global_tracks_queue,
homography_matrices_paths=HOMOGRAPHY_MATRIX_PATHS,
min_conf_threshold=MIN_CONF_THRESHOLD,
reid_model_path=REID_MODEL_PATH,
reid_device=REID_DEVICE,
sort_max_age=SORT_MAX_AGE,
sort_min_hits=SORT_N_INIT,
sort_iou_threshold=SORT_IOU_THRESHOLD,
sort_max_cosine_distance=SORT_MAX_COSINE_DISTANCE,
input_lidar_queue=g_lidar_queue, # 传递LiDAR输入队列
fusion_iou_threshold=FUSION_IOU_THRESHOLD # 传递融合IoU阈值
)
fusion_worker.start()
logger.info("主程序: 融合与追踪工作者线程 (DeepSORT + LiDAR Fusion) 已启动。")
# ... (5. 加载鸟瞰图背景，主显示循环 不变)
map_background = cv2.imread(MAP_BACKGROUND_PATH)
if map_background is None:
logger.error(f"无法加载鸟瞰图背景图像: {
MAP_BACKGROUND_PATH}。将使用黑色占位图。")
map_background = np.zeros((800, 800, 3), dtype=np.uint8)
cv2.putText(map_background, "MAP NOT LOADED!", (50, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
latest_global_tracks = [] # 存储最新的全局追踪结果
last_display_time = time.time()
try:
logger.info("主程序: 进入显示循环。按 'q' 退出。")
while True:
# 检查所有工作线程是否活跃
if not all(t.is_alive() for t in capture_threads) or \
not inference_worker.is_alive() or \
not lidar_detector.is_alive() or \
not fusion_worker.is_alive():
logger.error("主程序: 检测到有工作线程非正常停止，系统将退出。")
break
# 获取最新的全局追踪结果
latest_track_data = None
while not g_global_tracks_queue.empty():
try:
latest_track_data = g_global_tracks_queue.get_nowait()
except queue.Empty:
break
if latest_track_data:
latest_global_tracks = latest_track_data['tracked_objects']
current_time = time.time()
if (current_time - last_display_time) >= DISPLAY_UPDATE_INTERVAL:
last_display_time = current_time
# 绘制鸟瞰图与全局追踪轨迹
if map_background is not None:
map_with_tracks = draw_tracks_on_map(map_background, latest_global_tracks)
cv2.imshow("Global Bird's-Eye View Tracking (Camera+LiDAR)", map_with_tracks)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
logger.info("主程序: 用户请求退出。")
break
except KeyboardInterrupt:
logger.info("主程序: 捕获到用户中断信号 (Ctrl+C)。")
except Exception as e:
logger.critical(f"主程序: 发生未捕获的致命错误: {
e}", exc_info=True)
finally:
# --- 6. 安全地停止所有线程 ---
logger.info("主程序: 正在发送停止信号并等待所有工作线程终止...")
# 1. 停止捕获线程
for thread in capture_threads:
thread.stop()
# 2. 停止YOLO推理工作者
inference_worker.stop()
# 3. 停止模拟LiDAR检测器
lidar_detector.stop()
# 4. 停止融合与追踪工作者
fusion_worker.stop()
# 等待所有线程结束
for thread in capture_threads:
thread.join(timeout=5)
if thread.is_alive():
logger.warning(f"线程 '{
thread.name}' 未能在规定时间内停止。")
inference_worker.join(timeout=10)
if inference_worker.is_alive():
logger.warning(f"线程 '{
inference_worker.name}' 未能在规定时间内停止。")
lidar_detector.join(timeout=5)
if lidar_detector.is_alive():
logger.warning(f"线程 '{
lidar_detector.name}' 未能在规定时间内停止。")
fusion_worker.join(timeout=10)
if fusion_worker.is_alive():
logger.warning(f"线程 '{
fusion_worker.name}' 未能在规定时间内停止。")
cv2.destroyAllWindows()
logger.info("主程序: 所有工作线程已安全停止，系统退出。")
if __name__ == '__main__':
main_integrated_system()
至此，我们已经构建了一个能够集成多摄像头YOLO视觉信息和模拟LiDAR数据的多传感器融合目标追踪系统。这个系统通过模块化的设计，利用了Python的并发特性，并通过后期融合策略，结合了不同传感器的优势。这为我们进一步探索更复杂的感知挑战和更高级的AI应用打下了坚实的基础。
6.9 高层业务逻辑与事件触发：从感知到智能决策
6.9.1 引言：AI感知的价值转化
在许多实际应用场景中，我们不只是想知道“一个物体在哪里”或“它在向哪个方向移动”。我们更关心的是：
- “是否有未经授权的人员进入了禁区？”（区域入侵检测）
- “今天有多少车辆通过了这个路口？”（目标计数与流量统计）
- “这个包裹是否在运输带上停留了异常长的时间？”（异常行为识别）
- “目标A是否与目标B发生了近距离接触？”（行为分析）
- “当上述事件发生时，系统能否立即通知相关人员并记录下来？”（事件触发与通知）
将原始的感知数据转化为这些高层业务逻辑，是AI系统价值转化的关键。这一过程通常包括：
- 定义业务规则：明确什么条件构成一个“事件”。
- 状态监控：持续监控追踪目标的各种属性（位置、速度、类别、ID等）是否符合规则。
- 事件判断：根据规则判断事件是否发生。
- 事件触发：一旦事件发生，执行预设的动作（如日志记录、发送警报、更新数据库）。
我们将针对几个典型的业务场景，详细实现其高层逻辑模块。
6.9.2 区域入侵检测 (Region of Interest / Intrusion Detection)
区域入侵检测是最常见的智能监控功能之一。它用于检测是否有目标进入、离开或长时间停留在预定义的敏感区域。
概念：
- 敏感区域（Region of Interest, ROI）：通常是一个多边形或矩形区域，在我们的鸟瞰图（BEV）世界坐标系中定义。
- 入侵事件：当一个追踪到的目标，其在BEV中的位置进入了ROI，或者从ROI外部进入内部时，就可以触发入侵事件。
- 离开事件：当目标从ROI内部移动到外部时。
- 徘徊/停留事件：当目标在ROI内部停留的时间超过某个阈值时。
实现思路：
- 区域定义：用一组有序的BEV坐标点来定义多边形区域。
- 点在多边形内判断：使用几何算法（如
cv2.pointPolygonTest）判断目标中心点是否位于多边形内部。 - 状态追踪：为每个追踪目标维护一个历史区域状态（Inside/Outside/Unknown），以便检测状态变化。
- 事件生成：根据历史状态和当前状态的变化，生成入侵、离开或徘徊事件。
我们将创建一个RegionMonitor类来管理这些逻辑。
# business_logic_modules.py
import numpy as np # 导入NumPy库，用于数值计算
import cv2 # 导入OpenCV库，用于点在多边形内判断
import time # 导入时间模块
import logging # 导入日志模块
logger = logging.getLogger(__name__) # 获取日志器
class RegionMonitor:
"""
负责监控目标是否进入、离开或停留在预定义区域。
区域在BEV世界坐标系中定义。
"""
def __init__(self, region_name, polygon_points_bev, event_manager):
"""
初始化区域监控器。
:param region_name: 区域的名称（例如："禁区A", "入口区域"）。
:param polygon_points_bev: 定义区域的多边形顶点列表，格式为 [[x1, y1], [x2, y2], ...]
这些点都应在BEV世界坐标系中。
:param event_manager: 用于触发事件的EventManager实例。
"""
self.region_name = region_name # 存储区域名称
# 将多边形顶点转换为OpenCV所需的int32数组，并调整维度
self.polygon_points_bev = np.array(polygon_points_bev, dtype=np.int32).reshape((-1, 1, 2))
self.event_manager = event_manager # 存储事件管理器实例
# 用于追踪每个目标的区域状态
# {track_id: {'current_status': 'unknown'/'inside'/'outside', 'enter_time': float}}
self.track_status = {
} # 存储每个追踪目标的区域状态
logger.info(f"区域监控器 '{
self.region_name}' 已初始化，区域顶点: {
polygon_points_bev}") # 记录初始化信息
def update(self, tracked_objects):
"""
根据当前追踪到的目标更新区域状态并触发事件。
:param tracked_objects: 当前帧所有活跃且已确认的追踪结果列表，每个结果是Track对象或其简化字典。
包含 'track_id' 和 'bbox_world' (BEV坐标系下的 [cx, cy, w, h])。
"""
current_tracked_ids = {
obj['track_id'] for obj in tracked_objects} # 获取当前帧所有追踪到的ID集合
# 清理旧的轨迹状态，移除不再追踪的目标
ids_to_remove = [tid for tid in self.track_status if tid not in current_tracked_ids] # 找出不再追踪的ID
for tid in ids_to_remove: # 遍历要移除的ID
# 如果目标在区域内消失，可以触发一个“离开”事件
if self.track_status[tid]['current_status'] == 'inside': # 如果目标在区域内消失
self.event_manager.trigger_event(
'region_leave', # 事件类型
{
'region': self.region_name, 'track_id': tid, 'status': 'left', 'time': time.time()} # 事件数据
)
logger.info(f"事件: 目标 {
tid} 已离开区域 '{
self.region_name}' (消失)。") # 记录日志
del self.track_status[tid] # 从状态字典中删除该ID
# 遍历当前追踪到的目标，更新其区域状态
for obj in tracked_objects: # 遍历当前帧追踪到的目标
track_id = obj['track_id'] # 获取追踪ID
# 获取目标在BEV中的中心点
target_cx, target_cy = obj['bbox_world'][0], obj['bbox_world'][1] # 获取目标中心点坐标
target_point = (int(target_cx), int(target_cy)) # 转换为整数元组
# 判断点是否在多边形内
# cv2.pointPolygonTest 返回 >0 在内，=0 在边界，<0 在外
is_inside = cv2.pointPolygonTest(self.polygon_points_bev, target_point, False) >= 0 # 判断是否在区域内
# 获取目标的历史状态
previous_status = self.track_status.get(track_id, {
'current_status': 'unknown', 'enter_time': None}) # 获取历史状态
if is_inside: # 如果目标当前在区域内
if previous_status['current_status'] == 'outside' or previous_status['current_status'] == 'unknown':
# 从外部进入内部 -> 入侵事件
self.event_manager.trigger_event(
'region_enter', # 事件类型
{
'region': self.region_name, 'track_id': track_id, 'status': 'entered', 'time': time.time()} # 事件数据
)
self.track_status[track_id] = {
'current_status': 'inside', 'enter_time': time.time()} # 更新状态和进入时间
logger.info(f"事件: 目标 {
track_id} 已进入区域 '{
self.region_name}'。") # 记录日志
else: # 持续在区域内
self.track_status[track_id]['current_status'] = 'inside' # 保持在区域内状态
# 检查是否长时间停留
if previous_status['enter_time'] is not None: # 如果有进入时间
stay_duration = time.time() - previous_status['enter_time'] # 计算停留时间
# 假设我们有一个阈值来判断徘徊，例如5秒
if stay_duration > 5.0 and not previous_status.get('stay_alerted', False): # 如果停留超过5秒且未曾报警
self.event_manager.trigger_event(
'region_stay_long', # 事件类型
{
'region': self.region_name, 'track_id': track_id, 'duration': stay_duration, 'time': time.time()} # 事件数据
)
self.track_status[track_id]['stay_alerted'] = True # 标记已报警，避免重复触发
logger.warning(f"事件: 目标 {
track_id} 在区域 '{
self.region_name}' 停留时间过长 ({
stay_duration:.1f}秒)。") # 记录警告
else: # 如果目标当前在区域外
if previous_status['current_status'] == 'inside':
# 从内部离开外部 -> 离开事件
self.event_manager.trigger_event(
'region_leave', # 事件类型
{
'region': self.region_name, 'track_id': track_id, 'status': 'left', 'time': time.time()} # 事件数据
)
self.track_status[track_id] = {
'current_status': 'outside', 'enter_time': None} # 更新状态和清空进入时间
logger.info(f"事件: 目标 {
track_id} 已离开区域 '{
self.region_name}'。") # 记录日志
else: # 持续在区域外
self.track_status[track_id]['current_status'] = 'outside' # 保持在区域外状态
self.track_status[track_id]['enter_time'] = None # 确保进入时间被清空
if 'stay_alerted' in self.track_status[track_id]: # 如果有停留报警标记
del self.track_status[track_id]['stay_alerted'] # 清除停留报警标记
- 代码深度解析与RegionMonitor亮点：
- 区域定义 (
polygon_points_bev): 监控区域用BEV世界坐标系中的一个多边形来定义。np.array(..., dtype=np.int32).reshape((-1, 1, 2))将点列表转换为OpenCVcv2.pointPolygonTest函数所需的特定格式。 self.event_manager:RegionMonitor不直接处理事件（如打印到控制台），而是通过一个统一的EventManager实例来触发事件。这实现了职责分离，使得事件处理逻辑可以集中管理和配置。self.track_status: 这是一个字典，用于维护每个被追踪目标的当前区域状态（'inside','outside','unknown'）以及进入区域的时间戳。这对于检测状态变化（如从外部到内部的入侵，或长时间停留）至关重要。- 清理旧轨迹: 在
update方法开始时，会检查self.track_status中是否存在当前帧tracked_objects中不再出现的track_id。这些ID对应的目标可能已经消失，如果它们之前在区域内，就会触发一个“离开”事件。 cv2.pointPolygonTest: 这个OpenCV函数是判断点是否在多边形内的核心。它返回一个浮点数，通过与0比较来判断点是否在多边形内部、外部或边界上。- 状态机逻辑: 代码实现了简单的区域状态机：
outside->inside：触发region_enter事件。inside->outside：触发region_leave事件。inside且长时间：触发region_stay_long事件，并通过stay_alerted标志避免重复触发。
- 时间戳记录:
enter_time记录目标进入区域的精确时间，用于计算停留时长。 - 日志记录: 每当事件被触发时，都会记录详细的日志，便于调试和系统审计。
- 区域定义 (
6.9.3 目标计数与流量统计 (Object Counting / Traffic Flow)
目标计数是另一个常见的业务需求，例如统计通过某个路口、区域或生产线的车辆或行人数量。
概念：
- 计数线/区域：通常是一个在BEV世界坐标系中定义的线段或一个窄的矩形区域。
- 计数事件：当目标穿过计数线或区域时，增加计数。
实现思路：
- 计数线定义：用两个BEV坐标点来定义一条线段。
- 轨迹与线段的交点判断：判断追踪目标的运动轨迹是否与计数线发生交点。
- 方向判断：区分目标是从哪一侧穿过计数线（例如，从左到右，或从上到下），以便进行双向计数。
- 去重机制：确保同一个目标在同一条线上只被计数一次，避免重复计数（例如，目标在计数线上来回移动）。
我们将创建一个CounterMonitor类来实现这些逻辑。
# business_logic_modules.py (续)
# ... (RegionMonitor类之前的内容)
class CounterMonitor:
"""
负责监控目标是否穿过预定义线段进行计数和流量统计。
线段在BEV世界坐标系中定义。
"""
def __init__(self, counter_name, line_points_bev, event_manager, count_directions=None):
"""
初始化计数监控器。
:param counter_name: 计数器的名称（例如："入口计数器", "车流量统计"）。
:param line_points_bev: 定义计数线段的两个顶点列表，[[x1, y1], [x2, y2]]。
都应在BEV世界坐标系中。
:param event_manager: 用于触发事件的EventManager实例。
:param count_directions: 期望计数的方向列表，例如 ['positive', 'negative']。
'positive' 通常指从 line_points_bev[0] 到 line_points_bev[1] 的矢量方向。
'negative' 则相反。
如果为None，则只计数穿越事件，不区分方向。
"""
self.counter_name = counter_name # 存储计数器名称
self.line_points_bev = np.array(line_points_bev, dtype=np.float32) # 存储线段顶点
self.event_manager = event_manager # 存储事件管理器实例
self.count_directions = count_directions if count_directions is not None else [] # 存储计数方向
self.current_count = {
'total': 0, 'positive': 0, 'negative': 0} # 存储当前计数
# 用于追踪每个目标的跨线状态和历史位置，避免重复计数
# {track_id: {'last_pos': [x, y], 'crossed': False, 'crossed_direction': None}}
self.track_crossing_status = {
} # 存储每个目标的跨线状态
logger.info(f"计数监控器 '{
self.counter_name}' 已初始化，计数线: {
line_points_bev}") # 记录初始化信息
def _line_intersection_point(self, p1, p2, p3, p4):
"""
计算两条线段的交点。
线段1: (p1, p2), 线段2: (p3, p4)
:return: 交点坐标 [x, y] 或 None (如果不相交)。
"""
# https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect-in-3d
# Simplified for 2D
# p1, p2 are points on segment 1
# p3, p4 are points on segment 2
# Line AB (p1 to p2), Line CD (p3 to p4)
x1, y1 = p1
x2, y2 = p2
x3, y3 = p3
x4, y4 = p4
den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4) # 分母
if den == 0: # 平行或共线
return None # 不相交
t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / den # 参数t
u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / den # 参数u
if 0 <= t <= 1 and 0 <= u <= 1: # 如果交点在线段内
intersection_x = x1 + t * (x2 - x1) # 计算交点x
intersection_y = y1 + t * (y2 - y1) # 计算交点y
return np.array([intersection_x, intersection_y], dtype=np.float32) # 返回交点
return None # 不相交
def _point_on_side_of_line(self, point, line_p1, line_p2):
"""
判断点在线段的哪一侧。
:return: >0 在一侧，<0 在另一侧，=0 在线上。
"""
# 叉积法 (Cross product)
# (x_point - x_line1) * (y_line2 - y_line1) - (y_point - y_line1) * (x_line2 - x_line1)
return (point[0] - line_p1[0]) * (line_p2[1]