《摄影测量学基础》光束法MATLAB程序

• 实验目的

运用编程的方法进一步掌握光束法双向解析摄影测量。

• 实验要求

用Matlab编写光束法双向解析摄影测量程序。

• 实验数据

左片外方位元素近似值：Xs1 = 4999.0;Ys1 = 4999.0; Zs1 = 2000.0;

fi1 = 0.002;w1 = 1.400; k1 = 5.270;

右片外方位元素近似值：Xs2 = 5897.0;Ys2 = 5070.0; Zs2 = 2030.0;

fi2 = 0.490;w2 = 2.380; k2 = 6.190;

·实验步骤

本次实验主要分为以下几步：

1、读入坐标数据和外方位元素

2、初始化A、B、L、X、t矩阵

3、计算左片和右片的旋转矩阵

4、求左右片误差方程式中的常数项和系数项并组成矩阵

5、根据公式t=(N11-N12*((N22)^-1)*N12')\(u1-N12*(N22^-1)*u2)求解外方位元素改正数

6、根据公式X=(N22-N12'*((N11)^-1)*N12)\(u2-N12'*(N11^-1)*u1)计算坐标改正数

7、进行循环直至满足精度要求：线元素和坐标误差小于1cm，角元素小于1’

8、评估单位权中误差、外方位元素中误差和每个待定点坐标中误差

·实验代码

m = 9943; %像片比例尺
f = 150/ 1000; 
fid = fopen('C:\Users\小可爱\Desktop\摄影测量学实验\GCP.txt','r'); %将每行xyz坐标写在txt文本中，以空格分开
g = textscan(fid,'%f %f %f'); %读取地面摄影测量坐标
fclose(fid); 
GROUND = [g{1}, g{2}, g{3}]; 
fid = fopen('C:\Users\小可爱\Desktop\摄影测量学实验\XD1.txt','r'); %将左片相点坐标写在txt文本中，以空格分开
p = textscan(fid, '%f %f'); %读取左片像点坐标
fclose(fid); 
PHOTO1 = [p{1}, p{2}]; 
PHOTO1 = PHOTO1 / 1000.0;
fid = fopen('C:\Users\小可爱\Desktop\摄影测量学实验\XD2.txt','r'); %将右片相点坐标写在txt文本中，以空格分开
p = textscan(fid, '%f %f'); %读取右片像点坐标
fclose(fid); 
PHOTO2 = [p{1}, p{2}]; 
PHOTO2 = PHOTO2 / 1000.0;   
pointNum = size(GROUND,1);%总点数 
%左片外方位元素近似值
Xs1 = 4999.0;Ys1 = 4999.0; Zs1 = 2000.0; 
fi1=dms2degrees([0 0 20]);w1=dms2degrees([1 40 0]);k1=dms2degrees([5 27 0]);%将度分秒转换为十进制度
A1 = zeros(pointNum*2,6);A2 = zeros(pointNum*2,6);
B1 = zeros(pointNum*2,3);B2 = zeros(pointNum*2,3);%每个相点坐标的系数
L1 = zeros(pointNum*2,1);L2 = zeros(pointNum*2,1);%误差方程式常数项
dX1=0;dX2=0;dX3=0;dX4=0;dX5=0;
dY1=0;dY2=0;dY3=0;dY4=0;dY5=0;
dZ1=0;dZ2=0;dZ3=0;dZ4=0;dZ5=0;
X=[dX1 dY1 dZ1 dX2 dY2 dZ2 dX3 dY3 dZ3 dX4 dY4 dZ4 dX5 dY5 dZ5];
X=X(:);%将X转为一列
%右片外方位元素近似值
Xs2 = 5897.0;Ys2 = 5070.0; Zs2 = 2030.0; 
fi2=dms2degrees([0 49 0]);w2=dms2degrees([2 38 0]);k2=dms2degrees([6 19 0]);%将度分秒转换为十进制度
while (1)   %计算左右片旋转矩阵
    a1(1) = cos(fi1) * cos(k1) - sin(fi1) * sin(w1) * sin(k1);     
    a1(2) = -cos(fi1) * sin(k1) - sin(fi1) * sin(w1) * cos(k1);    
    a1(3) = -sin(fi1) * cos(w1);     
    b1(1) = cos(w1) * sin(k1);     
    b1(2) = cos(w1) * cos(k1);     
    b1(3) = -sin(w1);     
    c1(1) = sin(fi1) * cos(k1) + cos(fi1) * sin(w1) * sin(k1);     
    c1(2) = -sin(fi1) * sin(k1) + cos(fi1) * sin(w1) * cos(k1);     
    c1(3) = cos(fi1) * cos(w1); 
    a2(1) = cos(fi2) * cos(k2) - sin(fi2) * sin(w2) * sin(k2);     
    a2(2) = -cos(fi2) * sin(k2) - sin(fi2) * sin(w2) * cos(k2);    
    a2(3) = -sin(fi2) * cos(w2);     
    b2(1) = cos(w2) * sin(k2);     
    b2(2) = cos(w2) * cos(k2);     
    b2(3) = -sin(w2);     
    c2(1) = sin(fi2) * cos(k2) + cos(fi2) * sin(w2) * sin(k2);     
    c2(2) = -sin(fi2) * sin(k2) + cos(fi2) * sin(w2) * cos(k2);     
    c2(3) = cos(fi2) * cos(w2); 
    for i = 1:pointNum      %求左右片误差方程式中的常数项和系数项并组成矩阵
        ap1(1) = -f * (a1(1) * (GROUND(i,1) - Xs1) + b1(1) * (GROUND(i, 2) - Ys1) + c1(1) * (GROUND(i,3) - Zs1)) / (a1(3) * (GROUND(i, 1) - Xs1) + b1(3) * (GROUND(i, 2) - Ys1) + c1(3) * (GROUND(i, 3) - Zs1));%相片近似坐标         
        ap1(2) = -f * (a1(2) * (GROUND(i,1) - Xs1) + b1(2) * (GROUND(i, 2) - Ys1) + c1(2) * (GROUND(i,3) - Zs1)) / (a1(3) * (GROUND(i, 1) - Xs1) + b1(3) * (GROUND(i, 2) - Ys1) + c1(3) * (GROUND(i, 3) - Zs1));      
        Zbar1 = a1(3) * (GROUND(i, 1) - Xs1) + b1(3) * (GROUND(i, 2) - Ys1) + c1(3) * (GROUND(i, 3) - Zs1);        
        A1(i * 2 - 1, 1) = (a1(1) * f + a1(3) * PHOTO1(i, 1)) / Zbar1;        
        A1(i * 2 - 1, 2) = (b1(1) * f + b1(3) * PHOTO1(i, 1)) / Zbar1;         
        A1(i * 2 - 1, 3) = (c1(1) * f + c1(3) * PHOTO1(i, 1)) / Zbar1;        
        A1(i * 2 - 1, 4) = PHOTO1(i, 2) * sin(w1) - (PHOTO1(i, 1) * (PHOTO1(i, 1) * cos(k1) - PHOTO1(i, 2) * sin(k1)) / f + f * cos(k1)) * cos(w1);        
        A1(i * 2 - 1, 5) = -f * sin(k1) - PHOTO1(i, 1) * (PHOTO1(i, 1) * sin(k1) + PHOTO1(i, 2) * cos(k1)) / f;         
        A1(i * 2 - 1, 6) = PHOTO1(i, 2);        
        A1(i * 2, 1) = (a1(2) * f + a1(3) * PHOTO1(i, 2)) / Zbar1;         
        A1(i * 2, 2) = (b1(2) * f + b1(3) * PHOTO1(i, 2)) / Zbar1;         
        A1(i * 2, 3) = (c1(2) * f + c1(3) * PHOTO1(i, 2)) / Zbar1;        
        A1(i * 2, 4) = PHOTO1(i, 1) * sin(w1) - (PHOTO1(i, 2) * (PHOTO1(i, 1) * cos(k1) - PHOTO1(i, 2) * sin(k1)) / f - f * sin(k1)) * cos(w1);         
        A1(i * 2, 5) = -f * cos(k1) - PHOTO1(i, 2) * (PHOTO1(i, 1) * sin(k1) + PHOTO1(i, 2) * cos(k1)) / f;        
        A1(i * 2, 6) = -PHOTO1(i, 1); 
        L1(i * 2 - 1, 1) = PHOTO1(i, 1) - ap1(1);          
        L1(i * 2, 1) = PHOTO1(i, 2) - ap1(2); 
        ap2(1) = -f * (a2(1) * (GROUND(i,1) - Xs2) + b2(1) * (GROUND(i, 2) - Ys2) + c2(1) * (GROUND(i,3) - Zs2)) / (a2(3) * (GROUND(i, 1) - Xs2) + b2(3) * (GROUND(i, 2) - Ys2) + c2(3) * (GROUND(i, 3) - Zs2));         
    	ap2(2) = -f * (a2(2) * (GROUND(i,1) - Xs2) + b2(2) * (GROUND(i, 2) - Ys2) + c2(2) * (GROUND(i,3) - Zs2)) / (a2(3) * (GROUND(i, 1) - Xs2) + b2(3) * (GROUND(i, 2) - Ys2) + c2(3) * (GROUND(i, 3) - Zs2));         
    	Zbar2 = a2(3) * (GROUND(i, 1) - Xs2) + b2(3) * (GROUND(i, 2) - Ys2) + c2(3) * (GROUND(i, 3) - Zs2);        
    	A2(i * 2 - 1, 1) = (a2(1) * f + a2(3) * PHOTO2(i, 1)) / Zbar2;        
    	A2(i * 2 - 1, 2) = (b2(1) * f + b2(3) * PHOTO2(i, 1)) / Zbar2;         
    	A2(i * 2 - 1, 3) = (c2(1) * f + c2(3) * PHOTO2(i, 1)) / Zbar2;        
    	A2(i * 2 - 1, 4) = PHOTO2(i, 2) * sin(w2) - (PHOTO2(i, 1) * (PHOTO2(i, 1) * cos(k2) - PHOTO2(i, 2) * sin(k2)) / f + f * cos(k2)) * cos(w2);        
    	A2(i * 2 - 1, 5) = -f * sin(k2) - PHOTO2(i, 1) * (PHOTO2(i, 1) * sin(k2) + PHOTO2(i, 2) * cos(k2)) / f;         
    	A2(i * 2 - 1, 6) = PHOTO2(i, 2);        
    	A2(i * 2, 1) = (a2(2) * f + a2(3) * PHOTO2(i, 2)) / Zbar2;         
    	A2(i * 2, 2) = (b2(2) * f + b2(3) * PHOTO2(i, 2)) / Zbar2;         
    	A2(i * 2, 3) = (c2(2) * f + c2(3) * PHOTO2(i, 2)) / Zbar2;        
    	A2(i * 2, 4) = PHOTO2(i, 1) * sin(w2) - (PHOTO2(i, 2) * (PHOTO2(i, 1) * cos(k2) - PHOTO2(i, 2) * sin(k2)) / f - f * sin(k2)) * cos(w2);         
    	A2(i * 2, 5) = -f * cos(k2) - PHOTO2(i, 2) * (PHOTO2(i, 1) * sin(k2) + PHOTO2(i, 2) * cos(k2)) / f;        
    	A2(i * 2, 6) = -PHOTO2(i, 1);
    	L2(i * 2 - 1, 1) = PHOTO2(i, 1) - ap2(1);          
    	L2(i * 2, 1) = PHOTO2(i, 2) - ap2(2);
        
    end 
        A=[A1(1,1) A1(1,2) A1(1,3) A1(1,4) A1(1,5) A1(1,6) 0 0 0 0 0 0;
           A1(2,1) A1(2,2) A1(2,3) A1(2,4) A1(2,5) A1(2,6) 0 0 0 0 0 0;
           A1(3,1) A1(3,2) A1(3,3) A1(3,4) A1(3,5) A1(3,6) 0 0 0 0 0 0;
           A1(4,1) A1(4,2) A1(4,3) A1(4,4) A1(4,5) A1(4,6) 0 0 0 0 0 0;
           A1(5,1) A1(5,2) A1(5,3) A1(5,4) A1(5,5) A1(5,6) 0 0 0 0 0 0;
           A1(6,1) A1(6,2) A1(6,3) A1(6,4) A1(6,5) A1(6,6) 0 0 0 0 0 0;
           A1(7,1) A1(7,2) A1(7,3) A1(7,4) A1(7,5) A1(7,6) 0 0 0 0 0 0;
           A1(8,1) A1(8,2) A1(8,3) A1(8,4) A1(8,5) A1(8,6) 0 0 0 0 0 0;
           A1(9,1) A1(9,2) A1(9,3) A1(9,4) A1(9,5) A1(9,6) 0 0 0 0 0 0;
           A1(10,1) A1(10,2) A1(10,3) A1(10,4) A1(10,5) A1(10,6) 0 0 0 0 0 0;
           A1(11,1) A1(11,2) A1(11,3) A1(11,4) A1(11,5) A1(11,6) 0 0 0 0 0 0;
           A1(12,1) A1(12,2) A1(12,3) A1(12,4) A1(12,5) A1(12,6) 0 0 0 0 0 0;
           A1(13,1) A1(13,2) A1(13,3) A1(13,4) A1(13,5) A1(13,6) 0 0 0 0 0 0;
           A1(14,1) A1(14,2) A1(14,3) A1(14,4) A1(14,5) A1(14,6) 0 0 0 0 0 0;
           A1(15,1) A1(15,2) A1(15,3) A1(15,4) A1(15,5) A1(15,6) 0 0 0 0 0 0;
           A1(16,1) A1(16,2) A1(16,3) A1(16,4) A1(16,5) A1(16,6) 0 0 0 0 0 0;
           A1(17,1) A1(17,2) A1(17,3) A1(17,4) A1(17,5) A1(17,6) 0 0 0 0 0 0;
           A1(18,1) A1(18,2) A1(18,3) A1(18,4) A1(18,5) A1(18,6) 0 0 0 0 0 0;
           0 0 0 0 0 0 A2(1,1) A2(1,2) A2(1,3) A2(1,4) A2(1,5) A2(1,6);
           0 0 0 0 0 0 A2(2,1) A2(2,2) A2(2,3) A2(2,4) A2(2,5) A2(2,6);
           0 0 0 0 0 0 A2(3,1) A2(3,2) A2(3,3) A2(3,4) A2(3,5) A2(3,6);
           0 0 0 0 0 0 A2(4,1) A2(4,2) A2(4,3) A2(4,4) A2(4,5) A2(4,6);
           0 0 0 0 0 0 A2(5,1) A2(5,2) A2(5,3) A2(5,4) A2(5,5) A2(5,6);
           0 0 0 0 0 0 A2(6,1) A2(6,2) A2(6,3) A2(6,4) A2(6,5) A2(6,6);
           0 0 0 0 0 0 A2(7,1) A2(7,2) A2(7,3) A2(7,4) A2(7,5) A2(7,6);
           0 0 0 0 0 0 A2(8,1) A2(8,2) A2(8,3) A2(8,4) A2(8,5) A2(8,6);
           0 0 0 0 0 0 A2(9,1) A2(9,2) A2(9,3) A2(9,4) A2(9,5) A2(9,6);
           0 0 0 0 0 0 A2(10,1) A2(10,2) A2(10,3) A2(10,4) A2(10,5) A2(10,6);
           0 0 0 0 0 0 A2(11,1) A2(11,2) A2(11,3) A2(11,4) A2(11,5) A2(11,6);
           0 0 0 0 0 0 A2(12,1) A2(12,2) A2(12,3) A2(12,4) A2(12,5) A2(12,6);
           0 0 0 0 0 0 A2(13,1) A2(13,2) A2(13,3) A2(13,4) A2(13,5) A2(13,6);
           0 0 0 0 0 0 A2(14,1) A2(14,2) A2(14,3) A2(14,4) A2(14,5) A2(14,6);
           0 0 0 0 0 0 A2(15,1) A2(15,2) A2(15,3) A2(15,4) A2(15,5) A2(15,6);
           0 0 0 0 0 0 A2(16,1) A2(16,2) A2(16,3) A2(16,4) A2(16,5) A2(16,6);
           0 0 0 0 0 0 A2(17,1) A2(17,2) A2(17,3) A2(17,4) A2(17,5) A2(17,6);
           0 0 0 0 0 0 A2(18,1) A2(18,2) A2(18,3) A2(18,4) A2(18,5) A2(18,6);];
        B=[ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
            0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
            0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
            0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
            0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
            0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
            0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
            0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           -A1(9,1) -A1(9,2) -A1(9,3) 0 0 0 0 0 0 0 0 0 0 0 0;
           -A1(10,1) -A1(10,2) -A1(10,3) 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 -A1(11,1) -A1(11,2) -A1(11,3) 0 0 0 0 0 0 0 0 0;
           0 0 0 -A1(12,1) -A1(12,2) -A1(12,3) 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 -A1(13,1) -A1(13,2) -A1(13,3) 0 0 0 0 0 0;
           0 0 0 0 0 0 -A1(14,1) -A1(14,2) -A1(14,3) 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 -A1(15,1) -A1(15,2) -A1(15,3) 0 0 0;
           0 0 0 0 0 0 0 0 0 -A1(16,1) -A1(16,2) -A1(16,3) 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 -A1(17,1) -A1(17,2) -A1(17,3);
           0 0 0 0 0 0 0 0 0 0 0 0 -A1(18,1) -A1(18,2) -A1(18,3);
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
           -A2(9,1) -A2(9,2) -A2(9,3) 0 0 0 0 0 0 0 0 0 0 0 0;
           -A2(10,1) -A2(10,2) -A2(10,3) 0 0 0 0 0 0 0 0 0 0 0 0;
           0 0 0 -A2(11,1) -A2(11,2) -A2(11,3) 0 0 0 0 0 0 0 0 0;
           0 0 0 -A2(12,1) -A2(12,2) -A2(12,3) 0 0 0 0 0 0 0 0 0;
           0 0 0 0 0 0 -A2(13,1) -A2(13,2) -A2(13,3) 0 0 0 0 0 0;
           0 0 0 0 0 0 -A2(14,1) -A2(14,2) -A2(14,3) 0 0 0 0 0 0;
           0 0 0 0 0 0 0 0 0 -A2(15,1) -A2(15,2) -A2(15,3) 0 0 0;
           0 0 0 0 0 0 0 0 0 -A2(16,1) -A2(16,2) -A2(16,3) 0 0 0;
           0 0 0 0 0 0 0 0 0 0 0 0 -A2(17,1) -A2(17,2) -A2(17,3);
           0 0 0 0 0 0 0 0 0 0 0 0 -A2(18,1) -A2(18,2) -A2(18,3);];
       N11=A'*A;
       N12=A'*B;
       N21=B'*A;
       N22=B'*B;
       L=[L1  L2];
       L=L(:);  %把L变为列向量
       u1=A'*L;
       u2=B'*L;
    t=(N11-N12*((N22)^-1)*N12')\(u1-N12*(N22^-1)*u2);%求解外方位元素改正数
    Xs1 = t(1) + Xs1;     
    Ys1 = t(2) + Ys1;     
    Zs1 = t(3) + Zs1;    
    fi1 = t(4) + fi1;     
    w1 = t(5) + w1;    
    k1 = t(6) + k1; 
    Xs2 = t(7) + Xs2;     
    Ys2 = t(8) + Ys2;     
    Zs2 = t(9) + Zs2;    
    fi2 = t(10) + fi2;     
    w2 = t(11) + w2;    
    k2 = t(12) + k2;
    X=(N22-N12'*((N11)^-1)*N12)\(u2-N12'*(N11^-1)*u1);%计算坐标改正数
    X1=GROUND(5,1)+X(1);
    Y1=GROUND(5,2)+X(2);
    Z1=GROUND(5,3)+X(3);
    X2=GROUND(6,1)+X(4);
    Y2=GROUND(6,2)+X(5);
    Z2=GROUND(6,3)+X(6);
    X3=GROUND(7,1)+X(7);
    Y3=GROUND(7,2)+X(8);
    Z3=GROUND(7,3)+X(9);
    X4=GROUND(8,1)+X(10);
    Y4=GROUND(8,2)+X(11);
    Z4=GROUND(8,3)+X(12);
    X5=GROUND(9,1)+X(13);
    Y5=GROUND(9,2)+X(14);
    Z5=GROUND(9,3)+X(15);
    RANDOM_NUM=X(randperm(numel(X),1));%从X数组中随机取一个不重复改正数用于精度评定    
    if abs(t(4)) < 0.00000485 && abs(t(5)) < 0.00000485 && abs(t(6)) < 0.00000485 && abs(t(10)) < 0.00000485 && abs(t(11)) < 0.00000485 && abs(t(12)) < 0.00000485 &&  abs(t(1)) < 0.01 && abs(t(2)) < 0.01 && abs(t(3)) < 0.01 &&  abs(t(7)) < 0.01 && abs(t(8)) < 0.01 && abs(t(9)) < 0.01 && RANDOM_NUM < 0.01   
        break;     
    end 
end 
A1=[A,B];
V=A1*[t;X]-L;
m0=sqrt(V'*V/9);%单位权中误差
Q=(A1'*A1)^-1;%矩阵求逆
M=zeros(27);%给M预分配内存以提高运行速度
    for i=1:27
        M(i)=m0*sqrt(Q(i,i));%求每个待定点坐标中误差以及外方位元素中误差
    end
fprintf('Xs1 = %f\nYs1 = %f\nZs1= %f\n', Xs1, Ys1, Zs1); fprintf('fi1 = %f\nw1 = %f\nk1 = %f\n', fi1, w1, k1);fprintf('Xs2 = %f\nYs2 = %f\nZs2 = %f\n', Xs2, Ys2, Zs2);fprintf('fi2 = %f\nw2 = %f\nk2 = %f\n', fi2, w2, k2);
fprintf('X1 = %f\nY1 = %f\nZ1 = %f\nX2 = %f\nY2 = %f\nZ2 = %f\nX3 = %f\nY3 = %f\nZ3 = %f\nX4 = %f\nY4 = %f\nZ4 = %f\nX5 = %f\nY5 = %f\nZ5 = %f\n', X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, X4, Y4, Z4, X5, Y5, Z5);
fprintf('m0 = %f\n', m0);

·运行结果

·M阵已计算，需自行打开

·总结

       本次实验难度并不大，只是空间后方交会的延伸，且各元素近似值已给出（角元素近似值为度分秒形式，需转换），重点在于A、B两个矩阵的构造和理解，只要A、B矩阵能顺利列出，所有的问题便迎刃而解！