From fc55c688c7ad1f2789526ef41b65b5fd0ac45de5 Mon Sep 17 00:00:00 2001 From: haoliuaa <2121765139@qq.com> Date: Fri, 19 Dec 2025 16:15:57 +0800 Subject: [PATCH 1/4] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=BB=BF=E6=A1=86?= =?UTF-8?q?=E8=AF=86=E5=88=AB=E4=B8=8D=E5=87=86=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/InterconVis_PedSafety_Enh/cvips_utils.py | 120 +++++------ src/InterconVis_PedSafety_Enh/data_check.py | 203 +++++++------------ 2 files changed, 139 insertions(+), 184 deletions(-) diff --git a/src/InterconVis_PedSafety_Enh/cvips_utils.py b/src/InterconVis_PedSafety_Enh/cvips_utils.py index b5ce442a94..f337145572 100644 --- a/src/InterconVis_PedSafety_Enh/cvips_utils.py +++ b/src/InterconVis_PedSafety_Enh/cvips_utils.py @@ -1,10 +1,11 @@ +# --- START OF FILE cvips_utils.py --- import numpy as np import math import carla def build_projection_matrix(w, h, fov): """ - 构建相机的内参矩阵 (Intrinsic Matrix) + 构建相机内参矩阵 (Intrinsic Matrix) K """ focal = w / (2.0 * np.tan(fov * np.pi / 360.0)) K = np.identity(3) @@ -13,46 +14,20 @@ def build_projection_matrix(w, h, fov): K[1, 2] = h / 2.0 return K -def get_image_point(loc, K, w2c): - """ - 将 3D 世界坐标点 (loc) 投影到 2D 像素坐标 (u, v) - :param loc: carla.Location / carla.Vector3D - :param K: 内参矩阵 (3x3) - :param w2c: 世界到相机的变换矩阵 (4x4) - """ - # 1. 构建齐次坐标 [x, y, z, 1] - point = np.array([loc.x, loc.y, loc.z, 1]) - - # 2. 变换到相机坐标系 (Camera Coordinate) - point_camera = np.dot(w2c, point) - - # 3. 归一化相机平面 (除去深度 Z) [y, -z, x] -> standard [x, y, z] conversion happening inside w2c logic usually - # 但在 CARLA helper 中,我们直接用投影公式: - # 这里的 w2c 实际上已经包含了坐标轴的旋转 (UE4 -> OpenCV) - - # [x,y,z] -> [X_img, Y_img, 1] * depth - point_img = np.dot(K, point_camera[:3]) - - # 4. 归一化获取像素坐标 - pixel = [int(point_img[0] / point_img[2]), int(point_img[1] / point_img[2])] - return pixel - def get_matrix(transform): - """ - 将 CARLA 的 transform 对象转换为 4x4 变换矩阵 - """ - yaw = math.radians(transform.rotation.yaw) - pitch = math.radians(transform.rotation.pitch) - roll = math.radians(transform.rotation.roll) - - c_y = math.cos(yaw) - s_y = math.sin(yaw) - c_p = math.cos(pitch) - s_p = math.sin(pitch) - c_r = math.cos(roll) - s_r = math.sin(roll) - - matrix = np.identity(4) + """标准 CARLA 变换矩阵构建""" + rotation = transform.rotation + location = transform.location + c_y = math.cos(math.radians(rotation.yaw)) + s_y = math.sin(math.radians(rotation.yaw)) + c_r = math.cos(math.radians(rotation.roll)) + s_r = math.sin(math.radians(rotation.roll)) + c_p = math.cos(math.radians(rotation.pitch)) + s_p = math.sin(math.radians(rotation.pitch)) + matrix = np.matrix(np.identity(4)) + matrix[0, 3] = location.x + matrix[1, 3] = location.y + matrix[2, 3] = location.z matrix[0, 0] = c_p * c_y matrix[0, 1] = c_y * s_p * s_r - s_y * c_r matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r @@ -62,28 +37,57 @@ def get_matrix(transform): matrix[2, 0] = s_p matrix[2, 1] = -c_p * s_r matrix[2, 2] = c_p * c_r - matrix[0, 3] = transform.location.x - matrix[1, 3] = transform.location.y - matrix[2, 3] = transform.location.z - return matrix + return np.array(matrix) def build_world_to_camera_matrix(camera_transform): """ - 构建 世界坐标系 -> 相机像素坐标系 的外参矩阵 (Extrinsic Matrix) - 关键:包含坐标轴置换 (UE4 -> Standard CV) + 构建 [世界坐标] -> [相机坐标(OpenCV格式)] 的变换矩阵 (Extrinsic) + 关键修正:UE4 (X前,Y右,Z上) -> OpenCV (Z前,X右,Y下) """ - # 1. 获取相机在世界坐标系的位姿矩阵 M - cam_M = get_matrix(camera_transform) + # 1. 计算相机在世界坐标下的位姿矩阵 (World -> CameraActor) + cam_to_world = get_matrix(camera_transform) + + # 2. 求逆,得到 (CameraActor -> World) ... 实际上我们需要 World -> CameraActor + # 也就是将世界点变换到相机原本的坐标系中 + world_to_cam_ue = np.linalg.inv(cam_to_world) - # 2. 求逆,得到 世界->相机 的变换 - w2c = np.linalg.inv(cam_M) + # 3. 坐标轴修正矩阵 (UE4 -> OpenCV Standard) + # UE4: X(前), Y(右), Z(上) + # Cam: X(右), Y(下), Z(前) + # 变换关系: + # Cam_X = UE_Y + # Cam_Y = -UE_Z + # Cam_Z = UE_X + calibration = np.zeros((4, 4)) + calibration[0, 1] = 1 # Row 0 (Out X) takes Col 1 (In Y) + calibration[1, 2] = -1 # Row 1 (Out Y) takes Col 2 (In Z) * -1 + calibration[2, 0] = 1 # Row 2 (Out Z) takes Col 0 (In X) + calibration[3, 3] = 1 + + # 4. 组合矩阵: 先进行世界到UE相机的平移旋转,再进行轴变换 + w2c = np.dot(calibration, world_to_cam_ue) + return w2c + +def get_image_point(loc, K, w2c): + """ + 3D点 -> 2D像素 + """ + # [x, y, z, 1] + point = np.array([loc.x, loc.y, loc.z, 1]) + + # World -> Camera + point_camera = np.dot(w2c, point) + + # 深度裁剪 (在相机背后的点剔除) + if point_camera[2] <= 0: + return None + + # Camera -> Image Plane (归一化) + # [u*z, v*z, z] = K * [xc, yc, zc] + point_img = np.dot(K, point_camera[:3]) - # 3. 坐标轴修正矩阵 (UE4 x-forward, y-right, z-up -> Camera z-forward, x-right, y-down) - calibration = np.identity(4) - calibration[0, 2] = 1 # x -> z - calibration[1, 0] = -1 # y -> -x - calibration[2, 1] = -1 # z -> -y + # 归一化 + u = point_img[0] / point_img[2] + v = point_img[1] / point_img[2] - # 组合 - w2c = np.dot(calibration, w2c) - return w2c \ No newline at end of file + return [int(u), int(v)] \ No newline at end of file diff --git a/src/InterconVis_PedSafety_Enh/data_check.py b/src/InterconVis_PedSafety_Enh/data_check.py index f0c6416a8c..b3126b1b68 100644 --- a/src/InterconVis_PedSafety_Enh/data_check.py +++ b/src/InterconVis_PedSafety_Enh/data_check.py @@ -4,147 +4,98 @@ import os import glob import cvips_utils as utils - -# ================= 配置 ================= -# 这里的路径要和你 collector 生成的保持一致 -# 比如你刚跑了 Town05_rainy_night,这里就改对应的文件夹名 -DATASET_ROOT = "_out_dataset_final" -SCENE_NAME = "Town05_rainy_night" # 如果你跑的是默认参数,可能是 Town01_clear_day -# ======================================= - -def draw_3d_box(img, target, w2c, K): - """ - 核心绘画函数:将 3D 目标画在 2D 图上 - """ - # 1. 解析目标参数 - loc = target['location'] # [x, y, z] - rot = target['rotation'] # [pitch, yaw, roll] - extent = target['extent'] # [ex, ey, ez] - offset = target['center_offset'] # [ox, oy, oz] - - # 2. 恢复目标的位姿矩阵 (Local -> World) - # 注意:这里我们临时借用 carla 的 transform 对象来算矩阵, - # 如果不想依赖 carla 库,也可以手动写旋转矩阵公式,但用 carla 最方便 - import carla - t_loc = carla.Location(x=loc[0], y=loc[1], z=loc[2]) - t_rot = carla.Rotation(pitch=rot[0], yaw=rot[1], roll=rot[2]) - obj_transform = carla.Transform(t_loc, t_rot) - obj_matrix = utils.get_matrix(obj_transform) - - # 3. 定义包围盒的 8 个顶点 (相对于物体中心) - # CARLA extent 是半长,所以坐标是 +/- extent - dx, dy, dz = extent[0], extent[1], extent[2] +import carla + +def draw_box(img, target, w2c, K): + # 获取真值数据 + loc = target['location'] + rot = target['rotation'] + extent = target['extent'] + offset = target.get('center_offset', [0, 0, 0]) - # 8个顶点的局部坐标 (x, y, z, 1) + # 1. 构建物体的本地到世界的变换矩阵 + # 利用 carla 现成的 Transform 类来算旋转,最不容易出错 + obj_transform = carla.Transform( + carla.Location(x=loc[0], y=loc[1], z=loc[2]), + carla.Rotation(pitch=rot[0], yaw=rot[1], roll=rot[2]) + ) + obj_to_world = utils.get_matrix(obj_transform) + + # 2. 定义 3D 框的 8 个顶点 (加上 offset 修正中心点位置) + dx, dy, dz = extent[0], extent[1], extent[2] + ox, oy, oz = offset[0], offset[1], offset[2] corners_local = np.array([ - [dx, dy, dz, 1], [dx, -dy, dz, 1], [dx, -dy, -dz, 1], [dx, dy, -dz, 1], # 前面4个点 - [-dx, dy, dz, 1], [-dx, -dy, dz, 1], [-dx, -dy, -dz, 1], [-dx, dy, -dz, 1] # 后面4个点 - ]).T # 转置成 4x8 - - # 加上中心偏移量 (如果有的话) - corners_local[0, :] += offset[0] - corners_local[1, :] += offset[1] - corners_local[2, :] += offset[2] - - # 4. 坐标变换流水线 - # 4.1 Local -> World - corners_world = np.dot(obj_matrix, corners_local) # 4x8 - - # 4.2 World -> Pixel - img_points = [] - for i in range(8): - # 取出单个点的世界坐标 - p_world_vec = corners_world[:, i] - # 使用 utils 投影到像素 - # 注意 utils.get_image_point 需要 carla.Location 类型,我们手动构造一下 - p_loc = carla.Location(x=p_world_vec[0], y=p_world_vec[1], z=p_world_vec[2]) + [ox+dx, oy+dy, oz+dz, 1], [ox+dx, oy-dy, oz+dz, 1], + [ox+dx, oy-dy, oz-dz, 1], [ox+dx, oy+dy, oz-dz, 1], + [ox-dx, oy+dy, oz+dz, 1], [ox-dx, oy-dy, oz+dz, 1], + [ox-dx, oy-dy, oz-dz, 1], [ox-dx, oy+dy, oz-dz, 1] + ]) + + # 3. 投影到像素平面 + img_pts = [] + for pt in corners_local: + # 变换到世界坐标: World = Matrix * Local + w_pos = np.dot(obj_to_world, pt) + # 转换为 Location 传给 utils + world_loc = carla.Location(x=w_pos[0], y=w_pos[1], z=w_pos[2]) + pixel = utils.get_image_point(world_loc, K, w2c) + img_pts.append(pixel) - pixel = utils.get_image_point(p_loc, K, w2c) - img_points.append(pixel) - - # 5. 连线绘画 (12条棱边) - # 定义连接关系 - edges = [ - (0, 1), (1, 2), (2, 3), (3, 0), # 前面 - (4, 5), (5, 6), (6, 7), (7, 4), # 后面 - (0, 4), (1, 5), (2, 6), (3, 7) # 前后连接 - ] - - color = (0, 255, 0) # 绿色代表车辆 - if target['type'] == 'walker': - color = (0, 0, 255) # 红色代表行人 - - for p1_idx, p2_idx in edges: - p1 = tuple(img_points[p1_idx]) - p2 = tuple(img_points[p2_idx]) - # 画线 - cv2.line(img, p1, p2, color, 2) - + if any(p is None for p in img_pts): + return img + + # 4. 连线绘制 + edges = [(0,1), (1,2), (2,3), (3,0), (4,5), (5,6), (6,7), (7,4), (0,4), (1,5), (2,6), (3,7)] + color = (0, 255, 0) if target['type'] == 'vehicle' else (0, 0, 255) + for s, e in edges: + cv2.line(img, tuple(img_pts[s]), tuple(img_pts[e]), color, 2) return img def main(): - base_path = os.path.join(DATASET_ROOT, SCENE_NAME) - label_dir = os.path.join(base_path, "label") - - if not os.path.exists(label_dir): - print(f"❌ 错误: 找不到路径 {label_dir}") - print("请先运行 collector 脚本生成数据,或者检查 check_data.py 里的 SCENE_NAME 配置。") + # 自动查找最新的文件夹 + all_dirs = sorted(glob.glob("_out_dataset_final/*"), key=os.path.getmtime) + if not all_dirs: + print("没有找到数据目录") return - - # 获取所有 json 文件并排序 - json_files = sorted(glob.glob(os.path.join(label_dir, "*.json"))) - print(f"📂 发现 {len(json_files)} 帧数据,开始回放验证...") - print("⌨️ 按任意键下一帧,按 'q' 退出") - - for j_path in json_files: - with open(j_path, 'r') as f: - label_data = json.load(f) - - fid = label_data['frame_id'] + + target_dir = all_dirs[-1] # 默认看最新的 + print(f"正在检查目录: {target_dir}") + + json_files = sorted(glob.glob(os.path.join(target_dir, "label", "*.json"))) + + for jf in json_files: + with open(jf, 'r') as f: + data = json.load(f) + + fid = data['frame_id'] + ego_path = os.path.join(target_dir, "ego_rgb", f"{fid:08d}.jpg") + rsu_path = os.path.join(target_dir, "rsu_rgb", f"{fid:08d}.jpg") + + if not os.path.exists(ego_path): continue - # 读取图片 - ego_path = os.path.join(base_path, "ego_rgb", f"{fid:08d}.jpg") - rsu_path = os.path.join(base_path, "rsu_rgb", f"{fid:08d}.jpg") - - if not os.path.exists(ego_path) or not os.path.exists(rsu_path): - print(f"跳过缺失图片的帧: {fid}") - continue - img_ego = cv2.imread(ego_path) img_rsu = cv2.imread(rsu_path) - - # 获取矩阵参数 - # 注意: JSON 里存的是 list,转回 numpy array - ego_w2c = np.array(label_data['matrices']['ego_w2c']) - rsu_w2c = np.array(label_data['matrices']['rsu_w2c']) - # 内参 (从 JSON 读取或者用 utils 重新生成都可以,这里用 utils 生成) - h, w = img_ego.shape[:2] - K = utils.build_projection_matrix(w, h, label_data['camera_params']['fov']) - - # 遍历所有目标并画框 - targets = label_data['targets'] - for tgt in targets: - # 在主车视角画 - img_ego = draw_3d_box(img_ego, tgt, ego_w2c, K) - # 在路侧视角画 - img_rsu = draw_3d_box(img_rsu, tgt, rsu_w2c, K) - - # 拼接显示 - # 缩小一点方便看 - img_ego_s = cv2.resize(img_ego, (960, 540)) - img_rsu_s = cv2.resize(img_rsu, (960, 540)) + # 恢复矩阵 + ego_w2c = np.array(data['matrices']['ego_w2c']) + rsu_w2c = np.array(data['matrices']['rsu_w2c']) - # 上下拼接 - combined = np.vstack((img_ego_s, img_rsu_s)) + fov = data['camera_params']['fov'] + h, w = img_ego.shape[:2] + K = utils.build_projection_matrix(w, h, fov) - cv2.imshow(f"Validation - {SCENE_NAME}", combined) + for tgt in data['targets']: + img_ego = draw_box(img_ego, tgt, ego_w2c, K) + img_rsu = draw_box(img_rsu, tgt, rsu_w2c, K) + + # 拼接显示 + vis = np.vstack([img_ego, img_rsu]) + vis = cv2.resize(vis, (0, 0), fx=0.6, fy=0.6) # 缩小一点 - # 按键控制 - key = cv2.waitKey(0) # 0 表示无限等待,按键才继续;改成 30 可以自动播放 - if key == ord('q'): + cv2.imshow("Check Data", vis) + if cv2.waitKey(0) == ord('q'): break - + cv2.destroyAllWindows() if __name__ == "__main__": From 0502845238a97bb4a165956526c1f3061854d0d6 Mon Sep 17 00:00:00 2001 From: haoliuaa <2121765139@qq.com> Date: Fri, 19 Dec 2025 16:46:56 +0800 Subject: [PATCH 2/4] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=8F=AF=E8=A7=86?= =?UTF-8?q?=E5=8C=96=E7=AA=97=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../cvips_collector.py | 539 ++++++------------ 1 file changed, 189 insertions(+), 350 deletions(-) diff --git a/src/InterconVis_PedSafety_Enh/cvips_collector.py b/src/InterconVis_PedSafety_Enh/cvips_collector.py index 7b18ed01b0..742c186cb2 100644 --- a/src/InterconVis_PedSafety_Enh/cvips_collector.py +++ b/src/InterconVis_PedSafety_Enh/cvips_collector.py @@ -1,5 +1,3 @@ -# --- START OF FILE cvips_collector_final.py --- - import carla import random import queue @@ -9,392 +7,233 @@ import threading import json import numpy as np -import sys -import cvips_utils as utils # 必须确保同目录下有 cvips_utils.py +import cv2 +import cvips_utils as utils # ================= 配置区域 ================= -OUTPUT_FOLDER = "_out_dataset_final" # 修改输出目录名以示区别 -ENABLE_SAVING = True -SAVE_INTERVAL = 10 # 每 10 帧存一次 -TARGET_FPS = 30 +OUTPUT_FOLDER = "_out_dataset_final" +SAVE_INTERVAL = 5 +TARGET_FPS = 20 FIXED_DELTA_SECONDS = 1.0 / TARGET_FPS -# 相机参数 (必须与 spawn_actor 时设置的一致) -IMAGE_W = 1920 -IMAGE_H = 1080 -FOV = 90.0 +IMAGE_W = 1280 +IMAGE_H = 720 +FOV = 90.0 # =========================================== -# 全局控制标志 writing_thread_running = True -def configure_weather(world, weather_type, time_of_day): - """设置天气和光照""" - weather_presets = { - 'clear': carla.WeatherParameters.ClearNoon, - 'cloudy': carla.WeatherParameters.CloudyNoon, - 'rainy': carla.WeatherParameters.HardRainNoon, - 'wet': carla.WeatherParameters.WetNoon, +def configure_weather(world, weather_arg, time_arg): + """设置天气和时间的核心逻辑""" + presets = { + 'Clear': carla.WeatherParameters.ClearNoon, + 'Cloudy': carla.WeatherParameters.CloudyNoon, + 'Wet': carla.WeatherParameters.WetNoon, + 'Rain': carla.WeatherParameters.HardRainNoon, + 'Storm': carla.WeatherParameters.HardRainSunset } - weather = weather_presets.get(weather_type, carla.WeatherParameters.ClearNoon) - - if time_of_day == 'day': - weather.sun_altitude_angle = 75.0 - elif time_of_day == 'sunset': - weather.sun_altitude_angle = 10.0 - elif time_of_day == 'night': - weather.sun_altitude_angle = -90.0 - + weather = presets.get(weather_arg, carla.WeatherParameters.ClearNoon) + + # 修改太阳高度角模拟时间 + if time_arg == 'Day': weather.sun_altitude_angle = 60.0 + elif time_arg == 'Sunset': weather.sun_altitude_angle = 10.0 + elif time_arg == 'Night': weather.sun_altitude_angle = -90.0 + world.set_weather(weather) - - -def cleanup_previous_hero(world): - """启动前清理可能残留的主车""" - actors = world.get_actors() - potential_heroes = [x for x in actors if - x.type_id.startswith('vehicle') and x.attributes.get('role_name') == 'hero'] - if potential_heroes: - print(f"🧹 发现 {len(potential_heroes)} 辆残留的主车,正在清理...") - for h in potential_heroes: - h.destroy() - - -class pygame_clock: - def __init__(self): - self.start_time = time.time() - self.frame_count = 0 - self.fps = 0 - - def tick(self): - self.frame_count += 1 - elapsed = time.time() - self.start_time - if elapsed > 1.0: - self.fps = self.frame_count / elapsed - self.start_time = time.time() - self.frame_count = 0 - return self.fps - + print(f"🌦️ 环境已切换: {weather_arg} | {time_arg}") + +def is_visible(world, camera_actor, target_actor): + """射线检测:判断是否被遮挡""" + cam_trans = camera_actor.get_transform() + cam_loc = cam_trans.location + forward = cam_trans.get_forward_vector() + # 起点偏移防止撞到自己车头 + ray_start = cam_loc + carla.Location(x=forward.x*2.5, y=forward.y*2.5, z=forward.z*2.5) + + target_loc = target_actor.get_transform().location + target_loc.z += 1.0 + + hit_points = world.cast_ray(ray_start, target_loc) + for hit in hit_points: + if hit.location.distance(ray_start) < target_loc.distance(ray_start) - 2.0: + return False + return True + +def draw_box_simple(img, target, w2c, K): + """绘制3D绿框""" + loc, rot, extent = target['location'], target['rotation'], target['extent'] + offset = target.get('center_offset', [0, 0, 0]) + obj_t = carla.Transform(carla.Location(x=loc[0], y=loc[1], z=loc[2]), + carla.Rotation(pitch=rot[0], yaw=rot[1], roll=rot[2])) + obj_to_world = utils.get_matrix(obj_t) + dx, dy, dz = extent[0], extent[1], extent[2] + ox, oy, oz = offset[0], offset[1], offset[2] + corners_local = np.array([ + [ox+dx, oy+dy, oz+dz, 1], [ox+dx, oy-dy, oz+dz, 1], [ox+dx, oy-dy, oz-dz, 1], [ox+dx, oy+dy, oz-dz, 1], + [ox-dx, oy+dy, oz+dz, 1], [ox-dx, oy-dy, oz+dz, 1], [ox-dx, oy-dy, oz-dz, 1], [ox-dx, oy+dy, oz-dz, 1] + ]) + img_pts = [] + points_behind = 0 + for pt in corners_local: + w_pos = np.dot(obj_to_world, pt) + p = utils.get_image_point(carla.Location(x=w_pos[0], y=w_pos[1], z=w_pos[2]), K, w2c) + if p is None: + points_behind += 1 + img_pts.append((0,0)) + else: img_pts.append(p) + if points_behind > 4: return img + edges = [(0,1), (1,2), (2,3), (3,0), (4,5), (5,6), (6,7), (7,4), (0,4), (1,5), (2,6), (3,7)] + color = (0, 255, 0) if target['type'] == 'vehicle' else (0, 0, 255) + for s, e in edges: + if img_pts[s] != (0,0) and img_pts[e] != (0,0): + cv2.line(img, tuple(img_pts[s]), tuple(img_pts[e]), color, 2) + return img def save_worker(q): - """ - 后台保存线程:同时保存图片和 JSON 标签 - """ while writing_thread_running or not q.empty(): try: - # 获取任务包 task = q.get(timeout=0.1) - path = task['scene_path'] - - # 1. 保存图片 - images = task['image_data'] - ego_img = images['ego_rgb'] - rsu_img = images['rsu_rgb'] - - # 这里的 frame 编号统一使用 task 里传过来的,保证对齐 - fid = task['frame_id'] - - ego_img.save_to_disk(f"{path}/ego_rgb/{fid:08d}.jpg") - rsu_img.save_to_disk(f"{path}/rsu_rgb/{fid:08d}.jpg") - - # 2. 保存 JSON 标签 - label_data = task['label_data'] + path, fid = task['path'], task['fid'] + task['ego_img'].save_to_disk(f"{path}/ego_rgb/{fid:08d}.jpg") + task['rsu_img'].save_to_disk(f"{path}/rsu_rgb/{fid:08d}.jpg") with open(f"{path}/label/{fid:08d}.json", 'w') as f: - json.dump(label_data, f, indent=2) - + json.dump(task['json_data'], f, indent=2) q.task_done() - except queue.Empty: - pass - except Exception as e: - print(f"写入错误: {e}") - - -def get_environment_objects(world, ego_id): - """ - 获取环境中的车辆和行人信息 - """ - objects = [] - ego_actor = world.get_actor(ego_id) - ego_loc = ego_actor.get_transform().location - - for actor in world.get_actors(): - # 跳过主车自己(如果你想把主车也算作 RSU 的检测目标,可以注释掉下面两行) - if actor.id == ego_id: - continue - - if actor.type_id.startswith('vehicle') or actor.type_id.startswith('walker'): - # 距离过滤:只记录主车周围 100 米内的物体 - dist = actor.get_transform().location.distance(ego_loc) - if dist < 100: - bb = actor.bounding_box - t_loc = actor.get_transform().location - t_rot = actor.get_transform().rotation - - # 构建单个目标的数据字典 - obj_data = { - "id": actor.id, - "type": "vehicle" if actor.type_id.startswith('vehicle') else "walker", - "dist_to_ego": dist, - # 保存位置 (x, y, z) - "location": [t_loc.x, t_loc.y, t_loc.z], - # 保存旋转 (pitch, yaw, roll) - "rotation": [t_rot.pitch, t_rot.yaw, t_rot.roll], - # 保存包围盒半长宽高 (extent) - "extent": [bb.extent.x, bb.extent.y, bb.extent.z], - # 保存包围盒中心相对于物体原点的偏移 - "center_offset": [bb.location.x, bb.location.y, bb.location.z] - } - objects.append(obj_data) - return objects - + except: continue def main(): - argparser = argparse.ArgumentParser(description="CVIPS Final - 带标签采集版") - argparser.add_argument('--town', default='Town01', help='地图名称') - argparser.add_argument('--num_vehicles', default=40, type=int, help='车辆数') - argparser.add_argument('--num_walkers', default=40, type=int, help='行人数') - argparser.add_argument('--weather', default='clear', choices=['clear', 'rainy', 'cloudy', 'wet'], help='天气') - argparser.add_argument('--time_of_day', default='day', choices=['day', 'sunset', 'night'], help='时间') - argparser.add_argument('--max_frames', default=1000, type=int, help='采集多少帧后自动停止') - - args = argparser.parse_args() - - # 路径准备 - scene_name = f"{args.town}_{args.weather}_{args.time_of_day}" - scene_output_path = os.path.join(OUTPUT_FOLDER, scene_name) + parser = argparse.ArgumentParser() + parser.add_argument('--town', default='Town05') + parser.add_argument('--weather', default='Clear', choices=['Clear', 'Cloudy', 'Wet', 'Rain', 'Storm']) + parser.add_argument('--time', default='Day', choices=['Day', 'Sunset', 'Night']) + parser.add_argument('--num_vehicles', default=50, type=int) + parser.add_argument('--num_walkers', default=50, type=int) # 行人数量 + parser.add_argument('--max_frames', default=1000, type=int) + args = parser.parse_args() + + scene_id = f"{args.town}_{args.weather}_{args.time}" + save_path = os.path.join(OUTPUT_FOLDER, scene_id) + for sub in ['ego_rgb', 'rsu_rgb', 'label']: os.makedirs(os.path.join(save_path, sub), exist_ok=True) + + client = carla.Client('localhost', 2000) + client.set_timeout(20.0) + world = client.load_world(args.town) + + # 核心:设置天气和同步模式 + configure_weather(world, args.weather, args.time) + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = FIXED_DELTA_SECONDS + world.apply_settings(settings) - if ENABLE_SAVING: - os.makedirs(f"{scene_output_path}/ego_rgb", exist_ok=True) - os.makedirs(f"{scene_output_path}/rsu_rgb", exist_ok=True) - os.makedirs(f"{scene_output_path}/label", exist_ok=True) - print(f"📂 数据将保存在: {scene_output_path}") + tm = client.get_trafficmanager(8000) + tm.set_synchronous_mode(True) - client = None - world = None actor_list = [] - - # 传感器数据队列 (只用于暂时接收 Carla 回调数据) - sensor_queue = queue.Queue() - # 存盘队列 (用于发送给 save_worker) - save_queue = queue.Queue() - - global writing_thread_running - try: - client = carla.Client('127.0.0.1', 2000) - client.set_timeout(10.0) - - world = client.get_world() - if world.get_map().name.split('/')[-1] != args.town: - print(f"🗺️ 正在切换地图至 {args.town} ...") - world = client.load_world(args.town) - else: - print(f"🗺️ 当前已是 {args.town},准备就绪。") - - cleanup_previous_hero(world) - configure_weather(world, args.weather, args.time_of_day) - - # 设置同步模式 - settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = FIXED_DELTA_SECONDS - world.apply_settings(settings) - - traffic_manager = client.get_trafficmanager(8000) - traffic_manager.set_synchronous_mode(True) - traffic_manager.set_global_distance_to_leading_vehicle(2.5) - - # 启动保存线程 - writing_thread_running = True - save_thread = threading.Thread(target=save_worker, args=(save_queue,)) - save_thread.daemon = True - save_thread.start() - - # --- 1. 生成交通流 (NPC) --- - print("🚗 生成交通流...") - blueprint_library = world.get_blueprint_library() + bp_lib = world.get_blueprint_library() spawn_points = world.get_map().get_spawn_points() - random.shuffle(spawn_points) - - ego_spawn_point = spawn_points[0] - npc_spawn_points = spawn_points[1:] - - # 生成车辆 - bg_vehicle_bp = blueprint_library.filter('vehicle.*') - bg_vehicle_bp = [x for x in bg_vehicle_bp if int(x.get_attribute('number_of_wheels')) == 4] - - batch = [] - for n, transform in enumerate(npc_spawn_points): - if n >= args.num_vehicles: break - bp = random.choice(bg_vehicle_bp) - if bp.has_attribute('color'): - bp.set_attribute('color', random.choice(bp.get_attribute('color').recommended_values)) - batch.append(carla.command.SpawnActor(bp, transform).then( - carla.command.SetAutopilot(carla.command.FutureActor, True, traffic_manager.get_port()))) - - for response in client.apply_batch_sync(batch, True): - if not response.error: actor_list.append(response.actor_id) - - # 生成行人 - walker_bp = blueprint_library.filter('walker.pedestrian.*')[0] + + # 1. 主车 + ego_vehicle = world.spawn_actor(bp_lib.find('vehicle.tesla.model3'), spawn_points[0]) + ego_vehicle.set_autopilot(True, tm.get_port()) + actor_list.append(ego_vehicle) + + # 2. 背景车辆 + for i in range(1, min(len(spawn_points), args.num_vehicles)): + bp = random.choice(bp_lib.filter('vehicle.*')) + npc = world.try_spawn_actor(bp, spawn_points[i]) + if npc: + npc.set_autopilot(True, tm.get_port()) + actor_list.append(npc) + + # 3. 生成行人 + print(f"🚶 正在生成 {args.num_walkers} 个行人...") + walker_bp_lib = bp_lib.filter('walker.pedestrian.*') for _ in range(args.num_walkers): loc = world.get_random_location_from_navigation() if loc: + walker_bp = random.choice(walker_bp_lib) w = world.try_spawn_actor(walker_bp, carla.Transform(loc)) - if w: actor_list.append(w.id) - - # (可选) 给行人加上控制器,让他们动起来,这里简化处理,如有需要需添加 WalkerController - - # --- 2. 生成主车 (Ego) --- - print("🚘 生成主车...") - ego_bp = blueprint_library.find('vehicle.tesla.model3') - ego_bp.set_attribute('role_name', 'hero') - ego_vehicle = world.spawn_actor(ego_bp, ego_spawn_point) - ego_vehicle.set_autopilot(True) - actor_list.append(ego_vehicle.id) - - # --- 3. 生成传感器 (Ego & RSU) --- - # 3.1 确定位置 - # RSU: 主车生成位置上方 10米,稍微偏一点 - rsu_loc = ego_spawn_point.location - rsu_loc.z += 10.0 - rsu_loc.x += 8.0 - rsu_transform = carla.Transform(rsu_loc, carla.Rotation(pitch=-60, yaw=ego_spawn_point.rotation.yaw)) + if w: actor_list.append(w) + + # 4. 传感器 + cam_bp = bp_lib.find('sensor.camera.rgb') + cam_bp.set_attribute('image_size_x', str(IMAGE_W)); cam_bp.set_attribute('image_size_y', str(IMAGE_H)) + cam_bp.set_attribute('fov', str(FOV)) + ego_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(z=2.0, x=1.2)), attach_to=ego_vehicle) + rsu_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(x=20, y=10, z=15), carla.Rotation(pitch=-60))) + actor_list.extend([ego_cam, rsu_cam]) + + image_queue = queue.Queue() + ego_cam.listen(lambda data: image_queue.put(('ego', data))) + rsu_cam.listen(lambda data: image_queue.put(('rsu', data))) - # Ego Cam: 挂载在主车上 - cam_transform = carla.Transform(carla.Location(x=-1.5, z=2.4), carla.Rotation(pitch=-15)) + save_queue = queue.Queue() + threading.Thread(target=save_worker, args=(save_queue,), daemon=True).start() - # 3.2 配置蓝图 - camera_bp = blueprint_library.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', str(IMAGE_W)) - camera_bp.set_attribute('image_size_y', str(IMAGE_H)) - camera_bp.set_attribute('fov', str(FOV)) - camera_bp.set_attribute('sensor_tick', str(FIXED_DELTA_SECONDS)) - - # 3.3 Spawn - ego_cam = world.spawn_actor(camera_bp, cam_transform, attach_to=ego_vehicle) - rsu_cam = world.spawn_actor(camera_bp, rsu_transform) # RSU 是固定的(static),不 attach - actor_list.append(ego_cam.id) - actor_list.append(rsu_cam.id) - - # 3.4 监听数据 - ego_cam.listen(lambda image: sensor_queue.put((image.frame, 'ego_rgb', image))) - rsu_cam.listen(lambda image: sensor_queue.put((image.frame, 'rsu_rgb', image))) - - print(f"\n🚀 采集开始! 目标: {args.max_frames} 帧. 按 Ctrl+C 停止") - - frame_number = 0 spectator = world.get_spectator() - clock = pygame_clock() + K = utils.build_projection_matrix(IMAGE_W, IMAGE_H, FOV) - # --- 4. 主循环 --- - while True: - # 仿真步进 + print("\n🚀 运行中![Q]键退出 | 天气:", args.weather, "| 时间:", args.time) + + for frame_idx in range(args.max_frames): world.tick() - w_frame = world.get_snapshot().frame - fps = clock.tick() - # 更新观察者视角跟随主车 - spectator.set_transform(ego_cam.get_transform()) - - if args.max_frames > 0 and frame_number >= args.max_frames: - print("\n✅ 已达到目标帧数,自动停止。") - break - - try: - # 获取 RGB 图片数据 - current_frame_images = {} - timeout = 0 - # 等待两个相机的图片都到齐 - while len(current_frame_images) < 2 and timeout < 10: - data = sensor_queue.get(timeout=1.0) - fid, stype, img = data - # 只收当前帧的数据,防止错位 - if abs(fid - w_frame) <= 2: - current_frame_images[stype] = img - timeout += 1 - - # 只有当两个相机数据都齐了,并且满足保存间隔时,才进行标签计算和保存 - if len(current_frame_images) == 2 and ENABLE_SAVING and (frame_number % SAVE_INTERVAL == 0): - - # --- 核心新增逻辑: 计算矩阵和标签 --- - - # 1. 获取相机位姿 - ego_trans = ego_cam.get_transform() - rsu_trans = rsu_cam.get_transform() - - # 2. 计算 外参矩阵 (World -> Camera) - # 这一步需要 cvips_utils.py 的支持 - ego_w2c = utils.build_world_to_camera_matrix(ego_trans) - rsu_w2c = utils.build_world_to_camera_matrix(rsu_trans) - - # 3. 获取所有目标物体 (3D框) - targets = get_environment_objects(world, ego_vehicle.id) - - # 4. 组装 JSON 标签数据 - frame_label_data = { - "frame_id": frame_number, - "timestamp": world.get_snapshot().timestamp.elapsed_seconds, - "camera_params": { - "fov": FOV, - "width": IMAGE_W, - "height": IMAGE_H - }, - "matrices": { - "ego_w2c": ego_w2c.tolist(), - "rsu_w2c": rsu_w2c.tolist() - }, - "targets": targets - } - - # 5. 打包发送给后台线程 - save_task = { - "scene_path": scene_output_path, - "frame_id": frame_number, - "image_data": current_frame_images, - "label_data": frame_label_data - } - save_queue.put(save_task) - - print(f"FPS: {fps:.1f} | Frame: {frame_number} | Saved: ✅ | Queue: {save_queue.qsize()} ", end='\r') - else: - # 不保存的帧,仅打印进度 - print(f"FPS: {fps:.1f} | Frame: {frame_number} | Saved: ⏭️ | Queue: {save_queue.qsize()} ", end='\r') - - except queue.Empty: - pass + # 自动跟随视角 + ego_t = ego_vehicle.get_transform() + spectator.set_transform(carla.Transform(ego_t.location + carla.Location(x=-12, z=6), + carla.Rotation(pitch=-25, yaw=ego_t.rotation.yaw))) + + data = {} + for _ in range(2): + name, img = image_queue.get() + data[name] = img + + ego_w2c = utils.build_world_to_camera_matrix(ego_cam.get_transform()) + rsu_w2c = utils.build_world_to_camera_matrix(rsu_cam.get_transform()) - frame_number += 1 + # 寻找范围内物体 + all_actors = list(world.get_actors().filter('vehicle.*')) + list(world.get_actors().filter('walker.pedestrian.*')) + visible_targets = [] + for actor in all_actors: + if actor.id == ego_vehicle.id: continue + if actor.get_transform().location.distance(ego_t.location) > 80: continue + + # 遮挡检测逻辑 + if is_visible(world, ego_cam, actor): + t, bb = actor.get_transform(), actor.bounding_box + visible_targets.append({ + "id": actor.id, "type": "vehicle" if actor.type_id.startswith('vehicle') else "walker", + "location": [t.location.x, t.location.y, t.location.z], + "rotation": [t.rotation.pitch, t.rotation.yaw, t.rotation.roll], + "extent": [bb.extent.x, bb.extent.y, bb.extent.z], + "center_offset": [bb.location.x, bb.location.y, bb.location.z] + }) + + # 实时预览 + array = np.frombuffer(data['ego'].raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (IMAGE_H, IMAGE_W, 4)) + frame = array[:, :, :3].copy() + for t in visible_targets: frame = draw_box_simple(frame, t, ego_w2c, K) + cv2.imshow("Preview", frame) + if cv2.waitKey(1) & 0xFF == ord('q'): break + + # 保存 + if frame_idx % SAVE_INTERVAL == 0: + json_data = {"frame_id": frame_idx, "camera_params": {"fov": FOV, "w": IMAGE_W, "h": IMAGE_H}, + "matrices": {"ego_w2c": ego_w2c.tolist(), "rsu_w2c": rsu_w2c.tolist()}, + "targets": visible_targets} + save_queue.put({"path": save_path, "fid": frame_idx, "ego_img": data['ego'], "rsu_img": data['rsu'], "json_data": json_data}) - except KeyboardInterrupt: - print("\n\n🛑 用户中断") - except Exception as e: - print(f"\n❌ 错误: {e}") - import traceback - traceback.print_exc() finally: - print("\n♻️ 正在退出...") - - # 停止写入 + global writing_thread_running writing_thread_running = False - if 'save_thread' in locals() and save_thread.is_alive(): - print("⏳ 等待后台写入完成...", end='') - save_thread.join(timeout=5) - print("Done") - - # 销毁对象 - if client and actor_list: - print("💥 销毁 Actor...") - client.apply_batch([carla.command.DestroyActor(x) for x in actor_list]) - - # 恢复异步 - if world: - settings = world.get_settings() - settings.synchronous_mode = False - settings.fixed_delta_seconds = None - world.apply_settings(settings) - - print("✅ 资源已释放,强制返回终端。") - os._exit(0) + cv2.destroyAllWindows() + settings = world.get_settings(); settings.synchronous_mode = False; world.apply_settings(settings) + for actor in actor_list: + if actor: actor.destroy() + print("\n清理完毕。") if __name__ == '__main__': main() \ No newline at end of file From 522277f7eb949c231863d3e6853833e5d7aa1f87 Mon Sep 17 00:00:00 2001 From: haoliuaa <2121765139@qq.com> Date: Fri, 19 Dec 2025 18:54:54 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E4=BC=98=E5=8C=96=E7=BB=BF=E6=A1=86?= =?UTF-8?q?=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../cvips_collector.py | 187 ++++++++++-------- 1 file changed, 107 insertions(+), 80 deletions(-) diff --git a/src/InterconVis_PedSafety_Enh/cvips_collector.py b/src/InterconVis_PedSafety_Enh/cvips_collector.py index 742c186cb2..4246500e60 100644 --- a/src/InterconVis_PedSafety_Enh/cvips_collector.py +++ b/src/InterconVis_PedSafety_Enh/cvips_collector.py @@ -24,7 +24,9 @@ writing_thread_running = True def configure_weather(world, weather_arg, time_arg): - """设置天气和时间的核心逻辑""" + """ + 【完整保留】最初的天气和时间逻辑 + """ presets = { 'Clear': carla.WeatherParameters.ClearNoon, 'Cloudy': carla.WeatherParameters.CloudyNoon, @@ -32,100 +34,102 @@ def configure_weather(world, weather_arg, time_arg): 'Rain': carla.WeatherParameters.HardRainNoon, 'Storm': carla.WeatherParameters.HardRainSunset } + weather = presets.get(weather_arg, carla.WeatherParameters.ClearNoon) # 修改太阳高度角模拟时间 - if time_arg == 'Day': weather.sun_altitude_angle = 60.0 - elif time_arg == 'Sunset': weather.sun_altitude_angle = 10.0 - elif time_arg == 'Night': weather.sun_altitude_angle = -90.0 - + if time_arg == 'Day': + weather.sun_altitude_angle = 60.0 + elif time_arg == 'Sunset': + weather.sun_altitude_angle = 10.0 + elif time_arg == 'Night': + weather.sun_altitude_angle = -90.0 + world.set_weather(weather) - print(f"🌦️ 环境已切换: {weather_arg} | {time_arg}") + print(f"🌦️ 环境设置成功: {weather_arg} | {time_arg}") -def is_visible(world, camera_actor, target_actor): - """射线检测:判断是否被遮挡""" +def is_visible_refined(world, camera_actor, target_actor): + """ + 【修复版】可见性检测:针对两轮车做了高度优化 + """ cam_trans = camera_actor.get_transform() - cam_loc = cam_trans.location - forward = cam_trans.get_forward_vector() - # 起点偏移防止撞到自己车头 - ray_start = cam_loc + carla.Location(x=forward.x*2.5, y=forward.y*2.5, z=forward.z*2.5) + # 起点:相机位置 + 向上提一点,彻底避开主车车身 + ray_start = cam_trans.location + carla.Location(z=0.5) + cam_trans.get_forward_vector() * 2.0 - target_loc = target_actor.get_transform().location - target_loc.z += 1.0 + # 终点:两轮车较低,瞄准 z=0.6,轿车/行人瞄准 z=1.0 + target_z = 0.6 if 'bike' in target_actor.type_id or 'bicycle' in target_actor.type_id or 'motorcycle' in target_actor.type_id else 1.0 + target_loc = target_actor.get_transform().location + carla.Location(z=target_z) hit_points = world.cast_ray(ray_start, target_loc) - for hit in hit_points: - if hit.location.distance(ray_start) < target_loc.distance(ray_start) - 2.0: - return False - return True + if not hit_points: return True # 路径没撞击到任何东西,肯定可见 + + # 只要第一个碰撞点离目标中心较近,就判定为可见(防止因车体大而误判) + return hit_points[0].location.distance(target_loc) < 3.0 -def draw_box_simple(img, target, w2c, K): - """绘制3D绿框""" +def draw_box(img, target, w2c, K): + """ + 【稳定版】绘制3D框,行人红色,车绿色,两轮车紫色 + """ loc, rot, extent = target['location'], target['rotation'], target['extent'] offset = target.get('center_offset', [0, 0, 0]) + obj_t = carla.Transform(carla.Location(x=loc[0], y=loc[1], z=loc[2]), carla.Rotation(pitch=rot[0], yaw=rot[1], roll=rot[2])) obj_to_world = utils.get_matrix(obj_t) dx, dy, dz = extent[0], extent[1], extent[2] ox, oy, oz = offset[0], offset[1], offset[2] + corners_local = np.array([ [ox+dx, oy+dy, oz+dz, 1], [ox+dx, oy-dy, oz+dz, 1], [ox+dx, oy-dy, oz-dz, 1], [ox+dx, oy+dy, oz-dz, 1], [ox-dx, oy+dy, oz+dz, 1], [ox-dx, oy-dy, oz+dz, 1], [ox-dx, oy-dy, oz-dz, 1], [ox-dx, oy+dy, oz-dz, 1] ]) + img_pts = [] - points_behind = 0 for pt in corners_local: w_pos = np.dot(obj_to_world, pt) p = utils.get_image_point(carla.Location(x=w_pos[0], y=w_pos[1], z=w_pos[2]), K, w2c) - if p is None: - points_behind += 1 - img_pts.append((0,0)) - else: img_pts.append(p) - if points_behind > 4: return img + img_pts.append(p) + edges = [(0,1), (1,2), (2,3), (3,0), (4,5), (5,6), (6,7), (7,4), (0,4), (1,5), (2,6), (3,7)] - color = (0, 255, 0) if target['type'] == 'vehicle' else (0, 0, 255) + if target['type'] == 'walker': color = (0, 0, 255) + elif target['type'] == 'bike': color = (255, 0, 255) + else: color = (0, 255, 0) + for s, e in edges: - if img_pts[s] != (0,0) and img_pts[e] != (0,0): + if img_pts[s] and img_pts[e]: cv2.line(img, tuple(img_pts[s]), tuple(img_pts[e]), color, 2) return img -def save_worker(q): - while writing_thread_running or not q.empty(): - try: - task = q.get(timeout=0.1) - path, fid = task['path'], task['fid'] - task['ego_img'].save_to_disk(f"{path}/ego_rgb/{fid:08d}.jpg") - task['rsu_img'].save_to_disk(f"{path}/rsu_rgb/{fid:08d}.jpg") - with open(f"{path}/label/{fid:08d}.json", 'w') as f: - json.dump(task['json_data'], f, indent=2) - q.task_done() - except: continue - def main(): + # 【恢复】所有原始参数解析 parser = argparse.ArgumentParser() - parser.add_argument('--town', default='Town05') + parser.add_argument('--town', default='Town01') parser.add_argument('--weather', default='Clear', choices=['Clear', 'Cloudy', 'Wet', 'Rain', 'Storm']) parser.add_argument('--time', default='Day', choices=['Day', 'Sunset', 'Night']) parser.add_argument('--num_vehicles', default=50, type=int) - parser.add_argument('--num_walkers', default=50, type=int) # 行人数量 - parser.add_argument('--max_frames', default=1000, type=int) + parser.add_argument('--num_walkers', default=30, type=int) + parser.add_argument('--max_frames', default=500, type=int) args = parser.parse_args() - scene_id = f"{args.town}_{args.weather}_{args.time}" - save_path = os.path.join(OUTPUT_FOLDER, scene_id) - for sub in ['ego_rgb', 'rsu_rgb', 'label']: os.makedirs(os.path.join(save_path, sub), exist_ok=True) + scene_name = f"{args.town}_{args.weather}_{args.time}" + save_path = os.path.join(OUTPUT_FOLDER, scene_name) + for sub in ['ego_rgb', 'rsu_rgb', 'label']: + os.makedirs(os.path.join(save_path, sub), exist_ok=True) client = carla.Client('localhost', 2000) client.set_timeout(20.0) - world = client.load_world(args.town) + world = client.get_world() + if args.town not in world.get_map().name: + world = client.load_world(args.town) - # 核心:设置天气和同步模式 + # 【恢复】环境配置 configure_weather(world, args.weather, args.time) + settings = world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = FIXED_DELTA_SECONDS world.apply_settings(settings) - + tm = client.get_trafficmanager(8000) tm.set_synchronous_mode(True) @@ -133,35 +137,39 @@ def main(): try: bp_lib = world.get_blueprint_library() spawn_points = world.get_map().get_spawn_points() - - # 1. 主车 - ego_vehicle = world.spawn_actor(bp_lib.find('vehicle.tesla.model3'), spawn_points[0]) + random.shuffle(spawn_points) + + # 1. 主车 (Ego) + ego_bp = bp_lib.find('vehicle.tesla.model3') + ego_vehicle = world.spawn_actor(ego_bp, spawn_points[0]) ego_vehicle.set_autopilot(True, tm.get_port()) actor_list.append(ego_vehicle) - # 2. 背景车辆 - for i in range(1, min(len(spawn_points), args.num_vehicles)): + # 2. 生成背景车 (包含自行车/摩托车) + print(f"🚗 正在生成 {args.num_vehicles} 辆背景车...") + batch = [] + for i, transform in enumerate(spawn_points[1:]): + if i >= args.num_vehicles: break bp = random.choice(bp_lib.filter('vehicle.*')) - npc = world.try_spawn_actor(bp, spawn_points[i]) - if npc: - npc.set_autopilot(True, tm.get_port()) - actor_list.append(npc) + batch.append(carla.command.SpawnActor(bp, transform).then( + carla.command.SetAutopilot(carla.command.FutureActor, True, tm.get_port()))) + + for r in client.apply_batch_sync(batch): + if not r.error: actor_list.append(world.get_actor(r.actor_id)) - # 3. 生成行人 + # 3. 【恢复】生成行人 print(f"🚶 正在生成 {args.num_walkers} 个行人...") - walker_bp_lib = bp_lib.filter('walker.pedestrian.*') for _ in range(args.num_walkers): loc = world.get_random_location_from_navigation() if loc: - walker_bp = random.choice(walker_bp_lib) - w = world.try_spawn_actor(walker_bp, carla.Transform(loc)) + w = world.try_spawn_actor(random.choice(bp_lib.filter('walker.pedestrian.*')), carla.Transform(loc)) if w: actor_list.append(w) - # 4. 传感器 + # 4. 相机设置 cam_bp = bp_lib.find('sensor.camera.rgb') cam_bp.set_attribute('image_size_x', str(IMAGE_W)); cam_bp.set_attribute('image_size_y', str(IMAGE_H)) cam_bp.set_attribute('fov', str(FOV)) - ego_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(z=2.0, x=1.2)), attach_to=ego_vehicle) + ego_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(z=2.0, x=1.0)), attach_to=ego_vehicle) rsu_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(x=20, y=10, z=15), carla.Rotation(pitch=-60))) actor_list.extend([ego_cam, rsu_cam]) @@ -172,15 +180,15 @@ def main(): save_queue = queue.Queue() threading.Thread(target=save_worker, args=(save_queue,), daemon=True).start() - spectator = world.get_spectator() K = utils.build_projection_matrix(IMAGE_W, IMAGE_H, FOV) + spectator = world.get_spectator() - print("\n🚀 运行中![Q]键退出 | 天气:", args.weather, "| 时间:", args.time) + print("\n🚀 运行中... 按 Q 退出预览") for frame_idx in range(args.max_frames): world.tick() - # 自动跟随视角 + # 【保留】自动视角跟随 ego_t = ego_vehicle.get_transform() spectator.set_transform(carla.Transform(ego_t.location + carla.Location(x=-12, z=6), carla.Rotation(pitch=-25, yaw=ego_t.rotation.yaw))) @@ -193,37 +201,48 @@ def main(): ego_w2c = utils.build_world_to_camera_matrix(ego_cam.get_transform()) rsu_w2c = utils.build_world_to_camera_matrix(rsu_cam.get_transform()) - # 寻找范围内物体 - all_actors = list(world.get_actors().filter('vehicle.*')) + list(world.get_actors().filter('walker.pedestrian.*')) + # 标注可见目标 + all_actors = world.get_actors().filter('vehicle.*') + all_walkers = world.get_actors().filter('walker.pedestrian.*') + visible_targets = [] - for actor in all_actors: + for actor in list(all_actors) + list(all_walkers): if actor.id == ego_vehicle.id: continue if actor.get_transform().location.distance(ego_t.location) > 80: continue - # 遮挡检测逻辑 - if is_visible(world, ego_cam, actor): + # 遮挡检测判定 + if is_visible_refined(world, ego_cam, actor): t, bb = actor.get_transform(), actor.bounding_box + + # 识别类型 + atype = 'vehicle' + if 'walker' in actor.type_id: atype = 'walker' + elif any(x in actor.type_id for x in ['bike', 'bicycle', 'motorcycle']): atype = 'bike' + visible_targets.append({ - "id": actor.id, "type": "vehicle" if actor.type_id.startswith('vehicle') else "walker", + "id": actor.id, "type": atype, "location": [t.location.x, t.location.y, t.location.z], "rotation": [t.rotation.pitch, t.rotation.yaw, t.rotation.roll], "extent": [bb.extent.x, bb.extent.y, bb.extent.z], "center_offset": [bb.location.x, bb.location.y, bb.location.z] }) - # 实时预览 + # 预览 array = np.frombuffer(data['ego'].raw_data, dtype=np.dtype("uint8")) array = np.reshape(array, (IMAGE_H, IMAGE_W, 4)) frame = array[:, :, :3].copy() - for t in visible_targets: frame = draw_box_simple(frame, t, ego_w2c, K) - cv2.imshow("Preview", frame) + for t in visible_targets: frame = draw_box(frame, t, ego_w2c, K) + cv2.imshow("Final Stable Preview", frame) if cv2.waitKey(1) & 0xFF == ord('q'): break # 保存 if frame_idx % SAVE_INTERVAL == 0: - json_data = {"frame_id": frame_idx, "camera_params": {"fov": FOV, "w": IMAGE_W, "h": IMAGE_H}, - "matrices": {"ego_w2c": ego_w2c.tolist(), "rsu_w2c": rsu_w2c.tolist()}, - "targets": visible_targets} + json_data = { + "frame_id": frame_idx, + "camera_params": {"fov": FOV, "w": IMAGE_W, "h": IMAGE_H}, + "matrices": {"ego_w2c": ego_w2c.tolist(), "rsu_w2c": rsu_w2c.tolist()}, + "targets": visible_targets + } save_queue.put({"path": save_path, "fid": frame_idx, "ego_img": data['ego'], "rsu_img": data['rsu'], "json_data": json_data}) finally: @@ -233,7 +252,15 @@ def main(): settings = world.get_settings(); settings.synchronous_mode = False; world.apply_settings(settings) for actor in actor_list: if actor: actor.destroy() - print("\n清理完毕。") + +def save_worker(q): + while writing_thread_running or not q.empty(): + try: + task = q.get(timeout=0.1) + task['ego_img'].save_to_disk(f"{task['path']}/ego_rgb/{task['fid']:08d}.jpg") + task['rsu_img'].save_to_disk(f"{task['path']}/rsu_rgb/{task['fid']:08d}.jpg") + with open(f"{task['path']}/label/{task['fid']:08d}.json", 'w') as f: json.dump(task['json_data'], f, indent=2) + except: continue if __name__ == '__main__': main() \ No newline at end of file From dcfb11b9346d36d3f7ee46dd82569950baf42ef5 Mon Sep 17 00:00:00 2001 From: haoliuaa <2121765139@qq.com> Date: Sat, 20 Dec 2025 16:04:31 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=96=B0=E8=84=9A?= =?UTF-8?q?=E6=9C=AC=E5=8F=8A=E4=BF=AE=E6=94=B9=E5=8E=9F=E6=9C=89=E8=84=9A?= =?UTF-8?q?=E6=9C=AC=E4=BD=BF=E5=85=B6=E9=80=82=E9=85=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../cvips_collector.py | 120 ++++-------- .../cvips_collector_6cam.py | 176 ++++++++++++++++++ src/InterconVis_PedSafety_Enh/cvips_utils.py | 62 ++---- src/InterconVis_PedSafety_Enh/data_check.py | 55 +++--- 4 files changed, 255 insertions(+), 158 deletions(-) create mode 100644 src/InterconVis_PedSafety_Enh/cvips_collector_6cam.py diff --git a/src/InterconVis_PedSafety_Enh/cvips_collector.py b/src/InterconVis_PedSafety_Enh/cvips_collector.py index 4246500e60..6c8aa98cd6 100644 --- a/src/InterconVis_PedSafety_Enh/cvips_collector.py +++ b/src/InterconVis_PedSafety_Enh/cvips_collector.py @@ -24,9 +24,6 @@ writing_thread_running = True def configure_weather(world, weather_arg, time_arg): - """ - 【完整保留】最初的天气和时间逻辑 - """ presets = { 'Clear': carla.WeatherParameters.ClearNoon, 'Cloudy': carla.WeatherParameters.CloudyNoon, @@ -34,42 +31,22 @@ def configure_weather(world, weather_arg, time_arg): 'Rain': carla.WeatherParameters.HardRainNoon, 'Storm': carla.WeatherParameters.HardRainSunset } - weather = presets.get(weather_arg, carla.WeatherParameters.ClearNoon) - - # 修改太阳高度角模拟时间 - if time_arg == 'Day': - weather.sun_altitude_angle = 60.0 - elif time_arg == 'Sunset': - weather.sun_altitude_angle = 10.0 - elif time_arg == 'Night': - weather.sun_altitude_angle = -90.0 - + if time_arg == 'Day': weather.sun_altitude_angle = 60.0 + elif time_arg == 'Sunset': weather.sun_altitude_angle = 10.0 + elif time_arg == 'Night': weather.sun_altitude_angle = -90.0 world.set_weather(weather) - print(f"🌦️ 环境设置成功: {weather_arg} | {time_arg}") def is_visible_refined(world, camera_actor, target_actor): - """ - 【修复版】可见性检测:针对两轮车做了高度优化 - """ cam_trans = camera_actor.get_transform() - # 起点:相机位置 + 向上提一点,彻底避开主车车身 ray_start = cam_trans.location + carla.Location(z=0.5) + cam_trans.get_forward_vector() * 2.0 - - # 终点:两轮车较低,瞄准 z=0.6,轿车/行人瞄准 z=1.0 - target_z = 0.6 if 'bike' in target_actor.type_id or 'bicycle' in target_actor.type_id or 'motorcycle' in target_actor.type_id else 1.0 + target_z = 0.6 if any(x in target_actor.type_id for x in ['bike', 'bicycle', 'motorcycle']) else 1.0 target_loc = target_actor.get_transform().location + carla.Location(z=target_z) - hit_points = world.cast_ray(ray_start, target_loc) - if not hit_points: return True # 路径没撞击到任何东西,肯定可见 - - # 只要第一个碰撞点离目标中心较近,就判定为可见(防止因车体大而误判) + if not hit_points: return True return hit_points[0].location.distance(target_loc) < 3.0 def draw_box(img, target, w2c, K): - """ - 【稳定版】绘制3D框,行人红色,车绿色,两轮车紫色 - """ loc, rot, extent = target['location'], target['rotation'], target['extent'] offset = target.get('center_offset', [0, 0, 0]) @@ -79,6 +56,7 @@ def draw_box(img, target, w2c, K): dx, dy, dz = extent[0], extent[1], extent[2] ox, oy, oz = offset[0], offset[1], offset[2] + # 定义8个局部顶点 corners_local = np.array([ [ox+dx, oy+dy, oz+dz, 1], [ox+dx, oy-dy, oz+dz, 1], [ox+dx, oy-dy, oz-dz, 1], [ox+dx, oy+dy, oz-dz, 1], [ox-dx, oy+dy, oz+dz, 1], [ox-dx, oy-dy, oz+dz, 1], [ox-dx, oy-dy, oz-dz, 1], [ox-dx, oy+dy, oz-dz, 1] @@ -86,43 +64,44 @@ def draw_box(img, target, w2c, K): img_pts = [] for pt in corners_local: + # 局部 -> 世界 w_pos = np.dot(obj_to_world, pt) + # 世界 -> 像素 (使用 utils 的投影逻辑) p = utils.get_image_point(carla.Location(x=w_pos[0], y=w_pos[1], z=w_pos[2]), K, w2c) img_pts.append(p) + if any(p is None for p in img_pts): return img + edges = [(0,1), (1,2), (2,3), (3,0), (4,5), (5,6), (6,7), (7,4), (0,4), (1,5), (2,6), (3,7)] if target['type'] == 'walker': color = (0, 0, 255) elif target['type'] == 'bike': color = (255, 0, 255) else: color = (0, 255, 0) for s, e in edges: - if img_pts[s] and img_pts[e]: - cv2.line(img, tuple(img_pts[s]), tuple(img_pts[e]), color, 2) + cv2.line(img, tuple(img_pts[s]), tuple(img_pts[e]), color, 2) return img def main(): - # 【恢复】所有原始参数解析 parser = argparse.ArgumentParser() parser.add_argument('--town', default='Town01') - parser.add_argument('--weather', default='Clear', choices=['Clear', 'Cloudy', 'Wet', 'Rain', 'Storm']) - parser.add_argument('--time', default='Day', choices=['Day', 'Sunset', 'Night']) - parser.add_argument('--num_vehicles', default=50, type=int) - parser.add_argument('--num_walkers', default=30, type=int) + parser.add_argument('--weather', default='Clear') + parser.add_argument('--time', default='Day') + parser.add_argument('--num_vehicles', default=40, type=int) + parser.add_argument('--num_walkers', default=20, type=int) parser.add_argument('--max_frames', default=500, type=int) args = parser.parse_args() - scene_name = f"{args.town}_{args.weather}_{args.time}" + scene_name = f"{args.town}_{args.weather}_{args.time}_{int(time.time())}" save_path = os.path.join(OUTPUT_FOLDER, scene_name) for sub in ['ego_rgb', 'rsu_rgb', 'label']: os.makedirs(os.path.join(save_path, sub), exist_ok=True) client = carla.Client('localhost', 2000) - client.set_timeout(20.0) + client.set_timeout(15.0) world = client.get_world() if args.town not in world.get_map().name: world = client.load_world(args.town) - # 【恢复】环境配置 configure_weather(world, args.weather, args.time) settings = world.get_settings() @@ -139,38 +118,33 @@ def main(): spawn_points = world.get_map().get_spawn_points() random.shuffle(spawn_points) - # 1. 主车 (Ego) + # 1. 主车 ego_bp = bp_lib.find('vehicle.tesla.model3') ego_vehicle = world.spawn_actor(ego_bp, spawn_points[0]) ego_vehicle.set_autopilot(True, tm.get_port()) actor_list.append(ego_vehicle) - # 2. 生成背景车 (包含自行车/摩托车) - print(f"🚗 正在生成 {args.num_vehicles} 辆背景车...") - batch = [] - for i, transform in enumerate(spawn_points[1:]): - if i >= args.num_vehicles: break + # 2. 生成背景车和行人 + for i in range(1, min(len(spawn_points), args.num_vehicles + 1)): bp = random.choice(bp_lib.filter('vehicle.*')) - batch.append(carla.command.SpawnActor(bp, transform).then( - carla.command.SetAutopilot(carla.command.FutureActor, True, tm.get_port()))) - - for r in client.apply_batch_sync(batch): - if not r.error: actor_list.append(world.get_actor(r.actor_id)) + npc = world.try_spawn_actor(bp, spawn_points[i]) + if npc: + npc.set_autopilot(True, tm.get_port()) + actor_list.append(npc) - # 3. 【恢复】生成行人 - print(f"🚶 正在生成 {args.num_walkers} 个行人...") for _ in range(args.num_walkers): loc = world.get_random_location_from_navigation() if loc: - w = world.try_spawn_actor(random.choice(bp_lib.filter('walker.pedestrian.*')), carla.Transform(loc)) - if w: actor_list.append(w) + w_bp = random.choice(bp_lib.filter('walker.pedestrian.*')) + walker = world.try_spawn_actor(w_bp, carla.Transform(loc)) + if walker: actor_list.append(walker) - # 4. 相机设置 + # 3. 相机设置 cam_bp = bp_lib.find('sensor.camera.rgb') cam_bp.set_attribute('image_size_x', str(IMAGE_W)); cam_bp.set_attribute('image_size_y', str(IMAGE_H)) cam_bp.set_attribute('fov', str(FOV)) ego_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(z=2.0, x=1.0)), attach_to=ego_vehicle) - rsu_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(x=20, y=10, z=15), carla.Rotation(pitch=-60))) + rsu_cam = world.spawn_actor(cam_bp, carla.Transform(carla.Location(x=10, y=10, z=15), carla.Rotation(pitch=-45))) actor_list.extend([ego_cam, rsu_cam]) image_queue = queue.Queue() @@ -181,40 +155,28 @@ def main(): threading.Thread(target=save_worker, args=(save_queue,), daemon=True).start() K = utils.build_projection_matrix(IMAGE_W, IMAGE_H, FOV) - spectator = world.get_spectator() - print("\n🚀 运行中... 按 Q 退出预览") - for frame_idx in range(args.max_frames): world.tick() - - # 【保留】自动视角跟随 - ego_t = ego_vehicle.get_transform() - spectator.set_transform(carla.Transform(ego_t.location + carla.Location(x=-12, z=6), - carla.Rotation(pitch=-25, yaw=ego_t.rotation.yaw))) - data = {} for _ in range(2): - name, img = image_queue.get() - data[name] = img + name, img_data = image_queue.get() + data[name] = img_data ego_w2c = utils.build_world_to_camera_matrix(ego_cam.get_transform()) rsu_w2c = utils.build_world_to_camera_matrix(rsu_cam.get_transform()) - # 标注可见目标 - all_actors = world.get_actors().filter('vehicle.*') - all_walkers = world.get_actors().filter('walker.pedestrian.*') - visible_targets = [] - for actor in list(all_actors) + list(all_walkers): + ego_loc = ego_vehicle.get_location() + + # 扫描周围 80米内的车辆和行人 + for actor in world.get_actors(): if actor.id == ego_vehicle.id: continue - if actor.get_transform().location.distance(ego_t.location) > 80: continue + if 'vehicle' not in actor.type_id and 'walker' not in actor.type_id: continue + if actor.get_location().distance(ego_loc) > 80: continue - # 遮挡检测判定 if is_visible_refined(world, ego_cam, actor): t, bb = actor.get_transform(), actor.bounding_box - - # 识别类型 atype = 'vehicle' if 'walker' in actor.type_id: atype = 'walker' elif any(x in actor.type_id for x in ['bike', 'bicycle', 'motorcycle']): atype = 'bike' @@ -228,14 +190,12 @@ def main(): }) # 预览 - array = np.frombuffer(data['ego'].raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (IMAGE_H, IMAGE_W, 4)) + array = np.frombuffer(data['ego'].raw_data, dtype=np.uint8).reshape((IMAGE_H, IMAGE_W, 4)) frame = array[:, :, :3].copy() for t in visible_targets: frame = draw_box(frame, t, ego_w2c, K) - cv2.imshow("Final Stable Preview", frame) + cv2.imshow("Collector Preview", frame) if cv2.waitKey(1) & 0xFF == ord('q'): break - # 保存 if frame_idx % SAVE_INTERVAL == 0: json_data = { "frame_id": frame_idx, @@ -248,10 +208,10 @@ def main(): finally: global writing_thread_running writing_thread_running = False - cv2.destroyAllWindows() settings = world.get_settings(); settings.synchronous_mode = False; world.apply_settings(settings) for actor in actor_list: if actor: actor.destroy() + cv2.destroyAllWindows() def save_worker(q): while writing_thread_running or not q.empty(): diff --git a/src/InterconVis_PedSafety_Enh/cvips_collector_6cam.py b/src/InterconVis_PedSafety_Enh/cvips_collector_6cam.py new file mode 100644 index 0000000000..0806e03c2c --- /dev/null +++ b/src/InterconVis_PedSafety_Enh/cvips_collector_6cam.py @@ -0,0 +1,176 @@ +import carla +import random +import queue +import numpy as np +import cv2 +import cvips_utils as utils + +# ================= 配置区域 ================= +IMAGE_W, IMAGE_H = 640, 360 +FOV = 90.0 +TARGET_FPS = 30 +# =========================================== + +def get_w2c_matrix(cam_transform): + """构建绝对对齐的 W2C 矩阵""" + world_2_cam_ue = np.linalg.inv(utils.get_matrix(cam_transform)) + calibration = np.array([ + [0, 1, 0, 0], + [0, 0, -1, 0], + [1, 0, 0, 0], + [0, 0, 0, 1] + ]) + return np.dot(calibration, world_2_cam_ue) + +def draw_3d_box_generic(img, transform, bb, is_walker, K, w2c): + """ + 通用 3D 画框函数 + transform: 物体的世界变换 (carla.Transform) + bb: 物体的碰撞盒 (carla.BoundingBox) + is_walker: 是否为行人 (决定颜色) + """ + # 1. 获取物体的世界变换矩阵 + obj_to_world = utils.get_matrix(transform) + + # 2. 计算 8 个顶点 (考虑中心偏移 loc 和 范围 ext) + ext = bb.extent + loc = bb.location + corners = np.array([ + [loc.x+ext.x, loc.y+ext.y, loc.z+ext.z, 1], [loc.x+ext.x, loc.y-ext.y, loc.z+ext.z, 1], + [loc.x+ext.x, loc.y-ext.y, loc.z-ext.z, 1], [loc.x+ext.x, loc.y+ext.y, loc.z-ext.z, 1], + [loc.x-ext.x, loc.y+ext.y, loc.z+ext.z, 1], [loc.x-ext.x, loc.y-ext.y, loc.z+ext.z, 1], + [loc.x-ext.x, loc.y-ext.y, loc.z-ext.z, 1], [loc.x-ext.x, loc.y+ext.y, loc.z-ext.z, 1] + ]) + + pixels = [] + for corner in corners: + world_pos = np.dot(obj_to_world, corner) + p = utils.get_image_point(carla.Location(x=world_pos[0], y=world_pos[1], z=world_pos[2]), K, w2c) + if p is None: return img + pixels.append(tuple(p)) + + color = (0,0,255) if is_walker else (0,255,0) + edges = [(0,1),(1,2),(2,3),(3,0),(4,5),(5,6),(6,7),(7,4),(0,4),(1,5),(2,6),(3,7)] + for s, e in edges: + cv2.line(img, pixels[s], pixels[e], color, 1) + return img + +def main(): + # 1. 环境初始化 + client = carla.Client('localhost', 2000) + client.set_timeout(20.0) + world = client.get_world() + + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 1.0/TARGET_FPS + world.apply_settings(settings) + + tm = client.get_trafficmanager(8000) + tm.set_synchronous_mode(True) + + # 2. 健壮生成主车 + bp_lib = world.get_blueprint_library() + spawn_points = world.get_map().get_spawn_points() + ego_vehicle = None + print("正在寻找空位生成主车...") + while ego_vehicle is None: + spawn_point = random.choice(spawn_points) + ego_vehicle = world.try_spawn_actor(bp_lib.find('vehicle.tesla.model3'), spawn_point) + + ego_vehicle.set_autopilot(True, tm.get_port()) + print(f"主车已就绪,位置: {spawn_point.location}") + + # 3. 配置 6 摄像头 + cam_bp = bp_lib.find('sensor.camera.rgb') + cam_bp.set_attribute('image_size_x', str(IMAGE_W)) + cam_bp.set_attribute('image_size_y', str(IMAGE_H)) + cam_bp.set_attribute('fov', str(FOV)) + + mounts = { + 'Front': carla.Transform(carla.Location(x=1.5, z=2.0), carla.Rotation(yaw=0)), + 'FrontLeft': carla.Transform(carla.Location(x=1.5, z=2.0), carla.Rotation(yaw=-60)), + 'FrontRight': carla.Transform(carla.Location(x=1.5, z=2.0), carla.Rotation(yaw=60)), + 'Back': carla.Transform(carla.Location(x=-1.5, z=2.0), carla.Rotation(yaw=180)), + 'BackLeft': carla.Transform(carla.Location(x=-1.5, z=2.0), carla.Rotation(yaw=-120)), + 'BackRight': carla.Transform(carla.Location(x=-1.5, z=2.0), carla.Rotation(yaw=120)) + } + + cams = {}; queues = {} + for name, trans in mounts.items(): + c = world.spawn_actor(cam_bp, trans, attach_to=ego_vehicle) + q = queue.Queue(); c.listen(q.put) + cams[name] = c; queues[name] = q + + K = utils.build_projection_matrix(IMAGE_W, IMAGE_H, FOV) + + # 4. 扫描地图静态车 (ID 10 代表所有车辆类型) + print("正在扫描地图静态车辆...") + static_vehicles = world.get_environment_objects(10) + print(f"扫描完成:找到 {len(static_vehicles)} 辆静态车") + + # 5. 主循环 + print("\n🚀 全场景检测启动!按 'q' 退出") + try: + while True: + current_frame = world.tick() + snapshot = world.get_snapshot() + + # 严格帧对齐取图 + imgs = {} + for name, q in queues.items(): + while True: + data = q.get() + if data.frame == current_frame: + imgs[name] = data + break + if data.frame > current_frame: break # 防止死循环 + if len(imgs) < 6: continue + + # 获取动态 NPC + all_actors = list(world.get_actors().filter('vehicle.*')) + \ + list(world.get_actors().filter('walker.pedestrian.*')) + + frame_list = [] + display_order = ['FrontLeft', 'Front', 'FrontRight', 'BackLeft', 'Back', 'BackRight'] + + for name in display_order: + raw = imgs[name] + img = np.reshape(np.frombuffer(raw.raw_data, dtype="uint8"), (IMAGE_H, IMAGE_W, 4))[:,:,:3].copy() + cam_sn = snapshot.find(cams[name].id) + + if cam_sn: + w2c = get_w2c_matrix(cam_sn.get_transform()) + cam_loc = cam_sn.get_transform().location + + # --- 绘制动态物体 --- + for actor in all_actors: + if actor.id == ego_vehicle.id: continue + actor_sn = snapshot.find(actor.id) + if actor_sn: + if actor_sn.get_transform().location.distance(cam_loc) < 45: + is_walker = 'walker' in actor.type_id + img = draw_3d_box_generic(img, actor_sn.get_transform(), actor.bounding_box, is_walker, K, w2c) + + # --- 绘制静态物体 (路边车) --- + for obj in static_vehicles: + if obj.transform.location.distance(cam_loc) < 45: + img = draw_3d_box_generic(img, obj.transform, obj.bounding_box, False, K, w2c) + + cv2.putText(img, name, (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2) + frame_list.append(img) + + # 拼接显示 + combined = np.vstack([np.hstack(frame_list[:3]), np.hstack(frame_list[3:])]) + cv2.imshow("CVIPS Full Scene Detection", combined) + if cv2.waitKey(1) & 0xFF == ord('q'): break + + finally: + print("正在清理环境...") + settings = world.get_settings(); settings.synchronous_mode = False; world.apply_settings(settings) + ego_vehicle.destroy() + for c in cams.values(): c.destroy() + cv2.destroyAllWindows() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/InterconVis_PedSafety_Enh/cvips_utils.py b/src/InterconVis_PedSafety_Enh/cvips_utils.py index f337145572..9c2d07a0c4 100644 --- a/src/InterconVis_PedSafety_Enh/cvips_utils.py +++ b/src/InterconVis_PedSafety_Enh/cvips_utils.py @@ -1,12 +1,8 @@ -# --- START OF FILE cvips_utils.py --- import numpy as np import math import carla def build_projection_matrix(w, h, fov): - """ - 构建相机内参矩阵 (Intrinsic Matrix) K - """ focal = w / (2.0 * np.tan(fov * np.pi / 360.0)) K = np.identity(3) K[0, 0] = K[1, 1] = focal @@ -15,7 +11,7 @@ def build_projection_matrix(w, h, fov): return K def get_matrix(transform): - """标准 CARLA 变换矩阵构建""" + """Local to World 4x4 Matrix""" rotation = transform.rotation location = transform.location c_y = math.cos(math.radians(rotation.yaw)) @@ -24,7 +20,7 @@ def get_matrix(transform): s_r = math.sin(math.radians(rotation.roll)) c_p = math.cos(math.radians(rotation.pitch)) s_p = math.sin(math.radians(rotation.pitch)) - matrix = np.matrix(np.identity(4)) + matrix = np.identity(4) matrix[0, 3] = location.x matrix[1, 3] = location.y matrix[2, 3] = location.z @@ -37,57 +33,27 @@ def get_matrix(transform): matrix[2, 0] = s_p matrix[2, 1] = -c_p * s_r matrix[2, 2] = c_p * c_r - return np.array(matrix) + return matrix def build_world_to_camera_matrix(camera_transform): - """ - 构建 [世界坐标] -> [相机坐标(OpenCV格式)] 的变换矩阵 (Extrinsic) - 关键修正:UE4 (X前,Y右,Z上) -> OpenCV (Z前,X右,Y下) - """ - # 1. 计算相机在世界坐标下的位姿矩阵 (World -> CameraActor) + """World to Camera 4x4 Matrix (OpenCV format)""" cam_to_world = get_matrix(camera_transform) - - # 2. 求逆,得到 (CameraActor -> World) ... 实际上我们需要 World -> CameraActor - # 也就是将世界点变换到相机原本的坐标系中 world_to_cam_ue = np.linalg.inv(cam_to_world) - - # 3. 坐标轴修正矩阵 (UE4 -> OpenCV Standard) - # UE4: X(前), Y(右), Z(上) - # Cam: X(右), Y(下), Z(前) - # 变换关系: - # Cam_X = UE_Y - # Cam_Y = -UE_Z - # Cam_Z = UE_X - calibration = np.zeros((4, 4)) - calibration[0, 1] = 1 # Row 0 (Out X) takes Col 1 (In Y) - calibration[1, 2] = -1 # Row 1 (Out Y) takes Col 2 (In Z) * -1 - calibration[2, 0] = 1 # Row 2 (Out Z) takes Col 0 (In X) - calibration[3, 3] = 1 - - # 4. 组合矩阵: 先进行世界到UE相机的平移旋转,再进行轴变换 - w2c = np.dot(calibration, world_to_cam_ue) - return w2c + # 坐标轴转换:UE4 -> OpenCV + calibration = np.array([ + [0, 1, 0, 0], + [0, 0, -1, 0], + [1, 0, 0, 0], + [0, 0, 0, 1] + ]) + return np.dot(calibration, world_to_cam_ue) def get_image_point(loc, K, w2c): - """ - 3D点 -> 2D像素 - """ - # [x, y, z, 1] + """3D Loc -> 2D Pixel""" point = np.array([loc.x, loc.y, loc.z, 1]) - - # World -> Camera point_camera = np.dot(w2c, point) - - # 深度裁剪 (在相机背后的点剔除) - if point_camera[2] <= 0: - return None - - # Camera -> Image Plane (归一化) - # [u*z, v*z, z] = K * [xc, yc, zc] + if point_camera[2] <= 0: return None point_img = np.dot(K, point_camera[:3]) - - # 归一化 u = point_img[0] / point_img[2] v = point_img[1] / point_img[2] - return [int(u), int(v)] \ No newline at end of file diff --git a/src/InterconVis_PedSafety_Enh/data_check.py b/src/InterconVis_PedSafety_Enh/data_check.py index b3126b1b68..0edd0a1461 100644 --- a/src/InterconVis_PedSafety_Enh/data_check.py +++ b/src/InterconVis_PedSafety_Enh/data_check.py @@ -3,25 +3,21 @@ import numpy as np import os import glob +import carla import cvips_utils as utils -import carla def draw_box(img, target, w2c, K): - # 获取真值数据 - loc = target['location'] - rot = target['rotation'] - extent = target['extent'] + loc, rot, extent = target['location'], target['rotation'], target['extent'] offset = target.get('center_offset', [0, 0, 0]) - # 1. 构建物体的本地到世界的变换矩阵 - # 利用 carla 现成的 Transform 类来算旋转,最不容易出错 + # 1. 还原物体的世界变换矩阵 obj_transform = carla.Transform( carla.Location(x=loc[0], y=loc[1], z=loc[2]), carla.Rotation(pitch=rot[0], yaw=rot[1], roll=rot[2]) ) obj_to_world = utils.get_matrix(obj_transform) - # 2. 定义 3D 框的 8 个顶点 (加上 offset 修正中心点位置) + # 2. 定义 3D 框的 8 个顶点 (局部坐标) dx, dy, dz = extent[0], extent[1], extent[2] ox, oy, oz = offset[0], offset[1], offset[2] corners_local = np.array([ @@ -34,10 +30,10 @@ def draw_box(img, target, w2c, K): # 3. 投影到像素平面 img_pts = [] for pt in corners_local: - # 变换到世界坐标: World = Matrix * Local + # 局部 -> 世界 w_pos = np.dot(obj_to_world, pt) - # 转换为 Location 传给 utils world_loc = carla.Location(x=w_pos[0], y=w_pos[1], z=w_pos[2]) + # 世界 -> 像素 (使用 utils) pixel = utils.get_image_point(world_loc, K, w2c) img_pts.append(pixel) @@ -46,20 +42,23 @@ def draw_box(img, target, w2c, K): # 4. 连线绘制 edges = [(0,1), (1,2), (2,3), (3,0), (4,5), (5,6), (6,7), (7,4), (0,4), (1,5), (2,6), (3,7)] - color = (0, 255, 0) if target['type'] == 'vehicle' else (0, 0, 255) + if target['type'] == 'walker': color = (0, 0, 255) + elif target['type'] == 'bike': color = (255, 0, 255) + else: color = (0, 255, 0) + for s, e in edges: cv2.line(img, tuple(img_pts[s]), tuple(img_pts[e]), color, 2) return img def main(): - # 自动查找最新的文件夹 - all_dirs = sorted(glob.glob("_out_dataset_final/*"), key=os.path.getmtime) - if not all_dirs: - print("没有找到数据目录") + # 查找最新的数据文件夹 + dirs = sorted(glob.glob("_out_dataset_final/*"), key=os.path.getmtime) + if not dirs: + print("Error: No data found.") return - target_dir = all_dirs[-1] # 默认看最新的 - print(f"正在检查目录: {target_dir}") + target_dir = dirs[-1] + print(f"Checking: {target_dir}") json_files = sorted(glob.glob(os.path.join(target_dir, "label", "*.json"))) @@ -68,31 +67,27 @@ def main(): data = json.load(f) fid = data['frame_id'] - ego_path = os.path.join(target_dir, "ego_rgb", f"{fid:08d}.jpg") - rsu_path = os.path.join(target_dir, "rsu_rgb", f"{fid:08d}.jpg") - - if not os.path.exists(ego_path): continue + img_ego = cv2.imread(os.path.join(target_dir, "ego_rgb", f"{fid:08d}.jpg")) + img_rsu = cv2.imread(os.path.join(target_dir, "rsu_rgb", f"{fid:08d}.jpg")) - img_ego = cv2.imread(ego_path) - img_rsu = cv2.imread(rsu_path) + if img_ego is None or img_rsu is None: continue - # 恢复矩阵 + # 恢复矩阵和内参 ego_w2c = np.array(data['matrices']['ego_w2c']) rsu_w2c = np.array(data['matrices']['rsu_w2c']) - fov = data['camera_params']['fov'] - h, w = img_ego.shape[:2] - K = utils.build_projection_matrix(w, h, fov) + cam_p = data['camera_params'] + K = utils.build_projection_matrix(cam_p['w'], cam_p['h'], cam_p['fov']) for tgt in data['targets']: img_ego = draw_box(img_ego, tgt, ego_w2c, K) img_rsu = draw_box(img_rsu, tgt, rsu_w2c, K) - # 拼接显示 + # 拼接展示 vis = np.vstack([img_ego, img_rsu]) - vis = cv2.resize(vis, (0, 0), fx=0.6, fy=0.6) # 缩小一点 + vis = cv2.resize(vis, (0, 0), fx=0.6, fy=0.6) - cv2.imshow("Check Data", vis) + cv2.imshow("Data Validation (Q to quit, Any key for next)", vis) if cv2.waitKey(0) == ord('q'): break