点云与图像融合形成彩色点云

参考资料：

Kitti 的 calib_cam_to_cam.txt，calib_imu_to_velo.txt，calib_velo_to_cam.txt

 

点云到图像平面的投影 

https://blog.csdn.net/qq_33801763/article/details/78959205

 

matlab pointcloud类文档

http://www.mathworks.com/help/vision/ref/pointcloud-class.html

 

激光相机数据融合 

https://www.cnblogs.com/zoucheng/p/7860827.html

 

clear;close all; dbstop error; clc; 

% matlab中符号为'/'，与 Windows相反
base_dir = '.../point_cloud_projection'; % 图片目录
calib_dir = '.../point_cloud_projection'; % 相机参数目录

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：矫正后的投影矩阵，用于从矫正后的0号相机坐标系 投影到 X号相机的图像平面。

img = imread(sprintf('%s/%010d.png', base_dir,  frame)); 

fid = fopen(sprintf('%s/%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为点云在图像上的坐标
velo_img = project(velo(:,1:3),P_velo_to_img);

%原图像维数
img_d1 = size(img,1);
img_d2 = size(img,2);

%预分配内存
velo_img_rgb = ones(size(velo_img,1),3) * 255;

%取点云对应像素的RGB值
for i=1:size(velo_img,1)
    %取整(点云x,y轴和图像是反的)
    y=round(velo_img(i,1));
    x=round(velo_img(i,2));
    
    %排除在图像外的点云点（图像小点云多，点云并不能完全映射到图像上）
    if x>0 && x<=img_d1 && y>0 && y<=img_d2        
        velo_img_rgb(i,1:3) = img(x,y,1:3);
    end
end

%取原点云x,y,z
velo_xyz = velo(:,1:3);

% 颜色矩阵要用colormap
color_m = colormap(velo_img_rgb./255);

%构造一个pointCloud对象
ptCloud = pointCloud(velo_xyz,'Color',color_m);

pcshow(ptCloud);