function result = labs2

% First calibrate the cameras, get the intrinsics K, and the extrinsics R1,
% T1, R2, T2.

% Copy also in this script the positions of the points you chose (you don't
% want to do that every time you run the script so do it beforehand).

% Now do crazy stuff and reconstruct the 3D skeleton


% Intrinsic matrix
K = [2537.44818 0.0 1630.12777 0.0; 0.0 2546.49999 1128.69435 0.0; 0.0 0.0 1.0 0.0]

% Extrinsic matrices of the first camera
T1 = [ 222.970122 	 370.513555 	 1139.646442 ]
R1 = [   0.995197 	 -0.097475 	 -0.009011;
        -0.038933 	 -0.309668 	 -0.950047;
        0.089815 	 0.945835 	 -0.311976 ]
    
% Extrinsic matrices of the second camera
T2 = [ -470.845493 	 319.961790 	 961.459527 ]
R2 = [  0.872401 	 0.488756 	 0.005934;
        0.139432 	 -0.237207 	 -0.961401;
        -0.468483 	 0.839554 	 -0.275088 ]

%%%%%%%%%%%% Calculate P1 and P2
M1_w_to_c = [   R1(1,1) R1(1,2) R1(1,3) T1(1);
                R1(2,1) R1(2,2) R1(2,3) T1(2);
                R1(3,1) R1(3,2) R1(3,3) T1(3);
                0.0 0.0 0.0 1.0];

M2_w_to_c = [   R2(1,1) R2(1,2) R2(1,3) T2(1);
                R2(2,1) R2(2,2) R2(2,3) T2(2);
                R2(3,1) R2(3,2) R2(3,3) T2(3);
                0.0 0.0 0.0 1.0];
            
M1_c_to_w = inv(M1_w_to_c);
M2_c_to_w = inv(M2_w_to_c);

% WORLD_TO_IMAGE
P1 = K*M1_w_to_c;
    
P2 = K*M2_w_to_c,

% Points on the photo of CAM1
X1 = [1220.00000000000;1460;1301;1220;758;1049;1679;1505;1193;]
Y1 = [1871;1541.00000000000;1058.00000000000;407.000000000000;473.000000000000;137.000000000000;494.000000000000;848.000000000000;284.000000000000;]

% Points on the photo of CAM2
X2 = [1295.00000000000;1835.00000000000;1589;1559;1157;1265;2054;1724;1490;]
Y2 = [1844;1625.00000000000;1052.00000000000;371.000000000000;419.000000000000;70.9999999999996;485.000000000000;857.000000000000;233.000000000000;]

%%%%%%%%%%%%%%%%


% Do stuff here


%%%%%%%%%%%%%%%%


% Cam 1
drawCamera(M1_c_to_w);

% Cam 2
drawCamera(M2_c_to_w);

% Checkerboard
drawCheckerboard;

% the end
result = 0;

end

function void = drawCamera(M_camera_to_world)
    C_1 =  [0.0; 0.0; 0.0; 1.0];
    C_2 =  [50.0; 50.0; 100.0; 1.0];
    C_3 =  [50.0; -50.0; 100.0; 1.0];
    C_4 =  [-50.0; 50.0; 100.0; 1.0];
    C_5 =  [-50.0; -50.0; 100.0; 1.0];

    C_1_w = M_camera_to_world*C_1;
    C_2_w = M_camera_to_world*C_2;
    C_3_w = M_camera_to_world*C_3;
    C_4_w = M_camera_to_world*C_4;
    C_5_w = M_camera_to_world*C_5;

    C_1_w = C_1_w/C_1_w(end);
    C_2_w = C_2_w/C_2_w(end);
    C_3_w = C_3_w/C_3_w(end);
    C_4_w = C_4_w/C_4_w(end);
    C_5_w = C_5_w/C_5_w(end);

    joinPoints(C_1_w, C_2_w);
    joinPoints(C_1_w, C_3_w);
    joinPoints(C_1_w, C_4_w);
    joinPoints(C_1_w, C_5_w);
    joinPoints(C_2_w, C_3_w);
    joinPoints(C_2_w, C_4_w);
    joinPoints(C_4_w, C_5_w);
    joinPoints(C_5_w, C_3_w);
end

function void = drawCheckerboard
    C_1 =  [0.0; 0.0; 0.0; 1.0];
    C_2 =  [196.0; 0.0; 0.0; 1.0];
    C_3 =  [196.0; 140.0; 0.0; 1.0];
    C_4 =  [0.0; 140.0; 0.0; 1.0];

    joinPoints(C_1, C_2);
    joinPoints(C_2, C_3);
    joinPoints(C_3, C_4);
    joinPoints(C_4, C_1);
end

function void = joinPoints(P1, P2)
    line( [P1(1) P2(1)], [P1(2) P2(2)], [P1(3) P2(3)])
    void = 0;
end