<span style="font-size:18px;">function exercise clear all; clf; close all; %%定义一个相机1 au1=100;av1=120;uo1=128;vo1=128; %%定义第二个相机 au2=90;av2=110;uo2=128;vo2=128; ax = 0.1; by = pi/4; cz = 0.2; tx = -1000; ty = 190; tz = 230; %%两个相机的本征矩阵和转移矩阵 A1=[au1 0 uo1 0 av1 vo1 0 0 1]; A2=[au2 0 uo2 0 av2 uo2 0 0 1]; %相机到图像平面的 转移矩阵 %z轴的旋转矩阵 相机间的旋转矩阵 rz=[cos(cz) -sin(cz) 0 sin(cz) cos(cz) 0 0 0 1]; ry=[cos(by) 0 sin(by) 0 1 0 -sin(by) 0 cos(by)]; rx=[1 0 0 0 cos(ax) -sin(ax) 0 sin(ax) cos(ax)]; %旋转矩阵 R=rx*ry*rz; %平移矩阵 t=[tx;ty;tz]; K_2respto1=[R t]; %相机2到1的变换 K_1respto2=[R' -R'*t];%相机1到的变换 就是公式推导出来的 %% Step 4 Get the Fundamental matrix analytically as the product of matrices defined in step 3 t_antisym=[0 -tz ty tz 0 -tx -ty tx 0]; %反对称矩阵 F1=inv(A2')*R'*t_antisym*inv(A1);%求得是基础矩阵F F2=inv(A1')*t_antisym*R*inv(A2); %% Step 5 Define the following set of object points with respect to the % world coordinate system (orcamera 1 coordinate system) V(:,1) = [100;-400;2000;1]; V(:,2) = [300;-400;3000;1]; V(:,3) = [500;-400;4000;1]; V(:,4) = [700;-400;2000;1]; V(:,5) = [900;-400;3000;1]; V(:,6) = [100;-50;4000;1]; V(:,7) = [300;-50;2000;1]; V(:,8) = [500;-50;3000;1]; V(:,9) = [700;-50;4000;1]; V(:,10) = [900;-50;2000;1]; V(:,11) = [100;50;3000;1]; V(:,12) = [300;50;4000;1]; V(:,13) = [500;50;2000;1]; V(:,14) = [700;50;3000;1]; V(:,15) = [900;50;4000;1]; V(:,16) = [100;400;2000;1]; V(:,17) = [300;400;3000;1]; V(:,18) = [500;400;4000;1]; V(:,19) = [700;400;2000;1]; V(:,20) = [900;400;3000;1]; %% Step 6 Compute the couples of image points in both image planes by using the matrices of step3 %世界坐标的点转移到图像坐标 intr1=[A1 zeros(3,1)]; intr2=[A2 zeros(3,1)];%相机坐标系到图像坐标系的变化 K_wrespto1=[eye(3) zeros(3,1)];%世界坐标系到相机坐标系的变换 extr1=[K_wrespto1;zeros(1,3) 1]; K_wrespto2=K_1respto2; extr2=[K_wrespto2;zeros(1,3) 1]; V_I1=point_projection(intr1,extr1,V); V_I2=point_projection(intr2,extr2,V); %% Step 7 Open two windows in matlab, which will be used as both image %planes, and draw the 2D points obtained in step 6 figure(15) subplot(1,2,1);hold on; x=[0;256;256;0];y=[0;0;256;256]; fill(x,y,[0.6 0.6 1]); text(256,256,' image plane I1'); stem(V_I1(1,:),V_I1(2,:),'LineStyle','none','Marker','o','Color','m'); title('Camera 1'); subplot(1,2,2);hold on; fill(x,y,[1 0.6 1]); text(256,256,' image plane I2'); stem(V_I2(1,:),V_I2(2,:),'LineStyle','none','Marker','o','Color','r'); title('Camera 2'); figure(1) draw_object_points(V_I1,V_I2); %% Step 8. Compute the fundamental matrix by using the 8-point method and least-squares by means of the 2D points obtained in step 6 F1_8pm=eightpointsmethod1(V_I1,V_I2); %在极线配准那里的基础矩阵x2T * F *x1=0; F1_normal=F1/F1(3,3); F2_8pm=eightpointsmethod2(V_I1,V_I2); F2_normal=F2/F2(3,3); %% Step 9. Compare the step 8 matrix with the one obtained in step 4 different1=F1_8pm-F1_normal; different2=F2_8pm-F2_normal; %% Step 10. Draw in the windows of step 7 all the epipolar geometry figure(1) subplot(1,2,2); ylabel('epipolar geometry using LS 8 points method without noise'); %% %boundary of x-axis x1_max=600; x1_min=-600; x2_max=600; x2_min=-600; draw_epipolar_geometry(F1_normal,F2_normal,V_I1,V_I2,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %% Step 11 Add some Gaussian noise to the 2D points producing discrepancies % between the range [-1,+1] pixels for the 95% of points. mu=0; sigma=0.5102; V_I1_noise=V_I1+[normrnd(mu,sigma,2,20);zeros(1,20)];% add Gaussian noise % to the point V_I2_noise=V_I2+[normrnd(mu,sigma,2,20);zeros(1,20)]; %% Step 12. Again repeat step 8 up to 10 with the noisy 2D points. %Compute the fundamental matrix by using the 8-point method and %least-squares by means of the 2D points with noise obtained in step 11 F1_8pm_noise=eightpointsmethod1(V_I1_noise,V_I2_noise); F2_8pm_noise=eightpointsmethod2(V_I1_noise,V_I2_noise); %Draw all the epipolar geometry figure(2); subplot(1,2,2); ylabel('epipolar geometry using LS 8 points method with noise (no uniqueeipiplar)'); %boundary of x-axis x1_max=700; x1_min=-700; x2_max=700; x2_min=-700; draw_object_points(V_I1_noise,V_I2_noise); draw_epipolar_geometry(F1_8pm_noise,F2_8pm_noise,V_I1_noise,V_I2_noise,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %compute the closest rank-2 matrix [Q1,S1,V1] = svd(F1_8pm_noise); S1(3,3)=0; F1_8pm_noise_svd=Q1*S1*V1'; [Q2,S2,V2] = svd(F2_8pm_noise); S2(3,3)=0; F2_8pm_noise_svd=Q2*S2*V2'; figure(3) subplot(1,2,2); ylabel('epipolar geometry using LS 8 points method with noise (uniqueeipiplar)'); x1_max=700; x1_min=-700; x2_max=700; x2_min=-700; draw_object_points(V_I1_noise,V_I2_noise); draw_epipolar_geometry(F1_8pm_noise_svd,F2_8pm_noise_svd,V_I1_noise,V_I2_noise,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %% Step 13. Increase the Gaussian noise of step 11 (now in the range [-2,+2] % for the 95% of points) and repeat step 8-12. mu=0; sigma=0.5102; sigma2=2*sigma; V_I1_noise2=V_I1+[normrnd(mu,sigma2,2,20);zeros(1,20)];% add Gaussian noiseto the point V_I2_noise2=V_I2+[normrnd(mu,sigma2,2,20);zeros(1,20)]; %Compute the fundamental matrix by using the 8-point method and %least-squares by means of the 2D points with noise obtained above F1_8pm_noise2=eightpointsmethod1(V_I1_noise2,V_I2_noise2); F2_8pm_noise2=eightpointsmethod2(V_I1_noise2,V_I2_noise2); %Draw all the epipolar geometry figure(4) subplot(1,2,2); ylabel('epipolar geometry using LS 8 points method with noise2 (no uniqueeipiplar)'); %boundary of x-axis x1_max=700; x1_min=-700; x2_max=700; x2_min=-700; draw_object_points(V_I1_noise2,V_I2_noise2); draw_epipolar_geometry(F1_8pm_noise2,F2_8pm_noise2,V_I1_noise2,V_I2_noise2,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %compute the closest rank-2 matrix [Q1,S1,V1] = svd(F1_8pm_noise2); S1(3,3)=0; F1_8pm_noise2_svd=Q1*S1*V1'; [Q2,S2,V2] = svd(F2_8pm_noise2); S2(3,3)=0; F2_8pm_noise2_svd=Q2*S2*V2'; figure(5) subplot(1,2,2); ylabel('epipolar geometry using LS 8 points method with noise2 (uniqueeipiplar)'); x1_max=700; x1_min=-700; x2_max=700; x2_min=-700; draw_object_points(V_I1_noise2,V_I2_noise2); draw_epipolar_geometry(F1_8pm_noise2_svd,F2_8pm_noise2_svd,V_I1_noise2,V_I2_noise2,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %% Part 2. %% Step 14. Compute the fundamental matrix by using the 8-point method and SVD from the points 2D obtained in step 6 and without noise. F1_svd=svdmethod1(V_I1,V_I2); F2_svd=svdmethod2(V_I1,V_I2); different3=F1_svd-F1_normal; different4=F2_svd-F2_normal; %% Step 15. Repeat step 10 up to 13 (with the matrix of step 14 instead ofstep 8) for points with Gaussian noise %first Gaussian noise in the rang [-1, 1] for the 95% of points F1_svd_noise=svdmethod1(V_I1_noise,V_I2_noise); F2_svd_noise=svdmethod2(V_I1_noise,V_I2_noise); %Draw all the epipolar geometry figure(6) subplot(1,2,2); ylabel('epipolar geometry using SVD 8 points method with noise (no uniqueeipiplar)'); %boundary of x-axis x1_max=700; x1_min=-700; x2_max=700; x2_min=-700; draw_object_points(V_I1_noise,V_I2_noise); draw_epipolar_geometry(F1_svd_noise,F2_svd_noise,V_I1_noise,V_I2_noise,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %compute the closest rank-2 matrix [Q1,S1,V1] = svd(F1_svd_noise); S1(3,3)=0; F1_svd_noise_svd=Q1*S1*V1'; [Q2,S2,V2] = svd(F2_svd_noise); S2(3,3)=0; F2_svd_noise_svd=Q2*S2*V2'; figure(7) subplot(1,2,2); ylabel('epipolar geometry using SVD 8 points method with noise (uniqueeipiplar)'); x1_max=700; x1_min=-700; x2_max=700; x2_min=-700; draw_object_points(V_I1_noise,V_I2_noise); draw_epipolar_geometry(F1_svd_noise_svd,F2_svd_noise_svd,V_I1_noise,V_I2_noise,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %second Gaussian noise in the rang [-2, 2] for the 95% of points F1_svd_noise2=svdmethod1(V_I1_noise2,V_I2_noise2); F2_svd_noise2=svdmethod2(V_I1_noise2,V_I2_noise2); %Draw all the epipolar geometry figure(8) subplot(1,2,2); ylabel('epipolar geometry using SVD 8 points method with noise2 (no uniqueeipiplar)'); %boundary of x-axis x1_max=700; x1_min=-700; x2_max=700; x2_min=-700; draw_object_points(V_I1_noise2,V_I2_noise2); draw_epipolar_geometry(F1_svd_noise2,F2_svd_noise2,V_I1_noise2,V_I2_noise2,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %compute the closest rank-2 matrix [Q1,S1,V1] = svd(F1_svd_noise2); S1(3,3)=0; F1_svd_noise2_svd=Q1*S1*V1'; [Q2,S2,V2] = svd(F2_svd_noise2); S2(3,3)=0; F2_svd_noise2_svd=Q2*S2*V2'; figure(9) subplot(1,2,2); ylabel('epipolar geometry using SVD 8 points method with noise2 (uniqueeipiplar)'); x1_max=700; x1_min=-700; x2_max=700; x2_min=-700; draw_object_points(V_I1_noise2,V_I2_noise2); draw_epipolar_geometry(F1_svd_noise2_svd,F2_svd_noise2_svd,V_I1_noise2,V_I2_noise2,x1_min,x1_max,x2_min,x2_max); draw_focal_point(t,intr1,extr1,intr2,extr2); %% function %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function V_I1=point_projection(intr1,extr1,V) V_I1=intr1*extr1*V; V_I1(1,:)=V_I1(1,:)./V_I1(3,:); V_I1(2,:)=V_I1(2,:)./V_I1(3,:); V_I1(3,:)=1; %归一化后的处理 让图像上的z坐标为1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function draw_object_points(V_I1,V_I2) subplot(1,2,1);hold on; x=[0;256;256;0];y=[0;0;256;256]; fill(x,y,[0.6 0.6 1]); text(256,256,' image plane I1'); stem(V_I1(1,:),V_I1(2,:),'LineStyle','none','Marker','.','Color','m'); title('Camera 1'); subplot(1,2,2);hold on; fill(x,y,[1 0.6 1]); text(256,256,' image plane I2'); stem(V_I2(1,:),V_I2(2,:),'LineStyle','none','Marker','.','Color','r'); title('Camera 2'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function F1_8pm=eightpointsmethod1(V_I1,V_I2) U1=[V_I1(1,:).*V_I2(1,:) V_I1(2,:).*V_I2(1,:) V_I2(1,:) V_I1(1,:).*V_I2(2,:) V_I1(2,:).*V_I2(2,:) V_I2(2,:) V_I1(1,:) V_I1(2,:)]'; F1_8pm=-inv(U1'*U1)*U1'*ones(20,1); F1_8pm=[F1_8pm(1) F1_8pm(2) F1_8pm(3) F1_8pm(4) F1_8pm(5) F1_8pm(6) F1_8pm(7) F1_8pm(8) 1]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function F2_8pm=eightpointsmethod2(V_I1,V_I2) U2=[V_I2(1,:).*V_I1(1,:) V_I2(2,:).*V_I1(1,:) V_I1(1,:) V_I2(1,:).*V_I1(2,:) V_I2(2,:).*V_I1(2,:) V_I1(2,:) V_I2(1,:) V_I2(2,:)]'; F2_8pm=-inv(U2'*U2)*U2'*ones(20,1); F2_8pm=[F2_8pm(1) F2_8pm(2) F2_8pm(3) F2_8pm(4) F2_8pm(5) F2_8pm(6) F2_8pm(7) F2_8pm(8) 1]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function draw_epipolar_geometry(F1,F2,V_I1,V_I2,x1_min,x1_max,x2_min,x2_max) l2=F1*V_I1; %极线方程 l1=F2*V_I2; %由基础矩阵得到的就是极线方程 就是极线在另一个平面上的极线 x1=x1_min:x1_max; x2=x2_min:x2_max; m1=-l1(1,:)./l1(2,:); d1=-l1(3,:)./l1(2,:); m2=-l2(1,:)./l2(2,:); d2=-l2(3,:)./l2(2,:); for i=1:20 y1(i,:)=m1(i)*x1+d1(i); y2(i,:)=m2(i)*x2+d2(i); subplot(1,2,1);hold on; plot(x1,y1(i,:),'Color','b'); xlim([x1_min x1_max]); ylim([x1_min x1_max]); axis square; subplot(1,2,2);hold on; plot(x2,y2(i,:),'Color','m'); axis square; xlim([x2_min x2_max]); ylim([x2_min x2_max]); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function draw_focal_point(t,intr1,extr1,intr2,extr2) oc2_resptow=[t;1]; oc2_I1=intr1*extr1*oc2_resptow; oc2_I1=oc2_I1/oc2_I1(3); oc1_resptow=[0;0;0;1]; oc1_I2=intr2*extr2*oc1_resptow; oc1_I2=oc1_I2/oc1_I2(3); subplot(1,2,1);hold on; stem(oc2_I1(1),oc2_I1(2),'LineStyle','none','Color','r'); subplot(1,2,2);hold on; stem(oc1_I2(1),oc1_I2(2),'LineStyle','none','Color','b'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function F1_svd=svdmethod1(V_I1,V_I2) U1_svd=[V_I1(1,:).*V_I2(1,:) V_I1(2,:).*V_I2(1,:) V_I2(1,:) V_I1(1,:).*V_I2(2,:) V_I1(2,:).*V_I2(2,:) V_I2(2,:) V_I1(1,:) V_I1(2,:) ones(1,20)]'; [Q3,S3,V3] = svd(U1_svd); F1_svd=V3(:,9); F1_svd=F1_svd/F1_svd(9); F1_svd=[F1_svd(1) F1_svd(2) F1_svd(3) F1_svd(4) F1_svd(5) F1_svd(6) F1_svd(7) F1_svd(8) F1_svd(9)]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function F2_svd=svdmethod2(V_I1,V_I2) U2_svd=[V_I2(1,:).*V_I1(1,:) V_I2(2,:).*V_I1(1,:) V_I1(1,:) V_I2(1,:).*V_I1(2,:) V_I2(2,:).*V_I1(2,:) V_I1(2,:) V_I2(1,:) V_I2(2,:) ones(1,20)]'; [Q4,S4,V4] = svd(U2_svd); F2_svd=V4(:,9); F2_svd=F2_svd/F2_svd(9); F2_svd=[F2_svd(1) F2_svd(2) F2_svd(3) F2_svd(4) F2_svd(5) F2_svd(6) F2_svd(7) F2_svd(8) F2_svd(9)]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% </span>