stichVideo/script/getM_G2I.m

function [ M ] = getM_G2I( dH,fYaw, fPitch,fRoll,fAz,fPt,imgW,imgH,fd)
%GETM_G2I 此处显示有关此函数的摘要
%   此处显示详细说明
% 设备坐标系定义，ENG 东北地
%//[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]'


%% 深度矩阵
M_het = [1 0 0
    0 1 0
    0 0 dH];


%% 姿态矩阵
M_yaw = [cosd(fYaw)   -sind(fYaw) 0 
sind(fYaw)    cosd(fYaw)     0
0  0   1];

M_pitch=[1  0    0
	0 cosd(fPitch) -sind(fPitch)
	0   sind(fPitch)     cosd(fPitch)];	
	
M_roll = [	 cosd(fRoll)    0      sind(fRoll)
	0    1   0
	-sind(fRoll) 0   cosd(fRoll)
	];

%% 伺服矩阵
M_beta = [cosd(fAz)  -sind(fAz) 0
sind(fAz)    cosd(fAz)     0
0  0   1];

M_alaph = [1  0    0
	0 cosd(fPt) -sind(fPt)
	0   sind(fPt)     cosd(fPt)];

%% 内参矩阵
M_cam = [fd 0 imgW/2
    0 -fd imgH/2
    0 0 1];


M = M_cam*M_alaph*M_beta*M_roll*M_pitch*M_yaw  * M_het;


end