处理点云数据(四)：点云到图像平面的投影

点云到图像平面的投影

坐标系的定义

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