角锥体法进行求解影像的外方位元素matlab实现

close all; clc;clear; tic %% 读取已知信息 B=dlmread('Lor49.dat'); B=dlmread('Lor50.dat'); A_size=B(1,2);%点个数 P=B(2:A_size+1,2:6);%控制点及对应像点信息 lw=B(1,3);%角限差 ll=B(1,4);%线限差 f=B(1,5);%焦距 x=B(1,6);%x0 y=B(1,7);%y0 %% 计算Xs,Ys,Zs近似值 t=1; %计算控制点之间的距离以及对应像点之间的距离，以求近似比例尺 for i=1:A_size-1 for j=i+1:A_size D(t)=(P(i,1)-P(j,1))^2+(P(i,2)-P(j,2))^2+(P(i,3)-P(j,3))^2;%控制点之间的距离 d(t)=(P(i,4)-P(j,4))^2+(P(i,5)-P(j,5))^2;%对应像点之间的距离 m(t)=sqrt(D(t)/d(t)); t=t+1; end end m=mean(m);%近似比例尺 Xs=sum(P(:,1))/A_size;%初值 Ys=sum(P(:,2))/A_size; Zs=sum(P(:,3))/A_size+m*f; %% 像素坐标系与像平面坐标系转换 %像点横纵坐标转换 imx(:,1)=P(:,4)-x; imy(:,1)=y-P(:,5); x=0;y=0;%转换后像中心坐标为0 %% 求线元素 while 1 for i=1:A_size d(i) = sqrt((imx(i)-x)^2+(imy(i)-y)^2+f*f);%d(i) D(i) = sqrt((P(i,1)-Xs)^2+(P(i,2)-Ys)^2++(P(i,3)-Zs)^2);%D(i) end m=0; for i=1:A_size for j=i+1:A_size m=m+1; %系数阵和常数阵 imcos(m)= ((x-imx(i))*(x-imx(j))+(y-imy(i))*(y-imy(j))+f*f)/(d(i)*d(j));%像点组成的余弦 pcos(m)= ((Xs-P(i,1))*(Xs-P(j,1))+(Ys-P(i,2))*(Ys-P(j,2))+(Zs-P(i,3))*(Zs-P(j,3)))/(D(i)*D(j));%控制点组成的余弦 pro(m) = (1-D(i)*pcos(m)/D(j))/(D(i)*D(j)); prc(m) = (1-D(j)*pcos(m)/D(i))/(D(i)*D(j)); a(m) = (Xs-P(i,1))*prc(m)+(Xs-P(j,1))*pro(m);%系数 b(m) = (Ys-P(i,2))*prc(m)+(Ys-P(j,2))*pro(m); c(m) = (Zs-P(i,3))*prc(m)+(Zs-P(j,3))*pro(m); l(m) = imcos(m)-pcos(m);%常数项 end end %系数阵 B=[a;b;c]'; L=l'; Xt=pinv(B'*B)*B'*L%改正数 if norm(Xt)<ll %判断是否迭代 break; end Xs=Xs+Xt(1); Ys=Ys+Xt(2); Zs=Zs+Xt(3); line=[Xs;Ys;Zs]; end fprintf('线元素为\n %d \n %d \n %d \n',Xs,Ys,Zs) %% 求角元素 % 按《有多余控制点的角锥体法改进》一文求解 for i=1:A_size S(i,1)=sqrt((P(i,1)-Xs)^2+(P(i,2)-Ys)^2+(P(i,3)-Zs)^2);%控制点到摄影中心距离 s(i,1)=sqrt((imx(i)-x)^2+(imy(i)-y)^2+f^2);%像点到像主点距离 at(i,1)=(P(i,1)-Xs)/S(i,1);%系数 bt(i,1)=(P(i,2)-Ys)/S(i,1); ct(i,1)=(P(i,3)-Zs)/S(i,1); l1(i,1)=(imx(i)-x)/s(i,1); l2(i,1)=(imy(i)-y)/s(i,1); l3(i,1)=-f/s(i,1); end B=[at,bt,ct]; R1=pinv(B'*B)*B'*l1; R2=pinv(B'*B)*B'*l2; R3=pinv(B'*B)*B'*l3; R=[R1,R2,R3]'; fai=atan(-R3(1,1)/R3(3,1)); w=asin(-R3(2,1)); k=atan(R1(2,1)/R2(2,1)); horn=[fai;w;k]; %% 残差 for i=1:A_size resi_x(i)=-f*(R(1,1)*(P(i,1)-Xs)+R(1,2)*(P(i,2)-Ys)+R(1,3)*(P(i,3)-Zs))/(R(3,1)*(P(i,1)-Xs)+R(3,2)*(P(i,2)-Ys)+R(3,3)*(P(i,3)-Zs))+x-imx(i); resi_y(i)=-f*(R(2,1)*(P(i,1)-Xs)+R(2,2)*(P(i,2)-Ys)+R(2,3)*(P(i,3)-Zs))/(R(3,1)*(P(i,1)-Xs)+R(3,2)*(P(i,2)-Ys)+R(3,3)*(P(i,3)-Zs))+y-imy(i); end fprintf('角元素为\n %d \n %d \n %d \n ',fai,w,k) toc