基于matlab的相对定向元素求解（数据源自科研）

废话不多说，直接放代码

clc;clear;
disp("连续像对相对定向");
x_1 = [1.983,0.924,1.068,1.208,-0.514,1.293];
y_1 = [-6.091,7.098,4.538,6.858,-10.05,-8.089];
x_2 = [-3.202,-2.830,-2.878,-2.578,-5.642,-3.981];
y_2 = [-5.564,7.694,5.098,7.429,-9.152,-7.441];
%%u=0.0911;v=-0.0311;f=24; **这个是结果**
%%q = -0.0423;w=-0.0301;k=0.0975;
f=24;
q=0;w=0;
k=0;u=0;
v=0;n=6;
dq=1;dw=1;dk=1;du=1;dv=1;
limit1=0.00003;
X1=x_1;Y1=y_1;round=0;
for i=1:n
        Z1(i)=-f;
    end
while(1)
    a1=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);
    a2=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
    a3=-sin(q)*cos(w);
    b1=cos(w)*sin(k);
    b2=cos(w)*cos(k);
    b3=-sin(w);
    c1=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);
    c2=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k);
    c3=cos(q)*cos(w);
    R=[a1,a2,a3;
        b1,b2,b3;
        c1,c2,c3];
    
    for i=1:n
        a=(R*[x_2(i);y_2(i);-f])';
        X2(i)=a(1);Y2(i)=a(2);Z2(i)=a(3);
    end
    
    BX=x_1(1)-x_2(1);
    BY=u*BX;
    BZ=v*BX;
    coe= zeros(n,5);
    for i = 1:n
        N =(BX*Z2(i)-BZ*X2(i))/(X1(i)*Z2(i)-Z1(i)*X2(i));
        N_1=(BX*Z1(i)-BZ*X1(i))/(X1(i)*Z2(i)-Z1(i)*X2(i));
        Q=N*Y1(i)-N_1*Y2(i)-BY;
        L(i)=Q;
        coe(i,:)=[-X2(i)*Y2(i)*N_1/Z2(i),-(Z2(i)+Y2(i)^2/Z2(i))*N_1,X2(i)*N_1,BX,-Y2(i)/Z2(i)*BX];
    end
    dX=inv(coe'*coe)*coe'*L';
    dq=dX(1);dw=dX(2);dk=dX(3);du=dX(4);dv=dX(5);
    q=q+dq;w=w+dw;k=k+dk;u=u+du;v=v+dv;
    round = round+1;
    if(abs(dq)<limit1&&abs(dw)<limit1&&abs(dk)<limit1&&abs(du)<limit1&&abs(dv)<limit1)
   %if(round==2) 
        break;
   end
end



结果如下，完全附和

u =

    0.0911


v =

   -0.0314


q =

   -0.0423


w =

   -0.0301


k =

    0.0975

摄影测量matlab代码持续更新中