diff --git a/src/tracking_car/README.md b/src/tracking_car/README.md index e69de29bb2..7d4e5dc5c4 100644 --- a/src/tracking_car/README.md +++ b/src/tracking_car/README.md @@ -0,0 +1,284 @@ +# CARLA 多目标跟踪与行为分析系统 + +[![Python Version](https://img.shields.io/badge/python-3.8%2B-blue)](https://www.python.org/) +[![CARLA Version](https://img.shields.io/badge/CARLA-0.9.14%2B-orange)](https://carla.org/) +[![License](https://img.shields.io/badge/license-MIT-green)](LICENSE) + +一个基于 CARLA 仿真环境和 YOLOv8 的实时车辆多目标跟踪系统,支持 2D/3D 感知融合、车辆行为分析、多天气场景适配和数据记录功能。 + +## 🌟 核心功能 + +- 🚗 **实时目标检测与跟踪**:基于 YOLOv8 + SORT 算法实现车辆检测与多目标跟踪 +- 🌤️ **多天气场景适配**:支持晴天、雨天、雾天、夜晚、多云、雪天等天气,并自动调整图像增强策略 +- 📊 **车辆行为分析**:检测停车、超车、变道、刹车、危险接近等行为 +- 📡 **多传感器融合**:支持 RGB 相机 + LiDAR 点云融合检测 +- 📝 **数据记录**:自动记录跟踪结果、性能指标和配置参数 +- 🎮 **3D 可视化**:实时显示 LiDAR 点云数据和跟踪结果 +- ⚡ **高性能**:多线程架构,支持 GPU 加速和模型量化 + +## 📋 环境要求 + +### 基础环境 +- Python 3.7 +- CARLA 0.9.14 或更高版本 +- CUDA 11.8+ (推荐,用于 GPU 加速) + +### 硬件要求 +- CPU:4核以上 +- GPU:NVIDIA GPU (8GB 显存以上,推荐 RTX 3060+) +- 内存:16GB 以上 + +## 🛠️ 安装步骤 + +### 1. 安装 CARLA +参考 [CARLA 官方文档](https://carla.readthedocs.io/en/latest/start_quickstart/) 安装 CARLA 仿真环境: + +```bash +# 方式1:使用 pip 安装 +pip install carla + +# 方式2:下载预编译版本 +# https://github.com/carla-simulator/carla/releases +``` + +### 2. 安装依赖包 +```bash +# 克隆仓库 +git clone https://github.com/your-username/carla-object-tracking.git +cd carla-object-tracking + +# 安装依赖 +pip install -r requirements.txt +``` + +### 3. 依赖包列表 +核心依赖包: +```txt +carla>=0.9.14 +ultralytics>=8.0.0 +torch>=2.0.0 +opencv-python>=4.8.0 +numpy>=1.24.0 +open3d>=0.17.0 +scipy>=1.10.0 +scikit-learn>=1.2.0 +numba>=0.58.0 +loguru>=0.7.0 +pyyaml>=6.0 +psutil>=5.9.0 +dataclasses>=0.6 +``` + +## 🚀 快速开始 + +### 1. 启动 CARLA 服务器 +```bash +# 进入 CARLA 安装目录 +cd /path/to/carla/root + +# 启动服务器 +./CarlaUE4.sh -windowed -ResX=800 -ResY=600 +``` + +### 2. 运行跟踪程序 +```bash +# 基本运行 +python carla_tracking.py + +# 指定配置文件 +python carla_tracking.py --config config.yaml + +# 自定义参数 +python carla_tracking.py --host localhost --port 2000 --conf-thres 0.5 --weather rain +``` + +### 3. 交互控制 +| 按键 | 功能 | +|------|------| +| ESC | 退出程序 | +| W/w | 切换天气模式(晴天→雨天→雾天→夜晚→多云→雪天) | + +## ⚙️ 配置说明 + +### 配置文件格式 (config.yaml) +```yaml +# 基础配置 +host: "localhost" +port: 2000 +num_npcs: 20 + +# 图像配置 +img_width: 640 +img_height: 480 + +# 检测配置 +conf_thres: 0.5 +iou_thres: 0.3 +yolo_model: "yolov8n.pt" +yolo_imgsz_max: 320 +yolo_iou: 0.45 +yolo_quantize: false + +# 跟踪配置 +max_age: 5 +min_hits: 3 +kf_dt: 0.05 +max_speed: 50.0 + +# 行为分析配置 +stop_speed_thresh: 1.0 +stop_frames_thresh: 5 +overtake_speed_ratio: 1.5 +overtake_dist_thresh: 50.0 +lane_change_thresh: 0.5 +brake_accel_thresh: 2.0 +turn_angle_thresh: 15.0 +danger_dist_thresh: 10.0 +predict_frames: 10 + +# 可视化配置 +window_width: 1280 +window_height: 720 +display_fps: 30 +track_history_len: 20 + +# LiDAR 配置 +use_lidar: true +lidar_channels: 32 +lidar_range: 100.0 +lidar_points_per_second: 500000 +fuse_lidar_vision: true + +# 数据记录配置 +record_data: true +record_dir: "track_records" +record_format: "csv" +record_fps: 10 +save_screenshots: false + +# 3D 可视化配置 +use_3d_visualization: true +pcd_view_size: 800 +``` + +### 命令行参数 +| 参数 | 说明 | 默认值 | +|------|------|--------| +| --config | 配置文件路径 | None | +| --host | CARLA 服务器地址 | localhost | +| --port | CARLA 服务器端口 | 2000 | +| --conf-thres | 检测置信度阈值 | 0.5 | +| --weather | 初始天气模式 | clear | + +## 📁 输出文件结构 + +程序运行时会自动创建记录目录(默认:`track_records/`): +``` +track_records/ +└── 20250101_120000/ # 时间戳目录 + ├── config.yaml # 运行配置备份 + ├── performance.csv # 性能指标记录 + ├── track_results.csv # 跟踪结果记录 + └── screenshots/ # 截图目录(可选) + └── screenshot_clear_000001.png +``` + +### 跟踪结果字段说明 (track_results.csv) +| 字段 | 说明 | +|------|------| +| timestamp | 时间戳 | +| frame_id | 帧ID | +| track_id | 跟踪ID | +| x1,y1,x2,y2 | 检测框坐标 | +| cls_id | 类别ID | +| cls_name | 类别名称 (Car/Bus/Truck/Unknown) | +| behavior | 行为标签 | +| speed | 估计速度 | +| confidence | 检测置信度 | + +### 性能指标字段说明 (performance.csv) +| 字段 | 说明 | +|------|------| +| timestamp | 时间戳 | +| frame_id | 帧ID | +| fps | 帧率 | +| cpu_usage | CPU 使用率 (%) | +| memory_usage | 内存使用率 (%) | +| gpu_usage | GPU 使用率 (%) | +| detection_count | 检测目标数 | +| track_count | 跟踪目标数 | + +## 🎨 可视化界面说明 + +### 主可视化窗口 +- **顶部信息栏**:显示 FPS、天气、跟踪数量、行为统计、性能指标 +- **检测框**:蓝色边框,显示类别、置信度、跟踪ID +- **行为标签**:红色背景显示 STOP/DANGER 等关键行为 +- **轨迹线**:绿色线条显示车辆运动轨迹 + +### LiDAR 3D 窗口 +- 实时显示点云数据,Z轴高度用颜色编码(红→蓝) +- 支持鼠标交互旋转/缩放视角 + +## 📚 核心算法说明 + +### 目标检测 +- 使用 YOLOv8 作为基础检测器,支持 Car/Bus/Truck 三类车辆 +- 针对不同天气自动调整图像增强策略(去雾、去雨、去雪、降噪) + +### 多目标跟踪 +- 基于 SORT 算法,使用卡尔曼滤波预测目标位置 +- 匈牙利算法进行检测框匹配 +- IOU 作为匹配代价 + +### 行为分析 +- **停车**:速度低于阈值且持续多帧 +- **超车**:相对自车速度比超过阈值 +- **变道**:横向位移超过阈值 +- **刹车**:加速度低于负阈值 +- **危险接近**:距离自车过近 + +## 🔧 常见问题 + +### Q1: CARLA 连接失败 +``` +解决方法: +1. 确认 CARLA 服务器已启动 +2. 检查端口是否正确(默认 2000) +3. 关闭防火墙或添加例外 +``` + +### Q2: GPU 内存不足 +``` +解决方法: +1. 降低 yolo_imgsz_max 参数 +2. 使用更小的模型(如 yolov8n.pt 而非 yolov8x.pt) +3. 启用 yolo_quantize: true 量化模型 +``` + +### Q3: 帧率过低 +``` +解决方法: +1. 降低 display_fps 参数 +2. 关闭 LiDAR (use_lidar: false) +3. 减少 NPC 数量 (num_npcs) +4. 降低图像分辨率 (img_width/img_height) +``` + +### Q4: 自车生成失败 +``` +解决方法: +1. 检查 CARLA 地图是否加载完成 +2. 减少 NPC 数量避免碰撞 +3. 程序会自动尝试偏移位置重试 +``` + +## 📄 许可证 + +本项目采用 MIT 许可证 - 详见 [LICENSE](LICENSE) 文件 + +## 🙏 致谢 + +- [CARLA Simulator](https://carla.org/) - 开源自动驾驶仿真平台 +- [Ultralytics YOLOv8](https://github.com/ultralytics/ultralytics) - 目标检测模型 +- [SORT Algorithm](https://github.com/abewley/sort) - 多目标跟踪算法 diff --git a/src/tracking_car/config.yaml b/src/tracking_car/config.yaml index 642d8d90c8..a092fc5836 100644 --- a/src/tracking_car/config.yaml +++ b/src/tracking_car/config.yaml @@ -8,7 +8,7 @@ conf_thres: 0.5 iou_thres: 0.3 max_age: 5 min_hits: 3 -yolo_model: "yolov8n.pt" +yolo_model: '"D:/harudt/yolov8n.pt"' yolo_imgsz_max: 320 yolo_iou: 0.45 yolo_quantize: false diff --git a/src/tracking_car/main.py b/src/tracking_car/main.py index 6569b4e63c..13f899b9a5 100644 --- a/src/tracking_car/main.py +++ b/src/tracking_car/main.py @@ -1,851 +1,1072 @@ +""" +main.py - CARLA多目标跟踪系统主程序 +入口文件,协调各个模块运行 +""" + +import sys +import os +import time import argparse -import carla -import queue -import random import cv2 import numpy as np -import time -import os -import sys -import yaml -import threading +import carla import torch -import csv -import open3d as o3d -import psutil -from ultralytics import YOLO -from dataclasses import dataclass -from scipy.optimize import linear_sum_assignment -from datetime import datetime -from numba import njit -from loguru import logger -from sklearn.cluster import DBSCAN - -# ======================== 通用工具函数 ======================== -def valid_img(img): - return img is not None and len(img.shape)==3 and img.shape[2]==3 and img.size>0 +import queue -def clip_box(bbox, img_shape): - h,w = img_shape - return np.array([ - max(0,min(bbox[0],w-1)), - max(0,min(bbox[1],h-1)), - max(bbox[0]+1,min(bbox[2],w-1)), - max(bbox[1]+1,min(bbox[3],h-1)) - ], dtype=np.float32) +# 添加当前目录到路径,确保可以导入模块 +sys.path.append(os.path.dirname(os.path.abspath(__file__))) -def make_div(x, d=32): - return (x + d -1) // d * d +# 导入自定义模块 +try: + import utils + import sensors + import tracker + from loguru import logger +except ImportError as e: + print(f"❌ 导入模块失败: {e}") + print("请确保以下文件在同一目录下:") + print(" - utils.py") + print(" - sensors.py") + print(" - tracker.py") + sys.exit(1) -# ======================== 常量配置 ======================== -PLATFORM, IS_WIN, IS_LINUX = sys.platform, sys.platform.startswith('win'), sys.platform.startswith('linux') -WEATHER = { - 'clear':carla.WeatherParameters(0.0,0.0,0.0,0.0,180.0,75.0,0.0,0.0,1.0,0.0,0.0), - 'rain':carla.WeatherParameters(80.0,80.0,50.0,30.0,180.0,45.0,20.0,50.0,0.8,80.0,0.5), - 'fog':carla.WeatherParameters(90.0,0.0,0.0,10.0,180.0,30.0,70.0,20.0,0.5,10.0,0.8), - 'night':carla.WeatherParameters(20.0,0.0,0.0,0.0,0.0,-90.0,10.0,100.0,0.7,0.0,1.0), - 'cloudy':carla.WeatherParameters(90.0,0.0,0.0,20.0,180.0,60.0,10.0,100.0,0.9,0.0,0.3), - 'snow':carla.WeatherParameters(90.0,90.0,80.0,40.0,180.0,20.0,30.0,30.0,0.6,50.0,0.7) -} -VEHICLE_CLS = {2:"Car",5:"Bus",7:"Truck",-1:"Unknown"} -# ======================== 配置类 ======================== -@dataclass -class Config: - host, port, num_npcs = "localhost", 2000, 20 - img_width, img_height = 640, 480 - conf_thres, iou_thres, max_age, min_hits = 0.5, 0.3, 5, 3 - yolo_model, yolo_imgsz_max, yolo_iou, yolo_quantize = "yolov8n.pt", 320, 0.45, False - kf_dt, max_speed = 0.05, 50.0 - window_width, window_height, smooth_alpha, fps_window_size, display_fps = 1280,720,0.2,15,30 - track_history_len, track_line_width, track_alpha = 20,2,0.6 - stop_speed_thresh, stop_frames_thresh = 1.0,5 - overtake_speed_ratio, overtake_dist_thresh = 1.5,50.0 - lane_change_thresh, brake_accel_thresh, turn_angle_thresh, danger_dist_thresh, predict_frames = 0.5,2.0,15.0,10.0,10 - default_weather, auto_adjust_detection = "clear", True - use_lidar, lidar_channels, lidar_range, lidar_points_per_second, fuse_lidar_vision = True,32,100.0,500000,True - record_data, record_dir, record_format, record_fps, save_screenshots = True,"track_records","csv",10,False - use_3d_visualization, pcd_view_size = True,800 +# ======================== 配置管理 ======================== - @classmethod - def from_yaml(cls, p=None): - try: - p = p or os.path.join(os.path.dirname(os.path.abspath(__file__)), "config.yaml") - if not os.path.exists(p): return cls() - with open(p, "r", encoding="utf-8") as f: data = yaml.safe_load(f.read().strip().replace("\t"," ")) - if not isinstance(data, dict): return cls() - valid_keys = set(cls.__dataclass_fields__.keys()) - data = {k:v for k,v in data.items() if k in valid_keys} - for k,v in data.items(): - try: data[k] = cls.__dataclass_fields__[k].type(v) - except: del data[k] - return cls(**data) - except: return cls() +def load_config(config_path=None): + """ + 加载配置 + + Args: + config_path: 配置文件路径 + + Returns: + dict: 配置字典 + """ + # 默认配置 + default_config = { + # CARLA连接 + 'host': 'localhost', + 'port': 2000, + 'timeout': 20.0, + + # 传感器 + 'img_width': 640, + 'img_height': 480, + 'fov': 90, + 'sensor_tick': 0.05, + 'use_lidar': True, + 'lidar_channels': 32, + 'lidar_range': 100.0, + 'lidar_points_per_second': 500000, + + # 检测 + 'yolo_model': 'yolov8n.pt', + 'conf_thres': 0.5, + 'iou_thres': 0.3, + 'device': 'cuda' if torch.cuda.is_available() else 'cpu', + 'yolo_imgsz_max': 320, + + # 跟踪 + 'max_age': 5, + 'min_hits': 3, + 'kf_dt': 0.05, + 'max_speed': 50.0, + + # 行为分析 + 'stop_speed_thresh': 1.0, + 'stop_frames_thresh': 5, + 'overtake_speed_ratio': 1.5, + 'overtake_dist_thresh': 50.0, + 'lane_change_thresh': 0.5, + 'brake_accel_thresh': 2.0, + 'turn_angle_thresh': 15.0, + 'danger_dist_thresh': 10.0, + 'predict_frames': 10, + 'track_history_len': 20, + + # 可视化 + 'window_width': 1280, + 'window_height': 720, + 'display_fps': 30, + + # 天气 + 'weather': 'clear', + 'num_npcs': 20, + + # 自车 + 'ego_vehicle_filter': 'vehicle.tesla.model3', + 'ego_vehicle_color': '255,0,0', + } + + # 如果提供了配置文件,尝试加载 + if config_path and os.path.exists(config_path): + loaded_config = utils.load_yaml_config(config_path) + if loaded_config: + # 合并配置(加载的配置覆盖默认配置) + for key, value in loaded_config.items(): + if isinstance(value, dict) and key in default_config and isinstance(default_config[key], dict): + # 递归合并字典 + default_config[key].update(value) + else: + default_config[key] = value + logger.info(f"✅ 已加载配置文件: {config_path}") + + return default_config -# ======================== 天气图像增强 ======================== -class WeatherEnhancer: - def __init__(self, cfg): - self.cfg = cfg; self.weather = "clear" - self.params = { - 'clear':{'b':1.0,'c':1.0,'g':1.0}, - 'rain':{'b':1.1,'c':1.2,'g':0.9,'dh':True,'dr':True}, - 'fog':{'b':1.3,'c':1.4,'g':0.8,'dh':True}, - 'night':{'b':1.5,'c':1.3,'g':0.7,'dn':True}, - 'cloudy':{'b':1.2,'c':1.1,'g':1.0}, - 'snow':{'b':1.1,'c':1.3,'g':0.9,'dh':True,'ds':True} - } - def set_weather(self, w): - if w in WEATHER: self.weather = w - def enhance(self, img): - if not self.cfg.auto_adjust_detection or not valid_img(img): return img - p = self.params.get(self.weather, self.params['clear']) - enh = cv2.convertScaleAbs(img.copy(), alpha=p['c'], beta=int(p['b']*255-255)) - g = p['g']; inv_g = 1.0/g - g_table = np.array([((i/255.0)**inv_g)*255 for i in range(256)]).astype(np.uint8) - enh = cv2.LUT(enh, g_table) - if p.get('dh'): enh = self._dehaze(enh) - if p.get('dr'): enh = self._derain(enh) - if p.get('ds'): enh = self._desnow(enh) - if p.get('dn'): enh = cv2.fastNlMeansDenoisingColored(enh, None,10,10,7,21) - return enh - def _dehaze(self, img): - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - dc = cv2.erode(gray, np.ones((7,7), np.uint8)) - nz = dc[dc<10]; al = 255.0 if len(nz)==0 else np.max(img[dc<10]) - t = np.clip(1-0.1*(gray/al),0.1,1.0) - deh = np.zeros_like(img, dtype=np.float32) - for c in range(3): deh[:,:,c] = (img[:,:,c].astype(np.float32)-al)/t + al - return np.clip(deh,0,255).astype(np.uint8) - def _derain(self, img): - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - km = cv2.getStructuringElement(cv2.MORPH_RECT, (1,5)) - rm = cv2.morphologyEx(gray, cv2.MORPH_OPEN, km) - return cv2.inpaint(img, (255-rm).astype(np.uint8),3,cv2.INPAINT_TELEA) - def _desnow(self, img): - blur = cv2.GaussianBlur(img,(5,5),0) - gray = cv2.cvtColor(blur, cv2.COLOR_BGR2GRAY) - _, sm = cv2.threshold(gray,200,255,cv2.THRESH_BINARY) - return cv2.inpaint(img,sm,5,cv2.INPAINT_NS) -# ======================== LiDAR处理 ======================== -class LiDARProc: - def __init__(self, cfg): - self.cfg = cfg; self.q = queue.Queue(maxsize=2); self.data, self.trans = None, None - def cb(self, pc): +def setup_carla_client(config): + """ + 设置CARLA客户端 + + Args: + config: 配置字典 + + Returns: + tuple: (client, world) or (None, None) + """ + try: + logger.info(f"正在连接CARLA服务器 {config['host']}:{config['port']}...") + client = carla.Client(config['host'], config['port']) + client.set_timeout(config['timeout']) + + world = client.get_world() + + # 设置同步模式 + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + + # 设置交通管理器 try: - self.data = np.frombuffer(pc.raw_data, dtype=np.float32).reshape(-1,4)[:,:3] - self.trans = pc.transform - if self.q.full(): self.q.get_nowait() - self.q.put((self.data.copy(), self.trans)) - except: pass - def detect(self): - if self.data is None or len(self.data)<50: return [] - gm = self.data[:,2] < -1.0; ng = self.data[~gm] - if len(ng)<50: return [] - cls = DBSCAN(eps=0.8, min_samples=30).fit(ng[:,:2]) - boxes = [] - for l in set(cls.labels_): - if l == -1: continue - cp = ng[cls.labels_==l] - if len(cp)<30: continue - mn = cp.min(axis=0); mx = cp.max(axis=0) - boxes.append({'3d_bbox':[mn[0],mn[1],mn[2],mx[0],mx[1],mx[2]],'center':[(mn[0]+mx[0])/2,(mn[1]+mx[1])/2,(mn[2]+mx[2])/2],'size':[mx[0]-mn[0],mx[1]-mn[1],mx[2]-mn[2]],'num_points':len(cp)}) - return boxes - def get_3d(self): - if self.data is None: return None - pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(self.data) - z_min,z_max = self.data[:,2].min(),self.data[:,2].max() - c = (self.data[:,2]-z_min)/(z_max-z_min+1e-6) - cm = np.zeros((len(c),3)); cm[:,0]=1-c; cm[:,2]=c - pcd.colors = o3d.utility.Vector3dVector(cm) - return pcd + tm = client.get_trafficmanager(8000) + tm.set_global_distance_to_leading_vehicle(2.0) + tm.set_respawn_dormant_vehicles(True) + tm.set_hybrid_physics_mode(True) + tm.set_hybrid_physics_radius(50.0) + tm.global_percentage_speed_difference(0) + except Exception as e: + logger.warning(f"交通管理器设置失败: {e}") + + logger.info("✅ CARLA客户端连接成功") + return client, world + + except Exception as e: + logger.error(f"❌ 连接CARLA服务器失败: {e}") + return None, None -# ======================== 数据记录 ======================== -class Recorder: - def __init__(self, cfg): - self.cfg = cfg; self.dir = os.path.join(cfg.record_dir, datetime.now().strftime("%Y%m%d_%H%M%S")) - self.fr = 0; self.fs = {} - if cfg.record_data: - os.makedirs(self.dir, exist_ok=True) - os.makedirs(os.path.join(self.dir,"screenshots"), exist_ok=True) - self._init() - def _init(self): - tp = os.path.join(self.dir, "track_results.csv") - self.fs['tracks'] = open(tp, 'w', newline='', encoding='utf-8') - self.tw = csv.writer(self.fs['tracks']) - self.tw.writerow(['timestamp','frame_id','track_id','x1','y1','x2','y2','cls_id','cls_name','behavior','speed','confidence']) - pp = os.path.join(self.dir, "performance.csv") - self.fs['performance'] = open(pp, 'w', newline='', encoding='utf-8') - self.pw = csv.writer(self.fs['performance']) - self.pw.writerow(['timestamp','frame_id','fps','cpu_usage','memory_usage','gpu_usage','detection_count','track_count']) - cp = os.path.join(self.dir, "config.yaml") - with open(cp, 'w', encoding='utf-8') as f: yaml.dump(self.cfg.__dict__, f, indent=2) - def record(self, tracks, dets, fps): - if not self.cfg.record_data: return - if self.fr % (self.cfg.display_fps//self.cfg.record_fps) !=0: - self.fr +=1; return - ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f") - if tracks and len(tracks)>0: - for t in tracks: - try: - b = "stopped" if t.is_stopped else "overtaking" if t.is_overtaking else "lane_changing" if t.is_lane_changing else "braking" if t.is_braking else "dangerous" if t.is_dangerous else "normal" - s = t._calc_speed() if hasattr(t,'_calc_speed') else 0.0 - self.tw.writerow([ts,self.fr,t.track_id,t.bbox[0],t.bbox[1],t.bbox[2],t.bbox[3],t.cls_id,VEHICLE_CLS.get(t.cls_id,"Unknown"),b,s,t.conf if hasattr(t,'conf') else 0.0]) - except: pass - cpu = psutil.cpu_percent(); mem = psutil.virtual_memory().percent - gpu = torch.cuda.utilization() if (torch.cuda.is_available() and hasattr(torch.cuda,'utilization')) else 0.0 - self.pw.writerow([ts,self.fr,fps,cpu,mem,gpu,len(dets) if dets is not None else 0,len(tracks) if tracks is not None else 0]) - for f in self.fs.values(): f.flush() - self.fr +=1 - def save_ss(self, img, w): - if not self.cfg.save_screenshots or not valid_img(img): return - p = os.path.join(self.dir,"screenshots",f"screenshot_{w}_{self.fr:06d}.png") - cv2.imwrite(p, img) - def close(self): - if self.cfg.record_data: - for f in self.fs.values(): - try: f.close() - except: pass -# ======================== 卡尔曼滤波 ======================== -class KF: - def __init__(self, dt=0.05, ms=50.0): - self.dt = dt; self.ms = ms; self.x = np.zeros(8, dtype=np.float32) - self.F = np.array([[1,0,0,0,dt,0,0,0],[0,1,0,0,0,dt,0,0],[0,0,1,0,0,0,dt,0],[0,0,0,1,0,0,0,dt],[0,0,0,0,1,0,0,0],[0,0,0,0,0,1,0,0],[0,0,0,0,0,0,1,0],[0,0,0,0,0,0,0,1]], dtype=np.float32) - self.H = np.eye(4,8,dtype=np.float32) - self.Q = np.diag([1,1,1,1,5,5,5,5]).astype(np.float32) - self.R = np.diag([5,5,5,5]).astype(np.float32) - self.P = np.eye(8,dtype=np.float32)*50 - def predict(self): - self.x = self.F @ self.x - self.P = self.F @ self.P @ self.F.T + self.Q - return self.x[:4] - def update(self, z): - z = z.astype(np.float32) - y = z - self.H @ self.x - S = self.H @ self.P @ self.H.T + self.R - S_inv = np.linalg.pinv(S) if np.linalg.det(S)<1e-6 else np.linalg.inv(S) - K = self.P @ self.H.T @ S_inv - self.x = self.x + K @ y - self.P = (np.eye(8)-K@self.H) @ self.P - return self.x[:4] - def update_noise(self, s): - sf = min(1.0, s/self.ms) - self.Q = np.diag([1+sf*4]*4 + [5+sf*20]*4).astype(np.float32) +def set_weather(world, weather_name): + """ + 设置天气 + + Args: + world: CARLA世界对象 + weather_name: 天气名称 + """ + weather_presets = { + 'clear': carla.WeatherParameters.ClearNoon, + 'cloudy': carla.WeatherParameters.CloudyNoon, + 'rain': carla.WeatherParameters.HardRainNoon, + 'fog': carla.WeatherParameters.SoftRainNoon, + 'night': carla.WeatherParameters.ClearNight, + 'wet': carla.WeatherParameters.WetNoon, + 'wet_cloudy': carla.WeatherParameters.WetCloudyNoon, + } + + if weather_name in weather_presets: + world.set_weather(weather_presets[weather_name]) + logger.info(f"🌤️ 天气已设置为: {weather_name}") + else: + logger.warning(f"未知天气: {weather_name}, 使用晴天") -# ======================== IOU计算 ======================== -@njit -def iou(box1, box2): - ix1 = max(box1[0], box2[0]); iy1 = max(box1[1], box2[1]) - ix2 = min(box1[2], box2[2]); iy2 = min(box1[3], box2[3]) - ia = max(0, ix2-ix1)*max(0, iy2-iy1) - a1 = (box1[2]-box1[0])*(box1[3]-box1[1]) - a2 = (box2[2]-box2[0])*(box2[3]-box2[1]) - ua = a1+a2-ia - return ia/ua if ua>0 else 0 -# ======================== 跟踪目标 ======================== -class Track: - def __init__(self, tid, bbox, img_shape, kf_cfg, cfg): - self.track_id = tid; self.kf = KF(dt=kf_cfg["dt"], ms=kf_cfg["max_speed"]) - self.img_shape = img_shape; self.cfg = cfg; self.bbox = clip_box(bbox.astype(np.float32), img_shape) - self.kf.x[:4] = self.bbox; self.track_hist = []; self.speed_hist = []; self.accel_hist = [] - self.heading_hist = []; self.lat_dis = []; self.pred_traj = [] - self.is_stopped = self.is_overtaking = self.is_lane_changing = self.is_braking = self.is_accelerating = self.is_turning = self.is_dangerous = False - self.stop_fr = self.overtake_fr = self.lane_change_fr = self.brake_fr = self.turn_fr = 0 - self.hits = 1; self.age = 0; self.tsu = 0; self.cls_id = None; self.conf = 0.0 - self._update_hist() - def _update_hist(self): - cx = (self.bbox[0]+self.bbox[2])/2; cy = (self.bbox[1]+self.bbox[3])/2 - self.track_hist.append((cx, cy)) - if len(self.track_hist)>1: self.lat_dis.append(abs(cx - self.track_hist[-2][0])) - if len(self.track_hist)>self.cfg.track_history_len: self.track_hist.pop(0) - if len(self.lat_dis)>10: self.lat_dis.pop(0) - def _calc_speed(self): - if len(self.track_hist)<2: return 0.0 - pc = self.track_hist[-2]; cc = self.track_hist[-1] - s = np.linalg.norm(np.array(cc)-np.array(pc))/self.kf.dt - self.speed_hist.append(s) - if len(self.speed_hist)>1: self.accel_hist.append((s-self.speed_hist[-2])/self.kf.dt) - if len(self.speed_hist)>5: self.speed_hist.pop(0) - if len(self.accel_hist)>5: self.accel_hist.pop(0) - return np.mean(self.speed_hist) if self.speed_hist else 0.0 - def _calc_heading(self): - if len(self.track_hist)<3: return 0.0 - dx = self.track_hist[-1][0]-self.track_hist[-3][0] - dy = self.track_hist[-1][1]-self.track_hist[-3][1] - h = np.degrees(np.arctan2(dy, dx)) - self.heading_hist.append(h) - if len(self.heading_hist)>5: self.heading_hist.pop(0) - return h - def _pred_traj(self): - self.pred_traj = [] - if len(self.track_hist)<5: return - tkf = KF(dt=self.kf.dt, ms=self.kf.ms) - tkf.x = self.kf.x.copy(); tkf.P = self.kf.P.copy() - for _ in range(self.cfg.predict_frames): - pb = tkf.predict() - self.pred_traj.append(((pb[0]+pb[2])/2, (pb[1]+pb[3])/2)) - def _analyze_behavior(self, ego_center): - s = self._calc_speed(); h = self._calc_heading() - if s < self.cfg.stop_speed_thresh: - self.stop_fr +=1; self.is_stopped = self.stop_fr >= self.cfg.stop_frames_thresh - else: self.stop_fr = 0; self.is_stopped = False - if ego_center and len(self.track_hist)>=2: - d = np.linalg.norm(np.array(self.track_hist[-1])-np.array(ego_center)) - if d < self.cfg.overtake_dist_thresh: - es = getattr(self, 'ego_speed', 0.0) - if s > es*self.cfg.overtake_speed_ratio: - self.overtake_fr +=1; self.is_overtaking = self.overtake_fr >=3 - else: self.overtake_fr =0; self.is_overtaking = False - else: self.overtake_fr =0; self.is_overtaking = False - if len(self.lat_dis)>=5: - al = np.mean(self.lat_dis[-5:]) - if al>self.cfg.lane_change_thresh: - self.lane_change_fr +=1; self.is_lane_changing = self.lane_change_fr >=3 - else: self.lane_change_fr =0; self.is_lane_changing = False - if len(self.accel_hist)>=3: - aa = np.mean(self.accel_hist[-3:]) - if aa < -self.cfg.brake_accel_thresh: - self.brake_fr +=1; self.is_braking = self.brake_fr >=2; self.is_accelerating = False - elif aa > self.cfg.brake_accel_thresh: - self.is_accelerating = True; self.is_braking = False; self.brake_fr =0 - else: self.is_braking = self.is_accelerating = False; self.brake_fr =0 - if len(self.heading_hist)>=3: - hd = np.abs(self.heading_hist[-1]-self.heading_hist[-3]) - if hd>self.cfg.turn_angle_thresh: - self.turn_fr +=1; self.is_turning = self.turn_fr >=2 - else: self.turn_fr =0; self.is_turning = False - if ego_center: - d = np.linalg.norm(np.array(self.track_hist[-1])-np.array(ego_center)) - self.is_dangerous = d < self.cfg.danger_dist_thresh - self._pred_traj() - def predict(self): - if len(self.track_hist)>=2: - pc = np.array([(self.kf.x[0]+self.kf.x[2])/2, (self.kf.x[1]+self.kf.x[3])/2]) - cc = np.array([(self.bbox[0]+self.bbox[2])/2, (self.bbox[1]+self.bbox[3])/2]) - ps = np.linalg.norm(cc-pc)/self.kf.dt - mps = max(self.img_shape)/self.kf.dt - s = min(1.0, ps/mps)*self.kf.ms - else: s=0.0 - self.bbox = self.kf.predict() - self.bbox = clip_box(self.bbox, self.img_shape) - self._update_hist(); self.age +=1; self.tsu +=1; self.kf.update_noise(s) - return self.bbox - def update(self, bbox, cls_id, conf=0.0, ego_center=None): - self.bbox = self.kf.update(clip_box(bbox, self.img_shape)) - self._update_hist(); self.hits +=1; self.tsu =0; self.cls_id = cls_id; self.conf = conf - self._analyze_behavior(ego_center) +# ======================== 可视化(英文版) ======================== -# ======================== SORT跟踪器 ======================== -class SORT: - def __init__(self, cfg): - self.max_age = cfg.max_age; self.min_hits = cfg.min_hits; self.iou_th = cfg.iou_thres - self.img_shape = (cfg.img_height, cfg.img_width); self.kf_cfg = {"dt":cfg.kf_dt, "max_speed":cfg.max_speed} - self.cfg = cfg; self.tracks = []; self.next_id =1; self.ego_center = None; self.ego_speed =0.0 - def update(self, dets, ego_center=None, lidar_dets=None): - self.ego_center = ego_center - if dets is None or len(dets)==0: - if lidar_dets and len(lidar_dets)>0 and self.cfg.fuse_lidar_vision: - dets = self._lidar2d(lidar_dets) - self.tracks = [t for t in self.tracks if t.tsu <= self.max_age] - return np.array([]), np.array([]), np.array([]) - vd = [] - for d in dets: - if len(d)>=6: - x1,y1,x2,y2,conf,cls_id = d[:6] - if conf>0 and x2>x1 and y2>y1: vd.append([x1,y1,x2,y2,conf,int(cls_id)]) - vd = np.array(vd, dtype=np.float32) - if len(vd)==0: - self.tracks = [t for t in self.tracks if t.tsu <= self.max_age] - return np.array([]), np.array([]), np.array([]) - for t in self.tracks: t.predict() - if len(self.tracks)==0: - for d in vd: - self.tracks.append(Track(self.next_id, d[:4], self.img_shape, self.kf_cfg, self.cfg)) - self.next_id +=1 - return np.array([]), np.array([]), np.array([]) - try: - iou_mat = np.array([[iou(t.bbox, d[:4]) for t in self.tracks] for d in vd]) - cost_mat = 1 - iou_mat - t_idx, d_idx = linear_sum_assignment(cost_mat) - except: t_idx, d_idx = [], [] - matches, ud, ut = [], set(), set() - for ti, di in zip(t_idx, d_idx): - if tiself.iou_th: - matches.append((ti,di)); ud.add(di); ut.add(ti) - for ti, di in matches: - self.tracks[ti].update(vd[di][:4], int(vd[di][5]), vd[di][4], self.ego_center) - self.tracks[ti].ego_speed = self.ego_speed - for di in set(range(len(vd)))-ud: - self.tracks.append(Track(self.next_id, vd[di][:4], self.img_shape, self.kf_cfg, self.cfg)) - self.next_id +=1 - self.tracks = [t for t in self.tracks if t.tsu <= self.max_age] - vt = [t for t in self.tracks if t.hits >= self.min_hits] - if not vt: return np.array([]), np.array([]), np.array([]) - boxes = np.array([t.bbox.astype(int) for t in vt]) - ids = np.array([t.track_id for t in vt]) - cls = np.array([t.cls_id if t.cls_id is not None else -1 for t in vt]) - return boxes, ids, cls - def _lidar2d(self, lidar_dets): - dets = [] - for d in lidar_dets: - c = d['center']; s = d['size'] - x1 = c[0]*10 + self.img_shape[1]/2; y1 = c[1]*10 + self.img_shape[0]/2 - x2 = x1 + s[0]*5; y2 = y1 + s[1]*5 - dets.append([x1,y1,x2,y2,0.8,2]) - return np.array(dets) - -# ======================== 检测线程 ======================== -class DetThread(threading.Thread): - def __init__(self, det, cfg, enh, in_q, out_q, dev="cpu"): - super().__init__(daemon=True) - self.det = det; self.cfg = cfg; self.enh = enh; self.in_q = in_q; self.out_q = out_q - self.running = True; self.dev = dev - def run(self): - while self.running: +class Visualizer: + """可视化管理器(英文版,解决乱码问题)""" + + def __init__(self, config): + self.config = config + self.window_name = "CARLA Object Tracking" + + # 创建窗口 + cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL) + cv2.resizeWindow(self.window_name, + config.get('window_width', 1280), + config.get('window_height', 720)) + + # 车辆类别颜色映射 + self.class_colors = { + 'car': (255, 0, 0), # 蓝色 - 小汽车 + 'bus': (0, 255, 0), # 绿色 - 公交车 + 'truck': (0, 0, 255), # 红色 - 卡车 + 'default': (255, 255, 0) # 青色 - 默认 + } + + # 行为状态颜色映射(优先级从高到低) + self.behavior_colors = { + 'dangerous': (0, 0, 255), # 红色 - 危险 + 'stopped': (0, 255, 255), # 黄色 - 停车 + 'overtaking': (255, 0, 255), # 紫色 - 超车 + 'lane_changing': (0, 255, 255), # 青色 - 变道 + 'turning': (0, 255, 255), # 青色 - 转弯 + 'accelerating': (255, 0, 0), # 蓝色 - 加速 + 'braking': (0, 165, 255), # 橙色 - 刹车 + 'normal': (0, 255, 0) # 绿色 - 正常行驶 + } + + # 行为状态文本映射(使用英文) + self.behavior_texts = { + 'dangerous': 'DANGER', + 'stopped': 'STOP', + 'overtaking': 'OVERTAKE', + 'lane_changing': 'LANE CHANGE', + 'turning': 'TURNING', + 'accelerating': 'ACCEL', + 'braking': 'BRAKE', + 'normal': 'NORMAL' + } + + logger.info("✅ 可视化器初始化完成(英文版)") + + def _get_behavior_color(self, track_info): + """ + 根据行为状态返回对应颜色 + + Args: + track_info: 跟踪目标信息字典 + + Returns: + tuple: BGR颜色值 + """ + if not track_info: + return self.behavior_colors['normal'] + + # 优先级:危险 > 停车 > 超车 > 变道/转弯 > 加速/刹车 > 正常 + if track_info.get('is_dangerous', False): + return self.behavior_colors['dangerous'] + elif track_info.get('is_stopped', False): + return self.behavior_colors['stopped'] + elif track_info.get('is_overtaking', False): + return self.behavior_colors['overtaking'] + elif track_info.get('is_lane_changing', False): + return self.behavior_colors['lane_changing'] + elif track_info.get('is_turning', False): + return self.behavior_colors['turning'] + elif track_info.get('is_accelerating', False): + return self.behavior_colors['accelerating'] + elif track_info.get('is_braking', False): + return self.behavior_colors['braking'] + else: + return self.behavior_colors['normal'] + + def _get_behavior_text(self, track_info): + """ + 根据行为状态返回对应文本 + + Args: + track_info: 跟踪目标信息字典 + + Returns: + str: 行为文本 + """ + if not track_info: + return self.behavior_texts['normal'] + + # 优先级:危险 > 停车 > 超车 > 变道/转弯 > 加速/刹车 > 正常 + if track_info.get('is_dangerous', False): + return self.behavior_texts['dangerous'] + elif track_info.get('is_stopped', False): + return self.behavior_texts['stopped'] + elif track_info.get('is_overtaking', False): + return self.behavior_texts['overtaking'] + elif track_info.get('is_lane_changing', False): + return self.behavior_texts['lane_changing'] + elif track_info.get('is_turning', False): + return self.behavior_texts['turning'] + elif track_info.get('is_accelerating', False): + return self.behavior_texts['accelerating'] + elif track_info.get('is_braking', False): + return self.behavior_texts['braking'] + else: + return self.behavior_texts['normal'] + + def _get_class_name(self, class_id): + """ + 根据类别ID获取类别名称 + + Args: + class_id: 类别ID + + Returns: + str: 类别名称 + """ + class_map = { + 2: 'car', + 5: 'bus', + 7: 'truck', + } + return class_map.get(int(class_id), 'default') + + def _adjust_color_brightness(self, color, factor): + """ + 调整颜色亮度 + + Args: + color: 原始颜色 (B, G, R) + factor: 亮度因子 (0.0-1.0) + + Returns: + tuple: 调整后的颜色 + """ + return tuple(int(c * factor) for c in color) + + def draw_detections(self, image, boxes, ids, classes, tracks_info=None): + """ + 绘制检测和跟踪结果 + + Args: + image: 原始图像 + boxes: 边界框数组 + ids: 跟踪ID数组 + classes: 类别数组 + tracks_info: 跟踪详细信息 + + Returns: + np.ndarray: 绘制后的图像 + """ + if not utils.valid_img(image): + return image + + result = image.copy() + + # 绘制顶部信息栏 + result = self._draw_info_panel(result, len(boxes)) + + # 绘制边界框和ID + for i, (bbox, track_id, class_id) in enumerate(zip(boxes, ids, classes)): try: - img = self.in_q.get(timeout=1.0) - if not valid_img(img): - self.out_q.put((None, np.array([]))) + x1, y1, x2, y2 = map(int, bbox) + + # 确保坐标有效 + if x1 >= x2 or y1 >= y2: continue - img_enh = self.enh.enhance(img) - h,w = img.shape[:2]; r = min(self.cfg.yolo_imgsz_max/w, self.cfg.yolo_imgsz_max/h) - ws, hs = make_div(int(w*r)), make_div(int(h*r)) - res = self.det.predict(img_enh, conf=self.cfg.conf_thres, verbose=False, device=self.dev, agnostic_nms=True, imgsz=(hs,ws), iou=self.cfg.yolo_iou) - dets = [] - for r in res: - if hasattr(r, 'boxes') and r.boxes is not None and len(r.boxes)>0: - for b in r.boxes: - if b.cls is not None and b.conf is not None and b.xyxy is not None: - cid = int(b.cls[0]) - if cid in {2,5,7}: - xyxy = b.xyxy[0].cpu().numpy() - conf = float(b.conf[0]) - if xyxy[2]>xyxy[0] and xyxy[3]>xyxy[1] and conf>0: - dets.append([*xyxy, conf, cid]) - self.out_q.put((img, np.array(dets, dtype=np.float32))) - except queue.Empty: continue - except: self.out_q.put((None, np.array([]))) - def stop(self): self.running = False - -# ======================== 可视化工具(优化版) ======================== -class FrameBuf: - def __init__(self, sz=(480,640,3)): - self.df = np.zeros(sz, dtype=np.uint8) - cv2.putText(self.df, "Initializing...", (100,240), cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2) - self.cf = self.df.copy(); self.lock = threading.Lock() - def update(self, f): - if valid_img(f): - with self.lock: self.cf = f.copy() - def get(self): return self.cf.copy() - -class FPS: - def __init__(self, ws=15): - self.ws = ws; self.times = []; self.fps =0.0 - def update(self): - self.times.append(time.time()) - if len(self.times)>self.ws: self.times.pop(0) - if len(self.times)>=2: self.fps = (len(self.times)-1)/(self.times[-1]-self.times[0]) - return self.fps - -# ======================== 核心绘制函数(优化版) ======================== -def draw(img, boxes, ids, cls_ids, tracks, fps=0.0, det_cnt=0, cfg=None, w="clear", perf=None): - """优化版绘制函数,简洁高效,匹配截图样式""" - if not valid_img(img): - return np.zeros((480,640,3), dtype=np.uint8) - - di = img.copy() - - # 1. 绘制顶部信息栏(简洁版) - info_height = 60 - cv2.rectangle(di, (0, 0), (di.shape[1], info_height), (0, 0, 0), -1) - - # FPS和基本信息 - cv2.putText(di, f"FPS: {fps:.1f} | Weather: {w} | Tracks: {len(boxes)}", - (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1, cv2.LINE_AA) - - # 行为统计 - if tracks: - sc = sum(1 for t in tracks if t.is_stopped) - oc = sum(1 for t in tracks if t.is_overtaking) - dc = sum(1 for t in tracks if t.is_dangerous) - cv2.putText(di, f"Stop: {sc} | Overtake: {oc} | Danger: {dc}", - (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1, cv2.LINE_AA) - - # 2. 绘制检测框(简化版,匹配截图) - if boxes is not None and ids is not None: - for i, (b, tid) in enumerate(zip(boxes, ids)): - try: - if len(b) != 4: continue - x1, y1, x2, y2 = map(int, b) + # 获取当前目标的详细信息 + track_info = None + if tracks_info and i < len(tracks_info): + track_info = tracks_info[i] - # 检查坐标有效性 - if x1 >= x2 or y1 >= y2: continue + # 根据行为状态选择颜色 + behavior_color = self._get_behavior_color(track_info) - # 蓝色检测框(截图样式) - cv2.rectangle(di, (x1, y1), (x2, y2), (255, 0, 0), 2, cv2.LINE_AA) + # 根据车辆类别选择基础颜色 + class_name = self._get_class_name(class_id) + class_color = self.class_colors.get(class_name, self.class_colors['default']) - # 在左上角显示ID(白色背景,黑色文字) - id_text = f"{tid}" - text_size = cv2.getTextSize(id_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)[0] + # 融合颜色:70%行为颜色 + 30%类别颜色 + color = tuple( + int(behavior_color[j] * 0.7 + class_color[j] * 0.3) + for j in range(3) + ) - # 背景框 - bg_x2 = x1 + text_size[0] + 6 - bg_y2 = y1 + text_size[1] + 6 - cv2.rectangle(di, (x1, y1), (bg_x2, bg_y2), (255, 255, 255), -1) + # 绘制渐变色边框(外深内浅) + border_width = 3 + for thickness in range(border_width, 0, -1): + # 计算当前层的颜色亮度 + brightness = 0.3 + 0.7 * (thickness / border_width) + layer_color = self._adjust_color_brightness(color, brightness) + + # 绘制边框层 + offset = border_width - thickness + cv2.rectangle(result, + (x1 - offset, y1 - offset), + (x2 + offset, y2 + offset), + layer_color, + 1) - # 文字 - cv2.putText(di, id_text, (x1+3, y1+text_size[1]+3), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + # 绘制ID标签背景(使用行为颜色) + id_text = f"ID:{track_id}" + (text_width, text_height), baseline = cv2.getTextSize( + id_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1 + ) + + # 标签背景 + label_bg_top = y1 - text_height - 8 + label_bg_bottom = y1 + label_bg_right = x1 + text_width + 8 + + cv2.rectangle(result, + (x1, label_bg_top), + (label_bg_right, label_bg_bottom), + behavior_color, -1) + + # 标签边框 + cv2.rectangle(result, + (x1, label_bg_top), + (label_bg_right, label_bg_bottom), + (255, 255, 255), 1) + + # 绘制ID文本 + cv2.putText(result, id_text, + (x1 + 4, y1 - 4), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + # 绘制行为状态(如果可用) + if track_info: + # 获取行为文本 + behavior_text = self._get_behavior_text(track_info) + + # 在右上角绘制行为状态 + (text_width, text_height), _ = cv2.getTextSize( + behavior_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1 + ) + + # 文本位置(右上角) + text_x = x2 - text_width - 5 + text_y = y1 + text_height + 5 + + # 绘制文本背景 + cv2.rectangle(result, + (text_x - 3, text_y - text_height - 3), + (text_x + text_width + 3, text_y + 3), + behavior_color, -1) + + # 绘制文本 + cv2.putText(result, behavior_text, + (text_x, text_y), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + # 绘制速度信息(如果可用) + if 'speed' in track_info: + speed = track_info['speed'] + speed_text = f"{speed:.1f}m/s" + (speed_width, speed_height), _ = cv2.getTextSize( + speed_text, cv2.FONT_HERSHEY_SIMPLEX, 0.4, 1 + ) + + # 速度显示在左下角 + speed_x = x1 + 5 + speed_y = y2 - 5 + + # 速度背景 + cv2.rectangle(result, + (speed_x - 2, speed_y - speed_height - 2), + (speed_x + speed_width + 2, speed_y + 2), + (0, 0, 0), -1) + + # 速度文本 + cv2.putText(result, speed_text, + (speed_x, speed_y), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255), 1) except Exception as e: + logger.debug(f"绘制边界框时出错: {e}") continue + + return result - # 3. 绘制轨迹线(可选,简化版) - if tracks: - for track in tracks: - if len(track.track_hist) >= 2: - # 只绘制最近5帧轨迹 - for j in range(1, min(5, len(track.track_hist))): - try: - pt1 = (int(track.track_hist[-j-1][0]), int(track.track_hist[-j-1][1])) - pt2 = (int(track.track_hist[-j][0]), int(track.track_hist[-j][1])) - cv2.line(di, pt1, pt2, (0, 255, 0), 1, cv2.LINE_AA) - except: - pass + def _draw_info_panel(self, image, track_count): + """绘制信息面板(英文)""" + h, w = image.shape[:2] + + # 信息面板背景(半透明黑色) + panel_height = 80 + overlay = image.copy() + cv2.rectangle(overlay, (0, 0), (w, panel_height), (0, 0, 0), -1) + image = cv2.addWeighted(overlay, 0.7, image, 0.3, 0) + + # 标题(英文) + title = "CARLA Multi-Object Tracking System" + cv2.putText(image, title, (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2) + + # 状态信息(英文) + status_lines = [ + f"Tracking: {track_count} objects", + f"ESC: Exit | W: Weather | S: Screenshot", + f"P: Pause | M: Show/Hide Legend" + ] + + # 绘制状态信息 + font = cv2.FONT_HERSHEY_SIMPLEX + for i, line in enumerate(status_lines): + y_pos = 55 + i * 20 + cv2.putText(image, line, (10, y_pos), + font, 0.5, (255, 255, 255), 1) + + return image - return di - -# ======================== 工具函数 ======================== -def clear_actors(world, exclude=None): - ei = set(exclude) if exclude else set() - actors = world.get_actors() - actor_id_map = {a.id: a for a in actors if a.is_alive} - for at in ['vehicle.', 'sensor.']: - al = [a for a_id, a in actor_id_map.items() if a.type_id.startswith(at) and a_id not in ei] - for i in range(0, len(al),10): - for a in al[i:i+10]: - try: - if a.is_alive and a.id in actor_id_map: - a.destroy() - del actor_id_map[a.id] - except Exception as e: - logger.warning(f"跳过销毁Actor {a.id}:{str(e)[:30]}") - -def cam_cb(img, q): - try: - ia = np.frombuffer(img.raw_data, dtype=np.uint8).reshape((img.height, img.width,4)) - ir = cv2.GaussianBlur(ia[:,:,:3], (3,3),0) - if q.full(): q.get_nowait() - q.put(ir) - except: pass - -def spawn_npcs(world, num, sp): - bps = [bp for bp in world.get_blueprint_library().filter('vehicle') if int(bp.get_attribute('number_of_wheels'))==4 and not bp.id.endswith(('firetruck','ambulance','police'))] - if not bps: return 0 - cnt, used, max_att = 0, set(), num*3 - for _ in range(max_att): - if cnt>=num or len(used)>=len(sp): break - p = random.choice(sp) - k = (round(p.location.x,2), round(p.location.y,2), round(p.location.z,2)) - if k not in used: - used.add(k) - n = world.try_spawn_actor(random.choice(bps), p) - if n: - try: n.set_autopilot(True, tm_port=8000) - except: n.set_autopilot(True) - cnt +=1 - return cnt - -# 安全生成自车函数 -def safe_spawn_ego(world, spawn_points): - """安全生成自车,避免碰撞""" - ego_bp = random.choice(world.get_blueprint_library().filter('vehicle.tesla.model3')) - ego_bp.set_attribute('color', '255,0,0') - # 遍历生成点,直到找到无碰撞的位置 - for spawn_point in spawn_points: - ego = world.try_spawn_actor(ego_bp, spawn_point) - if ego is not None: - print(f"✅ 自车生成成功,位置:{spawn_point.location}") - return ego - # 若所有生成点都碰撞,随机偏移位置重试 - print("⚠️ 所有默认生成点有碰撞,尝试偏移位置...") - for spawn_point in spawn_points: - # 随机偏移x/y坐标(±2米) - spawn_point.location.x += random.uniform(-2, 2) - spawn_point.location.y += random.uniform(-2, 2) - ego = world.try_spawn_actor(ego_bp, spawn_point) - if ego is not None: - print(f"✅ 自车生成成功(偏移位置):{spawn_point.location}") - return ego - print("❌ 无法生成自车,所有位置都有碰撞") - return None - -# ======================== 主函数 ======================== -def main(): - parser = argparse.ArgumentParser() - parser.add_argument("--config", help="配置文件路径") - parser.add_argument("--host", help="CARLA主机") - parser.add_argument("--port", type=int, help="CARLA端口") - parser.add_argument("--conf-thres", type=float, help="检测置信度") - parser.add_argument("--weather", help="初始天气") - args = parser.parse_args() + def draw_color_legend(self, image): + """ + 绘制颜色说明图例(英文) + + Args: + image: 原始图像 + + Returns: + np.ndarray: 添加了图例的图像 + """ + h, w = image.shape[:2] + + # 图例背景(右侧半透明) + legend_width = 200 + legend_height = 300 + legend_x = w - legend_width - 20 + legend_y = 100 + + overlay = image.copy() + cv2.rectangle(overlay, + (legend_x, legend_y), + (legend_x + legend_width, legend_y + legend_height), + (40, 40, 40), -1) + image = cv2.addWeighted(overlay, 0.8, image, 0.2, 0) + + # 图例标题(英文) + cv2.putText(image, "Color Legend", (legend_x + 10, legend_y + 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + + # 行为状态颜色说明(英文) + behaviors = [ + ('dangerous', 'Dangerous'), + ('stopped', 'Stopped'), + ('overtaking', 'Overtaking'), + ('lane_changing', 'Lane Change'), + ('accelerating', 'Accelerating'), + ('braking', 'Braking'), + ('normal', 'Normal') + ] + + y_offset = 60 + for behavior_key, behavior_name in behaviors: + # 颜色方块 + color = self.behavior_colors.get(behavior_key, (255, 255, 255)) + cv2.rectangle(image, + (legend_x + 10, legend_y + y_offset), + (legend_x + 30, legend_y + y_offset + 15), + color, -1) + + # 行为名称 + text = behavior_name + cv2.putText(image, text, + (legend_x + 40, legend_y + y_offset + 12), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + y_offset += 25 + + # 车辆类别说明(英文) + cv2.putText(image, "Vehicle Types:", (legend_x + 10, legend_y + y_offset + 20), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) + + classes = [ + ('car', 'Car'), + ('bus', 'Bus'), + ('truck', 'Truck') + ] + + y_offset += 40 + for class_key, class_name in classes: + # 颜色方块 + color = self.class_colors.get(class_key, (255, 255, 255)) + cv2.rectangle(image, + (legend_x + 10, legend_y + y_offset), + (legend_x + 30, legend_y + y_offset + 15), + color, -1) + + # 类别名称 + text = class_name + cv2.putText(image, text, + (legend_x + 40, legend_y + y_offset + 12), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + y_offset += 25 + + return image - cfg = Config.from_yaml(args.config) - if args.host: cfg.host = args.host - if args.port: cfg.port = args.port - if args.conf_thres: cfg.conf_thres = args.conf_thres - if args.weather and args.weather in WEATHER: cfg.default_weather = args.weather - - # 初始化所有可能用到的变量 - ego = None - cam = None - lidar = None - lidar_proc = None - det_thread = None - vis = None - recorder = Recorder(cfg) - - try: - client = carla.Client(cfg.host, cfg.port) - client.set_timeout(20.0) - world = client.get_world() - try: - tm = client.get_trafficmanager(8000) - tm.set_global_distance_to_leading_vehicle(2.0) - tm.set_respawn_dormant_vehicles(True) - tm.set_hybrid_physics_mode(True) - tm.set_hybrid_physics_radius(50.0) - tm.global_percentage_speed_difference(0) - except: pass - settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 - settings.substepping = True - settings.max_substep_delta_time = 0.01 - settings.max_substeps = 10 - world.apply_settings(settings) - - # 设置初始天气 - world.set_weather(WEATHER[cfg.default_weather]) - we = WeatherEnhancer(cfg); we.set_weather(cfg.default_weather); cw = cfg.default_weather - - # 获取生成点并安全生成自车 - spawn_points = world.get_map().get_spawn_points() - if not spawn_points: - print("❌ 无可用生成点") - return + def show(self, image, wait_key=1): + """ + 显示图像 - ego = safe_spawn_ego(world, spawn_points) - if ego is None: - return - ego.set_autopilot(True, tm_port=8000) - - # 生成NPC - npc_count = spawn_npcs(world, cfg.num_npcs, spawn_points) - print(f"✅ 生成NPC车辆:{npc_count} 辆") - - # 初始化相机 - cam_bp = world.get_blueprint_library().find('sensor.camera.rgb') - cam_bp.set_attribute('image_size_x', str(cfg.img_width)) - cam_bp.set_attribute('image_size_y', str(cfg.img_height)) - cam_bp.set_attribute('fov', '90') - cam_bp.set_attribute('sensor_tick', '0.05') - cam_t = carla.Transform(carla.Location(x=2.0, z=1.8)) - cam = world.spawn_actor(cam_bp, cam_t, attach_to=ego) - cam_q = queue.Queue(maxsize=1) - cam.listen(lambda img: cam_cb(img, cam_q)) - print(f"✅ 相机传感器启动成功 (ID: {cam.id})") - - # 初始化LiDAR - if cfg.use_lidar: - lidar_proc = LiDARProc(cfg) - lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast') - lidar_bp.set_attribute('channels', str(cfg.lidar_channels)) - lidar_bp.set_attribute('range', str(cfg.lidar_range)) - lidar_bp.set_attribute('points_per_second', str(cfg.lidar_points_per_second)) - lidar_bp.set_attribute('rotation_frequency', '20') - lidar_bp.set_attribute('sensor_tick', '0.05') - lidar_t = carla.Transform(carla.Location(x=0.0, z=2.5)) - lidar = world.spawn_actor(lidar_bp, lidar_t, attach_to=ego) - lidar.listen(lidar_proc.cb) - print(f"✅ LiDAR传感器启动成功 (ID: {lidar.id})") - - # 初始化YOLO - dev = "cuda" if torch.cuda.is_available() else "cpu" - print(f"✅ 使用设备: {dev}") - model = YOLO(cfg.yolo_model) - if cfg.yolo_quantize and dev=="cuda": - model = model.quantize() - print("✅ YOLO模型已量化") - - # 启动检测线程 - in_q = queue.Queue(maxsize=2); out_q = queue.Queue(maxsize=2) - det_thread = DetThread(model, cfg, we, in_q, out_q, dev) - det_thread.start() - print("✅ 推理线程已启动") + Args: + image: 要显示的图像 + wait_key: 等待时间(毫秒) + """ + if utils.valid_img(image): + cv2.imshow(self.window_name, image) + return cv2.waitKey(wait_key) + return -1 + + def destroy(self): + """销毁窗口""" + cv2.destroyAllWindows() + logger.info("✅ 可视化窗口已关闭") - # 初始化跟踪器和可视化 - tracker = SORT(cfg) - fb = FrameBuf((cfg.img_height, cfg.img_width, 3)) - fps_cnt = FPS(cfg.fps_window_size) - cv2.namedWindow("CARLA Object Tracking", cv2.WINDOW_NORMAL) - cv2.resizeWindow("CARLA Object Tracking", cfg.window_width, cfg.window_height) - # 初始化3D可视化 - if cfg.use_3d_visualization and cfg.use_lidar and lidar_proc is not None: - vis = o3d.visualization.Visualizer() - vis.create_window(window_name="LiDAR Point Cloud", width=cfg.pcd_view_size, height=cfg.pcd_view_size) - print("✅ 3D点云可视化窗口已启动") - else: - vis = None +# ======================== 主程序 ======================== - # 主循环 - print("🚀 开始跟踪(按ESC退出,按W切换天气)") - fr_cnt = 0 - last_display_time = time.time() - while True: - world.tick() +class CarlaTrackingSystem: + """CARLA跟踪系统主类""" + + def __init__(self, config): + self.config = config + self.running = False + + # 核心组件 + self.client = None + self.world = None + self.ego_vehicle = None + self.sensor_manager = None + self.detector = None + self.tracker = None + self.visualizer = None + + # 性能监控 + self.fps_counter = utils.FPSCounter(window_size=15) + self.perf_monitor = utils.PerformanceMonitor() + + # 状态变量 + self.current_weather = config.get('weather', 'clear') + self.frame_count = 0 + self.show_legend = True # 是否显示颜色说明 + + # 检测线程相关 + self.detection_thread = None + self.image_queue = None + self.result_queue = None + + logger.info("✅ 跟踪系统初始化完成(英文版)") + + def initialize(self): + """初始化系统""" + try: + # 1. 连接CARLA + self.client, self.world = setup_carla_client(self.config) + if not self.client or not self.world: + return False - # 帧率控制 - current_time = time.time() - elapsed = current_time - last_display_time - target_interval = 1.0 / cfg.display_fps - if elapsed < target_interval: - time.sleep(target_interval - elapsed) - last_display_time = current_time + # 等待CARLA世界稳定 + logger.info("等待CARLA世界稳定...") + for i in range(10): + self.world.tick() + time.sleep(0.1) - # 获取相机图像 - try: - img = cam_q.get(timeout=0.1) - fb.update(img) - except: - img = fb.get() + # 2. 设置天气 + set_weather(self.world, self.current_weather) - # 提交推理任务 - if not in_q.full(): - in_q.put(img.copy()) + # 3. 清理现有的车辆 + logger.info("清理现有车辆...") + sensors.clear_all_actors(self.world, []) + time.sleep(1.0) - # 获取检测结果 - dets = np.array([]) - try: - _, dets = out_q.get_nowait() - except: - pass + # 4. 创建自车 + self.ego_vehicle = sensors.create_ego_vehicle(self.world, self.config) + if not self.ego_vehicle: + logger.error("❌ 创建自车失败") + return False - # LiDAR检测 - lidar_dets = lidar_proc.detect() if (cfg.use_lidar and lidar_proc) else [] + # 等待自车稳定 + time.sleep(0.5) - # 更新跟踪器 - boxes, ids, cls_ids = tracker.update(dets, (cfg.img_width//2, cfg.img_height//2), lidar_dets) + # 5. 生成NPC车辆 + npc_count = sensors.spawn_npc_vehicles(self.world, self.config) + logger.info(f"✅ 生成 {npc_count} 个NPC车辆") - # 性能计算 - fps = fps_cnt.update() - cpu = psutil.cpu_percent(); mem = psutil.virtual_memory().percent - gpu = torch.cuda.utilization() if (torch.cuda.is_available() and hasattr(torch.cuda,'utilization')) else 0.0 - perf = {'cpu':cpu, 'mem':mem, 'gpu':gpu, 'fps':fps, 'avg_fps':fps} + # 等待NPC车辆生成 + time.sleep(0.5) - # 绘制可视化(使用新的检测框样式) - di = draw(img, boxes, ids, cls_ids, tracker.tracks, fps=fps, det_cnt=len(dets), cfg=cfg, w=cw, perf=perf) - cv2.imshow("CARLA Object Tracking", di) + # 6. 初始化传感器 + self.sensor_manager = sensors.SensorManager(self.world, self.ego_vehicle, self.config) + if not self.sensor_manager.setup(): + logger.error("❌ 传感器初始化失败") + return False - # 3D点云更新 - if cfg.use_3d_visualization and vis and lidar_proc: - pcd = lidar_proc.get_3d() - if pcd: - vis.clear_geometries() - vis.add_geometry(pcd) - vis.poll_events() - vis.update_renderer() + # 7. 初始化检测器 + self.detector = tracker.YOLODetector(self.config) - # 数据记录 - recorder.record(tracker.tracks, dets, fps) - if cfg.save_screenshots and fr_cnt%30==0: - recorder.save_ss(di, cw) + # 8. 初始化跟踪器 + self.tracker = tracker.SORTTracker(self.config) - # 键盘事件处理 - key = cv2.waitKey(1) & 0xFF - if key ==27: - print("🛑 用户按下ESC,退出程序") - break - elif key == ord('w') or key == ord('W'): - wl = list(WEATHER.keys()) - cwi = wl.index(cw) - cw = wl[(cwi+1)%len(wl)] - world.set_weather(WEATHER[cw]) - we.set_weather(cw) - print(f"🌤️ 已切换天气到: {cw} (可选:{wl})") - fr_cnt +=1 - - except KeyboardInterrupt: - print("🛑 用户中断程序") - except Exception as e: - print(f"❌ 运行错误: {str(e)}") - finally: - # 资源清理 - print("🧹 开始清理资源...") + # 9. 初始化可视化器 + self.visualizer = Visualizer(self.config) + + # 10. 设置检测线程 + use_async = self.config.get('use_async_detection', True) + if use_async: + self._setup_detection_thread() + + logger.info("🎉 系统初始化完成,准备开始跟踪") + return True + + except Exception as e: + logger.error(f"❌ 系统初始化失败: {e}") + import traceback + traceback.print_exc() + return False + + def _setup_detection_thread(self): + """设置检测线程""" + try: + import queue + self.image_queue = queue.Queue(maxsize=2) + self.result_queue = queue.Queue(maxsize=2) + + self.detection_thread = tracker.DetectionThread( + detector=self.detector, + input_queue=self.image_queue, + output_queue=self.result_queue, + maxsize=2 + ) + self.detection_thread.start() + logger.info("✅ 检测线程已启动") + except Exception as e: + logger.warning(f"检测线程设置失败,使用同步模式: {e}") + self.detection_thread = None + + def run(self): + """运行主循环""" + import time + import queue - # 停止检测线程 - if det_thread: - det_thread.stop() - det_thread.join(timeout=2.0) + if not self.initialize(): + logger.error("❌ 系统初始化失败,无法运行") + return - # 关闭3D可视化窗口 - if vis is not None: - try: - vis.destroy_window() - except: - pass + self.running = True + logger.info("🚀 开始跟踪...") - # 关闭OpenCV窗口 - cv2.destroyAllWindows() + try: + while self.running: + # 开始帧计时 + self.perf_monitor.start_frame() + + # 1. 更新CARLA世界 + self.world.tick() + + # 2. 获取传感器数据 + sensor_data = self.sensor_manager.get_sensor_data() + image = sensor_data.get('image') + + if not utils.valid_img(image): + logger.warning("获取到无效图像,跳过本帧") + time.sleep(0.1) + continue + + # 3. 执行检测(同步或异步) + detections = [] + detection_start = time.time() + + if self.detection_thread and self.detection_thread.is_alive(): + # 异步检测 + if not self.image_queue.full(): + self.image_queue.put(image.copy()) + + try: + processed_image, detections = self.result_queue.get(timeout=0.05) + if processed_image is not None: + image = processed_image + except queue.Empty: + # 队列为空,使用上一次的检测结果 + pass + else: + # 同步检测 + detections = self.detector.detect(image) + + detection_time = time.time() - detection_start + self.perf_monitor.record_detection_time(detection_time) + + # 4. 更新跟踪器 + ego_center = (self.config['img_width'] // 2, self.config['img_height'] // 2) + + # 获取LiDAR检测结果(如果可用) + lidar_detections = sensor_data.get('lidar_objects', []) + + tracking_start = time.time() + boxes, ids, classes = self.tracker.update( + detections=detections, + ego_center=ego_center, + lidar_detections=lidar_detections if lidar_detections else None + ) + tracking_time = time.time() - tracking_start + self.perf_monitor.record_tracking_time(tracking_time) + + # 5. 获取跟踪详细信息 + tracks_info = self.tracker.get_tracks_info() + + # 6. 更新FPS + fps = self.fps_counter.update() + + # 7. 可视化 + result_image = self.visualizer.draw_detections( + image=image, + boxes=boxes, + ids=ids, + classes=classes, + tracks_info=tracks_info + ) + + # 添加颜色说明图例(如果启用) + if self.show_legend: + result_image = self.visualizer.draw_color_legend(result_image) + + # 在图像上显示FPS + if utils.valid_img(result_image): + fps_text = f"FPS: {fps:.1f}" + cv2.putText(result_image, fps_text, (self.config['img_width'] - 100, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) + + # 8. 显示结果 + key = self.visualizer.show(result_image, wait_key=1) + + # 9. 处理键盘输入 + self._handle_keyboard_input(key) + + # 10. 帧率控制 + self._control_frame_rate(fps) + + # 11. 更新状态 + self.frame_count += 1 + self.perf_monitor.end_frame() + + # 12. 定期打印状态 + if self.frame_count % 100 == 0: + self._print_status() + + except KeyboardInterrupt: + logger.info("🛑 用户中断程序") + except Exception as e: + logger.error(f"❌ 运行错误: {e}") + import traceback + traceback.print_exc() + finally: + self.cleanup() + + def _handle_keyboard_input(self, key): + """处理键盘输入""" + # ESC键退出 + if key == 27: # ESC + logger.info("🛑 ESC键按下,退出程序") + self.running = False - # 关闭数据记录 - recorder.close() + # W键切换天气 + elif key == ord('w') or key == ord('W'): + weather_list = ['clear', 'cloudy', 'rain', 'fog', 'night'] + current_idx = weather_list.index(self.current_weather) if self.current_weather in weather_list else 0 + next_idx = (current_idx + 1) % len(weather_list) + self.current_weather = weather_list[next_idx] + set_weather(self.world, self.current_weather) + logger.info(f"🌤️ 天气切换到: {self.current_weather}") - # 销毁LiDAR - if lidar and lidar.is_alive: - try: - lidar.stop() - lidar.destroy() - print("✅ LiDAR已销毁") - except Exception as e: - print(f"⚠️ 销毁LiDAR失败: {e}") - - # 销毁相机 - if cam and cam.is_alive: - try: - cam.stop() - cam.destroy() - print("✅ 相机已销毁") - except Exception as e: - print(f"⚠️ 销毁相机失败: {e}") + # S键保存截图 + elif key == ord('s') or key == ord('S'): + self._save_screenshot() - # 销毁自车 - if ego and ego.is_alive: - try: - ego.destroy() - print("✅ 自车已销毁") - except Exception as e: - print(f"⚠️ 销毁自车失败: {e}") + # P键暂停/继续 + elif key == ord('p') or key == ord('P'): + logger.info("⏸️ 程序暂停,按任意键继续...") + cv2.waitKey(0) + logger.info("▶️ 程序继续") + + # M键切换颜色说明显示 + elif key == ord('m') or key == ord('M'): + self.show_legend = not self.show_legend + status = "显示" if self.show_legend else "隐藏" + logger.info(f"🎨 颜色说明图例: {status}") + + def _control_frame_rate(self, current_fps): + """控制帧率""" + import time + target_fps = self.config.get('display_fps', 30) + if target_fps <= 0: + return - # 清理所有NPC和传感器 - clear_actors(world) + target_interval = 1.0 / target_fps + + # 如果帧率过高,适当休眠 + if current_fps > target_fps * 1.2: # 允许20%的波动 + sleep_time = max(0, target_interval - (1.0 / current_fps)) + time.sleep(sleep_time) + + def _save_screenshot(self): + """保存截图""" + try: + import time + timestamp = time.strftime("%Y%m%d_%H%M%S") + filename = f"screenshot_{timestamp}_{self.frame_count:06d}.png" + + # 获取当前显示的图像 + screenshot = self.sensor_manager.get_camera_image() + if utils.valid_img(screenshot): + utils.save_image(screenshot, filename) + logger.info(f"📸 截图已保存: {filename}") + except Exception as e: + logger.warning(f"保存截图失败: {e}") + + def _print_status(self): + """打印系统状态""" + stats = self.perf_monitor.get_stats() + tracks_info = self.tracker.get_tracks_info() + + # 统计行为类型 + behaviors = { + 'stopped': 0, + 'overtaking': 0, + 'lane_changing': 0, + 'turning': 0, + 'accelerating': 0, + 'braking': 0, + 'dangerous': 0, + 'normal': 0 + } + + for track in tracks_info: + if track.get('is_dangerous', False): + behaviors['dangerous'] += 1 + elif track.get('is_stopped', False): + behaviors['stopped'] += 1 + elif track.get('is_overtaking', False): + behaviors['overtaking'] += 1 + elif track.get('is_lane_changing', False): + behaviors['lane_changing'] += 1 + elif track.get('is_turning', False): + behaviors['turning'] += 1 + elif track.get('is_accelerating', False): + behaviors['accelerating'] += 1 + elif track.get('is_braking', False): + behaviors['braking'] += 1 + else: + behaviors['normal'] += 1 + + logger.info(f"📊 状态: 帧数={self.frame_count}, " + f"FPS={stats['avg_fps']:.1f}, " + f"目标数={len(tracks_info)}, " + f"危险={behaviors['dangerous']}, " + f"停车={behaviors['stopped']}, " + f"超车={behaviors['overtaking']}") + + def cleanup(self): + """清理资源""" + logger.info("🧹 正在清理资源...") + + # 停止检测线程 + if self.detection_thread: + self.detection_thread.stop() + self.detection_thread.join(timeout=2.0) + + # 销毁可视化器 + if self.visualizer: + self.visualizer.destroy() + + # 销毁传感器 + if self.sensor_manager: + self.sensor_manager.destroy() + + # 清理CARLA演员 + if self.world: + # 排除自车ID(如果存在) + exclude_ids = [self.ego_vehicle.id] if self.ego_vehicle and self.ego_vehicle.is_alive else [] + sensors.clear_all_actors(self.world, exclude_ids) # 恢复CARLA设置 - settings = world.get_settings() - settings.synchronous_mode = False - world.apply_settings(settings) - print("✅ 所有资源清理完成") + if self.world: + settings = self.world.get_settings() + settings.synchronous_mode = False + self.world.apply_settings(settings) + + # 打印最终性能统计 + if self.perf_monitor: + self.perf_monitor.print_stats() + + logger.info("✅ 资源清理完成") -if __name__ == "__main__": + +# ======================== 主函数 ======================== + +def main(): + """主函数""" + # 解析命令行参数 + parser = argparse.ArgumentParser(description='CARLA多目标跟踪系统') + parser.add_argument('--config', type=str, default='config.yaml', + help='配置文件路径 (默认: config.yaml)') + parser.add_argument('--host', type=str, default='localhost', + help='CARLA服务器地址 (默认: localhost)') + parser.add_argument('--port', type=int, default=2000, + help='CARLA服务器端口 (默认: 2000)') + parser.add_argument('--weather', type=str, default='clear', + choices=['clear', 'cloudy', 'rain', 'fog', 'night'], + help='初始天气 (默认: clear)') + parser.add_argument('--model', type=str, default='yolov8n.pt', + help='YOLO模型路径 (默认: yolov8n.pt)') + parser.add_argument('--conf-thres', type=float, default=0.5, + help='检测置信度阈值 (默认: 0.5)') + parser.add_argument('--no-lidar', action='store_true', + help='禁用LiDAR') + + args = parser.parse_args() + # 配置日志 logger.remove() - logger.add(sys.stdout, format="{time:HH:mm:ss} | {level: <8} | {message}", level="INFO") - logger.add(f"track_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.log", rotation="100 MB", retention="7 days", format="{time:YYYY-MM-DD HH:mm:ss} | {level: <8} | {message}", level="DEBUG") + logger.add(sys.stdout, + format="{time:HH:mm:ss} | {level: <8} | {message}", + level="INFO") + + # 记录开始时间 + start_time = time.time() + logger.info("=" * 50) + logger.info("🚗 CARLA多目标跟踪系统启动(英文版)") + logger.info("=" * 50) + + try: + # 1. 加载配置 + config = load_config(args.config) + + # 2. 用命令行参数覆盖配置 + if args.host: + config['host'] = args.host + if args.port: + config['port'] = args.port + if args.weather: + config['weather'] = args.weather + if args.model: + config['yolo_model'] = args.model + if args.conf_thres: + config['conf_thres'] = args.conf_thres + if args.no_lidar: + config['use_lidar'] = False + + # 3. 创建并运行跟踪系统 + system = CarlaTrackingSystem(config) + system.run() + + except Exception as e: + logger.error(f"❌ 程序运行异常: {e}") + import traceback + traceback.print_exc() + + finally: + # 计算运行时间 + run_time = time.time() - start_time + logger.info("=" * 50) + logger.info(f"⏱️ 程序运行时间: {run_time:.1f}秒") + logger.info("👋 程序结束") + logger.info("=" * 50) + + +if __name__ == "__main__": + # 检查必要的导入 + try: + import torch + except ImportError: + print("❌ 未找到PyTorch,请安装: pip install torch") + sys.exit(1) + + try: + import carla + except ImportError: + print("❌ 未找到CARLA Python API") + print("请从CARLA安装目录复制PythonAPI/carla到项目目录") + sys.exit(1) - # 启动主程序 + # 运行主程序 main() \ No newline at end of file diff --git a/src/tracking_car/sensors.py b/src/tracking_car/sensors.py new file mode 100644 index 0000000000..27e4d0ee42 --- /dev/null +++ b/src/tracking_car/sensors.py @@ -0,0 +1,761 @@ +""" +sensors.py - CARLA传感器管理 +包含:相机、LiDAR传感器封装和管理 +""" + +import carla +import cv2 +import numpy as np +import queue +import threading +import sys +import time + +# 配置日志 +try: + from loguru import logger +except ImportError: + # 使用标准logging作为回退 + import logging + logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s') + logger = logging.getLogger(__name__) + +import open3d as o3d +from sklearn.cluster import DBSCAN + + +class CameraManager: + """相机管理器""" + + def __init__(self, world, ego_vehicle, config): + """ + 初始化相机 + + Args: + world: CARLA世界对象 + ego_vehicle: 自车对象 + config: 配置字典 + """ + self.world = world + self.ego_vehicle = ego_vehicle + self.config = config + self.camera = None + self.image_queue = queue.Queue(maxsize=2) + self.current_image = None + self.frame_count = 0 + + def setup(self): + """设置相机""" + try: + # 获取相机蓝图 + camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') + + # 设置相机属性 + camera_bp.set_attribute('image_size_x', str(self.config.get('img_width', 640))) + camera_bp.set_attribute('image_size_y', str(self.config.get('img_height', 480))) + camera_bp.set_attribute('fov', str(self.config.get('fov', 90))) + camera_bp.set_attribute('sensor_tick', str(self.config.get('sensor_tick', 0.05))) + + # 设置相机位置(车顶前方) + camera_transform = carla.Transform( + carla.Location(x=2.0, z=1.8), + carla.Rotation(pitch=-10) # 略微向下倾斜 + ) + + # 生成相机 + self.camera = self.world.spawn_actor( + camera_bp, + camera_transform, + attach_to=self.ego_vehicle + ) + + # 绑定回调函数 + self.camera.listen(self._camera_callback) + + logger.info(f"✅ 相机初始化成功 (ID: {self.camera.id})") + return True + + except Exception as e: + logger.error(f"❌ 相机初始化失败: {e}") + return False + + def _camera_callback(self, image): + """相机数据回调函数""" + try: + # 将原始数据转换为numpy数组 + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape((image.height, image.width, 4)) + + # 提取RGB通道(去掉alpha通道) + rgb_array = array[:, :, :3] + + # 轻微高斯模糊减少噪声 + rgb_array = cv2.GaussianBlur(rgb_array, (3, 3), 0) + + # 更新当前图像 + self.current_image = rgb_array + + # 放入队列(如果队列已满,丢弃最旧的数据) + if self.image_queue.full(): + try: + self.image_queue.get_nowait() + except queue.Empty: + pass + + self.image_queue.put(rgb_array.copy()) + self.frame_count += 1 + + except Exception as e: + logger.warning(f"相机回调错误: {e}") + + def get_image(self, timeout=0.1): + """ + 获取最新图像 + + Args: + timeout: 超时时间(秒) + + Returns: + np.ndarray or None: 图像数据 + """ + try: + # 首先尝试从队列获取最新图像 + image = self.image_queue.get(timeout=timeout) + # 清空队列中的旧图像 + while not self.image_queue.empty(): + try: + self.image_queue.get_nowait() + except queue.Empty: + break + return image + except queue.Empty: + # 如果队列为空,返回当前图像 + return self.current_image + + def get_current_image(self): + """获取当前图像(不阻塞)""" + return self.current_image + + def destroy(self): + """销毁相机""" + if self.camera and self.camera.is_alive: + try: + self.camera.stop() + self.camera.destroy() + logger.info("✅ 相机已销毁") + except Exception as e: + logger.warning(f"销毁相机失败: {e}") + self.camera = None + + +class LiDARManager: + """LiDAR管理器""" + + def __init__(self, world, ego_vehicle, config): + """ + 初始化LiDAR + + Args: + world: CARLA世界对象 + ego_vehicle: 自车对象 + config: 配置字典 + """ + self.world = world + self.ego_vehicle = ego_vehicle + self.config = config + self.lidar = None + self.pointcloud_queue = queue.Queue(maxsize=2) + self.current_pointcloud = None + self.current_transform = None + + def setup(self): + """设置LiDAR""" + try: + if not self.config.get('use_lidar', True): + logger.info("LiDAR被禁用") + return True + + # 获取LiDAR蓝图 + lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast') + + # 设置LiDAR属性 + lidar_bp.set_attribute('channels', str(self.config.get('lidar_channels', 32))) + lidar_bp.set_attribute('range', str(self.config.get('lidar_range', 100.0))) + lidar_bp.set_attribute('points_per_second', + str(self.config.get('lidar_points_per_second', 500000))) + lidar_bp.set_attribute('rotation_frequency', str(self.config.get('rotation_frequency', 20))) + lidar_bp.set_attribute('sensor_tick', str(self.config.get('sensor_tick', 0.05))) + + # 设置LiDAR位置(车顶中央) + lidar_transform = carla.Transform( + carla.Location(x=0.0, z=2.5), + carla.Rotation() + ) + + # 生成LiDAR + self.lidar = self.world.spawn_actor( + lidar_bp, + lidar_transform, + attach_to=self.ego_vehicle + ) + + # 绑定回调函数 + self.lidar.listen(self._lidar_callback) + + logger.info(f"✅ LiDAR初始化成功 (ID: {self.lidar.id})") + return True + + except Exception as e: + logger.error(f"❌ LiDAR初始化失败: {e}") + return False + + def _lidar_callback(self, pointcloud): + """LiDAR数据回调函数""" + try: + # 将原始数据转换为numpy数组 + points = np.frombuffer(pointcloud.raw_data, dtype=np.float32) + points = points.reshape(-1, 4)[:, :3] # 只取xyz,忽略反射强度 + + # 过滤地面点(简单的高度过滤) + ground_mask = points[:, 2] < -1.0 + filtered_points = points[~ground_mask] + + # 更新当前点云 + self.current_pointcloud = filtered_points + self.current_transform = pointcloud.transform + + # 放入队列(如果队列已满,丢弃最旧的数据) + if self.pointcloud_queue.full(): + try: + self.pointcloud_queue.get_nowait() + except queue.Empty: + pass + + self.pointcloud_queue.put((filtered_points.copy(), pointcloud.transform)) + + except Exception as e: + logger.warning(f"LiDAR回调错误: {e}") + + def get_pointcloud(self, timeout=0.1): + """ + 获取最新点云数据 + + Args: + timeout: 超时时间(秒) + + Returns: + tuple: (points, transform) 或 (None, None) + """ + try: + points, transform = self.pointcloud_queue.get(timeout=timeout) + # 清空队列中的旧数据 + while not self.pointcloud_queue.empty(): + try: + self.pointcloud_queue.get_nowait() + except queue.Empty: + break + return points, transform + except queue.Empty: + # 如果队列为空,返回当前点云 + return self.current_pointcloud, self.current_transform + + def detect_objects(self, min_points=30): + """ + 从点云中检测物体 + + Args: + min_points: 最小点数阈值 + + Returns: + list: 检测到的物体列表 + """ + if self.current_pointcloud is None or len(self.current_pointcloud) < min_points: + return [] + + try: + # 使用DBSCAN聚类 + clustering = DBSCAN(eps=0.8, min_samples=30).fit(self.current_pointcloud[:, :2]) + + objects = [] + for label in set(clustering.labels_): + if label == -1: # 忽略噪声点 + continue + + # 获取该聚类的点 + cluster_points = self.current_pointcloud[clustering.labels_ == label] + + if len(cluster_points) < min_points: + continue + + # 计算3D边界框 + min_coords = cluster_points.min(axis=0) + max_coords = cluster_points.max(axis=0) + center = (min_coords + max_coords) / 2 + size = max_coords - min_coords + + # 估计物体类型(基于尺寸) + obj_type = self._estimate_object_type(size) + + objects.append({ + 'bbox_3d': [*min_coords, *max_coords], # [x_min, y_min, z_min, x_max, y_max, z_max] + 'center': center.tolist(), + 'size': size.tolist(), + 'num_points': len(cluster_points), + 'type': obj_type, + 'label': label + }) + + return objects + + except Exception as e: + logger.warning(f"LiDAR物体检测失败: {e}") + return [] + + def _estimate_object_type(self, size): + """根据尺寸估计物体类型""" + length, width, height = size + + # 简单的大小分类 + if height > 2.5: + return "truck" + elif width > 2.0: + return "bus" + elif length > 4.0: + return "car" + else: + return "unknown" + + def get_open3d_pointcloud(self): + """ + 获取Open3D格式的点云 + + Returns: + o3d.geometry.PointCloud or None: Open3D点云对象 + """ + if self.current_pointcloud is None or len(self.current_pointcloud) == 0: + return None + + try: + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(self.current_pointcloud) + + # 根据高度着色(低处蓝色,高处红色) + z_min = self.current_pointcloud[:, 2].min() + z_max = self.current_pointcloud[:, 2].max() + z_range = max(z_max - z_min, 1e-6) + + colors = np.zeros((len(self.current_pointcloud), 3)) + normalized_z = (self.current_pointcloud[:, 2] - z_min) / z_range + colors[:, 0] = normalized_z # 红色通道(高处) + colors[:, 2] = 1 - normalized_z # 蓝色通道(低处) + + pcd.colors = o3d.utility.Vector3dVector(colors) + + return pcd + + except Exception as e: + logger.warning(f"创建Open3D点云失败: {e}") + return None + + def destroy(self): + """销毁LiDAR""" + if self.lidar and self.lidar.is_alive: + try: + self.lidar.stop() + self.lidar.destroy() + logger.info("✅ LiDAR已销毁") + except Exception as e: + logger.warning(f"销毁LiDAR失败: {e}") + self.lidar = None + + +class SensorManager: + """传感器管理器(统一管理所有传感器)""" + + def __init__(self, world, ego_vehicle, config): + """ + 初始化传感器管理器 + + Args: + world: CARLA世界对象 + ego_vehicle: 自车对象 + config: 配置字典 + """ + self.world = world + self.ego_vehicle = ego_vehicle + self.config = config + + self.camera_manager = None + self.lidar_manager = None + self.is_setup = False + + def setup(self): + """设置所有传感器""" + logger.info("正在初始化传感器...") + + # 初始化相机 + self.camera_manager = CameraManager(self.world, self.ego_vehicle, self.config) + camera_success = self.camera_manager.setup() + + # 初始化LiDAR + lidar_success = True + if self.config.get('use_lidar', True): + self.lidar_manager = LiDARManager(self.world, self.ego_vehicle, self.config) + lidar_success = self.lidar_manager.setup() + else: + logger.info("LiDAR功能已禁用") + + self.is_setup = camera_success and lidar_success + + if self.is_setup: + logger.info("✅ 所有传感器初始化完成") + else: + logger.warning("⚠️ 传感器初始化不完全") + + return self.is_setup + + def get_sensor_data(self, timeout=0.05): + """ + 获取所有传感器数据 + + Args: + timeout: 超时时间(秒) + + Returns: + dict: 传感器数据字典 + """ + data = { + 'image': None, + 'pointcloud': None, + 'lidar_transform': None, + 'lidar_objects': [], + 'timestamp': time.time() + } + + # 获取相机图像 + if self.camera_manager: + data['image'] = self.camera_manager.get_image(timeout=timeout) + + # 获取LiDAR数据 + if self.lidar_manager: + points, transform = self.lidar_manager.get_pointcloud(timeout=timeout) + data['pointcloud'] = points + data['lidar_transform'] = transform + + # 检测物体 + if points is not None: + data['lidar_objects'] = self.lidar_manager.detect_objects() + + return data + + def get_camera_image(self): + """获取相机图像""" + if self.camera_manager: + return self.camera_manager.get_current_image() + return None + + def get_lidar_pointcloud(self): + """获取LiDAR点云""" + if self.lidar_manager: + return self.lidar_manager.current_pointcloud + return None + + def get_lidar_objects(self): + """获取LiDAR检测到的物体""" + if self.lidar_manager: + return self.lidar_manager.detect_objects() + return [] + + def get_open3d_pointcloud(self): + """获取Open3D格式的点云""" + if self.lidar_manager: + return self.lidar_manager.get_open3d_pointcloud() + return None + + def destroy(self): + """销毁所有传感器""" + logger.info("正在销毁传感器...") + + if self.camera_manager: + self.camera_manager.destroy() + + if self.lidar_manager: + self.lidar_manager.destroy() + + logger.info("✅ 所有传感器已销毁") + + +# ======================== 工具函数 ======================== + +def create_ego_vehicle(world, config, spawn_points=None): + """ + 创建自车 + + Args: + world: CARLA世界对象 + config: 配置字典 + spawn_points: 可选的自定义生成点列表 + + Returns: + carla.Vehicle or None: 自车对象 + """ + try: + # 获取生成点 + if spawn_points is None: + spawn_points = world.get_map().get_spawn_points() + + if not spawn_points: + logger.error("❌ 无可用生成点") + return None + + logger.info(f"找到 {len(spawn_points)} 个生成点") + + # 选择车辆蓝图 + vehicle_bp = None + vehicle_filter = config.get('ego_vehicle_filter', 'vehicle.tesla.model3') + + # 尝试首选车辆 + blueprint_library = world.get_blueprint_library() + for bp in blueprint_library.filter(vehicle_filter): + if int(bp.get_attribute('number_of_wheels')) == 4: + vehicle_bp = bp + logger.info(f"找到车辆蓝图: {bp.id}") + break + + # 如果没找到,选择任意四轮车辆 + if vehicle_bp is None: + logger.info("首选车辆未找到,尝试其他四轮车辆...") + for bp in blueprint_library.filter('vehicle.*'): + if int(bp.get_attribute('number_of_wheels')) == 4: + vehicle_bp = bp + logger.info(f"使用备用车辆蓝图: {bp.id}") + break + + if vehicle_bp is None: + logger.error("❌ 找不到合适的车辆蓝图") + return None + + # 设置车辆颜色 + color = config.get('ego_vehicle_color', '255,0,0') + vehicle_bp.set_attribute('color', color) + + # 尝试生成车辆 - 改进的碰撞避免策略 + max_attempts = config.get('spawn_max_attempts', 20) + + for attempt in range(max_attempts): + # 随机选择生成点 + if attempt < len(spawn_points): + spawn_point = spawn_points[attempt] + else: + # 随机选择一个生成点 + import random + spawn_point = random.choice(spawn_points) + + # 随机偏移位置以避免碰撞 + spawn_point.location.x += random.uniform(-3, 3) + spawn_point.location.y += random.uniform(-3, 3) + + logger.info(f"尝试生成自车 (尝试 {attempt + 1}/{max_attempts}) " + f"位置: x={spawn_point.location.x:.1f}, y={spawn_point.location.y:.1f}") + + # 设置生成点的高度为地面以上0.5米 + spawn_point.location.z += 0.5 + + ego_vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) + + if ego_vehicle is not None: + logger.info(f"✅ 自车生成成功 (尝试 {attempt + 1}/{max_attempts})") + logger.info(f" 位置: ({spawn_point.location.x:.1f}, {spawn_point.location.y:.1f}, {spawn_point.location.z:.1f})") + + # 等待一小段时间让车辆稳定 + world.tick() + + # 设置自动驾驶 + try: + ego_vehicle.set_autopilot(True, 8000) + logger.info("✅ 自车自动驾驶已启用") + except Exception as e: + logger.warning(f"设置自动驾驶失败: {e}") + try: + ego_vehicle.set_autopilot(True) + logger.info("✅ 自车自动驾驶已启用(备用方法)") + except: + logger.warning("无法设置自动驾驶,车辆将保持静止") + + return ego_vehicle + else: + logger.debug(f"生成失败,尝试下一个位置...") + + logger.error(f"❌ 经过 {max_attempts} 次尝试后仍无法生成自车") + logger.info("建议:") + logger.info("1. 重新启动CARLA服务器") + logger.info("2. 在CARLA中手动清理场景中的车辆") + logger.info("3. 尝试不同的生成点") + + return None + + except Exception as e: + logger.error(f"❌ 创建自车失败: {e}") + import traceback + traceback.print_exc() + return None + + +def spawn_npc_vehicles(world, config, count=None): + """ + 生成NPC车辆 + + Args: + world: CARLA世界对象 + config: 配置字典 + count: NPC数量(默认使用配置中的值) + + Returns: + int: 成功生成的NPC数量 + """ + try: + if count is None: + count = config.get('num_npcs', 20) + + spawn_points = world.get_map().get_spawn_points() + if not spawn_points: + logger.warning("无可用生成点,无法生成NPC") + return 0 + + # 过滤合适的车辆蓝图(四轮车辆,排除特殊车辆) + vehicle_bps = [] + for bp in world.get_blueprint_library().filter('vehicle.*'): + if int(bp.get_attribute('number_of_wheels')) == 4: + # 排除特殊车辆 + if not bp.id.endswith(('firetruck', 'ambulance', 'police', 'charger')): + vehicle_bps.append(bp) + + if not vehicle_bps: + logger.warning("找不到合适的NPC车辆蓝图") + return 0 + + spawned_count = 0 + used_spawn_points = set() + + for i in range(min(count * 3, len(spawn_points))): # 最多尝试3倍数量 + if spawned_count >= count: + break + + spawn_point = spawn_points[i] + + # 检查是否已使用该位置 + position_key = (round(spawn_point.location.x, 1), + round(spawn_point.location.y, 1)) + + if position_key in used_spawn_points: + continue + + # 随机选择车辆蓝图 + import random + vehicle_bp = random.choice(vehicle_bps) + + # 尝试生成 + npc = world.try_spawn_actor(vehicle_bp, spawn_point) + + if npc is not None: + used_spawn_points.add(position_key) + spawned_count += 1 + + # 设置自动驾驶 + try: + npc.set_autopilot(True, 8000) + except: + try: + npc.set_autopilot(True) + except: + pass + + logger.info(f"✅ 成功生成 {spawned_count}/{count} 个NPC车辆") + return spawned_count + + except Exception as e: + logger.error(f"生成NPC车辆失败: {e}") + return 0 + + +def clear_all_actors(world, exclude_ids=None): + """ + 清理所有演员(车辆和传感器) + + Args: + world: CARLA世界对象 + exclude_ids: 要排除的演员ID列表 + """ + try: + exclude_ids = set(exclude_ids) if exclude_ids else set() + + actors = world.get_actors() + + # 按类型分组清理 + vehicle_actors = [] + sensor_actors = [] + + for actor in actors: + if actor.id in exclude_ids: + continue + + if actor.type_id.startswith('vehicle.'): + vehicle_actors.append(actor) + elif actor.type_id.startswith('sensor.'): + sensor_actors.append(actor) + + # 先清理传感器 + logger.info(f"清理 {len(sensor_actors)} 个传感器...") + for sensor in sensor_actors: + try: + if sensor.is_alive: + sensor.destroy() + except: + pass + + # 再清理车辆(分批进行) + logger.info(f"清理 {len(vehicle_actors)} 个车辆...") + batch_size = 10 + for i in range(0, len(vehicle_actors), batch_size): + batch = vehicle_actors[i:i+batch_size] + for vehicle in batch: + try: + if vehicle.is_alive: + vehicle.destroy() + except: + pass + + logger.info("✅ 清理完成") + + except Exception as e: + logger.warning(f"清理演员时出错: {e}") + + +# ======================== 测试函数 ======================== + +def test_sensor_manager(): + """测试传感器管理器""" + print("=" * 50) + print("测试 sensors.py...") + print("=" * 50) + + # 模拟配置 + test_config = { + 'img_width': 640, + 'img_height': 480, + 'fov': 90, + 'sensor_tick': 0.05, + 'use_lidar': True, + 'lidar_channels': 32, + 'lidar_range': 100.0, + 'lidar_points_per_second': 500000, + } + + print("✅ sensors.py 结构测试通过") + print("注:完整测试需要CARLA环境") + + return True + + +if __name__ == "__main__": + test_sensor_manager() \ No newline at end of file diff --git a/src/tracking_car/tracker.py b/src/tracking_car/tracker.py new file mode 100644 index 0000000000..a15d34dd96 --- /dev/null +++ b/src/tracking_car/tracker.py @@ -0,0 +1,966 @@ +""" +tracker.py - 目标检测与跟踪核心算法 +包含:YOLO检测器、卡尔曼滤波、SORT跟踪器、行为分析 +""" + +import numpy as np +import cv2 +import torch +import queue +import threading +import time +import sys +import os +import queue + +# 配置日志 +try: + from loguru import logger +except ImportError: + # 使用标准logging作为回退 + import logging + logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s') + logger = logging.getLogger(__name__) + +from ultralytics import YOLO +from scipy.optimize import linear_sum_assignment +from dataclasses import dataclass +from typing import List, Tuple, Optional, Dict, Any + +# 导入utils模块中的工具函数 +try: + from utils import iou, iou_numpy, clip_box, valid_img, bbox_center +except ImportError: + # 如果在同一目录下,可以直接导入 + sys.path.append(os.path.dirname(os.path.abspath(__file__))) + from utils import iou, iou_numpy, clip_box, valid_img, bbox_center + +# ======================== 数据结构 ======================== + +@dataclass +class Detection: + """检测结果数据结构""" + bbox: np.ndarray # [x1, y1, x2, y2] + confidence: float + class_id: int + class_name: str = "Unknown" + + def __post_init__(self): + self.bbox = np.array(self.bbox, dtype=np.float32) + self.area = (self.bbox[2] - self.bbox[0]) * (self.bbox[3] - self.bbox[1]) + + +@dataclass +class TrackState: + """跟踪状态枚举""" + NEW = "new" + TRACKED = "tracked" + LOST = "lost" + REMOVED = "removed" + + +# ======================== 卡尔曼滤波器 ======================== + +class KalmanFilter: + """ + 卡尔曼滤波器 - 用于目标状态估计 + 8维状态: [x1, y1, x2, y2, vx1, vy1, vx2, vy2] + 4维观测: [x1, y1, x2, y2] + """ + + def __init__(self, dt=0.05, max_speed=50.0): + """ + 初始化卡尔曼滤波器 + + Args: + dt: 时间间隔(秒) + max_speed: 最大速度(像素/秒) + """ + self.dt = dt + self.max_speed = max_speed + + # 状态向量维度: 8 + # 观测向量维度: 4 + self.state_dim = 8 + self.measure_dim = 4 + + # 状态转移矩阵 F + self.F = np.eye(self.state_dim, dtype=np.float32) + for i in range(4): + self.F[i, i + 4] = dt + + # 观测矩阵 H + self.H = np.zeros((self.measure_dim, self.state_dim), dtype=np.float32) + for i in range(self.measure_dim): + self.H[i, i] = 1.0 + + # 过程噪声协方差矩阵 Q + self.Q = np.eye(self.state_dim, dtype=np.float32) + for i in range(4): + self.Q[i, i] = 1.0 + for i in range(4, 8): + self.Q[i, i] = 5.0 + + # 观测噪声协方差矩阵 R + self.R = np.eye(self.measure_dim, dtype=np.float32) * 5.0 + + # 状态协方差矩阵 P + self.P = np.eye(self.state_dim, dtype=np.float32) * 50.0 + + # 状态向量 x + self.x = np.zeros(self.state_dim, dtype=np.float32) + + # 首次更新标志 + self.first_update = True + + def init(self, bbox): + """ + 初始化滤波器状态 + + Args: + bbox: 初始边界框 [x1, y1, x2, y2] + """ + self.x[:4] = bbox + self.first_update = True + + def predict(self): + """ + 状态预测 + + Returns: + np.ndarray: 预测的边界框 + """ + # 状态预测 + self.x = self.F @ self.x + + # 协方差预测 + self.P = self.F @ self.P @ self.F.T + self.Q + + # 返回预测的边界框 + return self.x[:4].copy() + + def update(self, bbox): + """ + 状态更新 + + Args: + bbox: 观测到的边界框 [x1, y1, x2, y2] + + Returns: + np.ndarray: 更新后的边界框 + """ + z = np.array(bbox, dtype=np.float32) + + # 计算卡尔曼增益 + S = self.H @ self.P @ self.H.T + self.R + try: + K = self.P @ self.H.T @ np.linalg.inv(S) + except np.linalg.LinAlgError: + # 如果矩阵不可逆,使用伪逆 + K = self.P @ self.H.T @ np.linalg.pinv(S) + + # 计算新息 + y = z - self.H @ self.x + + # 状态更新 + self.x = self.x + K @ y + + # 协方差更新 + I = np.eye(self.state_dim, dtype=np.float32) + self.P = (I - K @ self.H) @ self.P + + self.first_update = False + + return self.x[:4].copy() + + def update_noise(self, speed): + """ + 根据速度更新过程噪声 + + Args: + speed: 估计的速度(像素/秒) + """ + # 速度归一化 + speed_factor = min(1.0, speed / self.max_speed) + + # 更新过程噪声协方差 + for i in range(4): + self.Q[i, i] = 1.0 + speed_factor * 4.0 + for i in range(4, 8): + self.Q[i, i] = 5.0 + speed_factor * 20.0 + + +# ======================== 跟踪目标 ======================== + +class TrackedObject: + """ + 单个跟踪目标 + """ + + def __init__(self, track_id: int, bbox: np.ndarray, + img_shape: Tuple[int, int], config: Dict[str, Any]): + """ + 初始化跟踪目标 + + Args: + track_id: 跟踪ID + bbox: 初始边界框 [x1, y1, x2, y2] + img_shape: 图像尺寸 (height, width) + config: 配置字典 + """ + self.track_id = track_id + self.img_shape = img_shape + self.config = config + + # 卡尔曼滤波器 + self.kf = KalmanFilter( + dt=config.get('kf_dt', 0.05), + max_speed=config.get('max_speed', 50.0) + ) + + # 边界框处理 + self.bbox = clip_box(bbox.astype(np.float32), img_shape) + self.kf.init(self.bbox) + + # 跟踪历史 + self.track_history: List[Tuple[float, float]] = [] # [(cx, cy), ...] + self.speed_history: List[float] = [] # 速度历史 + self.acceleration_history: List[float] = [] # 加速度历史 + + # 状态管理 + self.state = TrackState.NEW + self.age = 0 # 存在帧数 + self.time_since_update = 0 # 自上次更新以来的帧数 + self.hits = 1 # 匹配次数 + self.total_frames = 0 # 总跟踪帧数 + + # 检测信息 + self.class_id: Optional[int] = None + self.class_name: str = "Unknown" + self.confidence: float = 0.0 + + # 行为分析 + self.is_stopped = False + self.is_overtaking = False + self.is_lane_changing = False + self.is_braking = False + self.is_accelerating = False + self.is_turning = False + self.is_dangerous = False + + self.stop_frames = 0 + self.overtake_frames = 0 + self.lane_change_frames = 0 + self.brake_frames = 0 + self.turn_frames = 0 + + # 预测轨迹 + self.predicted_trajectory: List[Tuple[float, float]] = [] + + # 初始化历史 + self._update_history() + + def _update_history(self): + """更新跟踪历史""" + cx, cy = bbox_center(self.bbox) + self.track_history.append((cx, cy)) + + # 限制历史长度 + max_len = self.config.get('track_history_len', 20) + if len(self.track_history) > max_len: + self.track_history.pop(0) + + # 限制速度历史 + if len(self.speed_history) > 10: + self.speed_history.pop(0) + + # 限制加速度历史 + if len(self.acceleration_history) > 10: + self.acceleration_history.pop(0) + + def _calculate_speed(self) -> float: + """ + 计算当前速度 + + Returns: + float: 速度(像素/秒) + """ + if len(self.track_history) < 2: + return 0.0 + + # 计算最后两帧的位移 + prev_cx, prev_cy = self.track_history[-2] + curr_cx, curr_cy = self.track_history[-1] + + dx = curr_cx - prev_cx + dy = curr_cy - prev_cy + distance = np.sqrt(dx**2 + dy**2) + + # 计算速度 + speed = distance / self.kf.dt + + # 更新速度历史 + self.speed_history.append(speed) + + # 计算加速度 + if len(self.speed_history) >= 2: + acceleration = (self.speed_history[-1] - self.speed_history[-2]) / self.kf.dt + self.acceleration_history.append(acceleration) + + return speed + + def _calculate_heading(self) -> float: + """ + 计算当前航向角 + + Returns: + float: 航向角(度) + """ + if len(self.track_history) < 3: + return 0.0 + + # 使用最近三帧计算航向 + cx1, cy1 = self.track_history[-3] + cx2, cy2 = self.track_history[-1] + + dx = cx2 - cx1 + dy = cy2 - cy1 + + # 计算角度(弧度转度) + angle = np.degrees(np.arctan2(dy, dx)) + + return angle + + def _analyze_behavior(self, ego_center: Optional[Tuple[float, float]] = None): + """ + 分析目标行为 + + Args: + ego_center: 自车中心点坐标 + """ + # 计算基本状态 + speed = self._calculate_speed() + heading = self._calculate_heading() + + # 1. 停车检测 + stop_speed_thresh = self.config.get('stop_speed_thresh', 1.0) + stop_frames_thresh = self.config.get('stop_frames_thresh', 5) + + if speed < stop_speed_thresh: + self.stop_frames += 1 + self.is_stopped = self.stop_frames >= stop_frames_thresh + else: + self.stop_frames = 0 + self.is_stopped = False + + # 2. 超车检测 + overtake_speed_ratio = self.config.get('overtake_speed_ratio', 1.5) + overtake_dist_thresh = self.config.get('overtake_dist_thresh', 50.0) + + if ego_center and len(self.track_history) >= 2: + curr_cx, curr_cy = self.track_history[-1] + distance = np.sqrt((curr_cx - ego_center[0])**2 + (curr_cy - ego_center[1])**2) + + if distance < overtake_dist_thresh: + ego_speed = getattr(self, 'ego_speed', 0.0) + if speed > ego_speed * overtake_speed_ratio: + self.overtake_frames += 1 + self.is_overtaking = self.overtake_frames >= 3 + else: + self.overtake_frames = 0 + self.is_overtaking = False + else: + self.overtake_frames = 0 + self.is_overtaking = False + + # 3. 变道检测 + lane_change_thresh = self.config.get('lane_change_thresh', 0.5) + + if len(self.track_history) >= 5: + # 计算横向位移 + lateral_displacements = [] + for i in range(1, min(5, len(self.track_history))): + lateral_displacements.append( + abs(self.track_history[-i][0] - self.track_history[-i-1][0]) + ) + + avg_lateral = np.mean(lateral_displacements) if lateral_displacements else 0.0 + + if avg_lateral > lane_change_thresh: + self.lane_change_frames += 1 + self.is_lane_changing = self.lane_change_frames >= 3 + else: + self.lane_change_frames = 0 + self.is_lane_changing = False + + # 4. 刹车/加速检测 + brake_accel_thresh = self.config.get('brake_accel_thresh', 2.0) + + if len(self.acceleration_history) >= 3: + avg_accel = np.mean(self.acceleration_history[-3:]) + + if avg_accel < -brake_accel_thresh: + self.brake_frames += 1 + self.is_braking = self.brake_frames >= 2 + self.is_accelerating = False + elif avg_accel > brake_accel_thresh: + self.is_accelerating = True + self.is_braking = False + self.brake_frames = 0 + else: + self.is_braking = False + self.is_accelerating = False + self.brake_frames = 0 + + # 5. 转弯检测 + turn_angle_thresh = self.config.get('turn_angle_thresh', 15.0) + + if len(self.track_history) >= 3: + # 计算航向变化 + if hasattr(self, '_prev_heading'): + heading_change = abs(heading - self._prev_heading) + if heading_change > turn_angle_thresh: + self.turn_frames += 1 + self.is_turning = self.turn_frames >= 2 + else: + self.turn_frames = 0 + self.is_turning = False + self._prev_heading = heading + + # 6. 危险距离检测 + danger_dist_thresh = self.config.get('danger_dist_thresh', 10.0) + + if ego_center: + curr_cx, curr_cy = self.track_history[-1] + distance = np.sqrt((curr_cx - ego_center[0])**2 + (curr_cy - ego_center[1])**2) + self.is_dangerous = distance < danger_dist_thresh + + # 7. 预测轨迹 + self._predict_trajectory() + + def _predict_trajectory(self): + """预测未来轨迹""" + predict_frames = self.config.get('predict_frames', 10) + self.predicted_trajectory = [] + + if len(self.track_history) < 5: + return + + # 创建临时的卡尔曼滤波器用于预测 + temp_kf = KalmanFilter( + dt=self.kf.dt, + max_speed=self.kf.max_speed + ) + temp_kf.x = self.kf.x.copy() + temp_kf.P = self.kf.P.copy() + + # 预测未来位置 + for _ in range(predict_frames): + predicted_bbox = temp_kf.predict() + predicted_center = bbox_center(predicted_bbox) + self.predicted_trajectory.append(predicted_center) + + def predict(self) -> np.ndarray: + """ + 预测下一帧的位置 + + Returns: + np.ndarray: 预测的边界框 + """ + # 预测速度用于调整噪声 + if len(self.track_history) >= 2: + speed = self._calculate_speed() + self.kf.update_noise(speed) + + # 卡尔曼预测 + self.bbox = self.kf.predict() + self.bbox = clip_box(self.bbox, self.img_shape) + + # 更新状态 + self._update_history() + self.age += 1 + self.time_since_update += 1 + self.total_frames += 1 + + if self.time_since_update > 1: + self.state = TrackState.LOST + + return self.bbox + + def update(self, detection: Detection, ego_center: Optional[Tuple[float, float]] = None): + """ + 用新的检测更新跟踪 + + Args: + detection: 检测结果 + ego_center: 自车中心点坐标 + """ + # 卡尔曼更新 + self.bbox = self.kf.update(detection.bbox) + self.bbox = clip_box(self.bbox, self.img_shape) + + # 更新检测信息 + self.class_id = detection.class_id + self.class_name = detection.class_name + self.confidence = detection.confidence + + # 更新状态 + self._update_history() + self.hits += 1 + self.time_since_update = 0 + self.state = TrackState.TRACKED + + # 行为分析 + self._analyze_behavior(ego_center) + + def get_behavior_string(self) -> str: + """获取行为描述字符串""" + behaviors = [] + if self.is_stopped: + behaviors.append("停车") + if self.is_overtaking: + behaviors.append("超车") + if self.is_lane_changing: + behaviors.append("变道") + if self.is_braking: + behaviors.append("刹车") + if self.is_accelerating: + behaviors.append("加速") + if self.is_turning: + behaviors.append("转弯") + if self.is_dangerous: + behaviors.append("危险") + + return "|".join(behaviors) if behaviors else "正常" + + +# ======================== YOLO检测器 ======================== + +class YOLODetector: + """ + YOLOv8检测器 + """ + + def __init__(self, config: Dict[str, Any]): + """ + 初始化YOLO检测器 + + Args: + config: 配置字典 + """ + self.config = config + + # 模型配置 + model_path = config.get('yolo_model', 'yolov8n.pt') + self.device = config.get('device', 'cuda' if torch.cuda.is_available() else 'cpu') + self.conf_thres = config.get('conf_thres', 0.5) + self.iou_thres = config.get('iou_thres', 0.3) + self.imgsz_max = config.get('yolo_imgsz_max', 320) + self.quantize = config.get('yolo_quantize', False) + + # 类别过滤(只检测车辆) + self.vehicle_classes = {2: "Car", 5: "Bus", 7: "Truck"} + + # 加载模型 + self.model = self._load_model(model_path) + + logger.info(f"✅ YOLO检测器初始化完成 (设备: {self.device}, 模型: {model_path})") + + def _load_model(self, model_path: str): + """加载YOLO模型""" + try: + model = YOLO(model_path) + + if self.quantize and self.device == "cuda": + model = model.quantize() + + # 预热模型 + dummy_input = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8) + _ = model.predict(dummy_input, verbose=False, device=self.device) + + return model + + except Exception as e: + logger.error(f"❌ 加载YOLO模型失败: {e}") + raise + + def detect(self, image: np.ndarray) -> List[Detection]: + """ + 检测图像中的目标 + + Args: + image: 输入图像 + + Returns: + List[Detection]: 检测结果列表 + """ + if not valid_img(image): + return [] + + try: + # 调整图像尺寸 + h, w = image.shape[:2] + resize_ratio = min(self.imgsz_max / w, self.imgsz_max / h) + new_w = int(w * resize_ratio) + new_h = int(h * resize_ratio) + + # 确保尺寸是32的倍数 + new_w = (new_w + 31) // 32 * 32 + new_h = (new_h + 31) // 32 * 32 + + # 执行检测 + results = self.model.predict( + image, + conf=self.conf_thres, + iou=self.iou_thres, + imgsz=(new_h, new_w), + device=self.device, + verbose=False, + agnostic_nms=True + ) + + detections = [] + + for result in results: + if result.boxes is not None and len(result.boxes) > 0: + for box in result.boxes: + # 获取边界框 + xyxy = box.xyxy[0].cpu().numpy() + confidence = float(box.conf[0]) + class_id = int(box.cls[0]) + + # 只处理车辆类别 + if class_id in self.vehicle_classes: + # 确保边界框有效 + if xyxy[2] > xyxy[0] and xyxy[3] > xyxy[1] and confidence > 0: + detection = Detection( + bbox=xyxy, + confidence=confidence, + class_id=class_id, + class_name=self.vehicle_classes[class_id] + ) + detections.append(detection) + + return detections + + except Exception as e: + logger.error(f"❌ YOLO检测失败: {e}") + return [] + + +# ======================== SORT跟踪器 ======================== + +class SORTTracker: + """ + SORT (Simple Online and Realtime Tracking) 跟踪器 + """ + + def __init__(self, config: Dict[str, Any]): + """ + 初始化SORT跟踪器 + + Args: + config: 配置字典 + """ + self.config = config + + # 跟踪参数 + self.max_age = config.get('max_age', 5) + self.min_hits = config.get('min_hits', 3) + self.iou_threshold = config.get('iou_thres', 0.3) + + # 图像尺寸 + self.img_height = config.get('img_height', 480) + self.img_width = config.get('img_width', 640) + self.img_shape = (self.img_height, self.img_width) + + # 跟踪目标管理 + self.tracks: List[TrackedObject] = [] + self.next_track_id = 1 + + # 自车信息 + self.ego_center = (self.img_width // 2, self.img_height // 2) + self.ego_speed = 0.0 + + logger.info("✅ SORT跟踪器初始化完成") + + def update(self, detections: List[Detection], + ego_center: Optional[Tuple[float, float]] = None, + lidar_detections: Optional[List[Dict]] = None) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + """ + 更新跟踪器 + + Args: + detections: 检测结果列表 + ego_center: 自车中心点坐标 + lidar_detections: LiDAR检测结果(可选) + + Returns: + Tuple: (边界框数组, ID数组, 类别数组) + """ + # 更新自车信息 + if ego_center: + self.ego_center = ego_center + + # 如果没有检测结果,只进行预测 + if not detections: + # 预测所有现有目标 + for track in self.tracks: + track.predict() + + # 移除丢失时间过长的目标 + self.tracks = [t for t in self.tracks if t.time_since_update <= self.max_age] + + # 返回空结果 + return np.array([]), np.array([]), np.array([]) + + # 预测现有目标的位置 + for track in self.tracks: + track.predict() + + # 创建匹配成本矩阵 + if self.tracks: + # 计算IoU矩阵 + iou_matrix = np.zeros((len(detections), len(self.tracks)), dtype=np.float32) + + for i, det in enumerate(detections): + for j, track in enumerate(self.tracks): + iou_matrix[i, j] = iou(det.bbox, track.bbox) + + # 将IoU转换为成本(1 - IoU) + cost_matrix = 1.0 - iou_matrix + + # 使用匈牙利算法进行匹配 + try: + det_indices, track_indices = linear_sum_assignment(cost_matrix) + except ValueError: + det_indices, track_indices = [], [] + + # 根据阈值过滤匹配 + matched_pairs = [] + unmatched_detections = set(range(len(detections))) + unmatched_tracks = set(range(len(self.tracks))) + + for det_idx, track_idx in zip(det_indices, track_indices): + if iou_matrix[det_idx, track_idx] >= self.iou_threshold: + matched_pairs.append((det_idx, track_idx)) + unmatched_detections.discard(det_idx) + unmatched_tracks.discard(track_idx) + else: + matched_pairs = [] + unmatched_detections = set(range(len(detections))) + unmatched_tracks = set() + + # 更新匹配的目标 + for det_idx, track_idx in matched_pairs: + track = self.tracks[track_idx] + track.ego_speed = self.ego_speed # 传递自车速度用于行为分析 + track.update(detections[det_idx], self.ego_center) + + # 为未匹配的检测创建新目标 + for det_idx in unmatched_detections: + new_track = TrackedObject( + track_id=self.next_track_id, + bbox=detections[det_idx].bbox, + img_shape=self.img_shape, + config=self.config + ) + new_track.update(detections[det_idx], self.ego_center) + self.tracks.append(new_track) + self.next_track_id += 1 + + # 移除长时间未更新的目标 + self.tracks = [t for t in self.tracks if t.time_since_update <= self.max_age] + + # 返回跟踪结果(只返回满足最小匹配次数的目标) + active_tracks = [t for t in self.tracks if t.hits >= self.min_hits and t.state == TrackState.TRACKED] + + if not active_tracks: + return np.array([]), np.array([]), np.array([]) + + # 提取边界框、ID和类别 + boxes = np.array([t.bbox for t in active_tracks]) + ids = np.array([t.track_id for t in active_tracks]) + classes = np.array([t.class_id if t.class_id is not None else -1 for t in active_tracks]) + + return boxes, ids, classes + + def get_tracks_info(self) -> List[Dict[str, Any]]: + """ + 获取所有跟踪目标的详细信息 + + Returns: + List[Dict]: 跟踪目标信息列表 + """ + tracks_info = [] + + for track in self.tracks: + if track.hits >= self.min_hits and track.state == TrackState.TRACKED: + info = { + 'track_id': track.track_id, + 'bbox': track.bbox.tolist(), + 'class_id': track.class_id, + 'class_name': track.class_name, + 'confidence': track.confidence, + 'speed': track._calculate_speed(), + 'behavior': track.get_behavior_string(), + 'age': track.age, + 'hits': track.hits, + 'is_stopped': track.is_stopped, + 'is_overtaking': track.is_overtaking, + 'is_dangerous': track.is_dangerous, + } + tracks_info.append(info) + + return tracks_info + + def reset(self): + """重置跟踪器""" + self.tracks = [] + self.next_track_id = 1 + logger.info("✅ 跟踪器已重置") + + +# ======================== 检测线程 ======================== + +class DetectionThread(threading.Thread): + """ + 检测线程 - 将检测过程放到单独线程中 + """ + + def __init__(self, detector: YOLODetector, input_queue: queue.Queue, + output_queue: queue.Queue, maxsize: int = 2): + """ + 初始化检测线程 + + Args: + detector: YOLO检测器 + input_queue: 输入图像队列 + output_queue: 输出检测结果队列 + maxsize: 队列最大大小 + """ + super().__init__(daemon=True) + self.detector = detector + self.input_queue = input_queue + self.output_queue = output_queue + self.running = True + self.processed_count = 0 + + logger.info("✅ 检测线程初始化完成") + + def run(self): + """线程主函数""" + while self.running: + try: + # 从输入队列获取图像 + image = self.input_queue.get(timeout=1.0) + + if not valid_img(image): + self.output_queue.put((image, [])) + continue + + # 执行检测 + detections = self.detector.detect(image) + + # 放入输出队列 + if self.output_queue.full(): + try: + self.output_queue.get_nowait() + except queue.Empty: + pass + + self.output_queue.put((image, detections)) + self.processed_count += 1 + + except queue.Empty: + continue + except Exception as e: + logger.error(f"❌ 检测线程出错: {e}") + self.output_queue.put((None, [])) + + def stop(self): + """停止线程""" + self.running = False + logger.info("🛑 检测线程已停止") + + +# ======================== 测试函数 ======================== + +def test_tracker(): + """测试跟踪器""" + print("=" * 50) + print("测试 tracker.py...") + print("=" * 50) + + # 模拟配置 + test_config = { + 'yolo_model': 'yolov8n.pt', + 'conf_thres': 0.5, + 'iou_thres': 0.3, + 'max_age': 5, + 'min_hits': 3, + 'kf_dt': 0.05, + 'max_speed': 50.0, + 'img_width': 640, + 'img_height': 480, + 'track_history_len': 20, + 'stop_speed_thresh': 1.0, + 'stop_frames_thresh': 5, + 'overtake_speed_ratio': 1.5, + 'overtake_dist_thresh': 50.0, + 'lane_change_thresh': 0.5, + 'brake_accel_thresh': 2.0, + 'turn_angle_thresh': 15.0, + 'danger_dist_thresh': 10.0, + 'predict_frames': 10, + } + + # 测试数据结构 + print("1. 测试数据结构...") + bbox = np.array([100, 100, 200, 200], dtype=np.float32) + detection = Detection(bbox=bbox, confidence=0.9, class_id=2, class_name="Car") + assert detection.confidence == 0.9 + assert detection.class_id == 2 + print(" ✅ Detection数据结构测试通过") + + # 测试卡尔曼滤波器 + print("2. 测试卡尔曼滤波器...") + kf = KalmanFilter(dt=0.05) + kf.init(bbox) + predicted = kf.predict() + assert len(predicted) == 4 + updated = kf.update(bbox + 10) + assert len(updated) == 4 + print(" ✅ 卡尔曼滤波器测试通过") + + # 测试跟踪目标 + print("3. 测试跟踪目标...") + track = TrackedObject( + track_id=1, + bbox=bbox, + img_shape=(480, 640), + config=test_config + ) + track.update(detection, ego_center=(320, 240)) + assert track.track_id == 1 + assert track.class_id == 2 + track.predict() + print(" ✅ 跟踪目标测试通过") + + # 测试SORT跟踪器 + print("4. 测试SORT跟踪器...") + tracker = SORTTracker(test_config) + detections = [detection] + boxes, ids, classes = tracker.update(detections) + assert len(boxes) >= 0 # 可能没有匹配到 + print(" ✅ SORT跟踪器测试通过") + + print("=" * 50) + print("✅ tracker.py 所有测试通过") + print("注:完整测试需要YOLO模型文件") + + return True + + +if __name__ == "__main__": + test_tracker() \ No newline at end of file diff --git a/src/tracking_car/utils.py b/src/tracking_car/utils.py new file mode 100644 index 0000000000..b227fab51f --- /dev/null +++ b/src/tracking_car/utils.py @@ -0,0 +1,728 @@ +""" +utils.py - 通用工具函数 +包含:图像处理、几何计算、性能监控、文件操作等工具函数 +""" + +import cv2 +import numpy as np +import time +import os +import sys +from numba import njit +from datetime import datetime + +# 配置loguru logger +try: + from loguru import logger + # 移除默认配置,避免冲突 + logger.remove() + # 添加控制台输出 + logger.add(sys.stdout, format="{time:HH:mm:ss} | {level: <8} | {message}", level="INFO") +except ImportError: + # 创建简单的logger回退 + class SimpleLogger: + def debug(self, msg): print(f"DEBUG: {msg}") + def info(self, msg): print(f"INFO: {msg}") + def warning(self, msg): print(f"WARNING: {msg}") + def error(self, msg): print(f"ERROR: {msg}") + logger = SimpleLogger() + +# 尝试导入yaml,如果失败提供友好的错误信息 +try: + import yaml + YAML_AVAILABLE = True +except ImportError: + YAML_AVAILABLE = False + logger.warning("PyYAML未安装,配置文件功能将受限") + +# ======================== 图像处理工具 ======================== + +def valid_img(img): + """ + 检查图像是否有效 + + Args: + img: 输入图像 + + Returns: + bool: 图像是否有效 + """ + return img is not None and len(img.shape) == 3 and img.shape[2] == 3 and img.size > 0 + + +def clip_box(bbox, img_shape): + """ + 裁剪边界框到图像范围内 + + Args: + bbox: [x1, y1, x2, y2] 边界框坐标 + img_shape: (height, width) 图像尺寸 + + Returns: + np.ndarray: 裁剪后的边界框 + """ + h, w = img_shape[:2] + return np.array([ + max(0, min(bbox[0], w - 1)), + max(0, min(bbox[1], h - 1)), + max(bbox[0] + 1, min(bbox[2], w - 1)), + max(bbox[1] + 1, min(bbox[3], h - 1)) + ], dtype=np.float32) + + +def make_div(x, d=32): + """ + 将数值调整为d的倍数(用于YOLO输入尺寸) + + Args: + x: 原始数值 + d: 倍数(默认为32) + + Returns: + int: 调整后的数值 + """ + return (x + d - 1) // d * d + + +def resize_with_padding(image, target_size, color=(114, 114, 114)): + """ + 保持长宽比的resize,用指定颜色填充 + + Args: + image: 输入图像 + target_size: (width, height) 目标尺寸 + color: 填充颜色 + + Returns: + tuple: (resized_image, scale, padding) + """ + h, w = image.shape[:2] + target_w, target_h = target_size + + # 计算缩放比例 + scale = min(target_w / w, target_h / h) + new_w = int(w * scale) + new_h = int(h * scale) + + # 缩放图像 + if scale != 1: + image = cv2.resize(image, (new_w, new_h), interpolation=cv2.INTER_LINEAR) + + # 创建填充图像 + padded = np.full((target_h, target_w, 3), color, dtype=np.uint8) + + # 计算填充位置(居中) + dx = (target_w - new_w) // 2 + dy = (target_h - new_h) // 2 + + # 放置图像 + padded[dy:dy + new_h, dx:dx + new_w] = image + + return padded, scale, (dx, dy) + + +# ======================== 几何计算工具 ======================== + +@njit +def iou_numpy(box1, box2): + """ + 计算两个边界框的IoU(交并比)- 使用numpy数组版本 + + Args: + box1: np.array([x1, y1, x2, y2]) + box2: np.array([x1, y1, x2, y2]) + + Returns: + float: IoU值 + """ + ix1 = max(box1[0], box2[0]) + iy1 = max(box1[1], box2[1]) + ix2 = min(box1[2], box2[2]) + iy2 = min(box1[3], box2[3]) + + ia = max(0, ix2 - ix1) * max(0, iy2 - iy1) + a1 = (box1[2] - box1[0]) * (box1[3] - box1[1]) + a2 = (box2[2] - box2[0]) * (box2[3] - box2[1]) + ua = a1 + a2 - ia + + return ia / ua if ua > 0 else 0.0 + + +def iou(box1, box2): + """ + 计算两个边界框的IoU(兼容list和numpy数组) + + Args: + box1: [x1, y1, x2, y2] 或 np.array + box2: [x1, y1, x2, y2] 或 np.array + + Returns: + float: IoU值 + """ + # 转换为numpy数组 + box1_np = np.array(box1, dtype=np.float32) + box2_np = np.array(box2, dtype=np.float32) + return iou_numpy(box1_np, box2_np) + + +@njit +def iou_batch(boxes1, boxes2): + """ + 批量计算IoU矩阵 + + Args: + boxes1: (N, 4) 边界框数组 + boxes2: (M, 4) 边界框数组 + + Returns: + np.ndarray: (N, M) IoU矩阵 + """ + N = boxes1.shape[0] + M = boxes2.shape[0] + iou_matrix = np.zeros((N, M), dtype=np.float32) + + for i in range(N): + for j in range(M): + iou_matrix[i, j] = iou_numpy(boxes1[i], boxes2[j]) + + return iou_matrix + + +def bbox_center(bbox): + """ + 计算边界框中心点 + + Args: + bbox: [x1, y1, x2, y2] 边界框 + + Returns: + tuple: (cx, cy) 中心点坐标 + """ + return ((bbox[0] + bbox[2]) / 2, (bbox[1] + bbox[3]) / 2) + + +def bbox_area(bbox): + """ + 计算边界框面积 + + Args: + bbox: [x1, y1, x2, y2] 边界框 + + Returns: + float: 边界框面积 + """ + return max(0, bbox[2] - bbox[0]) * max(0, bbox[3] - bbox[1]) + + +def bbox_aspect_ratio(bbox): + """ + 计算边界框宽高比 + + Args: + bbox: [x1, y1, x2, y2] 边界框 + + Returns: + float: 宽高比(宽/高) + """ + width = max(0.1, bbox[2] - bbox[0]) + height = max(0.1, bbox[3] - bbox[1]) + return width / height + + +# ======================== 性能监控工具 ======================== + +class FPSCounter: + """ + FPS计数器 + """ + + def __init__(self, window_size=15): + """ + Args: + window_size: 滑动窗口大小 + """ + self.window_size = window_size + self.timestamps = [] + self.fps = 0.0 + self.avg_fps = 0.0 + self.fps_history = [] + + def update(self): + """ + 更新FPS计数 + + Returns: + float: 当前FPS + """ + self.timestamps.append(time.time()) + + if len(self.timestamps) > self.window_size: + self.timestamps.pop(0) + + if len(self.timestamps) >= 2: + self.fps = (len(self.timestamps) - 1) / (self.timestamps[-1] - self.timestamps[0]) + self.fps_history.append(self.fps) + + if len(self.fps_history) > 100: + self.fps_history.pop(0) + + self.avg_fps = np.mean(self.fps_history) if self.fps_history else self.fps + + return self.fps + + def reset(self): + """重置计数器""" + self.timestamps = [] + self.fps = 0.0 + self.fps_history = [] + self.avg_fps = 0.0 + + +class PerformanceMonitor: + """ + 性能监控器 + """ + + def __init__(self): + self.frame_count = 0 + self.start_time = time.time() + self.frame_times = [] + self.detection_times = [] + self.tracking_times = [] + + def start_frame(self): + """开始新帧计时""" + self.frame_start = time.time() + + def end_frame(self): + """结束帧计时""" + frame_time = time.time() - self.frame_start + self.frame_times.append(frame_time) + self.frame_count += 1 + + # 保留最近100帧的计时 + if len(self.frame_times) > 100: + self.frame_times.pop(0) + + def record_detection_time(self, dt): + """记录检测时间""" + self.detection_times.append(dt) + if len(self.detection_times) > 100: + self.detection_times.pop(0) + + def record_tracking_time(self, dt): + """记录跟踪时间""" + self.tracking_times.append(dt) + if len(self.tracking_times) > 100: + self.tracking_times.pop(0) + + def get_stats(self): + """获取性能统计""" + stats = { + 'total_frames': self.frame_count, + 'total_time': time.time() - self.start_time, + 'avg_fps': len(self.frame_times) / sum(self.frame_times) if self.frame_times else 0, + 'avg_frame_time': np.mean(self.frame_times) * 1000 if self.frame_times else 0, + 'avg_detection_time': np.mean(self.detection_times) * 1000 if self.detection_times else 0, + 'avg_tracking_time': np.mean(self.tracking_times) * 1000 if self.tracking_times else 0, + } + return stats + + def print_stats(self): + """打印性能统计""" + stats = self.get_stats() + logger.info(f"总帧数: {stats['total_frames']}") + logger.info(f"总时间: {stats['total_time']:.1f}s") + logger.info(f"平均FPS: {stats['avg_fps']:.1f}") + logger.info(f"平均帧时间: {stats['avg_frame_time']:.1f}ms") + logger.info(f"平均检测时间: {stats['avg_detection_time']:.1f}ms") + logger.info(f"平均跟踪时间: {stats['avg_tracking_time']:.1f}ms") + + +# ======================== 文件操作工具 ======================== + +def create_output_dir(base_dir="outputs"): + """ + 创建输出目录 + + Args: + base_dir: 基础目录名 + + Returns: + str: 创建的目录路径 + """ + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + output_dir = os.path.join(base_dir, timestamp) + + # 创建目录 + os.makedirs(output_dir, exist_ok=True) + os.makedirs(os.path.join(output_dir, "screenshots"), exist_ok=True) + os.makedirs(os.path.join(output_dir, "logs"), exist_ok=True) + + logger.info(f"创建输出目录: {output_dir}") + return output_dir + + +def save_image(image, path, create_dir=True): + """ + 保存图像 + + Args: + image: 要保存的图像 + path: 保存路径 + create_dir: 是否创建目录 + + Returns: + bool: 是否保存成功 + """ + if not valid_img(image): + logger.warning(f"无效图像,无法保存到 {path}") + return False + + try: + if create_dir: + os.makedirs(os.path.dirname(path), exist_ok=True) + + cv2.imwrite(path, image) + logger.debug(f"图像已保存: {path}") + return True + + except Exception as e: + logger.error(f"保存图像失败 {path}: {e}") + return False + + +def load_yaml_config(path): + """ + 加载YAML配置文件 + + Args: + path: 配置文件路径 + + Returns: + dict: 配置字典 + """ + if not YAML_AVAILABLE: + logger.error("无法加载YAML配置: PyYAML未安装") + return {} + + try: + with open(path, 'r', encoding='utf-8') as f: + config = yaml.safe_load(f) + logger.info(f"配置文件加载成功: {path}") + return config if config else {} + except FileNotFoundError: + logger.warning(f"配置文件不存在: {path}") + return {} + except Exception as e: + logger.error(f"加载配置文件失败 {path}: {e}") + return {} + + +def save_yaml_config(config, path): + """ + 保存配置到YAML文件 + + Args: + config: 配置字典 + path: 保存路径 + """ + if not YAML_AVAILABLE: + logger.error("无法保存YAML配置: PyYAML未安装") + return + + try: + os.makedirs(os.path.dirname(path), exist_ok=True) + with open(path, 'w', encoding='utf-8') as f: + yaml.dump(config, f, default_flow_style=False, indent=2) + logger.debug(f"配置已保存: {path}") + except Exception as e: + logger.error(f"保存配置失败 {path}: {e}") + + +# ======================== 可视化工具 ======================== + +def draw_bbox(image, bbox, color=(255, 0, 0), thickness=2, label=None): + """ + 在图像上绘制单个边界框 + + Args: + image: 输入图像 + bbox: [x1, y1, x2, y2] 边界框 + color: 颜色 (B, G, R) + thickness: 线宽 + label: 标签文本 + + Returns: + np.ndarray: 绘制后的图像 + """ + if not valid_img(image): + return image + + x1, y1, x2, y2 = map(int, bbox) + + # 检查坐标有效性 + if x1 >= x2 or y1 >= y2: + return image + + # 绘制边界框 + cv2.rectangle(image, (x1, y1), (x2, y2), color, thickness) + + # 绘制标签 + if label: + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 0.5 + + # 获取文本尺寸 + (text_width, text_height), baseline = cv2.getTextSize(label, font, font_scale, thickness) + + # 绘制标签背景 + cv2.rectangle(image, (x1, y1 - text_height - 5), + (x1 + text_width, y1), color, -1) + + # 绘制文本 + cv2.putText(image, label, (x1, y1 - 5), + font, font_scale, (255, 255, 255), thickness) + + return image + + +def draw_trajectory(image, points, color=(0, 255, 0), thickness=2, max_points=20): + """ + 在图像上绘制轨迹 + + Args: + image: 输入图像 + points: 轨迹点列表 [(x1, y1), (x2, y2), ...] + color: 轨迹颜色 + thickness: 线宽 + max_points: 最大显示点数 + + Returns: + np.ndarray: 绘制后的图像 + """ + if not valid_img(image) or len(points) < 2: + return image + + # 限制轨迹点数量 + points = points[-max_points:] + + # 绘制轨迹线 + for i in range(1, len(points)): + pt1 = (int(points[i-1][0]), int(points[i-1][1])) + pt2 = (int(points[i][0]), int(points[i][1])) + + # 检查点是否有效 + if 0 <= pt1[0] < image.shape[1] and 0 <= pt1[1] < image.shape[0] and \ + 0 <= pt2[0] < image.shape[1] and 0 <= pt2[1] < image.shape[0]: + cv2.line(image, pt1, pt2, color, thickness) + + return image + + +def draw_info_panel(image, info_dict, position="top_left"): + """ + 在图像上绘制信息面板 + + Args: + image: 输入图像 + info_dict: 信息字典 {key: value} + position: 位置 ("top_left", "top_right", "bottom_left", "bottom_right") + + Returns: + np.ndarray: 绘制后的图像 + """ + if not valid_img(image): + return image + + h, w = image.shape[:2] + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 0.6 + thickness = 1 + line_height = 25 + + # 确定起始位置 + if position == "top_left": + x, y = 10, 30 + elif position == "top_right": + x, y = w - 200, 30 + elif position == "bottom_left": + x, y = 10, h - 30 - len(info_dict) * line_height + elif position == "bottom_right": + x, y = w - 200, h - 30 - len(info_dict) * line_height + else: + x, y = 10, 30 + + # 绘制信息背景 + bg_height = len(info_dict) * line_height + 10 + cv2.rectangle(image, (x - 5, y - 25), (x + 190, y + bg_height - 20), (0, 0, 0), -1) + + # 绘制标题 + cv2.putText(image, "SYSTEM INFO", (x, y - 5), font, 0.7, (0, 255, 0), thickness) + + # 绘制信息项 + for i, (key, value) in enumerate(info_dict.items()): + text = f"{key}: {value}" + cv2.putText(image, text, (x, y + (i + 1) * line_height), + font, font_scale, (255, 255, 255), thickness) + + return image + + +# ======================== 测试函数 ======================== + +def run_self_tests(): + """运行自测试""" + print("=" * 50) + print("运行 utils.py 自测试...") + print("=" * 50) + + tests_passed = 0 + tests_failed = 0 + + # 测试 1: valid_img + try: + test_img = np.zeros((100, 100, 3), dtype=np.uint8) + assert valid_img(test_img) == True, "valid_img应该返回True" + assert valid_img(None) == False, "valid_img(None)应该返回False" + assert valid_img(np.zeros((100, 100), dtype=np.uint8)) == False, "灰度图应该返回False" + print("✅ valid_img测试通过") + tests_passed += 1 + except AssertionError as e: + print(f"❌ valid_img测试失败: {e}") + tests_failed += 1 + + # 测试 2: clip_box + try: + bbox = [10, 10, 200, 200] + clipped = clip_box(bbox, (150, 150)) + expected = [10, 10, 149, 149] # 索引从0开始,所以是149不是150 + assert np.allclose(clipped[:2], expected[:2]), f"clip_box坐标错误: {clipped[:2]} != {expected[:2]}" + assert clipped[2] <= 149 and clipped[3] <= 149, "clip_box应该限制在图像范围内" + print("✅ clip_box测试通过") + tests_passed += 1 + except AssertionError as e: + print(f"❌ clip_box测试失败: {e}") + tests_failed += 1 + + # 测试 3: iou (兼容性版本) + try: + box1 = [0, 0, 10, 10] + box2 = [5, 5, 15, 15] + iou_val = iou(box1, box2) + expected_iou = 25 / (100 + 100 - 25) # (5x5)/(100+100-25) = 25/175 ≈ 0.1429 + assert abs(iou_val - expected_iou) < 0.001, f"iou计算错误: {iou_val} != {expected_iou}" + + # 测试numpy数组版本 + box1_np = np.array(box1, dtype=np.float32) + box2_np = np.array(box2, dtype=np.float32) + iou_val_np = iou_numpy(box1_np, box2_np) + assert abs(iou_val_np - expected_iou) < 0.001, f"iou_numpy计算错误" + + print("✅ iou测试通过") + tests_passed += 1 + except AssertionError as e: + print(f"❌ iou测试失败: {e}") + tests_failed += 1 + + # 测试 4: make_div + try: + assert make_div(100) == 128, "make_div(100)应该返回128" + assert make_div(128) == 128, "make_div(128)应该返回128" + assert make_div(129) == 160, "make_div(129)应该返回160" + assert make_div(0, 32) == 0, "make_div(0)应该返回0" + print("✅ make_div测试通过") + tests_passed += 1 + except AssertionError as e: + print(f"❌ make_div测试失败: {e}") + tests_failed += 1 + + # 测试 5: FPSCounter (修复的测试) + try: + fps_counter = FPSCounter(window_size=3) + + # 第一次update会初始化但不会计算FPS(需要至少2个时间点) + fps1 = fps_counter.update() + time.sleep(0.05) # 等待50ms + + # 第二次update才会计算FPS + fps2 = fps_counter.update() + time.sleep(0.05) + + fps3 = fps_counter.update() + + # 现在应该有FPS值了 + assert fps3 > 0, f"FPS应该大于0,当前: {fps3}" + assert fps_counter.fps > 0, f"内部FPS应该大于0" + + print(f"✅ FPSCounter测试通过 (FPS: {fps3:.1f})") + tests_passed += 1 + except Exception as e: + print(f"❌ FPSCounter测试失败: {e}") + import traceback + traceback.print_exc() + tests_failed += 1 + + # 测试 6: 可视化函数 + try: + test_img = np.zeros((100, 100, 3), dtype=np.uint8) + # 测试draw_bbox + result1 = draw_bbox(test_img.copy(), [10, 10, 50, 50], label="test") + assert result1.shape == test_img.shape, "draw_bbox应该返回相同尺寸的图像" + + # 测试draw_trajectory + points = [(20, 20), (30, 30), (40, 40)] + result2 = draw_trajectory(test_img.copy(), points) + assert result2.shape == test_img.shape, "draw_trajectory应该返回相同尺寸的图像" + + # 测试draw_info_panel + info = {"FPS": "30.0", "Objects": "5"} + result3 = draw_info_panel(test_img.copy(), info) + assert result3.shape == test_img.shape, "draw_info_panel应该返回相同尺寸的图像" + + print("✅ 可视化函数测试通过") + tests_passed += 1 + except Exception as e: + print(f"❌ 可视化函数测试失败: {e}") + import traceback + traceback.print_exc() + tests_failed += 1 + + # 测试 7: bbox工具函数 + try: + bbox = [10, 20, 50, 80] + center = bbox_center(bbox) + area = bbox_area(bbox) + aspect = bbox_aspect_ratio(bbox) + + assert center == (30.0, 50.0), f"中心点计算错误: {center}" + assert area == 40 * 60, f"面积计算错误: {area}" + assert abs(aspect - 40/60) < 0.001, f"宽高比计算错误: {aspect}" + + print("✅ bbox工具函数测试通过") + tests_passed += 1 + except Exception as e: + print(f"❌ bbox工具函数测试失败: {e}") + tests_failed += 1 + + print("=" * 50) + print(f"测试结果: {tests_passed}通过, {tests_failed}失败") + + if tests_failed == 0: + print("🎉 所有测试通过!") + else: + print("⚠️ 有测试失败,请检查") + + return tests_failed == 0 + + +if __name__ == "__main__": + # 运行自测试 + success = run_self_tests() + + if success: + print("\nutils.py 可以安全使用") + sys.exit(0) + else: + print("\n⚠️ utils.py 有测试失败,请修复") + sys.exit(1) \ No newline at end of file