Road Vision System 是一个针对交通场景的轻量级道路视觉系统。该项目旨在利用单目摄像头实时采集视频流，经过图像预处理、目标检测、跟踪、几何投影和速度估计后，为道路监控和驾驶辅助提供结构化信息。整体系统采用模块化设计，既能在普通 PC 上运行，也支持在 Jetson Nano 等嵌入式平台部署。

项目链接(Github)：https://github.com/YJxyzxyz/road-vision-system

如果该文章及项目对你有帮助的话，请给我点一个Star⭐吧😀

模块编号	核心功能
Module 1	视频采集与时间戳，支持配置输入源、分辨率、帧率，并统计实际 FPS
Module 2	图像预处理插件框架，支持可开关的去雾/去雨等传统方法，可在 CPU 或 GPU 上运行
Module 3	车辆检测接口，统一Detection数据结构，后端可切换为 Ultralytics YOLO、ONNX、TensorRT 等
Modules 4–6	跟踪、单目测距和速度估计，引入 SORT 跟踪器及几何投影
Module 7	可视化与告警，负责绘制检测框、轨迹 ID、距离和速度，并支持原始帧与处理帧对比
Module 8	性能与异步流水线，通过多线程队列实现采集、推理、显示分离，支持跳帧和热配置更新
Module 9	Jetson Nano 部署与 TensorRT，提供模型转换脚本、GStreamer 相机接入和性能调优

配置模块

项目通过 src/config.py 定义默认配置并提供配置文件加载功能。默认配置中包含摄像头分辨率、预处理链、检测模型、跟踪器参数、几何投影器参数和运行时选项等。配置文件采用 YAML 格式，可以通过 load_config(path) 加载，函数会将用户配置与默认配置递归合并。

视频输入与性能测量

`VideoSource`

VideoSource 封装了 OpenCV 的视频读取接口，可以从设备索引、文件路径或 GStreamer 管道中打开输入源。它根据配置设置图像宽高和帧率，并在每次读取时附带捕获的时间戳。

`FPSMeter`

src/io_video/fps_meter.py 定义了 FPSMeter 类，用指数滑动平均计算实时帧率。

\mathrm{fps}_{t}=(1-\alpha) \mathrm{fps}_{t-1}+\alpha \cdot \frac{1}{\Delta t}

其中 α 是平滑系数，Δt 为两次调用之间的时间间隔

预处理模块

预处理管道

PreprocessPipeline 允许用户通过配置定义一串图像处理算子。管道包含两部分：

可选门控：如果 auto_gate.enable_low_contrast_gate 为真，则先估计当前图像的灰度直方图跨度。若跨度大于阈值则跳过预处理。
算子链执行：遍历配置的算子列表，依次调用每个算子的 __call__ 方法对图像进行处理。

预处理算子与注册表

所有预处理算子都继承自 PreprocessOp。src/preprocess/registry.py 注册了可用算子，并在运行时动态选择 CUDA 版本或 CPU 版本。常用算子包括：

中值去雨 (MedianDerain)：使用 OpenCV 的中值滤波来去除细小雨丝和噪点，内核大小由参数 ksize 决定。
CLAHE去雾 (CLAHEDehaze)：对 YCrCb 或 LAB 空间的亮度通道应用自适应直方图均衡 (CLAHE)，增强对比度并减轻雾霾。

算子既有 CPU 实现，也有利用 OpenCV CUDA 接口的加速实现，例如 CUDAMedianDerain 和 CUDACLAHEDehaze 。CUDA 版本会尝试在显存中完成滤波，否则回退到对应的 CPU 实现。

后续将会基于端侧的改进AOD-Net的去雾模型来进行实时去雾。

扩展细节：

注册表：src/preprocess/registry.py 将算子的名称映射到类，并在导入时尝试加载 CUDA 版本。配置文件中的 chain 字段给出一个算子列表，每项包含 name 和参数，管道构造函数通过 get_op_class 解析名称并实例化对应类。若存在同名 CUDA 类且当前设备支持 GPU，则优先使用 CUDA 版本；否则使用 CPU 版本。这种设计使得添加新的预处理算子只需在 registry.py 中注册即可。
自动门控详细实现：门控方法根据灰度图的最大值和最小值计算对比度范围 ΔI 。当 ΔI>τ 时说明图像已有足够对比度，不需要预处理。否则执行配置的预处理链。具体实现中使用 cv2.cvtColor 将输入图像转换为灰度，并通过 np.min 和 np.max 求出上下界，门控阈值 τ 在配置中定义。

数学原理

中值滤波：对窗口中的像素排序并取中位数，用于去除孤立的噪点。
CLAHE：将图像划分为网格，分别对每个网格的直方图进行裁剪并均衡化，可视为局部直方图均衡化，使对比度增强而不过度放大噪声。
自动门控：对比度衡量采用灰度图最大值与最小值的差值；若差值大于阈值则认为对比度足够，跳过预处理

目标检测模块

抽象接口

src/detect/base.py 定义了抽象类 Detector，其核心方法 infer 接受 BGR 图像并返回 Detection 对象列表。Detection 是一个包含边界框、置信度、类别及可选跟踪 ID 与距离信息的 dataclass。

检测器注册

build_detector(cfg) 根据配置中的 backend 字段实例化不同的检测器。支持两种后端：

Ultralytics YOLO：直接调用 Ultralytics 库进行推理。
TensorRT YOLO：加载导出的 TensorRT 模型，手动完成预处理、推理与后处理

TensorRT YOLO 实现

YOLOTensorRT 在初始化时加载 TensorRT 引擎并绑定输入输出缓冲区。预处理过程包括：

Letterbox 缩放与填充：计算输入尺寸与模型尺寸之间的缩放因子，将图像等比缩放后填充背景以保持长宽比。坐标映射参数 scale 、pad_x 和 pad_y 用于后处理。
归一化与通道调整：将像素值缩放到 [0,1][0,1][0,1] 范围，调整为 C×H×W 排列并增加批次维度。

推理通过 TensorRT 的异步执行完成，获得模型输出后需要扁平化 pred 数组。后处理步骤如下：

判定是否含有“objectness”维度：根据输出维度数和类别数量判断第 5 列是否为目标置信度。
合并类别分数与目标度：若存在目标度则计算 score，否则直接使用类别得分。
阈值过滤：筛选得分大于 conf_thres 的框，并按类别分组。
边界框解码：模型输出中的 (x,y,w,h)(x, y, w, h)(x,y,w,h) 为中心点和宽高，转换为左上和右下角坐标
坐标映射回原图：根据预处理时记录的缩放和填充，按
x' = \frac{x - \mathrm{pad}_{x}}{\mathrm{max}(\epsilon, \mathrm{scale})} y' = \frac{y - \mathrm{pad}_{y}}{\mathrm{max}(\epsilon, \mathrm{scale})}

将检测框映射回原始分辨率并截断到图像边界

6. 非极大值抑制 (NMS)：对每个类别的候选框按得分排序并执行 NMS；两个框的 IoU 超过阈值则保留得分较高的一个。IoU 定义为

\mathrm{IoU}(A, B) = \frac{|A \cap B|}{|A| + |B| - |A \cap B|}

最终得到的 Detection 对象包含类别名和置信度

Ultralytics YOLO 额外细节

除了简单调用 Ultralytics 的 model.predict 方法外，代码还将模型移动到指定设备并调用 model.fuse() 融合卷积与批归一化层，以减少推理算子数量。推理完成后通过 boxes.xyxy 获取每个检测框的左上角和右下角坐标，boxes.conf 返回置信度向量，boxes.cls 返回类别索引。如果用户配置了 classes_keep，推理结果会根据类别索引过滤，只保留感兴趣的类别。由于 Ultralytics 内部执行多尺度推理和 NMS，外层不必再手动执行非极大值抑制。

TensorRT YOLO 深入实现

引擎加载与内存绑定：构造函数接收导出的 .engine 文件路径，通过 trt.Runtime 反序列化引擎，然后创建执行上下文。为了支持可变批次，代码动态设置输入张量的形状并为输入输出分配页锁定的 host 内存和 device GPU 内存。
信号维度判断：由于不同版本的导出模型其输出张量维度可能为 num_dets×(4+1+num_classes) 或 num_dets×(4+num_classes)，实现通过比较输出列数和类别数量来判断是否包含 objectness 维度。若存在 objectness，则将类别得分乘以该值作为最终置信度；否则直接使用类别得分。
后处理细节：阈值过滤后，对每个类别的候选框按分数排序并循环：取分最高的框作为输出，与当前类别其余框计算 IoU；若大于阈值则删除该候选框。该算法近似于基于贪心的 NMS。输出检测数量限制为 max_det，避免输出过多低置信度框。
批量支持：当前实现批次固定为 1，但代码预留了扩展接口，可在外层堆叠多张图像并一次性推理。只需调整输入张量的批次维度并在后处理时分别处理各批次即可。

在 TensorRT 后端相比 Ultralytics 后端可极大降低延迟，但需要手动处理所有细节包括 letterbox 缩放、归一化、维度判断、解码与 NMS。用户应确保导出的模型与配置中的类别数量一致。

几何投影与测距

投影器支持将检测框底边中心映射到道路平面，并计算与投影原点之间的欧氏距离。用户可配置原点坐标与最大测距范围。

单应性求解推导

单应矩阵 H 用于将像素平面上的点 \mathbf{x} = (x, y, 1)^T 映射到道路平面上的齐次坐标 \mathbf{X} \propto \mathbf{H} x。设四个已知对应点 (x_{i}, y_{i}) \leftrightarrow (X_{i}, Y_{i})，使用最小二乘方法求解未知矩阵元素。每对点提供两个约束：

X_{i} = \frac{h_{11}x_{i} + h_{12}y_{i} + h_{13}}{h_{31}x_{i} + h_{32}y_{i} + h_{33}}, Y_{i} = \frac{h_{21}x_{i} + h_{22}y_{i} + h_{23}}{h_{31}x_{i} + h_{32}y_{i} + h_{33}}.

世界坐标与测距

求得 H 后，可将任意像素点映射到道路平面坐标：

(X, Y, 1)^{T} \propto \mathbf{H} (x, y, 1)^{T} X = \frac{H_{11}x + H_{12}y + H_{13}}{H_{31}x + H_{32}y + H_{33}} Y = \frac{H_{21}x + H_{22}y + H_{23}}{H_{31}x + H_{32}y + H_{33}}

系统默认使用检测框的底边中心作为行人或车辆在地面的投影点。通过设置原点并计算欧氏距离可获得目标与摄像头的距离。由于单应性只保证比例关系，该距离需要按预先测量的标定距离进行缩放。

如果使用 GroundProjector.distance_for_bbox 结合每帧时间差，可进一步计算目标的行驶速度并在可视化中显示。

多目标跟踪模块

算法概述

该系统采用 SORT (Simple Online and Realtime Tracking) 算法。每个目标跟踪轨迹维护一个七维状态向量\mathbf{x} = [c_{x}, c_{y}, s, r, v_{x}, v_{y}, \dot{s}]^{T}，其中 cx,cy 为框中心，s 为面积，r 为宽高比，vx,vy 是速度，s· 是面积增长率

状态转移矩阵 F 随时间间隔动态更新：

\mathbf{F} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & dt & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & dt & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & dt & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}

观测向量为测量的[c_{x}, c_{y}, s, r]^{T}。Kalman 滤波器的协方差矩阵根据运动噪声和测量噪声初始化。

IoU 与框转换

轨迹–检测关联使用交并比 IoU。对于两个框：

\mathrm{IoU}(A, B) = \frac{\max(0, \min(x_2, x'_2) - \max(x_1, x'_1)) \cdot \max(0, \min(y_2, y'_2) - \max(y_1, y'_1))}{|A| + |B| - |A \cap B|}

Kalman 滤波

卡尔曼滤波器由预测和更新两步组成。在预测步，利用上一时刻的状态和协方差推算当前状态

\hat{\mathbf{x}}_{t|t-1} = \mathbf{F} \hat{\mathbf{x}}_{t-1}

\mathbf{P}_{t|t-1} = \mathbf{F} \mathbf{P}_{t-1} \mathbf{F}^{T} + \mathbf{Q}

其中 Q 为过程噪声协方差，通过配置中的运动噪声系数计算。在更新步，给定测量与测量矩阵，计算卡尔曼增益：

\mathbf{K}_{t} = \mathbf{P}_{t|t-1} \mathbf{H}^{T} (\mathbf{H} \mathbf{P}_{t|t-1} \mathbf{H}^{T} + \mathbf{R})^{-1}

然后更新状态与协方差：

\hat{\mathbf{x}}_{t} = \hat{\mathbf{x}}_{t|t-1} + \mathbf{K}_{t} (\mathbf{z}_{t} - \mathbf{H} \hat{\mathbf{x}}_{t|t-1})

\mathbf{P}_{t} = (\mathbf{I} - \mathbf{K}_{t} \mathbf{H}) \mathbf{P}_{t|t-1}

当检测缺失时，仅执行预测步，轨迹失活计数增加；若失活时间超过 max_staleness 则将轨迹删除。初始化时，协方差的对角元素设置较大以反映对初始状态的不确定性。

速度估计与平滑

当几何投影器可用时，跟踪器在每次测量更新后将检测框映射到道路平面，并记录时间戳与位置。速度通过滑动窗口平均计算：

v_{t} = \frac{1}{K-1} \sum_{i=1}^{K-1} \frac{d_{t-i+1} - d_{t-i}}{t_{t-i+1} - t_{t-i}} ,

可视化模块

src/vis/draw.py 提供 draw_detections 函数，根据每个检测的类别选取颜色，在图像上绘制矩形框并显示类别名称、置信度、跟踪 ID 及测距/测速信息。标签绘制使用两个内置辅助函数分别在框上方和下方绘制带背景色的文本。

运行与主程序

Runtime 配置管理

src/runtime/config.py 提供线程安全的 RuntimeConfigStore 保存当前配置版本，并通过 ConfigWatcher 监视配置文件的修改，定时重新加载并更新存储。这样在异步模式下可实现热重载。

异步管道

AsyncPipeline 将采集、预处理与推理划分到不同线程，通过队列进行通信

捕获线程：_CaptureThread 循环读取视频帧，将图像和时间戳打包进 FramePacket 并放入队列。可选择在队列满时丢弃最老的帧。
推理工作线程：_InferWorker 从捕获队列读取帧，根据配置创建预处理管道和检测器，然后输出 InferencePacket 到结果队列。
输出合并：主线程按序号从结果队列取出包裹，调用跟踪器更新后进行绘制并显示，同时统计性能。

AsyncPipeline 支持多推理线程，利用多核 CPU/GPU 提升吞吐量。PerfAggregator 定期输出各阶段的平均耗时和队列占用。

队列管理与错误处理

捕获线程与推理线程之间通过 Queue 进行通信。可配置最大容量 max_queue_size，当队列已满且 drop_oldest 为真时，捕获线程丢弃队首元素保证最新帧能够入队，否则阻塞等待。推理线程从队列取出帧时采用阻塞模式，如捕获线程提前结束将放入一个终止信号使线程安全退出。

推理过程中任何异常都会被捕获并记录，包装成 InferencePacket 并通过结果队列传递给主线程。主线程在检查包裹时若发现异常则打印错误并停止管道

配置热更新

ConfigWatcher 在后台定时监视配置文件的修改时间。当检测到文件修改时，重新加载配置并更新 RuntimeConfigStore。异步管道中的推理线程每处理一帧都会读取当前配置实例，因此可以在不停止程序的情况下调整预处理链、阈值、颜色等参数，实现在线调试。

主线程渲染

异步模式下，主线程负责从结果队列按序号获取推理结果，调用跟踪器更新并绘制。因为推理工作可能乱序完成，队列元素包含 seq_id 用于确保帧顺序。主线程持续刷新 UI 窗口并响应按键事件，例如按 q 或 Esc 退出。在 preview.compare 模式下，会同时显示原图和处理后图像便于对比。

同步模式简单直观，但当推理延迟较大时会造成界面卡顿。异步模式通过分离采集与推理提升流畅度，适用于需要稳定实时帧率的场景。

同步模式

main_preview.py 提供同步预览运行模式，用单线程顺序完成采集、预处理、检测和跟踪。根据 preview.compare 配置，可以显示原图和处理后图像的并排或上下对比。程序在键盘按下 q 或 Esc 时退出，并在需要时热重载配置。

雾霾合成与增强

针对于后续的端侧去雾模型训练，设计了一个新的雾霾合成方式。

随机场与深度先验

src/augment/fog.py 实现了基于大气散射模型的道路雾合成。首先利用多八度随机场函数 rand_perlin 生成低频噪声，模拟雾团的不均匀性。然后构建近似深度图：

透视深度 d_{\mathrm{persp}} \propto 1/(y-y_h)，模拟越靠近地平线越远。
远点约束深度 d_{\mathrm{vanish}} \propto 1 / \sqrt{(x - x_v)^2 + (y - y_v)^2}。
将两者归一化并加权混合，再通过 S 形函数软化地平线过渡

大气散射模型

雾霾合成遵循经典的大气散射公式：

I(x) = J(x) t(x) + A(x) (1-t(x))

t(x) = e^{-\beta(x)d(x)}

代码中根据雾强度 presets 或气象能见度 MOR 选择 β 范围。然后利用 Perlin 噪声产生扰动的 beta_map，并根据深度图计算传输图。空气光 A_map 根据图像顶部亮度自适应获取并添加渐变与随机色偏。

为了更好的效果加入了如下的策略：

全局大气幕：按天空权重叠加一层均匀的气帘。
柔和光晕：对高亮区域进行高斯模糊并与原图融合，使远处光源呈现柔和晕影。
深度递增模糊：根据深度值决定模糊核大小，将远处区域模糊得更明显。
局部对比衰减：用双边滤波降低局部对比并保持边缘。
轻微色温/伽马调整与噪声：随机改变色温、施加伽马变换并加少量噪声，模拟传感器特性。

深入说明

Perlin 噪声生成：rand_perlin 函数通过多尺度叠加随机梯度的方式生成连贯的低频噪声。噪声的频率范围由 f_min 与 f_max 决定，振幅由指数分布随机采样。合成的噪声用作 beta_map 和 tint 的扰动，保证雾的分布不均匀且细节自然。
深度先验：深度图的构造基于几何常识：图像越接近地平线越远，越接近透视消失点越远。通过将两个深度代理加权，并对地平线附近应用 S 形函数，获得平滑的深度梯度。horizion_y 和 vanish_point 等参数可以从透视校准中获取，或者根据场景调整。
Beta 与传输图：消光系数 β(x) 来源于 beta_map 的基础值加噪声扰动。其范围根据输入能见度 (MOR) 或预设雾强度级别映射。随后按照散射模型计算传输图 t(x)。为避免边缘伪影，代码可选用 guided filter 对传输图进行引导滤波。
空气光估计：雾霾环境下，天空与地面反射的散射光会造成空白色调。compute_airlight 函数首先计算图像顶部一条横带的平均颜色，并通过最大光度原则估计环境光颜色。随后乘以随机增益和渐变因子生成 A_map，在雾图中模拟天空逐渐变亮的现象。
合成流程：最终的雾图按散射方程计算。然后施加全局大气幕、光晕、深度模糊、局部对比衰减、随机色温/伽马变化与噪声，形成具有真实感的道路雾。合成器返回雾图和元数据（如深度图、传输图和空气光），便于评估或作为进一步处理输入。