You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
73 lines
1.1 KiB
73 lines
1.1 KiB
5 months ago
|
% 吊舱下视坐标转换
|
||
|
|
format short g
|
||
|
|
% 相机信息
|
||
|
|
imgW = 1280;
|
||
|
|
imgH = 1024;
|
||
|
|
f = 48;
|
||
|
|
dsize = 7.5;
|
||
|
|
fd = f/dsize*1000;
|
||
|
|
|
||
|
|
%吊舱
|
||
|
|
fAz = 44.1876869;
|
||
|
|
fPt = 18.3241043;
|
||
|
|
|
||
|
|
% 姿态
|
||
|
|
fYaw = 165.08250427246094;
|
||
|
|
fPitch = 4.2472858428955078;
|
||
|
|
fRoll = -0.37968909740447998;
|
||
|
|
|
||
|
|
% 深度
|
||
|
|
dH = 2920;
|
||
|
|
|
||
|
|
M_G2I = getM_G2I( dH,fYaw, fPitch,fRoll,fAz,fPt,imgW,imgH,fd);
|
||
|
|
|
||
|
|
M_I2G = inv(M_G2I);
|
||
|
|
|
||
|
|
t = [0,0,1];
|
||
|
|
|
||
|
|
npoint = warpPoint([640,512],M_I2G,t);
|
||
|
|
|
||
|
|
|
||
|
|
[R1,t1] = invForm(M_I2G,t);
|
||
|
|
|
||
|
|
|
||
|
|
pxpoint = warpPoint(npoint',R1,t1);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
R1 * [npoint;1]/3144.6
|
||
|
|
|
||
|
|
for fAz = 44:5:60
|
||
|
|
for fPt = 18:10:45
|
||
|
|
M_G2I = getM_G2I( dH,fYaw, fPitch,fRoll,fAz,fPt,imgW,imgH,fd);
|
||
|
|
M_I2G = inv(M_G2I);
|
||
|
|
|
||
|
|
p1 = M_I2G*[0,0,1]';
|
||
|
|
p2 = M_I2G*[640,0,1]';
|
||
|
|
p3 = M_I2G*[640,512,1]';
|
||
|
|
p4 = M_I2G*[0,512,1]';
|
||
|
|
|
||
|
|
p1 = p1/p1(3);
|
||
|
|
p2 = p2/p2(3);
|
||
|
|
p3 = p3/p3(3);
|
||
|
|
p4 = p4/p4(3);
|
||
|
|
|
||
|
|
p5 = p1;
|
||
|
|
x(1) = p1(1); x(2) = p2(1); x(3) = p3(1); x(4) = p4(1);x(5) = p5(1);
|
||
|
|
y(1) = p1(2); y(2) = p2(2); y(3) = p3(2); y(4) = p4(2);y(5) = p5(2);
|
||
|
|
|
||
|
|
plot(x,y);
|
||
|
|
set(gca,'DataAspectRatio',[1 1 1])
|
||
|
|
hold on
|
||
|
|
pause(0.1)
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|