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.
|
|
|
|
|
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
|
|
|
|
|
|
|
