7일차 과제

[조수호]

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 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199

import numpy as np import cv2 import open3d as o3d # 1번  calib_file_path=r"C:/jsh/kitti/data_object_calib/training/calib/000010.txt" label_file_path=r"C:/jsh/kitti/data_object_label_2/training/label_2/000010.txt" image_file_path=r"C:/jsh/kitti/data_object_image_2/training/image_2/000010.png" pcd_file_path =r"C:/jsh/kitti/data_object_velodyne/training/velodyne/000010.bin"   def open_label(label_file_path):     labels = []     with open(label_file_path) as f:          for line in f.readlines():             obj_label = line.strip().split()             obj_type = obj_label[0]             truncated = float(obj_label[1])             occluded = int(obj_label[2])             alpha = float(obj_label[3])             bbox = np.array(obj_label[4:8]).astype(float)             dimension = np.array(obj_label[8:11]).astype(float)             location = np.array(obj_label[11:14]).astype(float)                          # 중심 y 좌표 수정             location[1] -= dimension[0] / 2                          rotation = float(obj_label[14])                          # 딕셔너리 타입으로 저장             label = {                 'type': obj_type,                 'truncated': truncated,                 'occluded': occluded,                 'alpha': alpha,                 'bbox': bbox,                 'dimensions': dimension,                 'location': location,                 'rotation_y': rotation             }             labels.append(label)     for data in labels:         print(data['type'], data['truncated'], data['occluded'], data['alpha'], data['bbox'],               data['dimensions'], data['location'], data['rotation_y'])     return labels # 파싱 결과 출력 def open_calib(calib_file_path):     with open(calib_file_path) as f:         lines = f.readlines()             P0 = np.array(lines[0].strip().split(":")[1].split(), dtype=np.float32)         P1 = np.array(lines[1].strip().split(":")[1].split(), dtype=np.float32)         P2 = np.array(lines[2].strip().split(":")[1].split(), dtype=np.float32)         P3 = np.array(lines[3].strip().split(":")[1].split(), dtype=np.float32)         R0_rect_ = np.array(lines[4].strip().split(":")[1].split(), dtype=np.float32)         Tr_velo_to_cam= np.array(lines[5].strip().split(":")[1].split(), dtype=np.float32)         Tr_imu_to_velo= np.array(lines[6].strip().split(":")[1].split(), dtype=np.float32)         Tr_velo_to_cam= np.vstack((Tr_velo_to_cam.reshape(3,4), [0., 0., 0., 1.]))         P2 = np.vstack((P2.reshape(3, 4), [0., 0., 0., 0.]))         R0_rect = np.eye(4)         R0_rect[:3, :3] = R0_rect_.reshape(3, 3)         calib_matrix= {         "P0": P0,         "P1": P1,         "P2": P2,         "P3": P3,         "R0_rect": R0_rect,         "Tr_velo_to_cam": Tr_velo_to_cam,         "Tr_imu_to_velo": Tr_imu_to_velo,         }          print("P2\n",calib_matrix["P2"])         print("R0_rect\n",calib_matrix["R0_rect"])         print("Tr_velo_to_cam\n",calib_matrix["Tr_velo_to_cam"])         return calib_matrix # Rectified 카메라좌표계에서 8개의 3D 꼭지점 좌표 구하기 def img_bbox_print(labels, calib_matrix):     print("개수 : " ,len(labels))     image = cv2.imread(image_file_path)     for data in labels:         h, w, l = data['dimensions']  # unpacking         x, y, z = data['location']  # unpacking         yaw = data['rotation_y']         P2 = calib_matrix["P2"]         R = np.array([[np.cos(yaw), 0, np.sin(yaw)],                      [0, 1, 0],                      [-np.sin(yaw), 0, np.cos(yaw)]])         x_corners = [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2]         y_corners = [h/2, h/2, h/2, h/2, -h/2, -h/2, -h/2, -h/2]         z_corners = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2]         corners_3d = np.dot(R, np.vstack([x_corners, y_corners, z_corners]))         corners_3d += np.vstack([x, y, z])           # 3x8 행렬로 출력, 열 단위로 꼭지점 좌표(x, y, z) 저장됨         print(corners_3d)           # 영상 좌표계에서 8개의 2D 꼭지점 좌표 구하기         corners_3d_hom = np.vstack([corners_3d, np.ones((1, 8))])  # 동차 좌표로 변환           # print(corners_3d_hom)         corners_2d_hom = np.dot(P2, corners_3d_hom)  # 8개 좌표를 투영 변환(3D -> 2D)           # print(corners_2d_hom)         corners_2d_hom[0, :] /= corners_2d_hom[2, :]  # 동차 좌표(4차원)에서 2차원 좌표 추출         corners_2d_hom[1, :] /= corners_2d_hom[2, :]           corners_2d = corners_2d_hom[0:2, :]  # 2x8 행렬로 출력, 열 단위로 꼭지점 좌표(x, y) 저장됨         print(corners_2d)           # 영상에 꼭지점 번호와 바운딩 박스 그리기                  for i in range(0, corners_2d.shape[1]):             cv2.putText(image, str(i), (int(corners_2d[0, i]), int(corners_2d[1, i])),                         cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255))                      lineset = np.array([[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 i in range(0, lineset.shape[0]):             cv2.line(image, (int(corners_2d[0, lineset[i, 0]]), int(corners_2d[1, lineset[i, 0]])),                     (int(corners_2d[0, lineset[i, 1]]), int(corners_2d[1, lineset[i, 1]])), (255, 0, 0), 1)                 cv2.imshow('2d bbox', image)     cv2.waitKey()   labels = open_label(label_file_path) calib_matrix = open_calib(calib_file_path) img_bbox_print(labels, calib_matrix)       # 2번 def pcd_bbox_print(pcd_file_path, labels, calib_matrix):     # 3d bbox 파라미터, 변환행렬 추출     # (N, 4) 형식의 numpy array로 변환     points = np.fromfile(pcd_file_path, dtype=np.float32).reshape(-1, 4)     print("배열 크기:", points.shape)  # 넘파이 ndarray 크기 출력     # open3d로 라이다 점 구름 및 바운딩박스 시각화     vis = o3d.visualization.Visualizer()     vis.create_window(window_name='kitti', width=960, height=540)     vis.get_render_option().point_size = 1.0     vis.get_render_option().background_color = np.zeros(3)       axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])     vis.add_geometry(axis_pcd)       pcd = o3d.geometry.PointCloud()     pcd.points = o3d.utility.Vector3dVector(points[:, :3])     pcd.colors = o3d.utility.Vector3dVector(np.ones((points.shape[0], 3)))       for data in labels:         h, w, l = data['dimensions']  # unpacking         x, y, z = data['location']  # unpacking         ry = data['rotation_y']           R0_rect = calib_matrix["R0_rect"]         R0_rect_inv = np.linalg.inv(R0_rect)         Tr_velo_to_cam = calib_matrix["Tr_velo_to_cam"]         Tr_velo_to_cam_inv = np.linalg.inv(Tr_velo_to_cam)           print(R0_rect_inv)         print(Tr_velo_to_cam_inv)              # 동차 변환을 이용하여 라이다 좌표계에서 3D bbox의 중심 좌표 (x, y, z) 계산         center3d_camrec = np.vstack([x, y, z, 1])         center3d_cam0 = np.dot(R0_rect_inv, center3d_camrec)         center3d_vel = np.dot(Tr_velo_to_cam_inv, center3d_cam0)         center3d_cam0 = center3d_cam0[0:3, :]         print(center3d_cam0)         center3d_vel = center3d_vel[0:3, :]         print(center3d_vel)         center = center3d_vel           # 라이다 좌표계에서 3D bbox 파라미터 계산 (l, w, h, yaw)         lwh = np.array([l, w, h])         angle = -ry - np.pi / 2         if angle >= np.pi:             angle -= np.pi         if angle < -np.pi:             angle = 2 * np.pi + angle         print("lidar_yaw:", angle)           # open3d 바운딩박스 생성         axis_angles = np.array([0, 0, angle])         rot = o3d.geometry.get_rotation_matrix_from_axis_angle(axis_angles)         open3d_bbox = o3d.geometry.OrientedBoundingBox(center, rot, lwh)         open3d_bbox.color = [1, 0, 0]  # Set the color to red           vis.add_geometry(pcd)         vis.add_geometry(open3d_bbox)           # set zoom, front, up, and lookat         vis.get_view_control().set_zoom(0.1)         vis.get_view_control().set_front([0, -1, 1])         vis.get_view_control().set_lookat([0, 0, 0])         vis.get_view_control().set_up([0, 1, 1])       vis.run()     vis.destroy_window()   pcd_bbox_print(pcd_file_path, labels, calib_matrix)   Colored by Color Scripter

cs