avatar

相对定向程序设计

注意

乌拉,乌拉,乌拉!!!

相对定向程序设计

matlab实现

  • relative.m

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    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);
  • 输入

    1
    2
    3
    4
    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)
  • 输出

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    相对定向后: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
文章作者: Gy
文章链接: http://sgyat.cn/2020/05/28/相对定向/
版权声明: 本博客所有文章除特别声明外,均采用 CC BY-NC-SA 4.0 许可协议。转载请注明来自 年轻没有梦
打赏
  • 微信
    微信
  • 支付宝
    支付宝

评论