1.m

空间后方交汇求解相机外方位元素

% x，y 控制点像点坐标
% X，Y，Z 控制点空间坐标
%f焦距
%X0，Y0，Z0，a，b，c六个外方位元素
%x0，y0，-f内方位元素：光心坐标
%cha，chb，chc：外方位角元素改正数
%count 记录迭代次数
%R 旋转矩阵
%A 线性化的偏导系数矩阵
%L 常数项矩阵
%M0 外方位元素矩阵
%M1 外方位元素改正数矩阵
clear all;clc;
%输入控制点坐标
x=[-86.15,-53.40,-14.78,10.46]/1000;
y=[-68.99,82.21,-76.63,64.43]/1000;
X=[36589.41,37631.08,39100.97,40426.54];
Y=[25273.32,31324.51,24934.98,30319.81];
Z=[2195.17,728.96,2386.50,757.31];
%焦距f；输入外方位元素位置元素X,Y,Z，角元素a，b，c，内方位元素x,y的初始值
f=153.24/1000;
X0=mean(X);
Y0=mean(Y);
a=0;
b=0;
c=0;
x0=0;y0=0;
Z0=((X(1)-X0)/(x(1)-x0)+(Y(1)-Y0)/(y(1)-y0))/2*f;
for i=2:1:4
    Z0=Z0+((X(i)-X0)/(x(i)-x0)+(Y(i)-Y0)/(y(i)-y0))/2*f;
end
Z0=Z0/4;
%进入循环,如果外方位角元素改正数小于限差0.1'或循环次数大于50，循环结束
cha=1;chb=1;chc=1;
count=0;
while((cha > 1/600*pi/180 | chb>1/600*pi/180 | chc>1/600*pi/180) )
  %旋转矩阵R  
    a1=cos(a)*cos(c)-sin(a)*sin(b)*sin(c);
    a2=-cos(a)*sin(c)-sin(a)*sin(b)*cos(c);
    a3=-sin(a)*cos(b);
    b1=cos(b)*sin(c);
    b2=cos(b)*cos(c);
    b3=-sin(b);
    c1=sin(a)*cos(c)+cos(a)*sin(b)*sin(c);
    c2=-sin(a)*sin(c)+cos(a)*sin(b)*cos(c);
    c3=cos(a)*cos(b);
    R=[a1,a2,a3;b1,b2,b3;c1,c2,c3];
    %求偏导系数矩阵A和常数矩阵L
    XX=a1*(X(1)-X0)+b1*(Y(1)-Y0)+c1*(Z(1)-Z0);
    YY=a2*(X(1)-X0)+b2*(Y(1)-Y0)+c2*(Z(1)-Z0);
    ZZ=a3*(X(1)-X0)+b3*(Y(1)-Y0)+c3*(Z(1)-Z0);
    a11=1/ZZ*(a1*f+a3*(x(1)-x0));
    a12=1/ZZ*(b1*f+b3*(x(1)-x0));
    a13=1/ZZ*(c1*f+c3*(x(1)-x0));
    a21=1/ZZ*(a2*f+a3*(y(1)-y0));
    a22=1/ZZ*(b2*f+b3*(y(1)-y0));
    a23=1/ZZ*(c2*f+c3*(y(1)-y0)); 
    a14=(y(1)-y0)*sin(b)-((x(1)-x0)/f*((x(1)-x0)*cos(c)-(y(1)-y0)*sin(c))+f*cos(c))*cos(b);
    a15=-f*sin(c)-(x(1)-x0)/f*((x(1)-x0)*sin(c)+(y(1)-y0)*cos(c));
    a16=y(1)-y0;
    a24=-(x(1)-x0)*sin(b)-((y(1)-y0)/f*((x(1)-x0)*cos(c)-(y(1)-y0)*sin(c))-f*cos(c))*cos(b);
    a25=-f*cos(c)-(y(1)-y0)/f*((x(1)-x0)*sin(c)+(y(1)-y0)*cos(c));
    a26=-(x(1)-x0);
    a17=-1*a26/f;
    a27=a16/f;
    a18=1;
    a28=0;
    a19=0;a29=1;    
    A=[a11,a12,a13,a14,a15,a16;
       a21,a22,a23,a24,a25,a26];
    L=[x(1)-x0+f*XX/ZZ, y(1)-y0+f*YY/ZZ]';
    for i=2:4
        ZZ=a3*(X(i)-X0)+b3*(Y(i)-Y0)+c3*(Z(i)-Z0);
        a11=1/ZZ*(a1*f+a3*(x(i)-x0));
        a12=1/ZZ*(b1*f+b3*(x(i)-x0));
        a13=1/ZZ*(c1*f+c3*(x(i)-x0));
        a21=1/ZZ*(a1*f+a3*(y(i)-y0));
        a22=1/ZZ*(b1*f+b3*(y(i)-y0));
        a23=1/ZZ*(c1*f+c3*(y(i)-y0)); 
        a14=(y(i)-y0)*sin(b)-((x(i)-x0)/f*((x(i)-x0)*cos(c)-(y(i)-y0)*sin(c))+f*cos(c))*cos(b);
        a15=-f*sin(c)-(x(i)-x0)/f*((x(i)-x0)*sin(c)+(y(i)-y0)*cos(c));
        a16=y(i)-y0;
        a24=-(x(i)-x0)*sin(b)-((y(i)-y0)/f*((x(i)-x0)*cos(c)-(y(i)-y0)*sin(c))-f*cos(c))*cos(b);
        a25=-f*cos(c)-(y(i)-y0)/f*((x(i)-x0)*sin(c)+(y(i)-y0)*cos(c));
        a26=-(x(i)-x0);
        a17=-1*a26/f;
        a27=a16/f;
        a18=1;a28=0;
        a19=0;a29=1;    
        AA=[a11,a12,a13,a14,a15,a16;
            a21,a22,a23,a24,a25,a26];
         LL=[x(i)-x0+f*(a1*(X(i)-X0)+b1*(Y(i)-Y0)+c1*(Z(i)-Z0))/(a3*(X(i)-X0)+b3*(Y(i)-Y0)+c3*(Z(i)-Z0)), y(i)-y0+f*(a2*(X(i)-X0)+b2*(Y(i)-Y0)+c2*(Z(i)-Z0))/(a3*(X(i)-X0)+b3*(Y(i)-Y0)+c3*(Z(i)-Z0)) ]';
        A=[A;AA];
        L=[L;LL];
    end
    %求改正项矩阵M1和修正后的外方位元素矩阵M0
    M0=[X0,Y0,Z0,a,b,c]';
    M1=inv(A'*A)*A'*L;
    cha=M1(4);chb=M1(5);chc=M1(6);
   M0=M0+M1;
   X0=M0(1);
   Y0=M0(2);
   Z0=M0(3);
   a=M0(4);
   b=M0(5);
   c=M0(6);
   %迭代一次后计数器增加
    count=count+1;
end
%检验迭代次数
if count>50
    '迭代次数大于限差'
end

M0