open3D 11

[김위제]

11

01

예측확률이 0.5이상인 객체만 바운딩 박스를 그려라

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

#import argparse import glob from pathlib import Path import open3d #from visual_utils import open3d_vis_utils as V import numpy as np import torch from pcdet.config import cfg, cfg_from_yaml_file from pcdet.datasets import DatasetTemplate from pcdet.models import build_network, load_data_to_gpu from pcdet.utils import common_utils   class DemoDataset(DatasetTemplate):     def __init__(self, dataset_cfg, class_names, training=True, root_path=None, logger=None, ext='.bin'):         """         Args:             root_path:             dataset_cfg:             class_names:             training:             logger:         """         super().__init__(             dataset_cfg=dataset_cfg, class_names=class_names, training=training, root_path=root_path, logger=logger         )         self.root_path = root_path         self.ext = ext         data_file_list = glob.glob(str(root_path / f'*{self.ext}')) if self.root_path.is_dir() else [self.root_path]           data_file_list.sort()         self.sample_file_list = data_file_list       def __len__(self):         return len(self.sample_file_list)       def __getitem__(self, index):         if self.ext == '.bin':             points = np.fromfile(self.sample_file_list[index], dtype=np.float32).reshape(-1, 4)         elif self.ext == '.npy':             points = np.load(self.sample_file_list[index])         else:             raise NotImplementedError           input_dict = {             'points': points,             'frame_id': index,         }           data_dict = self.prepare_data(data_dict=input_dict)         return data_dict   def model_inference(cfg_file, data_path, ckpt_path):     #ext = '.bin'     cfg_from_yaml_file(cfg_file, cfg)     logger = common_utils.create_logger()          demo_dataset = DemoDataset(dataset_cfg=cfg.DATA_CONFIG, class_names=cfg.CLASS_NAMES, training=False, root_path=Path(data_path))     print(f'Total number of samples: \t{len(demo_dataset)}')     model = build_network(model_cfg=cfg.MODEL, num_class=len(cfg.CLASS_NAMES), dataset=demo_dataset)     model.load_params_from_file(filename=ckpt_path, logger=logger, to_cpu=True)     model.cuda()     model.eval‎()     pcd = []     pred_dict = {}     with torch.no_grad():         for idx, data_dict in enumerate(demo_dataset):             logger.info(f'Visualized sample index: \t{idx + 1}')             data_dict = demo_dataset.collate_batch([data_dict])             load_data_to_gpu(data_dict)             pred_dicts, _ = model.forward(data_dict)             pcd = torch.Tensor.tolist(data_dict['points'][:, 1:4])             pred_dict['pred_boxes'] = torch.Tensor.tolist(pred_dicts[0]['pred_boxes'])             pred_dict['pred_scores'] = torch.Tensor.tolist(pred_dicts[0]['pred_scores'])             pred_dict['pred_labels'] = torch.Tensor.tolist(pred_dicts[0]['pred_labels'])       return pred_dict, pcd   if __name__ == '__main__':       cfg_file = "pointpillar.yaml"     #cfg_file = "pointrcnn.yaml"     #cfg_file = "second.yaml"     #cfg_file = "voxel_rcnn_car.yaml"     #cfg_file = "pv_rcnn.yaml"       data_path = "000000.bin"     #data_path = "000008.bin"              ckpt_path = "pointpillar_7728.pth"     #ckpt_path = "pointrcnn_7870.pth"     #ckpt_path = "second_7862.pth"     #ckpt_path = "voxel_rcnn_car_84.54.pth"     #ckpt_path = "pv_rcnn_8369.pth"          pred_dict,pcd = model_inference(cfg_file=cfg_file, data_path=data_path, ckpt_path=ckpt_path)     print("scores: ", pred_dict)     vis = open3d.visualization.Visualizer()     vis.create_window(window_name="kitti")     vis.get_render_option().point_size = 2.0     vis.get_render_option().background_color = np.zeros(3)       axis_pcd = open3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])     vis.add_geometry(axis_pcd)       pts = open3d.geometry.PointCloud()     pts.points = open3d.utility.Vector3dVector(pcd)     pts.colors = open3d.utility.Vector3dVector(np.ones((len(pcd), 3)))     vis.add_geometry(pts)       box_color = [[0,1,0],[0,1,1],[1,1,0],[1,1,1]]       for i in range(len(pred_dict['pred_boxes'])):         if pred_dict['pred_scores'][i] > 0.5:             center = pred_dict['pred_boxes'][i][0:3]             lwh = pred_dict['pred_boxes'][i][3:6]             axis_angles = np.array([0, 0, pred_dict['pred_boxes'][i][6]])             rot = open3d.geometry.get_rotation_matrix_from_axis_angle(axis_angles)             box3d = open3d.geometry.OrientedBoundingBox(center, rot, lwh)             box3d.color = box_color[pred_dict['pred_labels'][i]-1]              vis.add_geometry(box3d)                         vis.run()     vis.destroy_window()         Colored by Color Scripter

cs

02

레이블 파일에 있는 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 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 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219

import numpy as np import open3d as o3d import labels as LB import calib_matrix as VM #import argparse import glob from pathlib import Path import open3d #from visual_utils import open3d_vis_utils as V import numpy as np import torch from pcdet.config import cfg, cfg_from_yaml_file from pcdet.datasets import DatasetTemplate from pcdet.models import build_network, load_data_to_gpu from pcdet.utils import common_utils   class DemoDataset(DatasetTemplate):     def __init__(self, dataset_cfg, class_names, training=True, root_path=None, logger=None, ext='.bin'):         """         Args:             root_path:             dataset_cfg:             class_names:             training:             logger:         """         super().__init__(             dataset_cfg=dataset_cfg, class_names=class_names, training=training, root_path=root_path, logger=logger         )         self.root_path = root_path         self.ext = ext         data_file_list = glob.glob(str(root_path / f'*{self.ext}')) if self.root_path.is_dir() else [self.root_path]           data_file_list.sort()         self.sample_file_list = data_file_list       def __len__(self):         return len(self.sample_file_list)       def __getitem__(self, index):         if self.ext == '.bin':             points = np.fromfile(self.sample_file_list[index], dtype=np.float32).reshape(-1, 4)         elif self.ext == '.npy':             points = np.load(self.sample_file_list[index])         else:             raise NotImplementedError           input_dict = {             'points': points,             'frame_id': index,         }           data_dict = self.prepare_data(data_dict=input_dict)         return data_dict   def model_inference(cfg_file, data_path, ckpt_path):     #ext = '.bin'     cfg_from_yaml_file(cfg_file, cfg)     logger = common_utils.create_logger()          demo_dataset = DemoDataset(dataset_cfg=cfg.DATA_CONFIG, class_names=cfg.CLASS_NAMES, training=False, root_path=Path(data_path))     print(f'Total number of samples: /t{len(demo_dataset)}')     model = build_network(model_cfg=cfg.MODEL, num_class=len(cfg.CLASS_NAMES), dataset=demo_dataset)     model.load_params_from_file(filename=ckpt_path, logger=logger, to_cpu=True)     model.cuda()     model.eval‎()     pcd = []     pred_dict = {}     with torch.no_grad():         for idx, data_dict in enumerate(demo_dataset):             logger.info(f'Visualized sample index: /t{idx + 1}')             data_dict = demo_dataset.collate_batch([data_dict])             load_data_to_gpu(data_dict)             pred_dicts, _ = model.forward(data_dict)             pcd = torch.Tensor.tolist(data_dict['points'][:, 1:4])             pred_dict['pred_boxes'] = torch.Tensor.tolist(pred_dicts[0]['pred_boxes'])             pred_dict['pred_scores'] = torch.Tensor.tolist(pred_dicts[0]['pred_scores'])             pred_dict['pred_labels'] = torch.Tensor.tolist(pred_dicts[0]['pred_labels'])       return pred_dict, pcd       def transform_bbox_to_lidar(data, calib):     h, w, l = data['dimensions']     x, y, z = data['location']     ry = data['rotation_y']       R0_rect_inv = np.linalg.inv(calib["R0_rect"])     Tr_velo_to_cam_inv = np.linalg.inv(calib["Tr_velo_to_cam"])       center_cam_rec = np.array([[x], [y], [z], [1]])     center_cam0 = R0_rect_inv @ center_cam_rec     center_lidar = Tr_velo_to_cam_inv @ center_cam0     center = center_lidar[:3].flatten()       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       return center, lwh, angle     def create_open3d_bbox(center, lwh, angle, color=[1, 0, 0]):          axis_angles = np.array([0, 0, angle])     rot = o3d.geometry.get_rotation_matrix_from_axis_angle(axis_angles)     box = o3d.geometry.OrientedBoundingBox(center, rot, lwh)     box.color = color     return box     def load_point_cloud(file_path):          points = np.fromfile(file_path, dtype=np.float32).reshape(-1, 4)     return points[:, :3]     def assign_colors_by_bbox(pc_xyz, bboxes):          colors = np.ones((pc_xyz.shape[0], 3))  # 기본 흰색     for bbox in bboxes:         idx = bbox.get_point_indices_within_bounding_box(o3d.utility.Vector3dVector(pc_xyz))         colors[idx] = [0, 0, 1]  # 파란색     return colors     def visualize_scene(pc_xyz, colors, bboxes):       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 = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])     vis.add_geometry(axis)       pcd = o3d.geometry.PointCloud()     pcd.points = o3d.utility.Vector3dVector(pc_xyz)     pcd.colors = o3d.utility.Vector3dVector(colors)     vis.add_geometry(pcd)       for bbox in bboxes:         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()     def main(vis, pc_xyz):     calib = VM.calib_matrix  # calib matrix 불러오기     bbox_list = []     for data in LB.labels:         center, lwh, angle = transform_bbox_to_lidar(data, calib)         bbox = create_open3d_bbox(center, lwh, angle, color=[1, 0, 0])  # 빨간색 정답 박스         bbox_list.append(bbox)         vis.add_geometry(bbox)         if __name__ == "__main__":     cfg_file = "pointpillar.yaml"     #cfg_file = "pointrcnn.yaml"     #cfg_file = "second.yaml"     #cfg_file = "voxel_rcnn_car.yaml"     #cfg_file = "pv_rcnn.yaml"       data_path = "000000.bin"     #data_path = "000008.bin"              ckpt_path = "pointpillar_7728.pth"     #ckpt_path = "pointrcnn_7870.pth"     #ckpt_path = "second_7862.pth"     #ckpt_path = "voxel_rcnn_car_84.54.pth"     #ckpt_path = "pv_rcnn_8369.pth"          pred_dict,pcd = model_inference(cfg_file=cfg_file, data_path=data_path, ckpt_path=ckpt_path)     print("scores: ", pred_dict)     vis = open3d.visualization.Visualizer()     vis.create_window(window_name="kitti")     vis.get_render_option().point_size = 2.0     vis.get_render_option().background_color = np.zeros(3)       axis_pcd = open3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])     vis.add_geometry(axis_pcd)       pts = open3d.geometry.PointCloud()     pts.points = open3d.utility.Vector3dVector(pcd)     pts.colors = open3d.utility.Vector3dVector(np.ones((len(pcd), 3)))     vis.add_geometry(pts)       box_color = [[0,1,0],[0,1,1],[1,1,0],[1,1,1]]       for i in range(len(pred_dict['pred_boxes'])):         center = pred_dict['pred_boxes'][i][0:3]         lwh = pred_dict['pred_boxes'][i][3:6]         axis_angles = np.array([0, 0, pred_dict['pred_boxes'][i][6]])         rot = open3d.geometry.get_rotation_matrix_from_axis_angle(axis_angles)         box3d = open3d.geometry.OrientedBoundingBox(center, rot, lwh)         box3d.color = (0,1,1)         vis.add_geometry(box3d)                # 기존 vis 생성 부분 밑에     main(vis, np.asarray(pts.points))  # pc_xyz를 pts.points로 넘겨줌         vis.run()     vis.destroy_window()       Colored by Color Scripter

cs

03

추론 결과를 이용하여 영상 파일에 3d 바운딩 박스를 그려라, 먼저 라이다 좌표계에서 8개의 3차원 꼭지점 좌표(get_box_points 함수 이용)를 구하고 좌표변환 행렬(Tr_vel_to_cam -> R0_rect -> P2)을 이용하여 영상 좌표계의 8개의 2차원 꼭지점으로 변환하고 opencv 함수를 이용하여 라인을 그려라

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

import numpy as np import cv2 import glob from pathlib import Path import torch from pcdet.config import cfg, cfg_from_yaml_file from pcdet.datasets import DatasetTemplate from pcdet.models import build_network, load_data_to_gpu from pcdet.utils import common_utils import calib_matrix as VM  # P2, R0_rect, Tr_velo_to_cam 포함한 모듈   ### 추론용 Dataset 클래스 class DemoDataset(DatasetTemplate):     def __init__(self, dataset_cfg, class_names, training=True, root_path=None, logger=None, ext='.bin'):         super().__init__(dataset_cfg=dataset_cfg, class_names=class_names, training=training, root_path=root_path, logger=logger)         self.root_path = root_path         self.ext = ext         data_file_list = glob.glob(str(root_path / f'*{self.ext}')) if self.root_path.is_dir() else [self.root_path]         data_file_list.sort()         self.sample_file_list = data_file_list       def __len__(self):         return len(self.sample_file_list)       def __getitem__(self, index):         if self.ext == '.bin':             points = np.fromfile(self.sample_file_list[index], dtype=np.float32).reshape(-1, 4)         elif self.ext == '.npy':             points = np.load(self.sample_file_list[index])         else:             raise NotImplementedError         input_dict = {'points': points, 'frame_id': index}         return self.prepare_data(input_dict)     ### 모델 추론 함수 def model_inference(cfg_file, data_path, ckpt_path):     cfg_from_yaml_file(cfg_file, cfg)     logger = common_utils.create_logger()     dataset = DemoDataset(dataset_cfg=cfg.DATA_CONFIG, class_names=cfg.CLASS_NAMES, training=False, root_path=Path(data_path))     model = build_network(model_cfg=cfg.MODEL, num_class=len(cfg.CLASS_NAMES), dataset=dataset)     model.load_params_from_file(filename=ckpt_path, logger=logger, to_cpu=True)     model.cuda()     model.eval‎()       with torch.no_grad():         for data_dict in dataset:             data_dict = dataset.collate_batch([data_dict])             load_data_to_gpu(data_dict)             pred_dicts, _ = model.forward(data_dict)               pred_dict = {                 'boxes': pred_dicts[0]['pred_boxes'].cpu().numpy(),                 'scores': pred_dicts[0]['pred_scores'].cpu().numpy(),                 'labels': pred_dicts[0]['pred_labels'].cpu().numpy()             }               return pred_dict     # ### 3D 바운딩 박스 → 8개 꼭짓점 (LiDAR 좌표계) # def get_box_points(box): #     x, y, z, l, w, h, yaw = box #     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 = [0, 0, 0, 0, -h, -h, -h, -h] #     z_corners = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2] #     corners = np.dot(R, np.vstack([x_corners, y_corners, z_corners])) #     corners += np.array([[x], [y], [z]]) #     return corners  # (3,8)   def get_box_points(box):     x, y, z, l, w, h, yaw = box       # LiDAR 기준: yaw는 z축 기준 회전     R = np.array([         [np.cos(yaw), -np.sin(yaw), 0],         [np.sin(yaw),  np.cos(yaw), 0],         [0,            0,           1]     ])       # 중심 기준 좌표 (KITTI 기준 중심은 박스의 중간, 즉 z축 기준 ± h/2)     x_corners = [ l/2,  l/2, -l/2, -l/2,  l/2,  l/2, -l/2, -l/2]     y_corners = [ w/2, -w/2, -w/2,  w/2,  w/2, -w/2, -w/2,  w/2]     z_corners = [ h/2,  h/2,  h/2,  h/2, -h/2, -h/2, -h/2, -h/2]  # 중앙 기준으로 위/아래 나눔       corners = R @ np.vstack([x_corners, y_corners, z_corners])     corners += np.array([[x], [y], [z]])  # 중심 위치 적용       return corners  # shape: (3, 8)         ### LiDAR 좌표계 → 이미지 좌표계 def project_lidar_to_image(corners, Tr_velo_to_cam, R0_rect, P2):     # corners: (3, 8)     corners_hom = np.vstack((corners, np.ones((1, corners.shape[1]))))  # (4, 8)       # Tr_velo_to_cam: (3x4) or (4x4)     if Tr_velo_to_cam.shape == (4, 4):         Tr = Tr_velo_to_cam[:3, :]  # (3x4)로 자름     else:         Tr = Tr_velo_to_cam       # LiDAR → Camera 좌표계     cam_corners = Tr @ corners_hom  # (3, 8)       # Rectify     cam_corners = R0_rect @ cam_corners  # (3, 8)       # Camera → Image 투영     img_pts = P2 @ np.vstack((cam_corners, np.ones((1, cam_corners.shape[1]))))  # (3, 8)     img_pts[:2] /= img_pts[2]       return img_pts[:2]  # (2, 8)     ### OpenCV 이미지 위에 3D 박스 그리기 def draw_projected_box_on_image(img, corners_2d, color=(0, 255, 0)):     corners_2d = corners_2d.T.astype(int)     lines = [         [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 start, end in lines:         pt1, pt2 = tuple(corners_2d[start]), tuple(corners_2d[end])         cv2.line(img, pt1, pt2, color, 2)     return img     ### 메인 실행 코드 if __name__ == '__main__':     cfg_file = "pointpillar.yaml"     data_path = "000000.bin"     ckpt_path = "pointpillar_7728.pth"     image_path = "000000.png"       image = cv2.imread(image_path)     pred_dict = model_inference(cfg_file, data_path, ckpt_path)       P2 = VM.calib_matrix["P2"]     R0_rect = VM.calib_matrix["R0_rect"]     Tr_velo_to_cam = VM.calib_matrix["Tr_velo_to_cam"]       for i, box in enumerate(pred_dict['boxes']):         if pred_dict['scores'][i] >= 0.15:             box_pts = get_box_points(box)             box_img_pts = project_lidar_to_image(box_pts, Tr_velo_to_cam, R0_rect, P2)             image = draw_projected_box_on_image(image, box_img_pts)       cv2.imshow("3D Bounding Boxes", image)     cv2.waitKey(0)     cv2.destroyAllWindows()   Colored by Color Scripter

cs