點雲與圖像融合形成彩色點雲

參考資料：

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);