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 4ac5299d2392303f238a97eead1bd7a08489d831 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Sun, 21 Dec 2025 16:59:56 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/car1.py | 561 +++++++++++++++++++++++---------------- 1 file changed, 335 insertions(+), 226 deletions(-) diff --git a/src/car_tracking/car1.py b/src/car_tracking/car1.py index 6298523f1c..1f7a2aff0c 100644 --- a/src/car_tracking/car1.py +++ b/src/car_tracking/car1.py @@ -3,263 +3,372 @@ import random import cv2 import numpy as np - -# 注意:移除了所有deep_sort相关导入 -# 保留必要的工具函数导入(如果utils模块不存在,我会替换为内置实现) -from utils.projection import build_projection_matrix, get_image_point, get_2d_box_from_3d_edges, point_in_canvas -from utils.world import clear_npc, clear_static_vehicle - -# --------------------------- -# 全局配置(可根据需要调整) -# --------------------------- -MAP_NAME = "Town07" # 指定要加载的地图,可替换为Town01-Town12 -NPC_VEHICLE_NUM = 50 # NPC车辆数量 -CAMERA_RESOLUTION = (640, 640) # 相机分辨率 -SYNC_DELTA_SECONDS = 0.05 # 同步模式的固定时间步长 -VEHICLE_DISTANCE_THRESHOLD = 50 # 检测车辆的距离阈值(米) -SPEED_DISPLAY_UNIT = "km/h" # 速度单位:m/s 或 km/h - -# --------------------------- -# 替代draw_bounding_boxes的内置函数(如果utils.box_utils不存在) -# 如果你有utils.box_utils,可注释这部分,保留原导入 -# --------------------------- -def draw_bounding_boxes(image, bboxes, labels, class_names, ids): +import time # 用于计算FPS +from collections import deque # 用于平滑FPS计算 + +# 替换为你实际的导入路径,如果没有这些模块,先实现基础版本 +# 这里先实现缺失的工具函数,避免代码报错 +# ---------------------- 缺失工具函数的基础实现 ---------------------- +COCO_CLASS_NAMES = [ + 'person', 'bicycle', 'car', 'motorcycle', 'airplane', 'bus', + 'train', 'truck', 'boat', 'traffic light', 'fire hydrant', + 'stop sign', 'parking meter', 'bench', 'bird', 'cat', 'dog', + 'horse', 'sheep', 'cow', 'elephant', 'bear', 'zebra', 'giraffe', + 'backpack', 'umbrella', 'handbag', 'tie', 'suitcase', 'frisbee', + 'skis', 'snowboard', 'sports ball', 'kite', 'baseball bat', + 'baseball glove', 'skateboard', 'surfboard', 'tennis racket', + 'bottle', 'wine glass', 'cup', 'fork', 'knife', 'spoon', 'bowl', + 'banana', 'apple', 'sandwich', 'orange', 'broccoli', 'carrot', + 'hot dog', 'pizza', 'donut', 'cake', 'chair', 'couch', + 'potted plant', 'bed', 'dining table', 'toilet', 'tv', 'laptop', + 'mouse', 'remote', 'keyboard', 'cell phone', 'microwave', 'oven', + 'toaster', 'sink', 'refrigerator', 'book', 'clock', 'vase', + 'scissors', 'teddy bear', 'hair drier', 'toothbrush' +] + +def draw_bounding_boxes(image, boxes, labels, class_names, ids=None): """ - 绘制包围盒和ID标签 - :param image: 输入图像(BGR格式) - :param bboxes: 包围盒列表,格式为[[x1,y1,x2,y2], ...] - :param labels: 类别标签列表 - :param class_names: 类别名称列表 - :param ids: 跟踪ID列表 - :return: 绘制后的图像 + 绘制边界框(基础实现) """ - for i, (bbox, id) in enumerate(zip(bboxes, ids)): - x1, y1, x2, y2 = bbox.astype(int) + img = image.copy() + # 转换为BGR格式(Carla的图像是RGBA,OpenCV是BGR) + img = img[:, :, :3] + img = cv2.cvtColor(img, cv2.COLOR_RGBA2BGR) + for i, box in enumerate(boxes): + x1, y1, x2, y2 = map(int, box) + label = class_names[labels[i]] if labels[i] < len(class_names) else 'unknown' + id_text = f'ID: {ids[i]}' if ids is not None else '' # 绘制矩形框 - cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2) - # 绘制标签(类别 + ID) - label = f"{class_names[labels[i]]} | ID: {id}" - cv2.putText(image, label, (x1, y1 - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) - return image + cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 0), 2) + # 绘制标签和ID + cv2.putText(img, f'{label} {id_text}', (x1, y1 - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) + return img + +def build_projection_matrix(w, h, fov, is_behind_camera=False): + """ + 构建相机投影矩阵(基础实现) + """ + focal = w / (2.0 * np.tan(fov * np.pi / 360.0)) + K = np.identity(3) + K[0, 0] = K[1, 1] = focal + K[0, 2] = w / 2.0 + K[1, 2] = h / 2.0 + if is_behind_camera: + K[1, 1] = -K[1, 1] # 翻转y轴 + return K + +def get_image_point(world_point, K, world_to_camera): + """ + 将3D世界坐标转换为2D图像坐标 + """ + # 将Carla Location转换为齐次坐标 + point = np.array([world_point.x, world_point.y, world_point.z, 1]) + # 转换到相机坐标系 + camera_point = np.dot(world_to_camera, point) + # 转换到图像坐标系(归一化) + image_point = np.dot(K, camera_point[:3]) + # 透视除法 + image_point = image_point / image_point[2] + return np.array([image_point[0], image_point[1]]) + +def get_2d_box_from_3d_edges(points_2d, edges, h, w): + """ + 从3D边缘的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)) + x_max = min(w, max(x_coords)) + y_min = max(0, min(y_coords)) + y_max = min(h, max(y_coords)) + return x_min, x_max, y_min, y_max + +def point_in_canvas(point, h, w): + """ + 判断点是否在图像画布内 + """ + x, y = point + return 0 <= x <= w and 0 <= y <= h + +def clear_npc(world): + """ + 清除现有NPC车辆 + """ + for actor in world.get_actors().filter('*vehicle*'): + actor.destroy() + +def clear_static_vehicle(world): + """ + 清除静态车辆(与clear_npc功能重复,这里简化) + """ + pass + +def clear(world, *actors): + """ + 清除指定的actor + """ + settings = world.get_settings() + settings.synchronous_mode = False # 关闭同步模式 + world.apply_settings(settings) + for actor in actors: + if actor.is_alive: + actor.destroy() + # 清除剩余的NPC + for actor in world.get_actors().filter('*vehicle*'): + actor.destroy() +# ---------------------- 工具函数结束 ---------------------- def camera_callback(image, rgb_image_queue): - """相机回调函数,将图像数据存入队列""" + """ + 相机回调函数,将图像数据存入队列 + """ rgb_image_queue.put(np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))) -def calculate_vehicle_speed(velocity, unit="km/h"): - """计算车辆速度,转换为指定单位""" - speed_m_s = np.linalg.norm([velocity.x, velocity.y, velocity.z]) - if unit == "km/h": - return speed_m_s * 3.6 # 1 m/s = 3.6 km/h - return speed_m_s +def get_vehicle_speed(vehicle): + """ + 获取车辆的速度(km/h) + """ + velocity = vehicle.get_velocity() + speed = 3.6 * np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + return speed def main(): - # --------------------------- - # 1. 初始化Carla客户端,加快地图加载 - # --------------------------- - try: - client = carla.Client('localhost', 2000) - client.set_timeout(30.0) # 增加超时时间,避免地图加载时超时 - - # 方案3:查看所有可用地图 - print("Carla支持的地图列表:") - available_maps = client.get_available_maps() - print(available_maps) - - # 方案2:使用当前已加载的地图,避免map not found - world = client.get_world() - # 如果想指定地图,可从available_maps中选一个,比如: - # world = client.load_world("Town01") - - spawn_points = world.get_map().get_spawn_points() - if not spawn_points: - print("警告:未获取到地图生成点,程序将退出") - return - # 后续代码不变... - - # --------------------------- - # 2. 设置同步模式,优化仿真效率 - # --------------------------- - settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = SYNC_DELTA_SECONDS - world.apply_settings(settings) - - # --------------------------- - # 3. 生成Ego车辆 - # --------------------------- - bp_lib = world.get_blueprint_library() - vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') - ego_vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) - if not ego_vehicle: - print("警告:未能生成Ego车辆,程序将退出") - return - - # --------------------------- - # 4. 生成相机传感器 - # --------------------------- - camera_bp = bp_lib.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', str(CAMERA_RESOLUTION[0])) - camera_bp.set_attribute('image_size_y', str(CAMERA_RESOLUTION[1])) - camera_init_trans = carla.Transform(carla.Location(x=1, z=2)) - camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=ego_vehicle) - - # 图像队列(设置最大长度,避免内存溢出) - image_queue = queue.Queue(maxsize=1) - camera.listen(lambda image: camera_callback(image, image_queue)) - - # --------------------------- - # 5. 清理现有NPC和静态车辆 - # --------------------------- - clear_npc(world) - clear_static_vehicle(world) - - # --------------------------- - # 6. 预计算相机相关参数 - # --------------------------- - 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, image_h = CAMERA_RESOLUTION - 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) - - # --------------------------- - # 7. 生成NPC车辆 - # --------------------------- + # 初始化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() + + # 生成ego车辆 + bp_lib = world.get_blueprint_library() + vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') + vehicle = None + # 循环尝试生成,避免生成失败 + while not vehicle: + vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) + + # 生成相机 + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '640') + camera_bp.set_attribute('image_size_y', '640') + camera_init_trans = carla.Transform(carla.Location(x=1, z=2)) + camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle) + + # 图像队列 + image_queue = queue.Queue() + camera.listen(lambda image: camera_callback(image, image_queue)) + + # 清除现有NPC + clear_npc(world) + clear_static_vehicle(world) + + # 3D边界框的边缘对 + 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) + + # 生成50个NPC车辆 + for i in range(50): car_bp = [bp for bp in bp_lib.filter('vehicle') if int(bp.get_attribute('number_of_wheels')) == 4] if car_bp: - for i in range(NPC_VEHICLE_NUM): - npc = world.try_spawn_actor(random.choice(car_bp), random.choice(spawn_points)) - if npc: - npc.set_autopilot(True) - - ego_vehicle.set_autopilot(True) - - # --------------------------- - # 8. 主循环(核心:用Actor ID实现跟踪) - # --------------------------- + npc = world.try_spawn_actor(random.choice(car_bp), random.choice(spawn_points)) + if npc: + npc.set_autopilot(True) + vehicle.set_autopilot(True) + + # ---------------------- 修复:天气预设(纯英文名称,解决问号问题) ---------------------- + # 1. 先打印当前版本支持的天气预设(可选,用于调试) + weather_attributes = [attr for attr in dir(carla.WeatherParameters) if not attr.startswith('_')] + print("="*50) + print("Current Carla Weather Presets:", weather_attributes) + print("="*50) + + # 2. 步骤1:定义仅使用你版本支持的**预设常量**,纯英文名称 + preset_weathers = [ + (carla.WeatherParameters.ClearNoon, "ClearNoon"), + (carla.WeatherParameters.CloudyNoon, "CloudyNoon"), + (carla.WeatherParameters.WetNoon, "WetNoon"), + (carla.WeatherParameters.WetCloudyNoon, "WetCloudyNoon"), + (carla.WeatherParameters.MidRainyNoon, "MidRainyNoon"), + (carla.WeatherParameters.HardRainNoon, "HardRainNoon"), + (carla.WeatherParameters.SoftRainNoon, "SoftRainNoon"), + (carla.WeatherParameters.ClearSunset, "ClearSunset"), + (carla.WeatherParameters.CloudySunset, "CloudySunset"), + (carla.WeatherParameters.WetSunset, "WetSunset") + ] + + # 3. 步骤2:创建自定义雾天天气(调优参数,雾天效果更明显) + foggy_weather = carla.WeatherParameters() + # 核心雾天参数(调优后) + foggy_weather.fog_density = 0.95 # 雾浓度(0-1,调至接近1,雾更浓) + foggy_weather.fog_distance = 5.0 # 雾的起始距离(越小,雾离相机越近) + foggy_weather.fog_falloff = 0.1 # 雾的衰减率(越小,雾的范围越大,更均匀) + # 辅助参数,增强雾天视觉效果 + foggy_weather.sun_altitude_angle = 10.0 # 太阳高度角(越小,光线越暗,雾更明显) + foggy_weather.cloudiness = 0.9 # 云量(越大,天空越暗,雾的对比越强) + foggy_weather.scattering_intensity = 0.1 # 散射强度(越小,光线越弱) + foggy_weather.wetness = 0.1 # 地面湿度(降低,避免和雨天混淆) + foggy_weather.precipitation = 0.0 # 关闭降水,纯雾天 + # 自定义雾天名称(纯英文) + foggy_weather_name = "CustomFoggyNoon (Heavy Fog)" + + # 4. 步骤3:合并预设和自定义天气,组成最终的天气列表(元组:天气对象,天气名称) + weather_presets = [(wp, name) for wp, name in preset_weathers] + [(foggy_weather, foggy_weather_name)] + random.shuffle(weather_presets) # 随机打乱天气顺序,切换更自然 + + current_weather_idx = 0 + # 初始设置天气 + initial_wp, initial_name = weather_presets[current_weather_idx] + world.set_weather(initial_wp) + print(f"Initial Weather: {initial_name}") + weather_switch_interval = 10.0 # 每10秒切换一次天气 + last_weather_switch = time.time() + + # ---------------------- FPS计算初始化 ---------------------- + frame_times = deque(maxlen=10) + last_frame_time = time.time() + # ---------------------- 初始化结束 ---------------------- + + # 主循环 + try: while True: world.tick() + current_time = time.time() # 移动spectator到车辆上方 - spectator = world.get_spectator() - transform = carla.Transform(ego_vehicle.get_transform().transform( + transform = carla.Transform(vehicle.get_transform().transform( carla.Location(x=-4, z=50)), carla.Rotation(yaw=-180, pitch=-90)) spectator.set_transform(transform) - # 获取相机图像 - if not image_queue.empty(): - image = image_queue.get() - else: - continue - - # 图像预处理(RGBA转BGR,减少一次颜色转换) - image = cv2.cvtColor(image, cv2.COLOR_RGBA2BGR) + # 获取图像 + image = image_queue.get() # 更新相机的世界到相机矩阵 world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) - # 存储:包围盒、车辆ID、速度 - bboxes = [] - vehicle_ids = [] - vehicle_speeds = [] - - # 遍历所有车辆Actor + # 检测车辆并计算2D边界框 + boxes = [] + ids = [] vehicles = world.get_actors().filter('*vehicle*') for npc in vehicles: - if npc.id == ego_vehicle.id: - # 输出Ego车辆速度 - ego_speed = calculate_vehicle_speed(npc.get_velocity(), SPEED_DISPLAY_UNIT) - print(f"Ego车辆速度:{ego_speed:.1f} {SPEED_DISPLAY_UNIT}") - continue - - # 过滤距离过远的车辆 - dist = npc.get_transform().location.distance(ego_vehicle.get_transform().location) - if dist > VEHICLE_DISTANCE_THRESHOLD: - continue - - # 只检测车辆前方的目标 - forward_vec = ego_vehicle.get_transform().get_forward_vector() - ray = npc.get_transform().location - ego_vehicle.get_transform().location - if forward_vec.dot(ray) <= 0: - continue - - # 计算NPC车辆速度 - npc_speed = calculate_vehicle_speed(npc.get_velocity(), SPEED_DISPLAY_UNIT) - - # 计算3D包围盒的2D投影 - bb = npc.bounding_box - verts = [v for v in bb.get_world_vertices(npc.get_transform())] - points_2d = [] - - 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) - points_2d.append(p) - - # 获取2D框 - x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w) - - # 过滤过小的框和超出画布的框 - box_area = (y_max - y_min) * (x_max - x_min) - box_width = x_max - x_min - if box_area > 100 and box_width > 20 and point_in_canvas((x_min, y_min), image_h, image_w) and point_in_canvas((x_max, y_max), image_h, image_w): - bboxes.append(np.array([x_min, y_min, x_max, y_max])) - vehicle_ids.append(npc.id) # 用Actor ID作为跟踪ID - vehicle_speeds.append(npc_speed) - - # --------------------------- - # 绘制包围盒、ID和速度(核心替代deep_sort的部分) - # --------------------------- - if len(bboxes) > 0: - # 绘制基础包围盒和ID(类别固定为car,对应COCO的2号类别) - labels = np.array([2] * len(bboxes)) - COCO_CLASS_NAMES = {2: 'car'} # 简化的类别映射,替代原导入 - image = draw_bounding_boxes(image, bboxes, labels, COCO_CLASS_NAMES, vehicle_ids) - - # 绘制速度标签 - for i, (bbox, speed) in enumerate(zip(bboxes, vehicle_speeds)): - x1, y1, x2, y2 = bbox.astype(int) - speed_label = f"Speed: {speed:.1f} {SPEED_DISPLAY_UNIT}" - # 在框下方绘制速度 - cv2.putText(image, speed_label, (x1, y2 + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) + if npc.id != vehicle.id: + bb = npc.bounding_box + dist = npc.get_transform().location.distance(vehicle.get_transform().location) + if dist < 50: + forward_vec = vehicle.get_transform().get_forward_vector() + ray = npc.get_transform().location - vehicle.get_transform().location + if forward_vec.dot(ray) > 0: + verts = [v for v in bb.get_world_vertices(npc.get_transform())] + points_2d = [] + 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) + 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) + 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): + ids.append(npc.id) + boxes.append(np.array([x_min, y_min, x_max, y_max])) + + # 绘制边界框 + boxes = np.array(boxes) + labels = np.array([2] * len(boxes)) # 2: car + probs = np.array([1.0] * len(boxes)) + if len(boxes) > 0: + output_image = draw_bounding_boxes(image, boxes, labels, COCO_CLASS_NAMES, ids) + else: + # 没有边界框时,直接转换图像格式 + output_image = image[:, :, :3] + output_image = cv2.cvtColor(output_image, cv2.COLOR_RGBA2BGR) + + # ---------------------- 实时天气切换(纯英文名称打印) ---------------------- + if current_time - last_weather_switch > weather_switch_interval: + current_weather_idx = (current_weather_idx + 1) % len(weather_presets) + current_wp, current_name = weather_presets[current_weather_idx] + world.set_weather(current_wp) + last_weather_switch = current_time + # 醒目打印天气切换信息 + print("-"*50) + print(f"Weather Switched to: {current_name}") + print("-"*50) + # ---------------------- 天气切换结束 ---------------------- + + # ---------------------- 实时统计面板绘制(纯英文,无问号) ---------------------- + # 1. 计算FPS + frame_time = current_time - last_frame_time + frame_times.append(frame_time) + fps = 1.0 / np.mean(frame_times) if frame_times else 0.0 + last_frame_time = current_time + + # 2. 统计数据 + total_vehicles = len(vehicles) # 总车辆数 + detected_vehicles = len(boxes) # 检测到的车辆数 + ego_speed = get_vehicle_speed(vehicle) # ego车辆速度(km/h) + # 获取当前天气名称(纯英文) + current_wp, current_weather = weather_presets[current_weather_idx] + + # 3. 调整面板大小(纯英文无需太宽) + panel_x = 10 + panel_y = 10 + panel_width = 380 # 从450调回380,适配纯英文 + panel_height = 160 + # 绘制半透明矩形背景 + overlay = output_image.copy() + cv2.rectangle(overlay, (panel_x, panel_y), (panel_x + panel_width, panel_y + panel_height), (0, 0, 0), -1) + alpha = 0.6 # 透明度 + cv2.addWeighted(overlay, alpha, output_image, 1 - alpha, 0, output_image) + + # 绘制统计文本(纯英文,字体清晰) + text_y_offset = 30 + text_params = (cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) # 字体、大小、颜色、粗细 + cv2.putText(output_image, f"FPS: {fps:.1f}", (panel_x + 15, panel_y + text_y_offset), *text_params) + cv2.putText(output_image, f"Total Vehicles: {total_vehicles}", (panel_x + 15, panel_y + text_y_offset * 2), *text_params) + cv2.putText(output_image, f"Detected Vehicles: {detected_vehicles}", (panel_x + 15, panel_y + text_y_offset * 3), *text_params) + cv2.putText(output_image, f"Ego Speed: {ego_speed:.1f} km/h", (panel_x + 15, panel_y + text_y_offset * 4), *text_params) + # 处理稍长的天气名称(如CustomFoggyNoon (Heavy Fog)) + if len(current_weather) > 22: + weather_part1 = current_weather[:22] + weather_part2 = current_weather[22:] + cv2.putText(output_image, f"Weather: {weather_part1}", (panel_x + 15, panel_y + text_y_offset * 5), *text_params) + cv2.putText(output_image, f" {weather_part2}", (panel_x + 15, panel_y + text_y_offset * 6), *text_params) + else: + cv2.putText(output_image, f"Weather: {current_weather}", (panel_x + 15, panel_y + text_y_offset * 5), *text_params) + # ---------------------- 统计面板绘制结束 ---------------------- # 显示图像 - cv2.imshow('2D Tracking with Actor ID (no deep_sort)', image) - - # 按q退出 - if cv2.waitKey(1) & 0xFF == ord('q'): + cv2.imshow('2D Ground Truth (with Stats & Weather)', output_image) + if cv2.waitKey(1) == ord('q'): break except KeyboardInterrupt: - print("\n程序被用户中断") - except Exception as e: - print(f"程序出错:{e}") + print("\nUser Interrupted the Program") finally: - # --------------------------- - # 资源清理 - # --------------------------- - print("正在清理资源...") - if 'world' in locals(): - settings = world.get_settings() - settings.synchronous_mode = False - world.apply_settings(settings) - if 'camera' in locals(): - camera.destroy() - if 'ego_vehicle' in locals(): - ego_vehicle.destroy() - # 清理所有NPC车辆 - if 'world' in locals(): - for vehicle in world.get_actors().filter('*vehicle*'): - vehicle.destroy() + # 清理资源 + clear(world, camera, vehicle) cv2.destroyAllWindows() - print("资源清理完成") + print("Program Ended, Resources Cleared") -if __name__ == "__main__": +if __name__ == '__main__': main() \ No newline at end of file