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