KITTI 데이터 사용하기 (LiDAR-카메라 변환)

최근에 LiDAR 데이터를 사용할 필요가 있어서, 테스트 목적으로 KITTI 데이터셋을 받아보았다. 최종 목적은 LiDAR 데이터를 카메라 이미지에 매핑해서 영상과 함께 거리값을 이용하는 것이다. 그런데, LiDAR를 처음 사용하다 보니 LiDAR 데이터 포맷도 모르겠고,  KITTI에서 제공하는 캘리브레이션 정보도 어떻게 해석해야 할지 모르겠다. 그래서, 여기 저기 검색해 보고 테스트해 보면서 나름 파악한 내용을 정리해 본다.

 

1.  LiDAR 데이터 포맷

 

KITTI에서 제공하는 LiDAR 데이터는 64채널 Velodyne 센서(HDL-64E) 데이터로서 확장자가 *.bin이다. 처음에는 거리값들이 순서대로 저장되어 있나 했는데, 그건 아니고 ray의 끝 점 (x, y, z) 좌표와 반사도(reflectance) 값이 순서대로 저장되어 있다. 각각의 데이터 값은 float 타입으로 4 byte를 차지한다.

 

그림1. LiDAR bin 파일 데이터 포맷

LiDAR 좌표계 기준은 LiDAR 센서 중심이 원점 (0, 0, 0)이고 전방(front)이 x축, 왼쪽이 y축, 상단(하늘 방향)이 z축이다. 센서 장착 위치 및 좌표축 방향은 KITTI Sensor Setup 페이지에 그림으로 잘 설명되어 있다.

 

이제 bin 파일을 실제 Matlab으로 불러서 가시화해 보자. Matlab 예제 코드 및 실행 결과는 다음과 같다. 샘플 데이터로는 KITTI 데이터셋 중에서 가장 용량이 적은 road dataset을 이용하였다.

 

fid = fopen('data_road/training/velodyne/uu_000024.bin', 'r'); data = fread(fid,'single'); fclose(fid); data = reshape(data,4,[])' ; pcshow(data(:,1:3));

 

코드에서 'single'은 단정밀도(single precision) 부동소수점으로 데이터를 읽는다는 의미이다. 실행하면 다음과 같이 멋진 lidar 데이터를 볼 수 있다.

그림2. LiDAR 데이터 가시화

 결과를 보면, 360도 전방향에 대해 데이터가 기록된 것임을 알 수 있다. 참고로, Velodyne HDL-64E의 수평시야각은 360도, 수직시야각은 26.8도이다. 개인적으로 내부 데이터를 분석해 보니 맨 위쪽 스캔라인(scane line) 정면방향부터 시작해서 반시계방향으로 회전하면서 맨 아래쪽 스캔라인까지 데이터가 순차적으로 기록되어 있다.

 

그리고, 위 LiDAR 데이터에 대응되는 이미지와 비교해 보면 가시화가 제대로 되었다는 것을 확인할 수 있다.

그림3. data_road/training/image_2/uu_000024.png

 

2.  LiDAR - 카메라 매핑

 

LiDAR 데이터를 카메라 이미지에 매핑하기 위해서는 둘 사이의 캘리브레이션 관계를 이해해야 한다. KITTI 데이터셋에는 센서간 캘리브레이션 파일이 같이 제공되는데, 예를 들어, 'data_road/training/calib/uu_000024.txt' 파일의 내용은 다음과 같다.

 

P0: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 0.000000000000e+00 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 P1: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.875744000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 P2: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 4.485728000000e+01 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.163791000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.745884000000e-03 P3: 7.215377000000e+02 0.000000000000e+00 6.095593000000e+02 -3.395242000000e+02 0.000000000000e+00 7.215377000000e+02 1.728540000000e+02 2.199936000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 2.729905000000e-03 R0_rect: 9.999239000000e-01 9.837760000000e-03 -7.445048000000e-03 -9.869795000000e-03 9.999421000000e-01 -4.278459000000e-03 7.402527000000e-03 4.351614000000e-03 9.999631000000e-01 Tr_velo_to_cam: 7.533745000000e-03 -9.999714000000e-01 -6.166020000000e-04 -4.069766000000e-03 1.480249000000e-02 7.280733000000e-04 -9.998902000000e-01 -7.631618000000e-02 9.998621000000e-01 7.523790000000e-03 1.480755000000e-02 -2.717806000000e-01 Tr_imu_to_velo: 9.999976000000e-01 7.553071000000e-04 -2.035826000000e-03 -8.086759000000e-01 -7.854027000000e-04 9.998898000000e-01 -1.482298000000e-02 3.195559000000e-01 2.024406000000e-03 1.482454000000e-02 9.998881000000e-01 -7.997231000000e-01 Tr_cam_to_road: 9.999069316651e-01 1.126439637474e-02 -7.700846908379e-03 -1.756802998348e-02 -1.132554323887e-02 9.999042860338e-01 -7.943238435040e-03 -1.626967196797e+00 7.610633921140e-03 8.029712858942e-03 9.999387540998e-01 2.839303758772e-01

 

내용을 보면 4개의 projection 행렬(P0, P1, P2, P3)과 1개의 rectification 행렬(R0_rect), 그리고 3개의 Tr 행렬이 제공된다.  KITTI Sensor Setup에 보면 Cam0, Cam1, Cam2, Cam3 총 4대의 카메라가 일렬로 나란히 배치되어 있는데, 이들 카메라 의 projection 행렬이 각각 P0, P1, P2, P3이다. 이들 중 가장 기본이 되는 카메라는 Cam2이며, 샘플로 다운로드 받은 road dataset에서도 Cam2를 사용하기 때문에 여기서는 P2만 사용하면 된다.

 

☞참고로, 카메라의 projection 행렬은 3D 월드좌표를 이미지 픽셀 좌표로 변환해주는 행렬을 의미한다. 3D 월드좌표 (X, Y, Z)가 있을 때, [R|t]를 곱해서 카메라좌표계로 변환한 후 이미지 평면에 투영하면 픽셀좌표 (x, y)가 얻어지는데, 이를 행렬 연산으로 표현하면 s[x y 1] = K[R|t][X Y Z 1]가 된다. 여기서, 3 x 4 행렬 P = K[R|t]를 카메라의 projection matrix라 부른다 (K는 3 x 3 camera matrix, R은 3 x 3 rotation matrix, t는 3 x 1 translation vector).

 

캘리브레이션 파일 내용에 보면 P2에 대해 "P2: p1 p2 p3 p4 p5 p6 p7 p8 p9 p10 p11 p12"와 같이 총 12개의 실수값이 저장되어 있는데, P2 = [p1 p2 p3 p4; p5 p6 p7 p8; p9 p10 p11 p12]와 같은 식으로 3 x 4 projection 행렬을 만들면 된다.

 

LiDAR 데이터를 카메라 이미지에 매핑하기 위해서는 1) LiDAR 좌표를 기준 좌표계로 변환한 후, 2) rectification을 수행, 3) 카메라 projection 행렬(여기서는 P2)을 이용해서 이미지에 투영하는 과정을 거친다. LiDAR 좌표가 (X, Y, Z)라면 Cam2로의 매핑 변환식은 다음과 같다 (만일, Cam1으로의 매핑이라면 P2만 P1으로 바꾸면 된다).

 

s(x, y, 1) = P2 * R0_rect * Tr_velo_to_cam * (X, Y, Z, 1)

 

먼저, Tr_velo_to_cam은 LiDAR 좌표를 기준 좌표계(reference coordinate)로 변환해주는 3 x 4 변환행렬로서, LiDAR 센서와 기준 좌표계 카메라와의 [R|t] 관계를 나타낸다. KITTI Sensor Setup 그림에 보면, Cam0가 기준 좌표계 카메라인 것으로 보인다. 다음으로, R0_rect를 곱해주는 rectification 과정이 조금 이해가 어려울 수 있다. R0_rect는 Cam0를 최종적인 월드평면으로 회전시켜주는 3 x 3 회전변환 행렬이다. KITTI Sensor Setup 그림에 보면 카메라 4대가 스테레오 카메라처럼 일렬로 나란히 배치된 것을 볼 수 있다. 스테레오 연산을 위해서는 이 4대의 카메라들이 모두 동일한 월드평면에 align이 되어야 하는데, Cam0를 이 월드평면에 align시켜주는 행렬이 R0_rect이다. R0_rect까지 곱해지고 나면 최종적인 월드좌표가 나오며 이 월드좌표를 각 카메라에 투영하면 카메라의 이미지 좌표가 얻어진다.

 

☞ 위 식에서 R0_rect * Tr_velo_to_cam * (X, Y, Z, 1)까지 계산하면 3 x 1 벡터(변환된 3차원 좌표)가 얻어진다. 여기에 3 x 4 행렬인 P2를 적용하기 위해서는 좌표의 끝에 1을 덧붙인 homogeneous 좌표로 변환한 후에 P2를 곱해야 한다. 또는 아래 Matlab 코드처럼 R0_rect를 4 x 4 행렬로 확장해도 동일한 효과를 얻는다 (확장된 원소값을 모두 0으로 설정하고, 마지막 4행 4열 원소만 1로 설정)

 

Matlab을 이용하여 KITTI 캘리브레이션 파일을 읽고, parsing하며, 이미지에 매핑하는 코드를 아래와 같이 작성해 보았다.

 

clear all; close all; % read calibration file and parse the matrices fid = fopen('data_road/training/calib/uu_000024.txt'); P0 = sscanf(fgetl(fid), 'P0: %f %f %f %f %f %f %f %f %f %f %f %f'); P1 = sscanf(fgetl(fid), 'P1: %f %f %f %f %f %f %f %f %f %f %f %f'); P2 = sscanf(fgetl(fid), 'P2: %f %f %f %f %f %f %f %f %f %f %f %f'); P3 = sscanf(fgetl(fid), 'P3: %f %f %f %f %f %f %f %f %f %f %f %f'); R0_rect = sscanf(fgetl(fid), 'R0_rect: %f %f %f %f %f %f %f %f %f'); Tr_velo_to_cam = sscanf(fgetl(fid), 'Tr_velo_to_cam: %f %f %f %f %f %f %f %f %f %f %f %f'); Tr_imu_to_velo = sscanf(fgetl(fid), 'Tr_imu_to_velo: %f %f %f %f %f %f %f %f %f %f %f %f'); Tr_cam_to_road = sscanf(fgetl(fid), 'Tr_cam_to_road: %f %f %f %f %f %f %f %f %f %f %f %f'); fclose(fid); Tr = [reshape(Tr_velo_to_cam, 4, [])'; 0 0 0 1]; R0 = eye(4); R0(1:3,1:3) = reshape(R0_rect, 3, [])'; P = reshape(P2, 4, [])'; % read image img = imread('data_road/training/image_2/uu_000024.png'); imshow(img); % read LiDAR data fid = fopen('data_road/training/velodyne/uu_000024.bin', 'r'); data = fread(fid,'single'); fclose(fid); data = reshape(data,4,[])'; figure;pcshow(data(:,1:3)); % mapping to image XYZ1 = [data(:,1:3)'; ones(1,size(data,1))]; xy1 = P*R0*Tr*XYZ1; s = xy1(3,:); x = xy1(1,:)./s; y = xy1(2,:)./s; figure; plot(x,y,'.'); ax = gca; ax.YDir = 'reverse'; img_h = size(img, 1); img_w = size(img, 2); xlim([1 img_w]); ylim([1 img_h]);

 

실행해 보면 다음과 같이 LiDAR 데이터를 이미지 평면에 투영한 결과를 얻을 수 있는데, 뭔가 조금 이상하다.

그림4. LiDAR 데이터의 이미지 투영 1

원인이 무엇일까 생각해 보니, 후방에 있는 LiDAR 데이터가 이미지 평면 뒷편에서 투영되었기 때문이다. 이 경우는 depth 성분인 s가 음수인 경우이기 때문에, s > 0인 경우로 투영을 한정하면 문제를 해결할 수 있다. 수정된 코드 및 결과는 다음과 같다.

 

% mapping to image XYZ1 = [data(:,1:3)'; ones(1,size(data,1))]; xy1 = P*R0*Tr*XYZ1; s = xy1(3,:); x = xy1(1,:)./s; y = xy1(2,:)./s; k = find(s>0); figure; plot(x(k),y(k),'.'); ax = gca; ax.YDir = 'reverse'; img_h = size(img, 1); img_w = size(img, 2); xlim([1 img_w]); ylim([1 img_h]);

 

그림5. LiDAR 데이터의 이미지 투영 2

 

이제 정상적으로 매핑된 것을 볼 수 있다. 마지막으로 실제 이미지에 LiDAR를 투영해 보면 아래 그림과 같은 결과를 얻을 수 있다.

 

% mapping to image img_mapped = img; img_h = size(img, 1); img_w = size(img, 2); XYZ1 = [data(:,1:3)'; ones(1,size(data,1))]; xy1 = P*R0*Tr*XYZ1; s = xy1(3,:); x = xy1(1,:)./s; y = xy1(2,:)./s; for i=1:length(s)     ix = int32(x(i) + 0.5);     iy = int32(y(i) + 0.5);     if (s(i)<=0 || ix<=0 || ix>img_w || iy<=0 || iy>img_h)         continue;     end     img_mapped(iy, ix, :) = [0 255 0]; end figure; imshow(img_mapped);

 

그림6. LiDAR 데이터의 이미지 투영 (최종 결과)

☞ 처음에는 LiDAR - 카메라간 매핑 관계만 있으면, 이미지의 모든 픽셀에 대해 depth를 구할 수 있을 것이라 생각했다. 그러나, 그림 6에서 보는 것처럼 LiDAR 데이터는 sparse하기 때문에 LiDAR가 매핑되는 픽셀에 대해서만 depth를 구할 수 있다. 그외 픽셀에 대해서는 보간(interpolation) 등의 방법을 사용할 수 있을 것이다. 그러나, 이미지 윗부분처럼 매핑된 LiDAR 데이터가 아에 없는 영역에 대해서는 depth를 전혀 구할 수 없다.

 

by 다크 프로그래머