matlab实现相机标定程序源码.zip

function M=matrix_projection() %imgPoint图像坐标系中的点 tic imgPoint=[200.5 68.6 1 200.5 85.9 1 200.5 103.3 1 200.5 120.7 1 200.4 138.1 1 200.5 155.5 1 217.8 68.5 1 217.9 85.8 1 217.9 103.2 1 217.8 120.6 1 217.8 138.1 1 217.8 155.5 1 235.2 68.5 1 235.2 85.8 1 235.3 103.2 1 235.3 120.6 1 235.2 138.1 1 235.3 155.5 1 252.5 68.4 1 252.6 85.7 1 252.6 103.1 1 252.6 120.6 1 252.6 138.2 1 252.6 155.5 1 269.9 68.3 1 269.9 85.6 1 269.9 102.9 1 270.0 120.5 1 270.0 137.9 1 270.1 155.4 1 287.3 68.2 1 287.4 85.5 1 287.4 102.8 1 287.4 120.4 1 287.5 137.8 1 287.5 155.3 1 304.7 67.9 1 304.8 85.4 1 304.8 102.7 1 304.9 120.2 1 304.8 137.8 1 305.0 155.1 1 322.3 67.7 1 322.3 85.1 1 322.4 102.5 1 322.4 120.0 1 322.4 137.5 1 322.5 154.9 1]; %世界坐标系中的点 wPoint=[193.9 138.5 1 193.9 110.8 1 193.9 83.1 1 193.9 55.4 1 193.9 27.7 1 193.9 0 1 166.2 138.5 1 166.2 110.8 1 166.2 83.1 1 166.2 55.4 1 166.2 27.7 1 166.2 0 1 138.5 138.5 1 138.5 110.8 1 138.5 83.1 1 138.5 55.4 1 138.5 27.7 1 138.5 0 1 110.8 138.5 1 110.8 110.8 1 110.8 83.1 1 110.8 55.4 1 110.8 27.7 1 110.8 0 1 83.1 138.5 1 83.1 110.8 1 83.1 83.1 1 83.1 55.4 1 83.1 27.7 1 83.1 0 1 55.4 138.5 1 55.4 110.8 1 55.4 83.1 1 55.4 55.4 1 55.4 27.7 1 55.4 0 1 27.7 138.5 1 27.7 110.8 1 27.7 83.1 1 27.7 55.4 1 27.7 27.7 1 27.7 0 1 0 138.5 1 0 110.8 1 0 83.1 1 0 55.4 1 0 27.7 1 0 0 1]; NumOfPoint=length(imgPoint); X=zeros(NumOfPoint,1); Y=zeros(NumOfPoint,1); U=zeros(NumOfPoint,1); V=zeros(NumOfPoint,1); J=zeros(2,9); H=zeros(NumOfPoint,1); g=zeros(NumOfPoint,1); h=zeros(NumOfPoint,9); E=zeros(NumOfPoint,1); h1=zeros(9,1); %透视变换矩阵初值 h(1,:)=[-0.6256 0.0047 322.4882 0.0056 -0.6287 155.0889 0.0000 0.0000 1.0000]; %设置Xi和Yi for i=1:NumOfPoint X(i,1)=wPoint(i,1); Y(i,1)=wPoint(i,2); end %设置Ui和Vi for i=1:NumOfPoint U(i,1)=imgPoint(i,1); V(i,1)=imgPoint(i,2); end I=eye(9); u=0.000001; % 确定下一个迭代点 for i=1:NumOfPoint k=h(i,7)*X(i,1)+h(i,8)*Y(i,1)+h(i,9); k1=h(i,1)*X(i,1)+h(i,2)*Y(i,1)+h(i,3); k2=h(i,4)*X(i,1)+h(i,5)*Y(i,1)+h(i,6); J=[-X(i,1)/k -Y(i,1)/k -1/k 0 0 0 k1*X(i,1)/k/k k1*Y(i,1)/k/k k1/k/k;0 0 0 -X(i,1)/k -Y(i,1)/k -1/k k2*X(i,1)/k/k k2*Y(i,1)/k/k k2/k/k]; e1=U(i,1)-(h(i,1)*X(i,1)+h(i,2)*Y(i,1)+h(i,3))/k; e2=V(i,1)-(h(i,4)*X(i,1)+h(i,5)*Y(i,1)+h(i,6))/k; e=[e1 e2]'; h1=h(i,:)'-inv(J'*J+u*I)*J'*e; %要加入uI修/////////////////////////////////// h(i+1,:)=h1'; end for i=1:NumOfPoint k=h(i,7)*X(i,1)+h(i,8)*Y(i,1)+h(i,9); e1=U(i,1)-(h(i,1)*X(i,1)+h(i,2)*Y(i,1)+h(i,3))/k; e2=V(i,1)-(h(i,4)*X(i,1)+h(i,5)*Y(i,1)+h(i,6))/k; E(i,1)=e1*e1+e2*e2; end jizhun=E(1,1); js=0; for i=1:NumOfPoint if(jizhun>E(i,1)) jizhun=E(i,1); js=i; end end M=[h(js,1) h(js,2) h(js,3) h(js,4) h(js,5) h(js,6) h(js,7) h(js,8) h(js,9)]; %由图像点计算实际点坐标 CptwPoint=zeros(3,NumOfPoint); IM=inv(M); CptwPoint=IM*imgPoint'; %normalize for i=1:NumOfPoint CptwPoint(:,i)=[CptwPoint(1,i)/CptwPoint(3,i);CptwPoint(2,i)/CptwPoint(3,i);1.0]; end %画图做比较 plot(wPoint(:,1), wPoint(:,2),'*b'); hold on; plot(CptwPoint(1,:), CptwPoint(2,:),'+g'); hold off; %误差比较 cwPoint=wPoint'; err=zeros(3,NumOfPoint); for i=1:NumOfPoint err(:,i)=[CptwPoint(1,i)-cwPoint(1,i);CptwPoint(2,i)-cwPoint(2,i);0]; end a=0; for i=1:NumOfPoint a=a+err(1,i); end x_=a/48; a=0; for i=1:NumOfPoint a=a+err(2,i); end y_=a/48; if x_<0.000001 x_=0; end if y_<0.000001 y_=0; end errx=0; for i=1:NumOfPoint errx=errx+(err(1,i)-x_)^2; end errx=sqrt(errx/48); erry=0; for i=1:NumOfPoint erry=erry+(err(2,i)-y_)^2; end erry=sqrt(erry/48); err=[errx,erry] toc