From 9a62891fd266bed17970aa2bc464498668849fc4 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Sun, 21 Dec 2025 12:40:01 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E6=B3=A8=E9=87=8A?= =?UTF-8?q?=E5=AE=8C=E5=96=84=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/main.py | 349 ++++++++++++++++++++++++++++++--------- 1 file changed, 270 insertions(+), 79 deletions(-) diff --git a/src/car_tracking/main.py b/src/car_tracking/main.py index 0f5e36de6b..01718c5c76 100644 --- a/src/car_tracking/main.py +++ b/src/car_tracking/main.py @@ -1,17 +1,28 @@ +# 导入CARLA仿真环境库 import carla +# 导入队列库,用于处理摄像头图像的异步获取 import queue +# 导入随机数库,用于随机生成车辆和位置 import random +# 导入OpenCV库,用于图像处理和显示 import cv2 +# 导入数值计算库,用于矩阵和数组操作 import numpy as np +# 导入数学库,用于几何计算和三角函数 import math -import os # 新增:处理文件路径 -# 修复Deep SORT的API弃用问题 +# 导入操作系统库,处理文件路径相关操作 +import os + +# 修复Deep SORT的API弃用问题:导入scipy的优化模块 import scipy.optimize as opt -# 替换原有的linear_assignment导入 -from deep_sort import nn_matching -from deep_sort.detection import Detection -from deep_sort.tracker import Tracker -# 若你的COCO_CLASS_NAMES不存在,手动定义(避免依赖缺失) + +# 导入Deep SORT的核心模块 +from deep_sort import nn_matching # 近邻匹配度量 +from deep_sort.detection import Detection # 检测结果类 +from deep_sort.tracker import Tracker # 追踪器类 + +# 定义COCO数据集的类别名称(避免依赖缺失,手动定义) +# 这里主要用到索引2的'car'类别 COCO_CLASS_NAMES = [ 'person', 'bicycle', 'car', 'motorcycle', 'airplane', 'bus', 'train', 'truck', 'boat', 'traffic light', 'fire hydrant', @@ -31,54 +42,103 @@ # ===================== 修复:替换Deep SORT的linear_assignment(解决弃用警告) ===================== def linear_assignment(cost_matrix): - """替换原有的linear_assignment,使用scipy的linear_sum_assignment""" + """ + 替换原有的linear_assignment函数,解决scipy旧版本API弃用问题 + 使用scipy的linear_sum_assignment实现匈牙利算法,用于匹配检测框和追踪框 + 参数: + cost_matrix: 代价矩阵,元素表示检测框和追踪框的匹配代价 + 返回: + 匹配的索引对数组,格式为[(检测框索引, 追踪框索引), ...] + """ + # linear_sum_assignment返回行和列的索引,对应最小代价匹配 x, y = opt.linear_sum_assignment(cost_matrix) return np.array(list(zip(x, y))) -# 覆盖deep_sort.utils.linear_assignment中的同名函数 +# 覆盖deep_sort.utils.linear_assignment中的同名函数,实现无缝替换 import deep_sort.utils.linear_assignment as la la.linear_assignment = linear_assignment # ===================== 修复:重新实现Box Encoder(解决pb文件读取错误) ===================== -# 由于mars-small128.pb读取失败,改用简单的特征编码(不依赖TensorFlow,保证运行) class SimpleBoxEncoder: + """ + 简单的边界框特征编码器(替代原有的mars-small128.pb模型编码) + 解决TensorFlow pb模型文件读取失败的问题,保证追踪功能正常运行 + 生成128维的特征向量,与原编码器输出维度一致 + """ def __init__(self): pass + def __call__(self, image, boxes): - """生成简单的框特征(替代原有的mars-small128编码,保证追踪功能运行)""" + """ + 生成边界框的特征向量 + 参数: + image: 输入图像(np.array),用于获取图像尺寸进行归一化 + boxes: 边界框列表,每个框格式为[x1, y1, w, h](左上角坐标+宽高) + 返回: + 128维特征向量的数组,形状为(len(boxes), 128) + """ features = [] for box in boxes: x1, y1, w, h = box - # 特征:框的宽高比、中心坐标归一化、面积归一化 + # 计算框的宽高比(避免除零错误) aspect_ratio = w / h if h != 0 else 1.0 + # 计算框的中心坐标并归一化(相对于图像宽度/高度) center_x = (x1 + w/2) / image.shape[1] center_y = (y1 + h/2) / image.shape[0] + # 计算框的面积并归一化(相对于图像总面积) area = (w * h) / (image.shape[0] * image.shape[1]) - feature = np.array([aspect_ratio, center_x, center_y, area] + [0.0]*124) # 凑128维 + # 拼接特征:前4维为几何特征,后124维补0,凑成128维 + feature = np.array([aspect_ratio, center_x, center_y, area] + [0.0]*124) features.append(feature) return np.array(features) def create_box_encoder(model_filename=None, batch_size=32): - """替换原有的create_box_encoder,返回简单编码器""" + """ + 替换原有的create_box_encoder函数,返回自定义的简单编码器 + 参数: + model_filename: 原模型文件名(此处无用,保留参数以兼容接口) + batch_size: 批处理大小(此处无用,保留参数以兼容接口) + 返回: + SimpleBoxEncoder实例 + """ return SimpleBoxEncoder() # ===================== 自定义工具函数(替代utils中的函数,避免依赖缺失) ===================== def get_image_point(vertex, K, world_to_camera): - """3D点投影到2D图像平面(简化版)""" - # 3D点(齐次坐标) + """ + 将3D世界坐标点投影到2D图像平面(简化版投影计算) + 参数: + vertex: CARLA的3D顶点对象(包含x, y, z属性) + K: 相机内参矩阵(3x3) + world_to_camera: 世界坐标到相机坐标的变换矩阵(4x4) + 返回: + 2D图像坐标(x, y) + """ + # 将3D点转换为齐次坐标(4维) point_3d = np.array([vertex.x, vertex.y, vertex.z, 1.0]) - # 世界到相机 + # 世界坐标→相机坐标(矩阵乘法) point_camera = np.dot(world_to_camera, point_3d) - # 相机到图像(投影矩阵K) + # 相机坐标→图像坐标(使用内参矩阵K,取前3维) point_img = np.dot(K, point_camera[:3]) - # 归一化 + # 归一化(除以z分量,透视投影) point_img = point_img / point_img[2] return (point_img[0], point_img[1]) def get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w): - """从3D边的2D点生成2D边界框""" + """ + 从3D物体的2D投影点集生成2D边界框(包围盒) + 参数: + points_2d: 2D投影点列表,每个点为(x, y) + edges: 3D物体的边索引(此处未使用,保留参数兼容接口) + image_h: 图像高度 + image_w: 图像宽度 + 返回: + 边界框的x_min, x_max, y_min, y_max(限制在图像范围内) + """ + # 提取所有点的x和y坐标 x_coords = [p[0] for p in points_2d] y_coords = [p[1] for p in points_2d] + # 计算最小/最大坐标,限制在图像范围内(避免超出画布) x_min = max(0, min(x_coords)) x_max = min(image_w, max(x_coords)) y_min = max(0, min(y_coords)) @@ -86,296 +146,427 @@ def get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w): return x_min, x_max, y_min, y_max def point_in_canvas(point, image_h, image_w): - """判断点是否在画布内""" + """ + 判断2D点是否在图像画布内 + 参数: + point: 2D点(x, y) + image_h: 图像高度 + image_w: 图像宽度 + 返回: + 布尔值,True表示在画布内,False表示超出范围 + """ x, y = point return 0 <= x <= image_w and 0 <= y <= image_h def build_projection_matrix(w, h, fov, is_behind_camera=False): - """构建投影矩阵(简化版)""" + """ + 构建相机内参矩阵(投影矩阵),基于图像尺寸和视场角 + 参数: + w: 图像宽度 + h: 图像高度 + fov: 相机视场角(水平,单位:度) + is_behind_camera: 是否处理相机后方的点(翻转焦距符号) + 返回: + 3x3的内参矩阵K + """ + # 计算焦距:focal = 宽度 / (2 * tan(fov/2))(弧度转换) focal = w / (2.0 * math.tan(fov * math.pi / 360.0)) + # 初始化单位矩阵 K = np.identity(3) + # 设置x和y方向的焦距 K[0, 0] = K[1, 1] = focal + # 设置图像中心坐标(主点) K[0, 2] = w / 2.0 K[1, 2] = h / 2.0 + # 处理相机后方的点:翻转焦距符号 if is_behind_camera: K[0, 0] = -K[0, 0] return K def clear_npc(world): - """清理NPC车辆""" + """ + 清理CARLA世界中的NPC车辆(保留主角车辆) + 参数: + world: CARLA的世界对象 + """ for actor in world.get_actors().filter('*vehicle*'): + # 只销毁角色不是hero的车辆(主角车辆role_name为hero) if actor.attributes.get('role_name') != 'hero': actor.destroy() def clear_static_vehicle(world): - """清理静态车辆(空实现,避免报错)""" + """ + 清理静态车辆(空实现,避免依赖缺失导致的报错) + 参数: + world: CARLA的世界对象 + """ pass def clear(world, camera): - """清理资源""" + """ + 清理CARLA仿真环境的资源(销毁相机和所有车辆) + 参数: + world: CARLA的世界对象 + camera: 相机传感器对象 + """ + # 销毁相机 if camera: camera.destroy() + # 销毁所有车辆 for actor in world.get_actors().filter('*vehicle*'): actor.destroy() def draw_bounding_boxes(image, bboxes, labels, class_names, ids): - """绘制边界框和追踪ID(解决文字显示问题)""" + """ + 在图像上绘制边界框、类别标签和追踪ID + 参数: + image: 输入图像(np.array) + bboxes: 边界框列表,每个框为[x1, y1, x2, y2](左上角+右下角) + labels: 类别索引列表 + class_names: 类别名称列表 + ids: 追踪ID列表 + 返回: + 绘制后的图像 + """ for bbox, label, track_id in zip(bboxes, labels, ids): + # 将边界框坐标转换为整数 x1, y1, x2, y2 = bbox.astype(int) - # 绘制框 + # 绘制矩形边界框(绿色,线宽2) cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2) - # 绘制标签和ID + # 获取类别名称(默认car) class_name = class_names[label] if label < len(class_names) else 'car' + # 拼接标签文本:类别 + 追踪ID text = f"{class_name} | ID: {track_id}" - # 文字背景 + # 计算文本尺寸,绘制文本背景(绿色填充) text_size = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)[0] cv2.rectangle(image, (x1, y1 - text_size[1] - 5), (x1 + text_size[0], y1), (0, 255, 0), -1) + # 绘制文本(白色,字体大小0.5,线宽2) cv2.putText(image, text, (x1, y1 - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) return image # ===================== 可视化信息绘制函数 ===================== def draw_info_text(image, speed_kmh, vehicle_count, map_name): - """在图像上绘制车速、车辆数量、地图名称等信息""" + """ + 在图像左上角绘制仿真信息:地图名称、车速、追踪车辆数量 + 参数: + image: 输入图像(np.array) + speed_kmh: 车辆速度(km/h) + vehicle_count: 追踪到的车辆数量 + map_name: 地图名称 + 返回: + 绘制后的图像 + """ + # 复制图像,避免修改原图像 image_copy = image.copy() + # 设置字体参数 font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.6 font_thickness = 2 - text_color = (255, 255, 255) - bg_color = (0, 0, 0) - padding = 5 + text_color = (255, 255, 255) # 白色文本 + bg_color = (0, 0, 0) # 黑色背景 + padding = 5 # 文本背景的内边距 + # 定义要显示的文本列表 text_list = [ - f"Map: {map_name}", - f"Speed: {speed_kmh:.1f} km/h", - f"Tracked Vehicles: {vehicle_count}" + f"Map: {map_name}", # 地图名称 + f"Speed: {speed_kmh:.1f} km/h", # 车速(保留1位小数) + f"Tracked Vehicles: {vehicle_count}" # 追踪车辆数量 ] + # 初始化文本y轴偏移量 y_offset = 30 for text in text_list: + # 计算文本尺寸 text_size = cv2.getTextSize(text, font, font_scale, font_thickness)[0] + # 绘制文本背景矩形 cv2.rectangle( image_copy, - (10, y_offset - text_size[1] - padding), - (10 + text_size[0] + padding * 2, y_offset + padding), - bg_color, - -1 + (10, y_offset - text_size[1] - padding), # 左上角 + (10 + text_size[0] + padding * 2, y_offset + padding), # 右下角 + bg_color, # 背景色 + -1 # 填充矩形 ) + # 绘制文本 cv2.putText( image_copy, text, - (10 + padding, y_offset), + (10 + padding, y_offset), # 文本起始位置 font, font_scale, text_color, font_thickness ) + # 更新y轴偏移量(换行) y_offset += text_size[1] + padding * 3 return image_copy def camera_callback(image, rgb_image_queue): - """摄像头回调函数""" - rgb_image_queue.put(np.reshape(np.copy(image.raw_data), - (image.height, image.width, 4))) + """ + 相机传感器的回调函数,将获取的图像存入队列 + 参数: + image: CARLA的图像对象 + rgb_image_queue: 存储图像的队列 + """ + # 将原始图像数据转换为numpy数组(BGRA格式,尺寸为height×width×4) + rgb_image = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4)) + # 将图像存入队列 + rgb_image_queue.put(rgb_image) def main(): + """ + 主函数:初始化CARLA环境、相机传感器、Deep SORT追踪器,运行主循环实现车辆追踪 + """ # Part 1: 初始化CARLA环境 + # 连接CARLA服务器(本地地址,端口2000) client = carla.Client('localhost', 2000) + # 设置超时时间(10秒) client.set_timeout(10.0) + # 获取CARLA世界对象 world = client.get_world() - # 设置同步模式 + # 设置CARLA同步模式(保证仿真步长固定,避免图像和数据不同步) settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 + settings.synchronous_mode = True # 启用同步模式 + settings.fixed_delta_seconds = 0.05 # 固定步长(20FPS) world.apply_settings(settings) - # 获取 spectator + # 获取旁观者(用于视角跟随) spectator = world.get_spectator() - # 获取生成点 + # 获取地图的生成点(车辆生成的位置) spawn_points = world.get_map().get_spawn_points() if not spawn_points: print("❌ 无可用生成点!") return - # 生成自车 + # 生成主角车辆(hero) + # 获取蓝图库 bp_lib = world.get_blueprint_library() + # 选择林肯MKZ 2020车型(如果不存在则选择第一个车辆蓝图) vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') if not vehicle_bp: vehicle_bp = bp_lib.filter('vehicle.*')[0] + # 随机选择一个生成点 spawn_point = random.choice(spawn_points) + # 尝试生成车辆 vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) if not vehicle: print("❌ 车辆生成失败!") return - # 生成摄像头 + # 生成RGB相机传感器 camera_bp = bp_lib.find('sensor.camera.rgb') + # 设置相机参数:图像宽度640,高度480,视场角90度 camera_bp.set_attribute('image_size_x', '640') camera_bp.set_attribute('image_size_y', '480') camera_bp.set_attribute('fov', '90') + # 设置相机挂载位置:车辆前方1.2m,上方2.0m,俯仰角-5度(稍微向下看) camera_init_trans = carla.Transform(carla.Location(x=1.2, z=2.0), carla.Rotation(pitch=-5)) + # 将相机挂载到主角车辆上 camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle) - # 图像队列 + # 创建图像队列(用于存储相机回调的图像,最大长度2) image_queue = queue.Queue(maxsize=2) + # 注册相机回调函数 camera.listen(lambda image: camera_callback(image, image_queue)) - # 清理现有NPC + # 清理现有NPC车辆和静态车辆 clear_npc(world) clear_static_vehicle(world) - # Part 2: 初始化追踪参数 + # Part 2: 初始化追踪相关参数 + # 3D车辆的边索引(用于生成2D包围盒,此处未实际使用) edges = [[0, 1], [1, 3], [3, 2], [2, 0], [0, 4], [4, 5], [5, 1], [5, 7], [7, 6], [6, 4], [6, 2], [7, 3]] - # 摄像头参数 + # 获取相机参数(宽度、高度、视场角) image_w = camera_bp.get_attribute("image_size_x").as_int() image_h = camera_bp.get_attribute("image_size_y").as_int() fov = camera_bp.get_attribute("fov").as_float() - # 投影矩阵 + # 构建相机内参矩阵(前向和后向) K = build_projection_matrix(image_w, image_h, fov) K_b = build_projection_matrix(image_w, image_h, fov, is_behind_camera=True) - # 生成NPC + # 生成NPC车辆(目标:20辆) npc_count = 20 - spawned_npcs = 0 + spawned_npcs = 0 # 实际生成的NPC数量 for i in range(npc_count): + # 筛选4轮车辆的蓝图 vehicle_bp_list = bp_lib.filter('vehicle') - car_bp = [bp for bp in vehicle_bp_list if int( - bp.get_attribute('number_of_wheels')) == 4] + car_bp = [bp for bp in vehicle_bp_list if int(bp.get_attribute('number_of_wheels')) == 4] if not car_bp: continue + # 随机选择生成点 random_spawn = random.choice(spawn_points) + # 避免生成在主角车辆附近(距离小于10m则跳过) if random_spawn.location.distance(vehicle.get_location()) < 10.0: continue + # 尝试生成NPC车辆 npc = world.try_spawn_actor(random.choice(car_bp), random_spawn) if npc: + # 启用自动驾驶 npc.set_autopilot(True) spawned_npcs += 1 print(f"✅ 生成{spawned_npcs}辆NPC车辆") + # 主角车辆启用自动驾驶 vehicle.set_autopilot(True) - # Deep SORT(使用修复后的编码器) - encoder = create_box_encoder("mars-small128.pb", batch_size=32) + # 初始化Deep SORT追踪器 + # 创建特征匹配度量(余弦距离,阈值0.2) metric = nn_matching.NearestNeighborDistanceMetric("cosine", 0.2, None) + # 创建追踪器(使用自定义的编码器) tracker = Tracker(metric) + # 初始化边界框编码器(替换原有的mars-small128编码器) + encoder = create_box_encoder("mars-small128.pb", batch_size=32) - # 地图名称 + # 获取地图名称(提取最后一部分) map_name = world.get_map().name.split('/')[-1] - # 主循环 + # 主循环(车辆追踪和可视化) while True: try: + # 推进仿真一步(同步模式必须调用) world.tick() - # 移动spectator - transform = carla.Transform(vehicle.get_transform().transform( - carla.Location(x=-4, z=50)), carla.Rotation(yaw=-180, pitch=-90)) + # 移动旁观者视角:车辆后方4m,上方50m,俯视视角(yaw=-180,pitch=-90) + transform = carla.Transform( + vehicle.get_transform().transform(carla.Location(x=-4, z=50)), + carla.Rotation(yaw=-180, pitch=-90) + ) spectator.set_transform(transform) - # 获取图像 + # 获取相机图像(队列为空则跳过) if image_queue.empty(): continue image = image_queue.get() - # 图像预处理:BGRA→BGR→水平翻转(解决反向) + # 图像预处理:BGRA→BGR(去除Alpha通道)→水平翻转(解决图像反向问题) image = cv2.cvtColor(image, cv2.COLOR_BGRA2BGR) image = cv2.flip(image, 1) - # 更新相机矩阵 + # 更新世界坐标到相机坐标的变换矩阵 world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + # 检测车辆:遍历所有车辆,生成2D边界框 boxes = [] for npc in world.get_actors().filter('*vehicle*'): + # 跳过主角车辆 if npc.id != vehicle.id: + # 获取车辆的包围盒 bb = npc.bounding_box + # 计算车辆与主角的距离(只处理50m内的车辆) dist = npc.get_transform().location.distance(vehicle.get_transform().location) if dist < 50: + # 筛选主角车辆前方的车辆(点积>0表示同向) forward_vec = vehicle.get_transform().get_forward_vector() ray = npc.get_transform().location - vehicle.get_transform().location if forward_vec.dot(ray) > 0: + # 获取车辆包围盒的所有3D顶点(世界坐标) verts = [v for v in bb.get_world_vertices(npc.get_transform())] points_2d = [] for vert in verts: + # 判断顶点是否在相机前方(点积>0) ray0 = vert - camera.get_transform().location cam_forward_vec = camera.get_transform().get_forward_vector() if (cam_forward_vec.dot(ray0) > 0): + # 相机前方的点:使用前向内参矩阵投影 p = get_image_point(vert, K, world_2_camera) else: + # 相机后方的点:使用后向内参矩阵投影 p = get_image_point(vert, K_b, world_2_camera) - # 翻转x坐标 + # 水平翻转x坐标(匹配图像翻转) p = (image_w - p[0], p[1]) points_2d.append(p) + # 从3D顶点的2D投影生成2D边界框 x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges( points_2d, edges, image_h, image_w) + # 过滤小框(面积>100,宽度>20,避免噪声) if (y_max - y_min) * (x_max - x_min) > 100 and (x_max - x_min) > 20: + # 确保边界框在画布内 if point_in_canvas((x_min, y_min), image_h, image_w) and point_in_canvas((x_max, y_max), image_h, image_w): + # 存储边界框(x1, y1, x2, y2) boxes.append(np.array([x_min, y_min, x_max, y_max])) + # 将边界框列表转换为numpy数组 boxes = np.array(boxes) + # 初始化检测结果列表 detections = [] if len(boxes) > 0: + # 复制边界框,转换为Deep SORT格式(x1, y1, w, h) sort_boxes = boxes.copy() for i, box in enumerate(sort_boxes): + # 计算宽度和高度(替换x2, y2) box[2] -= box[0] box[3] -= box[1] + # 提取边界框的特征(128维) feature = encoder(image, box.reshape(1, -1).copy()) + # 创建Detection对象(边界框,置信度1.0,特征) detections.append(Detection(box, 1.0, feature[0])) - # 更新追踪器 - tracker.predict() - tracker.update(detections) + # 更新Deep SORT追踪器 + tracker.predict() # 预测追踪框的位置 + tracker.update(detections) # 根据检测结果更新追踪器 + # 提取追踪结果(确认的追踪框,更新时间<=1) bboxes = [] ids = [] for track in tracker.tracks: if not track.is_confirmed() or track.time_since_update > 1: continue + # 转换为tlbr格式(top, left, bottom, right) bbox = track.to_tlbr() bboxes.append(bbox) ids.append(track.track_id) + # 转换为numpy数组,获取追踪车辆数量 bboxes = np.array(bboxes) tracked_vehicle_count = len(bboxes) + # 在图像上绘制边界框和追踪ID if len(bboxes) > 0: + # 类别标签:全部设为2(car) labels = np.array([2] * len(bboxes)) - image = draw_bounding_boxes( - image, bboxes, labels, COCO_CLASS_NAMES, ids) + image = draw_bounding_boxes(image, bboxes, labels, COCO_CLASS_NAMES, ids) - # 计算车速并绘制信息 + # 计算主角车辆的速度(m/s → km/h,乘以3.6) velocity = vehicle.get_velocity() - speed_ms = math.hypot(velocity.x, velocity.y) + speed_ms = math.hypot(velocity.x, velocity.y) # 计算xy平面的速度大小 speed_kmh = speed_ms * 3.6 + # 绘制仿真信息(地图名称、车速、追踪数量) image = draw_info_text(image, speed_kmh, tracked_vehicle_count, map_name) - # 显示图像 + # 显示图像窗口 cv2.imshow('2D Ground Truth Deep SORT (Fixed All Issues)', image) + # 按下q键退出循环 if cv2.waitKey(1) & 0xFF == ord('q'): break except KeyboardInterrupt as e: + # 捕获键盘中断(Ctrl+C),退出循环 break except Exception as e: + # 捕获其他异常,打印错误信息并继续循环 print(f"⚠️ 运行错误:{e}") continue # 清理资源 clear(world, camera) + # 关闭同步模式 settings = world.get_settings() settings.synchronous_mode = False world.apply_settings(settings) + # 关闭所有OpenCV窗口 cv2.destroyAllWindows() print("✅ 程序正常退出") +# 程序入口 if __name__ == '__main__': main() \ No newline at end of file From 775fcfcba924c8794e0c3b03188998b8fc4cbd34 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Sun, 21 Dec 2025 12:46:08 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E8=B0=83=E6=95=B4=E7=94=BB=E9=9D=A2?= =?UTF-8?q?=E6=B8=85=E6=99=B0=E5=BA=A6=EF=BC=8C=E6=8F=90=E5=8D=87=E6=80=A7?= =?UTF-8?q?=E8=83=BD=EF=BC=8C=E5=A2=9E=E5=8A=A0=E7=AA=97=E5=8F=A3=E4=BA=A4?= =?UTF-8?q?=E4=BA=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/main.py | 479 +++++++++++++-------------------------- 1 file changed, 156 insertions(+), 323 deletions(-) diff --git a/src/car_tracking/main.py b/src/car_tracking/main.py index 01718c5c76..15c50f9115 100644 --- a/src/car_tracking/main.py +++ b/src/car_tracking/main.py @@ -1,28 +1,19 @@ -# 导入CARLA仿真环境库 import carla -# 导入队列库,用于处理摄像头图像的异步获取 import queue -# 导入随机数库,用于随机生成车辆和位置 import random -# 导入OpenCV库,用于图像处理和显示 import cv2 -# 导入数值计算库,用于矩阵和数组操作 import numpy as np -# 导入数学库,用于几何计算和三角函数 import math -# 导入操作系统库,处理文件路径相关操作 import os +import time # 新增:计算帧率 -# 修复Deep SORT的API弃用问题:导入scipy的优化模块 +# 修复Deep SORT的API弃用问题 import scipy.optimize as opt +from deep_sort import nn_matching +from deep_sort.detection import Detection +from deep_sort.tracker import Tracker -# 导入Deep SORT的核心模块 -from deep_sort import nn_matching # 近邻匹配度量 -from deep_sort.detection import Detection # 检测结果类 -from deep_sort.tracker import Tracker # 追踪器类 - -# 定义COCO数据集的类别名称(避免依赖缺失,手动定义) -# 这里主要用到索引2的'car'类别 +# COCO类别名称 COCO_CLASS_NAMES = [ 'person', 'bicycle', 'car', 'motorcycle', 'airplane', 'bus', 'train', 'truck', 'boat', 'traffic light', 'fire hydrant', @@ -40,533 +31,375 @@ 'scissors', 'teddy bear', 'hair drier', 'toothbrush' ] -# ===================== 修复:替换Deep SORT的linear_assignment(解决弃用警告) ===================== + +# ===================== Deep SORT修复函数 ===================== def linear_assignment(cost_matrix): - """ - 替换原有的linear_assignment函数,解决scipy旧版本API弃用问题 - 使用scipy的linear_sum_assignment实现匈牙利算法,用于匹配检测框和追踪框 - 参数: - cost_matrix: 代价矩阵,元素表示检测框和追踪框的匹配代价 - 返回: - 匹配的索引对数组,格式为[(检测框索引, 追踪框索引), ...] - """ - # linear_sum_assignment返回行和列的索引,对应最小代价匹配 x, y = opt.linear_sum_assignment(cost_matrix) return np.array(list(zip(x, y))) -# 覆盖deep_sort.utils.linear_assignment中的同名函数,实现无缝替换 + import deep_sort.utils.linear_assignment as la + la.linear_assignment = linear_assignment -# ===================== 修复:重新实现Box Encoder(解决pb文件读取错误) ===================== + class SimpleBoxEncoder: - """ - 简单的边界框特征编码器(替代原有的mars-small128.pb模型编码) - 解决TensorFlow pb模型文件读取失败的问题,保证追踪功能正常运行 - 生成128维的特征向量,与原编码器输出维度一致 - """ def __init__(self): pass def __call__(self, image, boxes): - """ - 生成边界框的特征向量 - 参数: - image: 输入图像(np.array),用于获取图像尺寸进行归一化 - boxes: 边界框列表,每个框格式为[x1, y1, w, h](左上角坐标+宽高) - 返回: - 128维特征向量的数组,形状为(len(boxes), 128) - """ features = [] for box in boxes: x1, y1, w, h = box - # 计算框的宽高比(避免除零错误) aspect_ratio = w / h if h != 0 else 1.0 - # 计算框的中心坐标并归一化(相对于图像宽度/高度) - center_x = (x1 + w/2) / image.shape[1] - center_y = (y1 + h/2) / image.shape[0] - # 计算框的面积并归一化(相对于图像总面积) + center_x = (x1 + w / 2) / image.shape[1] + center_y = (y1 + h / 2) / image.shape[0] area = (w * h) / (image.shape[0] * image.shape[1]) - # 拼接特征:前4维为几何特征,后124维补0,凑成128维 - feature = np.array([aspect_ratio, center_x, center_y, area] + [0.0]*124) + feature = np.array([aspect_ratio, center_x, center_y, area] + [0.0] * 124) features.append(feature) return np.array(features) + def create_box_encoder(model_filename=None, batch_size=32): - """ - 替换原有的create_box_encoder函数,返回自定义的简单编码器 - 参数: - model_filename: 原模型文件名(此处无用,保留参数以兼容接口) - batch_size: 批处理大小(此处无用,保留参数以兼容接口) - 返回: - SimpleBoxEncoder实例 - """ return SimpleBoxEncoder() -# ===================== 自定义工具函数(替代utils中的函数,避免依赖缺失) ===================== + +# ===================== 工具函数 ===================== def get_image_point(vertex, K, world_to_camera): - """ - 将3D世界坐标点投影到2D图像平面(简化版投影计算) - 参数: - vertex: CARLA的3D顶点对象(包含x, y, z属性) - K: 相机内参矩阵(3x3) - world_to_camera: 世界坐标到相机坐标的变换矩阵(4x4) - 返回: - 2D图像坐标(x, y) - """ - # 将3D点转换为齐次坐标(4维) point_3d = np.array([vertex.x, vertex.y, vertex.z, 1.0]) - # 世界坐标→相机坐标(矩阵乘法) point_camera = np.dot(world_to_camera, point_3d) - # 相机坐标→图像坐标(使用内参矩阵K,取前3维) point_img = np.dot(K, point_camera[:3]) - # 归一化(除以z分量,透视投影) point_img = point_img / point_img[2] return (point_img[0], point_img[1]) + def get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w): - """ - 从3D物体的2D投影点集生成2D边界框(包围盒) - 参数: - points_2d: 2D投影点列表,每个点为(x, y) - edges: 3D物体的边索引(此处未使用,保留参数兼容接口) - image_h: 图像高度 - image_w: 图像宽度 - 返回: - 边界框的x_min, x_max, y_min, y_max(限制在图像范围内) - """ - # 提取所有点的x和y坐标 x_coords = [p[0] for p in points_2d] y_coords = [p[1] for p in points_2d] - # 计算最小/最大坐标,限制在图像范围内(避免超出画布) x_min = max(0, min(x_coords)) x_max = min(image_w, max(x_coords)) y_min = max(0, min(y_coords)) y_max = min(image_h, max(y_coords)) return x_min, x_max, y_min, y_max + def point_in_canvas(point, image_h, image_w): - """ - 判断2D点是否在图像画布内 - 参数: - point: 2D点(x, y) - image_h: 图像高度 - image_w: 图像宽度 - 返回: - 布尔值,True表示在画布内,False表示超出范围 - """ x, y = point return 0 <= x <= image_w and 0 <= y <= image_h + def build_projection_matrix(w, h, fov, is_behind_camera=False): - """ - 构建相机内参矩阵(投影矩阵),基于图像尺寸和视场角 - 参数: - w: 图像宽度 - h: 图像高度 - fov: 相机视场角(水平,单位:度) - is_behind_camera: 是否处理相机后方的点(翻转焦距符号) - 返回: - 3x3的内参矩阵K - """ - # 计算焦距:focal = 宽度 / (2 * tan(fov/2))(弧度转换) focal = w / (2.0 * math.tan(fov * math.pi / 360.0)) - # 初始化单位矩阵 K = np.identity(3) - # 设置x和y方向的焦距 K[0, 0] = K[1, 1] = focal - # 设置图像中心坐标(主点) K[0, 2] = w / 2.0 K[1, 2] = h / 2.0 - # 处理相机后方的点:翻转焦距符号 if is_behind_camera: K[0, 0] = -K[0, 0] return K + def clear_npc(world): - """ - 清理CARLA世界中的NPC车辆(保留主角车辆) - 参数: - world: CARLA的世界对象 - """ for actor in world.get_actors().filter('*vehicle*'): - # 只销毁角色不是hero的车辆(主角车辆role_name为hero) if actor.attributes.get('role_name') != 'hero': actor.destroy() + def clear_static_vehicle(world): - """ - 清理静态车辆(空实现,避免依赖缺失导致的报错) - 参数: - world: CARLA的世界对象 - """ pass + def clear(world, camera): - """ - 清理CARLA仿真环境的资源(销毁相机和所有车辆) - 参数: - world: CARLA的世界对象 - camera: 相机传感器对象 - """ - # 销毁相机 if camera: camera.destroy() - # 销毁所有车辆 for actor in world.get_actors().filter('*vehicle*'): actor.destroy() + +# ===================== 【完善】可视化函数 ===================== def draw_bounding_boxes(image, bboxes, labels, class_names, ids): - """ - 在图像上绘制边界框、类别标签和追踪ID - 参数: - image: 输入图像(np.array) - bboxes: 边界框列表,每个框为[x1, y1, x2, y2](左上角+右下角) - labels: 类别索引列表 - class_names: 类别名称列表 - ids: 追踪ID列表 - 返回: - 绘制后的图像 - """ + """优化框的颜色,不同ID使用不同颜色""" + + # 生成固定的颜色映射(基于ID的哈希值) + def get_color(track_id): + np.random.seed(track_id) + return tuple(np.random.randint(0, 255, 3).tolist()) + for bbox, label, track_id in zip(bboxes, labels, ids): - # 将边界框坐标转换为整数 x1, y1, x2, y2 = bbox.astype(int) - # 绘制矩形边界框(绿色,线宽2) - cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2) - # 获取类别名称(默认car) + color = get_color(track_id) + # 绘制框和背景 + cv2.rectangle(image, (x1, y1), (x2, y2), color, 2) class_name = class_names[label] if label < len(class_names) else 'car' - # 拼接标签文本:类别 + 追踪ID - text = f"{class_name} | ID: {track_id}" - # 计算文本尺寸,绘制文本背景(绿色填充) + text = f"ID:{track_id} | {class_name}" text_size = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)[0] - cv2.rectangle(image, (x1, y1 - text_size[1] - 5), (x1 + text_size[0], y1), (0, 255, 0), -1) - # 绘制文本(白色,字体大小0.5,线宽2) + cv2.rectangle(image, (x1, y1 - text_size[1] - 5), (x1 + text_size[0], y1), color, -1) cv2.putText(image, text, (x1, y1 - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) return image -# ===================== 可视化信息绘制函数 ===================== -def draw_info_text(image, speed_kmh, vehicle_count, map_name): - """ - 在图像左上角绘制仿真信息:地图名称、车速、追踪车辆数量 - 参数: - image: 输入图像(np.array) - speed_kmh: 车辆速度(km/h) - vehicle_count: 追踪到的车辆数量 - map_name: 地图名称 - 返回: - 绘制后的图像 - """ - # 复制图像,避免修改原图像 + +def draw_info_text(image, speed_kmh, vehicle_count, map_name, fps): + """新增帧率显示,优化信息排版""" image_copy = image.copy() - # 设置字体参数 font = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.6 font_thickness = 2 - text_color = (255, 255, 255) # 白色文本 - bg_color = (0, 0, 0) # 黑色背景 - padding = 5 # 文本背景的内边距 + text_color = (255, 255, 255) + bg_color = (0, 0, 0) + padding = 5 - # 定义要显示的文本列表 text_list = [ - f"Map: {map_name}", # 地图名称 - f"Speed: {speed_kmh:.1f} km/h", # 车速(保留1位小数) - f"Tracked Vehicles: {vehicle_count}" # 追踪车辆数量 + f"Map: {map_name}", + f"Speed: {speed_kmh:.1f} km/h", + f"Tracked Vehicles: {vehicle_count}", + f"FPS: {fps:.1f}" # 新增帧率 ] - # 初始化文本y轴偏移量 y_offset = 30 for text in text_list: - # 计算文本尺寸 text_size = cv2.getTextSize(text, font, font_scale, font_thickness)[0] - # 绘制文本背景矩形 cv2.rectangle( image_copy, - (10, y_offset - text_size[1] - padding), # 左上角 - (10 + text_size[0] + padding * 2, y_offset + padding), # 右下角 - bg_color, # 背景色 - -1 # 填充矩形 + (10, y_offset - text_size[1] - padding), + (10 + text_size[0] + padding * 2, y_offset + padding), + bg_color, -1 ) - # 绘制文本 - cv2.putText( - image_copy, - text, - (10 + padding, y_offset), # 文本起始位置 - font, - font_scale, - text_color, - font_thickness - ) - # 更新y轴偏移量(换行) + cv2.putText(image_copy, text, (10 + padding, y_offset), font, font_scale, text_color, font_thickness) y_offset += text_size[1] + padding * 3 - return image_copy + def camera_callback(image, rgb_image_queue): - """ - 相机传感器的回调函数,将获取的图像存入队列 - 参数: - image: CARLA的图像对象 - rgb_image_queue: 存储图像的队列 - """ - # 将原始图像数据转换为numpy数组(BGRA格式,尺寸为height×width×4) rgb_image = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4)) - # 将图像存入队列 rgb_image_queue.put(rgb_image) + +# ===================== 【新增】窗口工具函数 ===================== +def init_window(window_name, width, height): + """初始化窗口:置顶、自适应大小、显示提示""" + cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) # 允许调整大小 + cv2.resizeWindow(window_name, width, height) + cv2.setWindowProperty(window_name, cv2.WND_PROP_TOPMOST, 1) # 窗口置顶 + # 显示初始提示文字 + init_img = np.zeros((height, width, 3), dtype=np.uint8) + cv2.putText(init_img, "CARLA DeepSORT Tracking", (width // 4, height // 2), cv2.FONT_HERSHEY_SIMPLEX, 1, + (255, 255, 255), 2) + cv2.imshow(window_name, init_img) + cv2.waitKey(1) + + +def confirm_exit(): + """退出前弹出确认窗口""" + confirm_img = np.zeros((200, 400, 3), dtype=np.uint8) + cv2.putText(confirm_img, "Quit? (Y/N)", (100, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + cv2.imshow("Confirm Exit", confirm_img) + key = cv2.waitKey(0) & 0xFF + cv2.destroyWindow("Confirm Exit") + return key == ord('y') or key == ord('Y') + + +# ===================== 主函数 ===================== def main(): - """ - 主函数:初始化CARLA环境、相机传感器、Deep SORT追踪器,运行主循环实现车辆追踪 - """ - # Part 1: 初始化CARLA环境 - # 连接CARLA服务器(本地地址,端口2000) + # 窗口配置 + WINDOW_NAME = "CARLA 2D Tracking (Enhanced Window)" + CAMERA_WIDTH = 640 + CAMERA_HEIGHT = 480 + + # 初始化CARLA client = carla.Client('localhost', 2000) - # 设置超时时间(10秒) client.set_timeout(10.0) - # 获取CARLA世界对象 world = client.get_world() - # 设置CARLA同步模式(保证仿真步长固定,避免图像和数据不同步) settings = world.get_settings() - settings.synchronous_mode = True # 启用同步模式 - settings.fixed_delta_seconds = 0.05 # 固定步长(20FPS) + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 world.apply_settings(settings) - # 获取旁观者(用于视角跟随) spectator = world.get_spectator() - - # 获取地图的生成点(车辆生成的位置) spawn_points = world.get_map().get_spawn_points() if not spawn_points: print("❌ 无可用生成点!") return - # 生成主角车辆(hero) - # 获取蓝图库 + # 生成自车 bp_lib = world.get_blueprint_library() - # 选择林肯MKZ 2020车型(如果不存在则选择第一个车辆蓝图) - vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') - if not vehicle_bp: - vehicle_bp = bp_lib.filter('vehicle.*')[0] - # 随机选择一个生成点 + vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') or bp_lib.filter('vehicle.*')[0] spawn_point = random.choice(spawn_points) - # 尝试生成车辆 vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) if not vehicle: print("❌ 车辆生成失败!") return - # 生成RGB相机传感器 + # 生成相机 camera_bp = bp_lib.find('sensor.camera.rgb') - # 设置相机参数:图像宽度640,高度480,视场角90度 - camera_bp.set_attribute('image_size_x', '640') - camera_bp.set_attribute('image_size_y', '480') + camera_bp.set_attribute('image_size_x', str(CAMERA_WIDTH)) + camera_bp.set_attribute('image_size_y', str(CAMERA_HEIGHT)) camera_bp.set_attribute('fov', '90') - - # 设置相机挂载位置:车辆前方1.2m,上方2.0m,俯仰角-5度(稍微向下看) camera_init_trans = carla.Transform(carla.Location(x=1.2, z=2.0), carla.Rotation(pitch=-5)) - # 将相机挂载到主角车辆上 camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle) - # 创建图像队列(用于存储相机回调的图像,最大长度2) + # 初始化窗口 + init_window(WINDOW_NAME, CAMERA_WIDTH, CAMERA_HEIGHT) + image_queue = queue.Queue(maxsize=2) - # 注册相机回调函数 camera.listen(lambda image: camera_callback(image, image_queue)) - # 清理现有NPC车辆和静态车辆 clear_npc(world) clear_static_vehicle(world) - # Part 2: 初始化追踪相关参数 - # 3D车辆的边索引(用于生成2D包围盒,此处未实际使用) + # 追踪参数 edges = [[0, 1], [1, 3], [3, 2], [2, 0], [0, 4], [4, 5], [5, 1], [5, 7], [7, 6], [6, 4], [6, 2], [7, 3]] + K = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, 90) + K_b = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, 90, is_behind_camera=True) - # 获取相机参数(宽度、高度、视场角) - image_w = camera_bp.get_attribute("image_size_x").as_int() - image_h = camera_bp.get_attribute("image_size_y").as_int() - fov = camera_bp.get_attribute("fov").as_float() - - # 构建相机内参矩阵(前向和后向) - K = build_projection_matrix(image_w, image_h, fov) - K_b = build_projection_matrix(image_w, image_h, fov, is_behind_camera=True) - - # 生成NPC车辆(目标:20辆) + # 生成NPC npc_count = 20 - spawned_npcs = 0 # 实际生成的NPC数量 + spawned_npcs = 0 for i in range(npc_count): - # 筛选4轮车辆的蓝图 vehicle_bp_list = bp_lib.filter('vehicle') car_bp = [bp for bp in vehicle_bp_list if int(bp.get_attribute('number_of_wheels')) == 4] if not car_bp: continue - # 随机选择生成点 random_spawn = random.choice(spawn_points) - # 避免生成在主角车辆附近(距离小于10m则跳过) if random_spawn.location.distance(vehicle.get_location()) < 10.0: continue - # 尝试生成NPC车辆 npc = world.try_spawn_actor(random.choice(car_bp), random_spawn) if npc: - # 启用自动驾驶 npc.set_autopilot(True) spawned_npcs += 1 print(f"✅ 生成{spawned_npcs}辆NPC车辆") - # 主角车辆启用自动驾驶 vehicle.set_autopilot(True) - # 初始化Deep SORT追踪器 - # 创建特征匹配度量(余弦距离,阈值0.2) + # 初始化追踪器 + encoder = create_box_encoder() metric = nn_matching.NearestNeighborDistanceMetric("cosine", 0.2, None) - # 创建追踪器(使用自定义的编码器) tracker = Tracker(metric) - # 初始化边界框编码器(替换原有的mars-small128编码器) - encoder = create_box_encoder("mars-small128.pb", batch_size=32) - - # 获取地图名称(提取最后一部分) map_name = world.get_map().name.split('/')[-1] - # 主循环(车辆追踪和可视化) + # 帧率计算变量 + frame_count = 0 + start_time = time.time() + fps = 0.0 + + # 主循环 while True: try: - # 推进仿真一步(同步模式必须调用) world.tick() + frame_count += 1 - # 移动旁观者视角:车辆后方4m,上方50m,俯视视角(yaw=-180,pitch=-90) + # 计算帧率(每10帧更新一次) + if frame_count % 10 == 0: + end_time = time.time() + fps = 10 / (end_time - start_time) + start_time = end_time + + # 旁观者视角 transform = carla.Transform( vehicle.get_transform().transform(carla.Location(x=-4, z=50)), carla.Rotation(yaw=-180, pitch=-90) ) spectator.set_transform(transform) - # 获取相机图像(队列为空则跳过) + # 获取图像 if image_queue.empty(): continue image = image_queue.get() - - # 图像预处理:BGRA→BGR(去除Alpha通道)→水平翻转(解决图像反向问题) image = cv2.cvtColor(image, cv2.COLOR_BGRA2BGR) image = cv2.flip(image, 1) - # 更新世界坐标到相机坐标的变换矩阵 + # 3D转2D检测框 world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) - - # 检测车辆:遍历所有车辆,生成2D边界框 boxes = [] for npc in world.get_actors().filter('*vehicle*'): - # 跳过主角车辆 if npc.id != vehicle.id: - # 获取车辆的包围盒 bb = npc.bounding_box - # 计算车辆与主角的距离(只处理50m内的车辆) dist = npc.get_transform().location.distance(vehicle.get_transform().location) if dist < 50: - # 筛选主角车辆前方的车辆(点积>0表示同向) forward_vec = vehicle.get_transform().get_forward_vector() ray = npc.get_transform().location - vehicle.get_transform().location if forward_vec.dot(ray) > 0: - # 获取车辆包围盒的所有3D顶点(世界坐标) verts = [v for v in bb.get_world_vertices(npc.get_transform())] points_2d = [] for vert in verts: - # 判断顶点是否在相机前方(点积>0) ray0 = vert - camera.get_transform().location cam_forward_vec = camera.get_transform().get_forward_vector() - if (cam_forward_vec.dot(ray0) > 0): - # 相机前方的点:使用前向内参矩阵投影 - p = get_image_point(vert, K, world_2_camera) - else: - # 相机后方的点:使用后向内参矩阵投影 - p = get_image_point(vert, K_b, world_2_camera) - # 水平翻转x坐标(匹配图像翻转) - p = (image_w - p[0], p[1]) + p = get_image_point(vert, K, world_2_camera) if cam_forward_vec.dot( + ray0) > 0 else get_image_point(vert, K_b, world_2_camera) + p = (CAMERA_WIDTH - p[0], p[1]) points_2d.append(p) - - # 从3D顶点的2D投影生成2D边界框 - x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges( - points_2d, edges, image_h, image_w) - - # 过滤小框(面积>100,宽度>20,避免噪声) + x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges(points_2d, edges, CAMERA_HEIGHT, + CAMERA_WIDTH) if (y_max - y_min) * (x_max - x_min) > 100 and (x_max - x_min) > 20: - # 确保边界框在画布内 - if point_in_canvas((x_min, y_min), image_h, image_w) and point_in_canvas((x_max, y_max), image_h, image_w): - # 存储边界框(x1, y1, x2, y2) + if point_in_canvas((x_min, y_min), CAMERA_HEIGHT, CAMERA_WIDTH) and point_in_canvas( + (x_max, y_max), CAMERA_HEIGHT, CAMERA_WIDTH): boxes.append(np.array([x_min, y_min, x_max, y_max])) - # 将边界框列表转换为numpy数组 boxes = np.array(boxes) - - # 初始化检测结果列表 detections = [] if len(boxes) > 0: - # 复制边界框,转换为Deep SORT格式(x1, y1, w, h) sort_boxes = boxes.copy() for i, box in enumerate(sort_boxes): - # 计算宽度和高度(替换x2, y2) box[2] -= box[0] box[3] -= box[1] - # 提取边界框的特征(128维) feature = encoder(image, box.reshape(1, -1).copy()) - # 创建Detection对象(边界框,置信度1.0,特征) detections.append(Detection(box, 1.0, feature[0])) - # 更新Deep SORT追踪器 - tracker.predict() # 预测追踪框的位置 - tracker.update(detections) # 根据检测结果更新追踪器 + # 更新追踪器 + tracker.predict() + tracker.update(detections) - # 提取追踪结果(确认的追踪框,更新时间<=1) - bboxes = [] - ids = [] + # 绘制结果 + bboxes, ids = [], [] for track in tracker.tracks: - if not track.is_confirmed() or track.time_since_update > 1: - continue - # 转换为tlbr格式(top, left, bottom, right) - bbox = track.to_tlbr() - bboxes.append(bbox) - ids.append(track.track_id) - - # 转换为numpy数组,获取追踪车辆数量 + if track.is_confirmed() and track.time_since_update <= 1: + bboxes.append(track.to_tlbr()) + ids.append(track.track_id) bboxes = np.array(bboxes) tracked_vehicle_count = len(bboxes) - # 在图像上绘制边界框和追踪ID if len(bboxes) > 0: - # 类别标签:全部设为2(car) labels = np.array([2] * len(bboxes)) image = draw_bounding_boxes(image, bboxes, labels, COCO_CLASS_NAMES, ids) - # 计算主角车辆的速度(m/s → km/h,乘以3.6) + # 绘制信息(含帧率) velocity = vehicle.get_velocity() - speed_ms = math.hypot(velocity.x, velocity.y) # 计算xy平面的速度大小 + speed_ms = math.hypot(velocity.x, velocity.y) speed_kmh = speed_ms * 3.6 - # 绘制仿真信息(地图名称、车速、追踪数量) - image = draw_info_text(image, speed_kmh, tracked_vehicle_count, map_name) - - # 显示图像窗口 - cv2.imshow('2D Ground Truth Deep SORT (Fixed All Issues)', image) - - # 按下q键退出循环 - if cv2.waitKey(1) & 0xFF == ord('q'): + image = draw_info_text(image, speed_kmh, tracked_vehicle_count, map_name, fps) + + # 显示图像 + cv2.imshow(WINDOW_NAME, image) + + # 按键处理(完善退出逻辑) + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + if confirm_exit(): # 确认退出 + break + elif key == ord('f'): # F键切换全屏 + current_flag = cv2.getWindowProperty(WINDOW_NAME, cv2.WND_PROP_FULLSCREEN) + new_flag = cv2.WINDOW_FULLSCREEN if current_flag == 0 else cv2.WINDOW_NORMAL + cv2.setWindowProperty(WINDOW_NAME, cv2.WND_PROP_FULLSCREEN, new_flag) + elif key == ord('s'): # S键保存当前帧 + save_path = f"track_frame_{frame_count}.png" + cv2.imwrite(save_path, image) + print(f"💾 帧已保存至 {save_path}") + + except KeyboardInterrupt: + if confirm_exit(): break - - except KeyboardInterrupt as e: - # 捕获键盘中断(Ctrl+C),退出循环 - break except Exception as e: - # 捕获其他异常,打印错误信息并继续循环 print(f"⚠️ 运行错误:{e}") continue # 清理资源 clear(world, camera) - # 关闭同步模式 - settings = world.get_settings() settings.synchronous_mode = False world.apply_settings(settings) - # 关闭所有OpenCV窗口 cv2.destroyAllWindows() print("✅ 程序正常退出") -# 程序入口 + if __name__ == '__main__': main() \ No newline at end of file