reconstructioncube1.m

3D Reconstruction after using tsai calibration

function [X Y Z]=ReconstructionCube1(K1,K2,T,R)




PointsImage1 = [  59  192   257   257   129    59   189      
                  407  452  403   275   242   279   318     
                   1    1    1      1     1     1     1 ];

PointsImage2 =[ 47   86  189  188  147   44   82    
               307  369  355  233  185  198  246     
                 1    1    1    1    1    1   1 ];





% Dans le rep鑢e monde : 
pim1=pinv(K1(:,1:3))*PointsImage1;
pim2=pinv(K2(:,1:3))*PointsImage2;


% On calcule les coordonn閑s approximatives du point3D 

n=size(PointsImage1,2);
for i=1:n    
    w=cross(pim1(:,i),R'*pim2(:,i));
    D=[pim1(:,i),(-R')*pim2(:,i),w];
    ABC=inv(D)*T;
    % Calculs de P1 et P2 sur les droites w : P est le milieu de ces points.
    P1=ABC(1)*pim1(:,i);
    P2=T+ABC(2)*(R)'*pim2(:,i);
    Pvect=P1+((P2-P1)/2);
    P(i)=Pvect(1);
    P(n+i)=Pvect(2);
    P(2*n+i)=Pvect(3);
end


% Cr閍tion de vecteurs pour afficher 
for i=1:n
    X(i+1)=P(i);
    Y(i+1)=P(n+i);
    Z(i+1)=P(2*n+i);
end


% Cr閍tion du point n'apparaissant pas sur les images 
X(1)= X(6)-X(7)+X(2);
Y(1)= Y(6)-Y(7)+Y(2);
Z(1)= Z(6)-Z(7)+Z(2);

% On rajoute des points pour l'affichage, ils n'ont aucune signification
% sinon de pouvoir 阾re utilis閟 pour cr閑r des lignes entre les points
% lors de l'affichage

X(9)=X(3);
Y(9)=Y(3);
Z(9)=Z(3);

X(10)=X(4);
Y(10)=Y(4);
Z(10)=Z(4);


X(11)=X(1);
Y(11)=Y(1);
Z(11)=Z(1);

X(12)=X(6);
Y(12)=Y(6);
Z(12)=Z(6);

X(13)=X(7);
Y(13)=Y(7);
Z(13)=Z(7);

X(14)=X(2);
Y(14)=Y(2);
Z(14)=Z(2);

X(15)=X(3);
Y(15)=Y(3);
Z(15)=Z(3);

X(16)=X(8);
Y(16)=Y(8);
Z(16)=Z(8);

X(17)=X(5);
Y(17)=Y(5);
Z(17)=Z(5);