プロジェクトでVelodyneが画像に雲を向けているのはどうですか？ここで（キティデータセット）

Question

は画像にベロダインポイントを投影する私のコードです： readCalibration機能が

function [P, Tr_velo_to_cam, R_cam_to_rect] = readCalibration(calib_dir,img_idx,cam) 

% load 3x4 projection matrix 
P = dlmread(sprintf('%s/%06d.txt',calib_dir,img_idx),' ',0,1); 

Tr_velo_to_cam = P(6,:); 
R_cam_to_rect = P(5,1:9); 

P = P(cam+1,:); 
P = reshape(P ,[4,3])'; 
Tr_velo_to_cam = reshape(Tr_velo_to_cam ,[3,4])'; 
R_cam_to_rect = reshape(R_cam_to_rect ,[3,3])'; 

end 

として定義されているが、ここでの結果です

cam  = 2; 
frame  = 20; 

% compute projection matrix velodyne->image plane 
R_cam_to_rect = eye(4); 
[P, Tr_velo_to_cam, R] = readCalibration('D:/Shared/training/calib/',frame,cam) 
R_cam_to_rect(1:3,1:3) = R; 
P_velo_to_img = P*R_cam_to_rect*Tr_velo_to_cam; 


% load and display image 
img = imread(sprintf('D:/Shared/training/image_2/%06d.png',frame)); 
fig = figure('Position',[20 100 size(img,2) size(img,1)]); axes('Position',[0 0 1 1]); 
imshow(img); hold on; 

% load velodyne points 
fid = fopen(sprintf('D:/Shared/training/velodyne/%06d.bin',frame),'rb'); 
velo = fread(fid,[4 inf],'single')'; 
% remove every 5th point for display speed 
velo = velo(1:5:end,:); 
fclose(fid); 

% remove all points behind image plane (approximation 
idx = velo(:,1)<5; 
velo(idx,:) = []; 

% project to image plane (exclude luminance) 
velo_img = project(velo(:,1:3),P_velo_to_img); 

% plot points 
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,:)); 

：

私のコードに間違いがありますか？私は "カム"変数を0から3に変更し、いずれも機能しませんでした。キャリブレーションファイルのサンプルは、次のリンクから入手できます。 How to understand KITTI camera calibration files

Answer 1

私は自分で修正しました。 readCalibration関数の変更点は次のとおりです。

Tr_velo_to_cam = P(6,:); 
Tr_velo_to_cam = reshape(Tr_velo_to_cam ,[4,3])'; 
Tr_velo_to_cam = [Tr_velo_to_cam;0 0 0 1];