open3D 10. KITTI dataset visualization2

[한윤지]

10장

1. 예제에서 포인트 클라우드에서 객체에 해당하는 포인트를 파란색으로 출력하라.

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

import numpy as np import open3d as o3d import label as lb  # 레이블 파일 파싱 코드 import calib as cl  # 켈리브레이션 파일 파싱 코드   calib_file_path="../../dataset/data_object_calib/training/calib/000010.txt" label_file_path = "../../dataset/data_object_label_2/training/label_2/000010.txt" image_file_path = "../../dataset/data_object_image_2/training/image_2/000010.png" pcd_file_path = "../../dataset/data_object_velodyne/training/velodyne/000010.bin" # 레이블 파일 파싱 코드 생략 # 캘리브레이션 파일 파싱 코드 생략포인트 클라우드에 3d 바운딩박스 시각화   # 3d bbox 파라미터, 변환행렬 추출 data = lb.labels[1] #2번째 객체만 추출 h,w,l = data['dimensions'] #unpacking x,y,z = data['location'] #unpacking ry = data['rotation_y'] R0_rect = cl.calib_matrix["R0_rect"] R0_rect_inv = np.linalg.inv(R0_rect) Tr_velo_to_cam = cl.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포인트 클라우드에 3d 바운딩박스 시각화   # (N, 4) 형식의 numpy array로 변환 points = np.fromfile(pcd_file_path, dtype=np.float32).reshape(-1, 4)  print("배열 크기:", points.shape) # 넘파이 ndarray 크기 출력   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]) # 1. 바운딩박스 내부에 있는 포인트 인덱스 추출 indices = open3d_bbox.get_point_indices_within_bounding_box(pcd.points) # 2. 모든 포인트를 흰색으로 초기화 colors = np.ones((points.shape[0], 3))  # 기본 흰색 # 3. 바운딩박스 내부 포인트만 파란색으로 지정 colors[indices] = [0, 0, 1]  # BGR = 파란색 # 4. 색상 적용 pcd.colors = o3d.utility.Vector3dVector(colors)   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() Colored by Color Scripter

cs

2. 예제는 객체 1개에 대해서 그림을 그렸는데 모든 객체에 대하여 영상에 3d 바운딩 박스를 그려라 모든 세부기능을 함수로 정의하고 사용하라

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

import numpy as np import cv2 import label as lb import calib as cl   # 캘리브레이션 행렬(P2) 반환 def load_calibration():     return cl.calib_matrix["P2"]   # 객체 1개의 3D 바운딩 박스 꼭짓점 8개 (3x8)를 rectified camera 좌표계로 계산 def get_3d_box_corners(data):     h, w, l = data['dimensions']     x, y, z = data['location']     yaw = data['rotation_y']       # 회전 행렬     R = np.array([         [np.cos(yaw), 0, np.sin(yaw)],         [0, 1, 0],         [-np.sin(yaw), 0, np.cos(yaw)]     ])       # 바운딩 박스 8개 꼭짓점 (중심 기준)     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 = R @ np.vstack([x_corners, y_corners, z_corners]) + np.array([[x], [y], [z]])     return corners_3d  # shape (3, 8)   # 3D 점들(3xN)을 P 행렬로 2D 이미지 좌표계(2xN)로 투영 def project_to_image(corners_3d, P):     corners_3d_hom = np.vstack([corners_3d, np.ones((1, corners_3d.shape[1]))])     corners_2d_hom = P @ corners_3d_hom     corners_2d_hom[:2, :] /= corners_2d_hom[2, :]  # x/z, y/z     return corners_2d_hom[:2, :]   # 2D 영상 위에 바운딩 박스의 꼭짓점 번호와 선을 그림 def draw_3d_box(image, corners_2d, box_id=None):     # 각 꼭짓점 번호 출력     for i in range(corners_2d.shape[1]):         pt = (int(corners_2d[0, i]), int(corners_2d[1, i]))         cv2.putText(image, str(i), pt, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)       # 3D 박스 연결선     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, j in lineset:         pt1 = (int(corners_2d[0, i]), int(corners_2d[1, i]))         pt2 = (int(corners_2d[0, j]), int(corners_2d[1, j]))         cv2.line(image, pt1, pt2, (255, 0, 0), 2)       # 객체 ID가 주어졌다면 왼쪽 위에 출력     if box_id is not None:         pt = (int(np.min(corners_2d[0])), int(np.min(corners_2d[1])) - 5)         cv2.putText(image, f"Obj {box_id}", pt, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2)   # main 함수 def main():     # 파일 경로     image_file_path = "../../dataset/data_object_image_2/training/image_2/000010.png"       # 이미지 불러오기     image = cv2.imread(image_file_path)     if image is None:         raise FileNotFoundError(f"이미지를 불러올 수 없습니다: {image_file_path}")       # P2 행렬 불러오기     P2 = load_calibration()       # 모든 객체에 대해 바운딩 박스 처리     for idx, data in enumerate(lb.labels):         corners_3d = get_3d_box_corners(data)         corners_2d = project_to_image(corners_3d, P2)         draw_3d_box(image, corners_2d, box_id=idx)       # 시각화     cv2.imshow('3D Bounding Boxes on Image', image)     cv2.waitKey(0)     cv2.destroyAllWindows()   # 실행 if __name__ == "__main__":     main()   Colored by Color Scripter

cs

3. 예제는 객체 1개에 대해서 그림을 그렸는데 모든 객체에 대하여 포인트 클라우드에 3d 바운딩 박스를 그려라 모든 세부기능을 함수로 정의하고 사용하라

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

import numpy as np import open3d as o3d import label as lb  # 레이블 파싱 모듈 import calib as cl  # 캘리브레이션 파싱 모듈   # 캘리브레이션 행렬에서 두 행렬 불러와 역행렬로 반환하는 함수 def load_calibration():     R0_rect = cl.calib_matrix["R0_rect"]     R0_rect_inv = np.linalg.inv(R0_rect)     Tr_velo_to_cam = cl.calib_matrix["Tr_velo_to_cam"]     Tr_velo_to_cam_inv = np.linalg.inv(Tr_velo_to_cam)     return R0_rect_inv, Tr_velo_to_cam_inv   # 좌표(x,y,z)를 카메라 좌표계->라이다 좌표계 반환하는 함수 def get_lidar_center(x, y, z, R0_rect_inv, Tr_velo_to_cam_inv):     center_cam = np.vstack([x, y, z, 1])  # 4x1     center_cam0 = R0_rect_inv @ center_cam  # 4x1     center_velo = Tr_velo_to_cam_inv @ center_cam0  # 4x1     return center_velo[0:3]  # 3x1   # 바운딩 박스 생성하는 함수 def create_bbox_from_label(data, R0_rect_inv, Tr_velo_to_cam_inv):     h, w, l = data['dimensions']     x, y, z = data['location']     ry = data['rotation_y']       center = get_lidar_center(x, y, z, R0_rect_inv, Tr_velo_to_cam_inv)     size = np.array([l, w, h])  # Lidar 좌표계에서는 순서 주의       yaw = -ry - np.pi / 2     if yaw >= np.pi:         yaw -= np.pi     if yaw < -np.pi:         yaw += 2 * np.pi       axis_angle = np.array([0, 0, yaw])     rot = o3d.geometry.get_rotation_matrix_from_axis_angle(axis_angle)       bbox = o3d.geometry.OrientedBoundingBox(center, rot, size)     bbox.color = [1, 0, 0]  # 빨간색     return bbox   # bin 파일 읽어 pcd 객체로 반환하는 함수 def load_point_cloud(pcd_file_path):     points = np.fromfile(pcd_file_path, dtype=np.float32).reshape(-1, 4)     pcd = o3d.geometry.PointCloud()     pcd.points = o3d.utility.Vector3dVector(points[:, :3])     pcd.paint_uniform_color([1, 1, 1])  # 흰색     return pcd   # 포인트 클라우드와 모든 3D 바운딩 박스 Open3D로 시각화 def visualize(pcd, bbox_list):     vis = o3d.visualization.Visualizer()     vis.create_window(window_name='KITTI 3D Bounding Boxes', width=960, height=540)     vis.get_render_option().point_size = 1.0     vis.get_render_option().background_color = np.zeros(3)       vis.add_geometry(pcd)     vis.add_geometry(o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0]))     for bbox in bbox_list:         vis.add_geometry(bbox)       ctr = vis.get_view_control()     ctr.set_zoom(0.1)     ctr.set_front([0, -1, 1])     ctr.set_lookat([0, 0, 0])     ctr.set_up([0, 1, 1])       vis.run()     vis.destroy_window()   # main 함수 def main():     pcd_file_path = "../../dataset/data_object_velodyne/training/velodyne/000010.bin"     R0_rect_inv, Tr_velo_to_cam_inv = load_calibration()       pcd = load_point_cloud(pcd_file_path)       bbox_list = []     for data in lb.labels:         bbox = create_bbox_from_label(data, R0_rect_inv, Tr_velo_to_cam_inv)         bbox_list.append(bbox)       visualize(pcd, bbox_list)   # 실행 if __name__ == "__main__":     main()   Colored by Color Scripter

cs