function mvg_ex06_prac % Read input images: image1 = double(imread('batinria0.tif')); image2 = double(imread('batinria1.tif')); % Number of corresponding point pairs: nPoints = 12; % Get corresponding point pairs: [x1,y1,x2,y2] = getpoints(image1,image2,nPoints); %[x1,y1,x2,y2] = getpoints2(); % Intrinsic camera paramter matrices K1 = [844.310547 0 243.413315; 0 1202.508301 281.529236; 0 0 1]; K2 = [852.721008 0 252.021805; 0 1215.657349 288.587189; 0 0 1]; K1 = inv(K1); K2 = inv(K2); % Transform image coordinates with inverse camera matrices: for i=1:nPoints l = K1 * [x1(i); y1(i); 1]; r = K2 * [x2(i); y2(i); 1]; x1(i) = l(1); y1(i) = l(2); x2(i) = r(1); y2(i) = r(2); end % Compute constraint matrix chi: chi = zeros(nPoints,9); for i=1:nPoints chi(i,:) = kron([x1(i) y1(i) 1],[x2(i) y2(i) 1])'; end rank_chi = rank(chi) % Find minimizer for |chi*E|: [UChi,DChi,VChi] = svd(chi); % Unstacked ninth column of V: E = reshape(VChi(:,9),3,3); % SVD of E [U,D,V] = svd(E); if det(U) < 0 || det(V) < 0 [U,D,V] = svd(-E); end % Project E onto essential space (replace eigenvalues): D(1,1) = 1; D(2,2) = 1; D(3,3) = 0; % Final essential matrix: E = U * D * V'; % Recover R and T from the essential matrix E: Rz1 = [0 1 0; -1 0 0; 0 0 1]'; Rz2 = [0 -1 0; 1 0 0; 0 0 1]'; R1 = U * Rz1' * V'; R2 = U * Rz2' * V'; T_hat1 = U * Rz1 * D * U'; T_hat2 = U * Rz2 * D * U'; % Translation belonging to T_hat T1 = [ -T_hat1(2,3); T_hat1(1,3); -T_hat1(1,2) ]; T2 = [ -T_hat2(2,3); T_hat2(1,3); -T_hat2(1,2) ]; % Compute scene reconstruction and correct combination of R and T: reconstruction(R1,T1,x1,y1,x2,y2,nPoints); reconstruction(R1,T2,x1,y1,x2,y2,nPoints); reconstruction(R2,T1,x1,y1,x2,y2,nPoints); reconstruction(R2,T2,x1,y1,x2,y2,nPoints); end % Compute correct combination of R and T and reconstruction of 3D points function reconstruction(R,T,x1,y1,x2,y2,nPoints) % Structure reconstruction matrix M: M = zeros(3*nPoints,nPoints+1); for i = 1:nPoints x2_hat = hat([x2(i) y2(i) 1]); M(3*i-2:3*i,i) = x2_hat * R * [x1(i); y1(i); 1]; M(3*i-2:3*i,nPoints+1) = x2_hat * T; end % Get depth values (eigenvector to the smallest eigenvalue of M'M): [V,D] = eig(M'*M); lambda = V(1:nPoints,1); gamma = V(nPoints+1,1); % Determine correct combination of R and T: if lambda >= zeros(nPoints,1) display(R); display(T); display(lambda); display(gamma); % Visualize the 3D points: plot3(x1,y1,lambda,'+'); end end function A = hat(v) A = [0 -v(3) v(2) ; v(3) 0 -v(1) ; -v(2) v(1) 0]; end function [x1,y1,x2,y2] = getpoints(image1,image2,nPoints) x1 = zeros(nPoints,1); y1 = zeros(nPoints,1); x2 = zeros(nPoints,1); y2 = zeros(nPoints,1); % Click points in image1: figure; imshow(uint8(image1)); hold on; for i=1:nPoints [x,y] = ginput(1); x1(i) = double(x); y1(i) = double(y); plot(x,y,'r+'); end hold off; % Click points in image2: figure; imshow(uint8(image2)); hold on; for i=1:nPoints [x,y] = ginput(1); x2(i) = double(x); y2(i) = double(y); plot(x,y,'r+'); end hold off; end function [x1,y1,x2,y2] = getpoints2() x1 = [ 10.0000 92.0000 8.0000 92.0000 289.0000 354.0000 289.0000 353.0000 69.0000 294.0000 44.0000 336.0000 ]; y1 = [ 232.0000 230.0000 334.0000 333.0000 230.0000 278.0000 340.0000 332.0000 90.0000 149.0000 475.0000 433.0000 ]; x2 = [ 123.0000 203.0000 123.0000 202.0000 397.0000 472.0000 398.0000 472.0000 182.0000 401.0000 148.0000 447.0000 ]; y2 = [ 239.0000 237.0000 338.0000 338.0000 236.0000 286.0000 348.0000 341.0000 99.0000 153.0000 471.0000 445.0000 ]; end