clear all close all whos %im = imread('jehlan01.jpg'); width = 2272; % sirka obrazku height = 1704; % vyska obrazku s = sqrt(width^2 + height^2); T = [1/s 0 -height/2/s; 0 1/s -width/2/s; 0 0 1]; % pro normalizaci pN = 17; % pocet bodu pM = 8; % pocet kamer load corrx % homogenni souradnice x % normalizace x for i = 1:3:24 x(i:i+2,:) = T*x(i:i+2,:); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % vypocet F %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% prvni = 1; druhy = 8; for i = 1:pN A(i,:) = [x(3*prvni-2,i)*x(druhy*3-2,i) x(3*prvni-2,i)*x(druhy*3-1,i) x(3*prvni-2,i)*x(druhy*3,i) ... x(3*prvni-1,i)*x(druhy*3-2,i) x(3*prvni-1,i)*x(druhy*3-1,i) x(3*prvni-1,i)*x(druhy*3,i) ... x(3*prvni ,i)*x(druhy*3-2,i) x(3*prvni ,i)*x(druhy*3-1,i) x(3*prvni ,i)*x(druhy*3,i)]; end [U,S,V] = svd(A); N = V(:,9); F = [N(1:3)'; N(4:6)'; N(7:9)']; [U,S,V] = svd(F); S(3,3) = 0; F = U*S*V'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % vypocet P1, P2 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% P1 = eye(3); P1(:,4) = 0; ec = null(F); ec = ec/ec(3); ex = zeros(3); ex(1,2) = -ec(3); ex(1,3) = ec(2); ex(2,1) = ec(3); ex(2,3) = -ec(1); ex(3,1) = -ec(2); ex(3,2) = ec(1); P2 = [ex*F ec]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % vypocet X %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for i = 1:pN xx1 = [ 0 -x(3*prvni,i) x(3*prvni-1,i); ... x(3*prvni,i) 0 -x(3*prvni-2,i); ... -x(3*prvni-1,i) x(3*prvni-2,i) 0]; xx2 = [ 0 -x(3*druhy,i) x(3*druhy-1,i); ... x(3*druhy,i) 0 -x(3*druhy-2,i); ... -x(3*druhy-1,i) x(3*druhy-2,i) 0]; B = [xx1 * P1; xx2 * P2]; [U,S,V] = svd(B); X(:,i) = V(:,4); end X = X ./ kron(X(4,:), [1 1 1 1]'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % vypocet P1-8 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for j = 1:pM clear B; for i = 1:pN B(i*3 - 2, 1:4) = x(3*j-1,i)*X(:,i)'; B(i*3 - 2, 5: 8) = -x(3*j-2,i)*X(:,i)'; B(i*3 - 1, 1:4) = x(3*j ,i)*X(:,i)'; B(i*3 - 1, 9:12) = -x(3*j-2,i)*X(:,i)'; B(i*3 , 5:8) = x(3*j ,i)*X(:,i)'; B(i*3 , 9:12) = -x(3*j-1,i)*X(:,i)'; end [U,S,V] = svd(B); N = V(:,12); P{j} = [N(1:4)'; N(5:8)'; N(9:12)']; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Prepocet P1-8 a X %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for i = 1:pM alfa{i} = (P{i}*X) ./ x(i*3-2:i*3,:); end alfy = cat(1, alfa{:}); a = alfy(3:3:end,:); [U,S,V] = svd(kron(a, [1;1;1]) .* x); S(5:end,5:end) = 0; PP = U*sqrt(S); PP = PP(:,1:4); XX = sqrt(S)*V'; XX = XX(1:4,:); XX = XX ./ kron(XX(4,:), [1 1 1 1]'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Hledani nevlastniho bodu %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% line_11 = cross(inv(T)*PP(3*prvni-2:3*prvni,:)*XX(:,5),inv(T)*PP(3*prvni-2:3*prvni,:)*XX(:,8)); line_12 = cross(inv(T)*PP(3*prvni-2:3*prvni,:)*XX(:,6),inv(T)*PP(3*prvni-2:3*prvni,:)*XX(:,7)); prus1 = cross(line_11, line_12); line_21 = cross(inv(T)*PP(3*druhy-2:3*druhy,:)*XX(:,5),inv(T)*PP(3*druhy-2:3*druhy,:)*XX(:,8)); line_22 = cross(inv(T)*PP(3*druhy-2:3*druhy,:)*XX(:,6),inv(T)*PP(3*druhy-2:3*druhy,:)*XX(:,7)); prus2 = cross(line_21, line_22); xx1 = [ 0 -prus1(3) prus1(2); ... prus1(3) 0 -prus1(1); ... -prus1(2) prus1(1) 0]; xx2 = [ 0 -prus2(3) prus2(2); ... prus2(3) 0 -prus2(1); ... -prus2(2) prus2(1) 0]; B = [xx1*PP(3*prvni-2:3*prvni,:); xx2*PP(3*druhy-2:3*druhy,:)]; [U,S,V] = svd(B); Xinf = V(:,4); Xinf = Xinf/Xinf(4); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Vypocet H %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% XH = XX(:,13:end); XH(:,6) = Xinf; XH = XH ./ kron(XH(4,:), [1 1 1 1]'); YH = [0, 1, 0, 1]'; YH(:,2) = [1, 1, 0, 1]'; YH(:,3) = [1, 0, 0, 1]'; YH(:,4) = [0, 0, 0, 1]'; YH(:,5) = [0.5, 0.5, sqrt(0.5), 1]'; YH(:,6) = [0, 0, 1, 0]'; clear A; for i = 1:size(XH,2) A(i*6-5,1:4) = YH(2,i)*XH(:,i)'; A(i*6-5,5:8) = -YH(1,i)*XH(:,i)'; A(i*6-4,1:4) = YH(3,i)*XH(:,i)'; A(i*6-4,9:12) = -YH(1,i)*XH(:,i)'; A(i*6-3,1:4) = YH(4,i)*XH(:,i)'; A(i*6-3,13:16) = -YH(1,i)*XH(:,i)'; A(i*6-2,5:8) = YH(3,i)*XH(:,i)'; A(i*6-2,9:12) = -YH(2,i)*XH(:,i)'; A(i*6-1,5:8) = YH(4,i)*XH(:,i)'; A(i*6-1,13:16) = -YH(2,i)*XH(:,i)'; A(i*6 ,9:12) = YH(4,i)*XH(:,i)'; A(i*6 ,13:16) = -YH(3,i)*XH(:,i)'; end [U,S,V] = svd(A); N = V(:,end); H = [N(1:4)'; N(5:8)'; N(9:12)'; N(13:16)']; H = H/H(4,4); C = H*XH; C = C ./ kron(C(4,:), [1 1 1 1]'); %[YH(:,1:5);C(:,1:5)] figure(10); YY = H*XX; YY = YY ./ kron(YY(4,:), [1 1 1 1]'); plot3(YY(1,:), YY(2,:), YY(3,:), '.'); hold on; grid on; axis equal; K = YY(:,1:4); K(:,5) = K(:,1); plot3(K(1,:), K(2,:), K(3,:), 'r'); hold on; grid on; axis equal; K = YY(:,5:8); K(:,5) = K(:,1); plot3(K(1,:), K(2,:), K(3,:), 'g'); hold on; grid on; axis equal; K = YY(:,9:12); K(:,5) = K(:,1); plot3(K(1,:), K(2,:), K(3,:), 'b'); hold on; grid on; axis equal; K = YY(:,13:16); K(:,5) = K(:,1); plot3(K(1,:), K(2,:), K(3,:), 'c'); hold on; grid on; axis equal; K = YY(:,13); K(:,2) = YY(:,17); plot3(K(1,:), K(2,:), K(3,:), 'c'); hold on; grid on; axis equal; K = YY(:,14); K(:,2) = YY(:,17); plot3(K(1,:), K(2,:), K(3,:), 'c'); hold on; grid on; axis equal; K = YY(:,15); K(:,2) = YY(:,17); plot3(K(1,:), K(2,:), K(3,:), 'c'); hold on; grid on; axis equal; K = YY(:,16); K(:,2) = YY(:,17); plot3(K(1,:), K(2,:), K(3,:), 'c'); hold on; grid on; axis equal; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Kresleni %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if (0) for j = 1:pM figure(j); im{j} = imread(sprintf('jehlan%02d.jpg',j)); imshow(im{j}); hold on; Y{j} = inv(T)*P{j}*X; Y{j} = Y{j} ./ kron(Y{j}(3,:), [1 1 1]'); plot(Y{j}(1,:),Y{j}(2,:),'rx'); Z{j} = inv(T)*PP(3*j-2:3*j,:)*XX; Z{j} = Z{j} ./ kron(Z{j}(3,:), [1 1 1]'); plot(Z{j}(1,:),Z{j}(2,:),'bx'); axis auto; end figure(prvni); plot(prus1(1)/prus1(3), prus1(2)/prus1(3), 'r*'); Yinf = inv(T)*PP(3*prvni-2:3*prvni,:)*Xinf; % plot(Yinf(1)/Yinf(3), Yinf(2)/Yinf(3), 'g*'); figure(druhy); plot(prus2(1)/prus2(3), prus2(2)/prus2(3), 'r*'); Zinf = inv(T)*PP(3*druhy-2:3*druhy,:)*Xinf; % plot(Zinf(1)/Zinf(3), Zinf(2)/Zinf(3), 'g*'); end