%摄影测量后方交会计算 M脚本.......
% Az@Maoohang v1.00.10
%地面坐标
% dmzbXYZ =[36589.41 25273.32 2195.17;
%           37631.08 31324.51 728.69;
%          39100.97 24934.98 2386.50;
%           40426.54 30319.81 757.31];
%像点坐标
% xdzbXY=[-86.15 -68.99;
%         -53.40,82.21;
%         -14.78,-76.63;
%         10.46,64.43];
f=0.15324; %摄影机主距
dmzbXYZ=input('输入地面坐标的XYZ(注意输入矩阵啊...4x3的)');
xdzbXY=input('输入像点坐标的XY(注意输入矩阵啊...4x2的)');
a=0.0;w=0.0;k=0.0;counter=0;
xdzbXY=xdzbXY/1000;
d1=sqrt(sum((dmzbXYZ(1,:)-dmzbXYZ(2,:)).*(dmzbXYZ(1,:)-dmzbXYZ(2,:))));
d2=sqrt(sum((xdzbXY(1,:)-xdzbXY(2,:)).*(xdzbXY(1,:)-xdzbXY(2,:))));
m=d1/d2; %计算比例尺
%估计坐标
probablyXYZ=sum(dmzbXYZ(:,1:2));
probablyXYZ=[probablyXYZ/4 m*f];
%初始化X阵
X=[1 1 1 1 1 1]';

while((abs(X(4,1))>=2.9e-5)||(abs(X(5,1))>=2.9e-5)||(abs(X(6,1))>=2.9e-5))
    counter=counter+1; %计数器
    %旋转矩阵R
R=[cos(a)*cos(k)-sin(a)*sin(w)*sin(k) cos(w)*sin(k) sin(a)*cos(k)+cos(a)*sin(w)*sin(k);
    -cos(a)*sin(k)-sin(a)*sin(w)*cos(k) cos(w)*cos(k) -sin(a)*sin(k)+cos(a)*sin(w)*cos(k);
    -sin(a)*cos(w) -sin(w) cos(a)*cos(w)];
    %共线方程M阵
M=dmzbXYZ-[probablyXYZ;probablyXYZ;probablyXYZ;probablyXYZ];
    %估计值(x),(y)阵
L0=[];
for i=1:4
L0=[L0;-f*(sum(R(1,:).*M(i,:))/sum(R(3,:).*M(i,:)));-f*(sum(R(2,:).*M(i,:))/sum(R(3,:).*M(i,:)))]; %#ok<AGROW>
end;
%L阵
xdzbXY_vector=[xdzbXY(1,:)';xdzbXY(2,:)';xdzbXY(3,:)';xdzbXY(4,:)'];
L=xdzbXY_vector-L0;

%A阵
A=[];
for i=1:4
    x=xdzbXY(i,1);
    y=xdzbXY(i,2);
    z=sum(R(3,:).*M(i,:));
    A=[A;(R(1,1)*f+R(3,1)*x)/z (R(1,2)*f+R(3,2)*x)/z (R(1,3)*f+R(3,3)*x)/z y*sin(w)-(x*(x*cos(k)-y*sin(k))/f+f*cos(k))*cos(w) -f*sin(k)-x*(x*sin(k)+y*cos(k))/f y;
       (R(2,1)*f+R(3,1)*y)/z (R(2,2)*f+R(3,2)*y)/z (R(2,3)*f+R(3,3)*y)/z -x*sin(w)-(x*(x*cos(k)-y*sin(k))/f-f*sin(k))*cos(w) -f*cos(k)-y*(x*sin(k)+y*cos(k))/f -x]; %#ok<AGROW>
end;
%平差中最常用的公式.....
X=((A'*A)^-1)*A'*L;
probablyXYZ = probablyXYZ + X(1:3,1)';

a=a+X(4,1);
w=w+X(5,1);
k=k+X(6,1);

end;
fprintf(1,'R阵为:\n');
R %#ok<NOPTS>
fprintf(1,'\nX阵为:\n')
X %#ok<NOPTS>
wfwys=[probablyXYZ';a;w;k];
fprintf(1,'\n外方位元素为:');
wfwys %#ok<NOPTS>
fprintf(1,'\n执行次数:%d',counter);

PS: 摄影测量后方交会程序