相对定向程序设计

注意

乌拉，乌拉，乌拉！！！

相对定向程序设计

matlab实现

• relative.m

  function [dx] = relative(x1y1,x2y2,f)
  
  varphi=0; omega=0; kappa=0;           % 初始化右像片外方位角元素 
  u=0; v=0;                             % bx*u=by, bx*v=bz
  W = [u; v; varphi; omega; kappa];     % 组成矩阵便于运算   
  count = 0;                            % 记录迭代次数
  bx = sum(x1y1(:,1)-x2y2(:,1))/size(x1y1,1);
  
  while true
      % 计算右像片的旋转矩阵R
      a1 = cos(W(3))*cos(W(5))-sin(W(3))*sin(W(4))*sin(W(5));
      a2 = -cos(W(3))*sin(W(5))-sin(W(3))*sin(W(4))*cos(W(5));
      a3 = -sin(W(3))*cos(W(4));
      b1 = cos(W(4))*sin(W(5));
      b2 = cos(W(4))*cos(W(5));
      b3 = -sin(W(4));
      c1 = sin(W(3))*cos(W(5))+cos(W(3))*sin(W(4))*sin(W(5));
      c2 = -sin(W(3))*sin(W(5))+cos(W(3))*sin(W(4))*cos(W(5));
      c3 = cos(W(3))*cos(W(4));
      R = [[a1,a2,a3];[b1,b2,b3];[c1,c2,c3]];      % 组成旋转矩阵
      
      % 计算像点像空间辅助坐标
      X1Y1Z1 = [x1y1 -f*ones(size(x1y1,1),1)]';      % R1为单位阵
      X2Y2Z2 = (R * [x2y2 -f*ones(size(x2y2,1),1)]'); 
      
      % 计算by，bz   
      by = bx * W(1);
      bz = bx * W(2);
      
      % 计算N1, N2
      N = []; L = []; A = [];
      mode=size(x1y1,1);
      for i = 1:mode
          n = [(bx * X2Y2Z2(i*3) - bz * X2Y2Z2(1+(i-1)*3))/...
              (X1Y1Z1(1+(i-1)*3) * X2Y2Z2(i*3) - X2Y2Z2(1+(i-1)*3) * X1Y1Z1(i*3))
              (bx * X1Y1Z1(i*3) - bz * X1Y1Z1(1+(i-1)*3))/...
              (X1Y1Z1(1+(i-1)*3) * X2Y2Z2(i*3) - X2Y2Z2(1+(i-1)*3) * X1Y1Z1(i*3))
              ];
          l = n(1) * X1Y1Z1(2+(i-1)*3) - n(2) * X2Y2Z2(2+(i-1)*3) - by;
          N = [N; n];  L = [L; l];
          a = bx; b = -bx * X2Y2Z2(2+(i-1)*3)/X2Y2Z2(i*3); c = -n(2) * X2Y2Z2(1+(i-1)*3) * X2Y2Z2(2+(i-1)*3)/X2Y2Z2(i*3);
          d = -n(2) * (X2Y2Z2(i*3) + X2Y2Z2(2+(i-1)*3)^2 / X2Y2Z2(i*3));  e = X2Y2Z2(1+(i-1)*3) * n(2);
          A = [A; a b c d e];
      end
      
      % 求dx，并记录迭代次数
      dx = inv(A'*A)*(A'*L);
      W = W + dx;
      count = count + 1;
      
      % 限差为3e-5
      if dx(1)<3e-5 && dx(2)<3e-5 ...                
              && dx(3)<3e-5 && dx(4)<3e-5 && dx(5)<3e-5
          break;
      end
  end
  
  fprintf('相对定向后：');
  fprintf('bx=%f, by=%f, bz=%f, varphi=%f, omega=%f, kappa=%f\n',bx,bx*W(1),bx*W(2),W(3),W(4),W(5));
  fprintf('迭代了: %d 次\n',count);

• 输入

  f = 24;
  x1y1 = [1.983 -6.091; 0.924 7.098; 1.068 4.538; 1.208 6.858; -0.514 -10.05; 1.293 -8.089];
  x2y2 = [-3.2020 -5.5640; -2.8300 7.6940; -2.8780 5.0980; -2.5780 7.4290; -5.6420 -9.1520; -3.9810 -7.4410 ];
  relative(x1y1,x2y2,f)

• 输出

  相对定向后：bx=4.512167, by=0.411146, bz=-0.141690, varphi=-0.042277, omega=-0.030103, kappa=0.097478
  迭代了: 6 次
  
  ans =
  1.0e-04 *
  
  -0.1505
   0.0184
  -0.0386
   0.0077
   0.0514