carla bounding_box error 해결 및 방안

[이승현]

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17

[LiDAR] Location: -52.074 63.538 2.986 Rotation: 0.000 45.000 -0.000   [Ego Vehicle] Location: -52.074 63.538 0.586 Rotation: 0.000 45.000 0.000 BBox Center: -52.098 63.514 1.474 BBox Extent: 2.276 1.048 0.886   [Vehicle ID 46] Location: -48.565 85.645 0.586 Rotation: 0.000 45.000 0.000 BBox Center: -48.590 85.621 1.474 BBox Extent: 2.276 1.048 0.886     Colored by Color Scripter

cs

carla의 데이터를 ego, vehicle, lidar 의 location, rotation 정보와 bboxcenter,bboxextent 정보를 world기준으로 저장하였다

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123

#carla -> open3d  world 중심 시각화 코드   import open3d as o3d import numpy as np import os import cv2   def visualize_all(base_dir):     label_dir = os.path.join(base_dir, 'label')     vis = o3d.visualization.Visualizer()     vis.create_window(window_name="LiDAR + BBox + Axis",width=1080,height=800)     opt = vis.get_render_option()     opt.background_color = np.array([0, 0, 0])     opt.point_size = 1.0       world_axis = o3d.geometry.TriangleMesh.create_coordinate_frame(size=10.0, origin=[0,0,0])     vis.add_geometry(world_axis)       def parse_lidar_ego_pose(label_file):         with open(label_file, 'r') as f:             lines = f.readlines()         lidar_origin, lidar_yaw, ego_origin, ego_yaw = None, None, None, None         for idx, line in enumerate(lines):             if '[LiDAR]' in line:                 loc = list(map(float, lines[idx+1].split(':')[1].split()))                 rot = list(map(float, lines[idx+2].split(':')[1].split()))                 lidar_origin, lidar_yaw = loc, rot[1]             if '[Ego Vehicle]' in line:                 loc = list(map(float, lines[idx+1].split(':')[1].split()))                 rot = list(map(float, lines[idx+2].split(':')[1].split()))                 ego_origin, ego_yaw = loc, rot[1]         return lidar_origin, lidar_yaw, ego_origin, ego_yaw       def load_all_bboxes(label_file):         bboxes = []         with open(label_file, 'r') as f:             lines = f.readlines()           loc, rotation, center, extent = None, None, None, None           for idx, line in enumerate(lines):             if '[Ego Vehicle]' in line or '[Vehicle ID' in line:                 loc, rotation, center, extent = None, None, None, None               if 'Location:' in line and ('[Ego Vehicle]' in lines[idx - 1] or '[Vehicle ID' in lines[idx - 1]):                 loc = list(map(float, line.strip().split(':')[1].split()))             if 'Rotation:' in line and ('[Ego Vehicle]' in lines[idx - 2] or '[Vehicle ID' in lines[idx - 2]):                 rotation = list(map(float, line.strip().split(':')[1].split()))             if 'BBox Center:' in line:                 center = list(map(float, line.strip().split(':')[1].split()))             if 'BBox Extent:' in line:                 extent = list(map(float, line.strip().split(':')[1].split()))                 if loc and rotation and center and extent:                     c = np.array(center)                     c[1] *= -1                     obb = o3d.geometry.OrientedBoundingBox()                     obb.center = c                     obb.extent = np.array(extent) * 2                     yaw = np.deg2rad(rotation[1])                     R = o3d.geometry.get_rotation_matrix_from_axis_angle([0, 0, -yaw])                     obb.R = R                     obb.color = [1, 0, 0] if '[Vehicle ID' in lines[idx - 4] else [0, 1, 0]  # Vehicle 빨강, Ego 초록                     bboxes.append(obb)         return bboxes       frame_ids = sorted([int(f.split('_')[1].split('.')[0]) for f in os.listdir(base_dir) if f.startswith('pc_')])       for frame_id in frame_ids:         lidar_file = os.path.join(base_dir, f'pc_{frame_id}.ply')         label_file = os.path.join(label_dir, f'label_{frame_id}.txt')         camera_file = os.path.join(base_dir, f'{str(frame_id).zfill(8)}.png')           if os.path.exists(camera_file):             img = cv2.imread(camera_file)             cv2.imshow('Camera View', img)             cv2.waitKey(10)           lidar_origin, lidar_yaw, ego_origin, ego_yaw = parse_lidar_ego_pose(label_file)         lidar_origin[1] *= -1         ego_origin[1] *= -1         pcd = o3d.io.read_point_cloud(lidar_file)         pts = np.asarray(pcd.points)         pts[:,1] *= -1         pcd.points = o3d.utility.Vector3dVector(pts)         pcd.colors = o3d.utility.Vector3dVector(np.ones_like(pts))           pcd.translate(lidar_origin)         lidar_R = pcd.get_rotation_matrix_from_xyz((0, 0, np.pi * -lidar_yaw / 180))         pcd.rotate(lidar_R, center=lidar_origin)           bboxes = load_all_bboxes(label_file)           lidar_axis = o3d.geometry.TriangleMesh.create_coordinate_frame(size=3.0, origin=lidar_origin)         lidar_axis.rotate(lidar_R, center=lidar_origin)           ego_axis = o3d.geometry.TriangleMesh.create_coordinate_frame(size=3.0, origin=ego_origin)         ego_R = ego_axis.get_rotation_matrix_from_xyz((0,0,np.pi * -ego_yaw/180))         ego_axis.rotate(ego_R, center=ego_origin)           vis.clear_geometries()         vis.add_geometry(world_axis)         vis.add_geometry(pcd)         vis.add_geometry(lidar_axis)         #vis.add_geometry(ego_axis)         for box in bboxes:             vis.add_geometry(box)         vis.poll_events()         vis.update_renderer()         # set zoom, front, up, and lookat         vis.get_view_control().set_zoom(0.3)         vis.get_view_control().set_front([0, 0, 1])         vis.get_view_control().set_up([1, 0, 0])         vis.get_view_control().set_lookat(lidar_origin)         vis.run()       vis.destroy_window()     cv2.destroyAllWindows()     # ✅ 실행 base_dir = 'C:/Users/airlab/Carla-0.10.0/PythonAPI/examples/_out' visualize_all(base_dir)   Colored by Color Scripter

cs

저장한 ply,txt 파일을 불러와 시각화 하는 코드이다.

현재 world기준으로 pointcloud와 bbox를 그리고 시점을 ego차량으로 한 코드이다. 이것을 ego기준으로 pointcloud와 bbox를 그리는 코드로 바꿀 예정이다.