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 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__":