From 555bae867fbcaae0d5ae873905647c62691a87f4 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Sun, 21 Dec 2025 14:35:09 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E4=BC=98=E5=8C=96=E7=AA=97=E5=8F=A3?= =?UTF-8?q?=E7=95=8C=E9=9D=A2=EF=BC=8C=E9=A9=BE=E9=A9=B6=E6=9B=B4=E5=8A=A0?= =?UTF-8?q?=E6=B5=81=E7=95=85=EF=BC=8C=E5=AE=9E=E7=8E=B0=E5=AE=9E=E6=97=B6?= =?UTF-8?q?=E6=80=A7=E8=83=BD=E7=9B=91=E6=8E=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/main.py | 268 +++++++++++++++++++++------------------ 1 file changed, 146 insertions(+), 122 deletions(-) diff --git a/src/car_tracking/main.py b/src/car_tracking/main.py index 0f5e36de6b..15c50f9115 100644 --- a/src/car_tracking/main.py +++ b/src/car_tracking/main.py @@ -4,14 +4,16 @@ import cv2 import numpy as np import math -import os # 新增:处理文件路径 +import os +import time # 新增:计算帧率 + # 修复Deep SORT的API弃用问题 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不存在,手动定义(避免依赖缺失) + +# COCO类别名称 COCO_CLASS_NAMES = [ 'person', 'bicycle', 'car', 'motorcycle', 'airplane', 'bus', 'train', 'truck', 'boat', 'traffic light', 'fire hydrant', @@ -29,54 +31,49 @@ 'scissors', 'teddy bear', 'hair drier', 'toothbrush' ] -# ===================== 修复:替换Deep SORT的linear_assignment(解决弃用警告) ===================== + +# ===================== Deep SORT修复函数 ===================== def linear_assignment(cost_matrix): - """替换原有的linear_assignment,使用scipy的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文件读取错误) ===================== -# 由于mars-small128.pb读取失败,改用简单的特征编码(不依赖TensorFlow,保证运行) + class SimpleBoxEncoder: def __init__(self): pass + def __call__(self, image, boxes): - """生成简单的框特征(替代原有的mars-small128编码,保证追踪功能运行)""" 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]) - feature = np.array([aspect_ratio, center_x, center_y, area] + [0.0]*124) # 凑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,返回简单编码器""" return SimpleBoxEncoder() -# ===================== 自定义工具函数(替代utils中的函数,避免依赖缺失) ===================== + +# ===================== 工具函数 ===================== def get_image_point(vertex, K, world_to_camera): - """3D点投影到2D图像平面(简化版)""" - # 3D点(齐次坐标) point_3d = np.array([vertex.x, vertex.y, vertex.z, 1.0]) - # 世界到相机 point_camera = np.dot(world_to_camera, point_3d) - # 相机到图像(投影矩阵K) point_img = np.dot(K, point_camera[:3]) - # 归一化 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边界框""" 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)) @@ -85,13 +82,13 @@ def get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w): 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): - """判断点是否在画布内""" x, y = point return 0 <= x <= image_w and 0 <= y <= image_h + def build_projection_matrix(w, h, fov, is_behind_camera=False): - """构建投影矩阵(简化版)""" focal = w / (2.0 * math.tan(fov * math.pi / 360.0)) K = np.identity(3) K[0, 0] = K[1, 1] = focal @@ -101,41 +98,48 @@ def build_projection_matrix(w, h, fov, is_behind_camera=False): K[0, 0] = -K[0, 0] return K + def clear_npc(world): - """清理NPC车辆""" for actor in world.get_actors().filter('*vehicle*'): if actor.attributes.get('role_name') != 'hero': actor.destroy() + def clear_static_vehicle(world): - """清理静态车辆(空实现,避免报错)""" pass + def clear(world, 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使用不同颜色""" + + # 生成固定的颜色映射(基于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) - # 绘制框 - cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2) - # 绘制标签和ID + 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' - 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) + 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): - """在图像上绘制车速、车辆数量、地图名称等信息""" + +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 @@ -147,7 +151,8 @@ def draw_info_text(image, speed_kmh, vehicle_count, map_name): text_list = [ f"Map: {map_name}", f"Speed: {speed_kmh:.1f} km/h", - f"Tracked Vehicles: {vehicle_count}" + f"Tracked Vehicles: {vehicle_count}", + f"FPS: {fps:.1f}" # 新增帧率 ] y_offset = 30 @@ -157,43 +162,60 @@ def draw_info_text(image, speed_kmh, vehicle_count, map_name): image_copy, (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 + bg_color, -1 ) + 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): - """摄像头回调函数""" - rgb_image_queue.put(np.reshape(np.copy(image.raw_data), - (image.height, image.width, 4))) +def camera_callback(image, rgb_image_queue): + 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(): - # Part 1: 初始化CARLA环境 + # 窗口配置 + WINDOW_NAME = "CARLA 2D Tracking (Enhanced Window)" + CAMERA_WIDTH = 640 + CAMERA_HEIGHT = 480 + + # 初始化CARLA client = carla.Client('localhost', 2000) client.set_timeout(10.0) world = client.get_world() - # 设置同步模式 settings = world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = 0.05 world.apply_settings(settings) - # 获取 spectator spectator = world.get_spectator() - - # 获取生成点 spawn_points = world.get_map().get_spawn_points() if not spawn_points: print("❌ 无可用生成点!") @@ -201,52 +223,42 @@ def main(): # 生成自车 bp_lib = world.get_blueprint_library() - 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 - # 生成摄像头 + # 生成相机 camera_bp = bp_lib.find('sensor.camera.rgb') - 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') - 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) - # 图像队列 + # 初始化窗口 + 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: 初始化追踪参数 + # 追踪参数 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) + K = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, 90) + K_b = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, 90, is_behind_camera=True) # 生成NPC npc_count = 20 spawned_npcs = 0 for i in range(npc_count): 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) @@ -260,36 +272,45 @@ def main(): vehicle.set_autopilot(True) - # Deep SORT(使用修复后的编码器) - encoder = create_box_encoder("mars-small128.pb", batch_size=32) + # 初始化追踪器 + encoder = create_box_encoder() metric = nn_matching.NearestNeighborDistanceMetric("cosine", 0.2, None) tracker = Tracker(metric) - - # 地图名称 map_name = world.get_map().name.split('/')[-1] + # 帧率计算变量 + frame_count = 0 + start_time = time.time() + fps = 0.0 + # 主循环 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)) + frame_count += 1 + + # 计算帧率(每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→水平翻转(解决反向) 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()) - boxes = [] for npc in world.get_actors().filter('*vehicle*'): if npc.id != vehicle.id: @@ -304,23 +325,18 @@ def main(): for vert in verts: 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) - - x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges( - points_2d, edges, image_h, image_w) - + 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): + 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])) boxes = np.array(boxes) - detections = [] if len(boxes) > 0: sort_boxes = boxes.copy() @@ -334,48 +350,56 @@ def main(): tracker.predict() tracker.update(detections) - bboxes = [] - ids = [] + # 绘制结果 + bboxes, ids = [], [] for track in tracker.tracks: - if not track.is_confirmed() or track.time_since_update > 1: - continue - bbox = track.to_tlbr() - bboxes.append(bbox) - ids.append(track.track_id) - + 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) if len(bboxes) > 0: 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) - # 计算车速并绘制信息 + # 绘制信息(含帧率) velocity = vehicle.get_velocity() 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) + image = draw_info_text(image, speed_kmh, tracked_vehicle_count, map_name, fps) # 显示图像 - cv2.imshow('2D Ground Truth Deep SORT (Fixed All Issues)', image) - - if cv2.waitKey(1) & 0xFF == ord('q'): + 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: - break except Exception as e: print(f"⚠️ 运行错误:{e}") continue # 清理资源 clear(world, camera) - settings = world.get_settings() settings.synchronous_mode = False world.apply_settings(settings) cv2.destroyAllWindows() print("✅ 程序正常退出") + if __name__ == '__main__': main() \ No newline at end of file From 3c98ffff00a11225048e0231bc743a48c4edc827 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Sun, 21 Dec 2025 15:33:34 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E5=88=A0=E9=99=A4README.md=E5=86=97?= =?UTF-8?q?=E4=BD=99=E9=83=A8=E5=88=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/README.md | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/car_tracking/README.md b/src/car_tracking/README.md index 214860f1cb..1965362492 100644 --- a/src/car_tracking/README.md +++ b/src/car_tracking/README.md @@ -17,10 +17,4 @@ - 自动生成自车和20辆NPC(自动驾驶) - Deep SORT实时追踪车辆,显示追踪ID和类别 - 实时显示车速、追踪车辆数、地图名称 -- 自动清理资源,避免残留进程 - -## 已修复问题 -1. 图像文字/边界框反向 -2. `mars-small128.pb`编码读取错误 -3. Deep SORT API弃用警告 -4. 外部依赖缺失问题 \ No newline at end of file +- 自动清理资源,避免残留进程 \ No newline at end of file