show_extrinsics
MATLAB camera calibrator
close all clear load calib_data
fc = cameraParams.FocalLength; cc = cameraParams.PrincipalPoint; dX = 3; dY = 3; nx = 4608; ny = 2592; alpha_c = 0; fprintf('Focal lengths: %g %g\n',fc); fprintf('Principal point: %g %g\n',cc); fprintf('Image Size: %d x %d\n',nx,ny); fprintf('Checker size: %g\n',dX);
Focal lengths: 6584.63 6584.47 Principal point: 2414.87 1335.39 Image Size: 4608 x 2592 Checker size: 3
plot camera
pts = [0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; K = 5*dX*[1 -alpha_c 0;0 1 0;0 0 1]*[1/fc(1) 0 0;0 1/fc(2) 0;0 0 1]*[1 0 -cc(1);0 1 -cc(2);0 0 1]; IP = K*pts; BASE = 5*dX*([0 1 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 0 1]); IP = reshape( [IP; BASE(:,1)*ones(1,5); IP] ,3,15); plot3(BASE(1,:),BASE(3,:),-BASE(2,:),'b-','linewidth',2); grid xlabel('x'); ylabel('y'); zlabel('z'); draw_frame = false; hold on; if draw_frame plot3(IP(1,:),IP(3,:),-IP(2,:),'r-','linewidth',2); end text(6*dX,0,0,'X_c'); text(-dX,5*dX,0,'Z_c'); text(0,0,-6*dX,'Y_c'); text(-dX,-dX,dX,'O_c'); hold off axis equal
no_grid = false; colors = 'brgkcm'; for kk = 7; XX_kk = [worldPoints zeros(64,1)]; R = cameraParams.RotationMatrices(:,:,kk); Tc = cameraParams.TranslationVectors(kk,:); N_kk = size(XX_kk,2); no_grid = false; n_sq_x = 7; n_sq_y = 7; YY_kk = XX_kk * R + ones(length(XX_kk),1)*Tc; uu = [-dX -dY 0]/2; uu = uu*R + Tc; if ~no_grid, YYx = zeros(n_sq_x+1,n_sq_y+1); YYy = zeros(n_sq_x+1,n_sq_y+1); YYz = zeros(n_sq_x+1,n_sq_y+1); YYx(:) = YY_kk(:,1); YYy(:) = YY_kk(:,2); YYz(:) = YY_kk(:,3); hold on hhh= mesh(YYx,YYz,-YYy); set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none'); %plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['o' colors(rem(kk-1,6)+1)]); text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1)); hold off else hold on plot3(YY_kk(:,1),YY_kk(:,3),-YY_kk(:,2),['.' colors(rem(kk-1,6)+1)]); text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1)); hold off end; end;
pts = [0 0 0; Tc(1) 0 0; Tc(1) Tc(2) 0; Tc]; hold on plot3(pts(:,1),pts(:,3),-pts(:,2),'r','LineWidth',2); hold off %Xcam = Xw * R + Tc; % Xw = (Xcam-Tc)*R';
Tw = -Tc*R'; fprintf('distance origin to camera: %g %g %g\n',Tw/12) pts = [0 0 0; Tw(1) 0 0; Tw(1) Tw(2) 0; Tw]; cpts = pts*R + ones(length(pts),1)*Tc; hold on plot3(cpts(:,1),cpts(:,3),-cpts(:,2),'g','LineWidth',2); hold off
distance origin to camera: 10.0599 1.75648 -10.1847