處理點雲資料(四):點雲到影象平面的投影
阿新 • • 發佈:2019-02-02
點雲到影象平面的投影
座標系的定義
相機(x:右,y:下,z:前)
點雲(x:前,y:左,z:上)
讀取感測器校準引數
在KITTI資料集raw_data中有兩個感測器校準引數檔案calib_cam_to_cam.txt(相機到相機的標定) 和 calib_velo_to_cam.txt(點雲到相機的定位)。
base_dir = 'D:/KITTI/data_set/2011_09_26/2011_09_26_drive_0009_sync'; % 圖片目錄
calib_dir = 'D:/KITTI/data_set/data_object_calib/2011_09_26'; % 相機引數目錄
cam = 2 ; % 第3個攝像頭
frame = 0; % 第1幀(第一張圖片)
calib = loadCalibrationCamToCam(fullfile(calib_dir,'calib_cam_to_cam.txt'));
Tr_velo_to_cam = loadCalibrationRigid(fullfile(calib_dir,'calib_velo_to_cam.txt'));
calib_cam_to_cam.txt(相機到相機的標定):
其中:
- S_xx:1x2 矯正前的影象xx的大小
- K_xx:3x3 矯正前攝像機xx的校準矩陣
- D_xx:1x5 矯正前攝像頭xx的失真向量
- R_xx:3x3 (外部)的旋轉矩陣(從相機0到相機xx)
- T_xx:3x1 (外部)的平移向量(從相機0到相機xx)
- S_rect_xx:1x2 矯正後的影象xx的大小
- R_rect_xx:3x3 糾正旋轉矩陣(使影象平面共面)
- P_rect_xx:3x4 矯正後的投影矩陣
KITTI中有四個攝像頭,cell一行四列中的四列分別代表了四個不同的攝像頭。
calib_velo_to_cam.txt(點雲到相機的定位):
其中,
- R:3x3旋轉矩陣
- T:3x1平移向量
- delta_f:棄用
- delta_c:棄用
計算點雲到影象平面的投影矩陣
計算點雲到影象的投影矩陣需要三個引數,分別是P_rect(相機內參矩陣)和R_rect(參考相機0到相機xx影象平面的旋轉矩陣)以及Tr_velo_to_cam(點雲到相機的[R T]外參矩陣)。
% 計算點雲到影象平面的投影矩陣
R_cam_to_rect = eye (4);
R_cam_to_rect(1:3,1:3) = calib.R_rect{1}; % 參考相機0到相機xx影象平面的旋轉矩陣
P_velo_to_img = calib.P_rect{cam+1}*R_cam_to_rect*Tr_velo_to_cam; % 內外引數
讀取點雲資料
fid = fopen(sprintf('%s/velodyne_points/data/%010d.bin',base_dir,frame),'rb');
velo = fread(fid,[4 inf],'single')';
velo = velo(1:5:end,:); % 顯示速度每5點移除一次
fclose(fid);
% 刪除影象平面後面的所有點(近似值)
idx = velo(:,1)<5;
velo(idx,:) = [];
N*4的矩陣每一列分別代表X, Y, Z軸座標和反射率
點雲投影到影象
function p_out = project(p_in,T)
% p_in為點雲座標,T為點雲到影象的投影矩陣
% 資料和投影矩陣的維數
dim_norm = size(T,1); % 3維
dim_proj = size(T,2); % 4維
% 在齊次座標中進行轉換
p2_in = p_in;
if size(p2_in,2)<dim_proj
p2_in(:,dim_proj) = 1;
end
p2_out = (T*p2_in')';
% 歸一化齊次座標:
p_out = p2_out(:,1:dim_norm-1)./(p2_out(:,dim_norm)*ones(1,dim_norm-1));
畫點
點雲資料中,X軸座標代表了點雲深度,所以用點雲深度的大小代表了顏色的深度。
cols = jet;
% velo_img為點雲在影象上的座標
for i=1:size(velo_img,1)
col_idx = round(64*5/velo(i,1));
plot(velo_img(i,1),velo_img(i,2),'o','LineWidth',4,'MarkerSize',1,'Color',cols(col_idx,:));
end
完整程式碼
clear;close all; dbstop error; clc;
base_dir = 'D:/KITTI/data_set/2011_09_26/2011_09_26_drive_0009_sync'; % 圖片目錄
calib_dir = 'D:/KITTI/data_set/data_object_calib/2011_09_26'; % 相機引數目錄
cam = 2; % 第3個攝像頭
frame = 5; % 第0幀(第一張圖片)
calib = loadCalibrationCamToCam(fullfile(calib_dir,'calib_cam_to_cam.txt'));
Tr_velo_to_cam = loadCalibrationRigid(fullfile(calib_dir,'calib_velo_to_cam.txt'));
% 計算點雲到影象平面的投影矩陣
R_cam_to_rect = eye(4);
R_cam_to_rect(1:3,1:3) = calib.R_rect{1}; % R_rect:糾正旋轉使影象平面共面
P_velo_to_img = calib.P_rect{cam+1}*R_cam_to_rect*Tr_velo_to_cam; % 內外引數 P_rect:矯正後的投影矩陣
img = imread(sprintf('%s/image_%02d/data/%010d.png', base_dir, cam, frame));
imshow(img); hold on;
fid = fopen(sprintf('%s/velodyne_points/data/%010d.bin',base_dir,frame),'rb');
velo = fread(fid,[4 inf],'single')';
velo = velo(1:5:end,:); % 顯示速度每5點移除一次
fclose(fid);
% 刪除影象平面後面的所有點(近似值)
idx = velo(:,1)<5;
velo(idx,:) = [];
% 投影到影象平面(排除亮度)
velo_img = project(velo(:,1:3),P_velo_to_img);
% 畫點
cols = jet;
for i=1:size(velo_img,1)
col_idx = round(64*5/velo(i,1));
plot(velo_img(i,1),velo_img(i,2),'o','LineWidth',4,'MarkerSize',1,'Color',cols(col_idx,:));
end