注意
乌拉,乌拉,乌拉!!!
相对定向程序设计
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
60function [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
4f = 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
本博客所有文章除特别声明外,均采用 CC BY-NC-SA 4.0 许可协议。转载请注明来自 年轻没有梦!
评论