% 吊舱下视坐标转换
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