[open3d] 5. KITTI 영상에 바운딩박스 그리기

[Sungryul Lee]

1.레이블정보를 이용하여 영상에 바운딩박스 그리기

- 레이블 파일에 저장된 3차원 바운딩 박스(gt box, ground truth box) 정보를 이용하여 영상에 그려주는 코드

- 레이블파일의 모든 좌표는 rectified camera coordinate 기준으로 작성됨

- 레이블파일의 3d 바운딩박스 중심점 정보는 전체박스의 중심점이 아니고 박스의 밑면의 중심점임을 주의할것

- 영상은 camera 2로 촬영한 영상임 -> 영상에 그림을 그릴려면 2번 카메라 좌표계의 2차원 좌표로 변환해야함

- 변환순서

레이블파일의 바운딩박스정보(h,w,l,x,y,z,ry) -> 바운딩박스의 모서리 3d좌표 8개 8*(x,y,z) -> camera2 영상 좌표계의 2차원 좌표 8*(x,y)

위 그림에서 Fc 좌표계가 3차원 카메라 좌표계를 의미하고 회색평면에 있는 x,y축이 2차원 카메라 좌표계를 의미

3차원 카메라 좌표(x,y,z)를 2차원 카메라(영상) 좌표(u,v)로 사영(project) 시키는 원리를 설명해주는 그림임

3d 객체의 2d 바운딩 박스와 3d 바운딩 박스의 정의

객체의 3d 바운딩박스는 중심점 좌표(x,y,z)와 폭(w, width), 높이(h, height), 길이(l, length), z축 기준의 방향각(yaw) 정의됨

(x,y,z,l,w,h, yaw(rz))

위 그림은 레이블파일에 저장되어 있는 파라미터의 정의를 보여줌, 레이블파일의 모든 값은 카메라 좌표계(y축이 아래방향, z축이 전방, x축이 오른쪽를 기준으로 측정됨

<소스코드>

kitti_utils.py

24.64KB

import numpy as np

import kitti_utils as kitti

import cv2

cv_box_colormap = [

    [255, 0, 0],

    [0, 255, 0],

    [0, 0, 255],

    [1, 0, 255],

]

lidar_file_path = r"D:\Users\2sungryul\Dropbox\Work\Dataset\KITTI\data_object_velodyne\training\velodyne\000010.bin"

label_file_path = r"D:\Users\2sungryul\Dropbox\Work\Dataset\KITTI\data_object_label_2\training\label_2\000010.txt"

calib_file_path = r"D:\Users\2sungryul\Dropbox\Work\Dataset\KITTI\data_object_calib\training\calib\000010.txt"

image_file_path = r"D:\Users\2sungryul\Dropbox\Work\Dataset\KITTI\data_object_image_2\training\image_2\000010.png"

labels_feature_dict, class_ids, bbox2D_cam, bbox3D_cam = kitti.parse_label_file(label_file_path)

print(class_ids, labels_feature_dict["labels/obj_center_cam"], bbox3D_cam)

calib_feature_dict, matrix_proj_2, matrix_tr_velo_to_cam, R_cam_to_rect = kitti.parse_calib_file(calib_file_path)

image = cv2.imread(image_file_path)

for i in range(len(bbox3D_cam)):

    corners_3d_cam2 = kitti.compute_3d_box_cam2(bbox3D_cam[i])

    print(corners_3d_cam2)

    pts_2d = kitti.project_rect_to_image(matrix_proj_2, corners_3d_cam2.T)

    print(pts_2d)

    kitti.draw_box3d_on_image(image, pts_2d, cv_box_colormap[class_ids[i]], thickness=2)

cv2.imshow("Image", image)

cv2.waitKey()

<실행결과>

2. 라이다좌표를 이용하여 영상에 바운딩박스 그리기

3차원 객체 검출 모델에서 예측한 바운딩 박스의 정보를 2번 카메라에서 촬영한 영상에 그려보자

3차원 객체 검출 모델의 출력은 라이다 좌표계에서 계산된 좌표임 (x,y,z,l,w,h,yaw(rz) ) 

따라서, 영상에 바운딩박스를 그리기 위해서는 다음과 같은 순서로 좌표변환이 필요함

라이다좌표계(x,y,z,l,w,h,yaw(rz) ) -> 라이다좌표계에서 바운딩박스의 모서리 8개 좌표(x,y,z) 계산(open3d 라이브러리 이용) -> 카메라 좌표계로 변환(3d 포인트 8개) -> rectified camera 좌표계로 변환(3d 포인트 8개) -> 2번 카메라 영상 좌표계(2d 포인트(x,y) 8개)

<소스코드>

import numpy as np

import kitti_utils as kitti

import cv2

cv_box_colormap = [

    [255, 0, 0],

    [0, 255, 0],

    [0, 0, 255],

    [1, 0, 255],

]

lidar_file_path = r"D:\Users\2sungryul\Dropbox\Work\Dataset\KITTI\data_object_velodyne\training\velodyne\000010.bin"

label_file_path = r"D:\Users\2sungryul\Dropbox\Work\Dataset\KITTI\data_object_label_2\training\label_2\000010.txt"

calib_file_path = r"D:\Users\2sungryul\Dropbox\Work\Dataset\KITTI\data_object_calib\training\calib\000010.txt"

image_file_path = r"D:\Users\2sungryul\Dropbox\Work\Dataset\KITTI\data_object_image_2\training\image_2\000010.png"

# lidar 3d bbox (x,y,z,l,w,h,rz)

lidar_3bbox = np.array([ [ 5.4826984, -4.4219365, -0.9296398,  3.35,        1.65,        1.57,  -0.15079632],

                [12.081573,    2.3993492,  -0.8686094,   3.95,        1.7,         1.43,   2.952389  ],

                [23.789532,   -8.322558,   -0.48450655,  1.09,        0.72,        1.96,   2.962389  ],

                [16.782623,   -5.8402405,  -0.84653795,  3.24,        1.6,         1.51,  -0.13079633],

                [22.332664,   -6.859388,   -0.80930686,  4.1,        1.74,        1.45,  -0.18079633],

                [23.921867,    0.39140925, -0.81109357,  3.79,        1.68,        1.54,   2.932389  ],

                [29.351864,   -0.6278057,  -0.7701172,   3.35,        1.52,        1.49,   2.922389  ],

                [28.813488,   -7.8675747,  -0.8422416,   4.37,        1.65,        1.53,  -0.17079632],

                [43.13186,    -4.4860353,  -0.6518749,   3.48,        1.45,        1.64,   2.692389  ]]).astype(float)

# 레이블 파일 읽기

# labels_feature_dict, class_ids, bbox2D_cam, bbox3D_cam = kitti.parse_label_file(label_file_path)

# print(class_ids, labels_feature_dict["labels/obj_center_cam"], bbox3D_cam)

# 캘리브레이션(calib) 파일 읽기 -> 변환행렬 읽기

calib_feature_dict, matrix_proj_2, matrix_tr_velo_to_cam, R_cam_to_rect = kitti.parse_calib_file(calib_file_path)

image = cv2.imread(image_file_path)

for i in range(len(lidar_3bbox)):

# lidar 3d bbox (x,y,z,l,w,h,rz) -> open3d 바운딩박스 생성

    open3d_bbox = kitti.create_open3d_bounding_box(lidar_3bbox[i]) 

# open3d 바운딩박스의 모서리 8개의 좌표 읽기

    open3d_bbox_corner = np.asarray(open3d_bbox.get_box_points())

    print("open3d_bbox_corner",open3d_bbox_corner)

# open3d 바운딩박스의 모서리 8개의 좌표를 카메라 좌표계로 변환    

    ref_3bbox_cornder = kitti.project_velo_to_ref(matrix_tr_velo_to_cam, open3d_bbox_corner)

    print("ref_3bbox_cornder",ref_3bbox_cornder)

# 카메라 좌표계로 변환된 8개좌표 -> rectified camera coordinate 좌표로 변환

    rect_3bbox_cornder = kitti.project_ref_to_rect(R_cam_to_rect, ref_3bbox_cornder)

    print("rect_3bbox_cornder",rect_3bbox_cornder)

# rectified camera coordinate 좌표 -> 2번 카메라 영상 좌표계로 변환 

    pts_2d = kitti.project_rect_home_to_image(matrix_proj_2, rect_3bbox_cornder)

    print(pts_2d)

#  2번 카메라 영상 좌표 8개로 바운딩박스 그리기 

    kitti.draw_open3d_box3d_on_image(image, pts_2d, cv_box_colormap[class_ids[i]], thickness=2)

cv2.imshow("Image", image)

cv2.waitKey()

<실행결과>