點雲到影象平面的投影

座標系的定義

相機（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