旋转投影相机的实现

我正在尝试将空间中的点投影到图像上。我的投影相机需要在俯仰、滚动和偏航中旋转。我也知道对于 ，投影矩阵P = K*[R|t]的K = [fx,0,cx; 0,fy,cy; 0,0,1]形式t = [1,0,0,-xc; 0,1,0,-yc; 0,0,1,-zc]为 yc,zc) ，其中相机所在的位置。那些。此外，为了获得图像上点的坐标，您需要计算u = x/z和v = y/z，其中[x;y;z] = P*[X;Y;Z;1]。我的问题似乎出在旋转后的显示上，因为没有它显示是正确的：我显示红点，相机矩阵的大小为 [w,h]，黑线是世界坐标系，彩色线是相机坐标系。

问题是要了解需要更改或添加的内容，以便在转弯时不会出现故障。

我的设计功能：

function [u,v,P,R,point1,pointNew] = point2camProjection(point,f,w,h,xc,yc,zc,alpha,beta,gamma,cx,cy)
  pix=6.5e-3; %mm
  K = [f/pix,0,cx; 0,f/pix,cy; 0,0,1]; % camera calibration matrix
  
  %alpha - altitude (pitch) - Y
  %beta - heading (yaw) - Z -> X
  %gamma - bank (roll) - X -> Z
  R_heading = quaternion(cos(beta/2),sin(beta/2),0,0);
  R_alt     = quaternion(cos(alpha/2),0,sin(alpha/2),0);
  R_bank    = quaternion(cos(gamma/2),0,0,sin(gamma/2));
  
  R = R_heading*R_alt*R_bank;
  
  axX = quaternion(0,1,0,0);
  axY = quaternion(0,0,1,0);
  axZ = quaternion(0,0,0,1);
  
  axX = R*axX*conj(R);
  axY = R*axY*conj(R);
  axZ = R*axZ*conj(R);

  line([xc xc+axX.x*5000],[yc yc+axX.y*5000],[zc zc+axX.z*5000],'color','g','linewidth',2); % X
  line([xc xc+axY.x*5000],[yc yc+axY.y*5000],[zc zc+axY.z*5000],'color','m','linewidth',2); %Y
  line([xc xc+axZ.x*5000],[yc yc+axZ.y*5000],[zc zc+axZ.z*5000],'color','c','linewidth',2); %Z
  
  pix=6.5e-3; %mm
  
  q1 = R*4*quaternion(0,-w/2,h/2,f/pix)*conj(R);
  q2 = R*4*quaternion(0,-w/2,-h/2,f/pix)*conj(R);
  q3 = R*4*quaternion(0,w/2,-h/2,f/pix)*conj(R);
  q4 = R*4*quaternion(0,w/2,h/2,f/pix)*conj(R);

  mat = [R.w^2+R.x^2-R.y^2-R.z^2, 2*(R.x*R.y-R.w*R.z), 2*(R.x*R.z+R.w*R.y);...
         2*(R.x*R.y+R.w*R.z), R.w^2-R.x^2+R.y^2-R.z^2, 2*(R.y*R.z-R.w*R.x);...
         2*(R.x*R.z-R.w*R.y), 2*(R.y*R.z+R.w*R.x), R.w^2-R.x^2-R.y^2+R.z^2];
  T = [1,0,0,-xc;0,1,0,-yc;0,0,1,-zc];
  P = K*mat*T; % camera matrix
 
  [m,n]=size(point);
  point1 = [];
  point2 = [];
  phi1 = atan(w*pix/(2*f)); % Oxz
  phi2 = atan(h*pix/(2*f)); % Oyz
  Cconj = conj(R)*quaternion(0,xc,yc,zc)*R;
  
  for k=1:m
    point_conj = conj(R).*quaternion(0,point(k,1),point(k,2),point(k,3))*R;
    if abs(point_conj.x-Cconj.x)<abs(point_conj.z-Cconj.z)*tan(phi1) && ...
       abs(point_conj.y-Cconj.y)<abs(point_conj.z-Cconj.z)*tan(phi2) && ...
       point_conj.z>=Cconj.z
       point1 = [point1; point(k,:)];
    end
  end
    
  [m,n] = size(point1);
  homogen_point=[];
  if m~=0
    homogen_point = [point1,ones(m,1)];
  end
  pointNew = [];
  [m,n] = size(homogen_point);
  if m~=0 
      
    for k = 1:m
      pointNew = [pointNew; (P*homogen_point(k,:)')'];
    end
    u = pointNew(:,1)./pointNew(:,3);
    v = pointNew(:,2)./pointNew(:,3);
  else
    u=[];
    v=[];
  end

在相机所在的给定点的构造：

f=4.5; %mm
w=2042; h=1856; %pixels
##
al=pi*(rand()-0.5);
bet=2*pi*(rand()-0.5);
g=2*pi*(rand()-0.5);

##al=0;
##bet=0;
##g=0;

points = [Px'; Py'; Pz']';
[u,v,P,R,point1,pointNew] = point2camProjection(points,f,w,h,xc,yc,zc,al,bet,g,w/2-1, h/2-1);
[p1,p2] = size(point1);
if p1~=0 && p2~=0
  plot3(point1(:,1),point1(:,2),point1(:,3),'ro')
end
if length(u)~=0
  
##  plot3(pointNew(:,1)./pointNew(:,3),pointNew(:,2)./pointNew(:,3),ones(p1,1),'+g')
  
  figure; grid on; hold on;
  UV=[];
  for k=1:length(u)
  ##  if (u(k)<=w)&(v(k)<=h)&&(u(k)>=0)&(v(k)>=0)
      UV=[UV;[u(k),v(k)]];
  ##  end
  end
  if length(UV)==0
    disp(['al = ' num2str(al) ', bet = ' num2str(bet) ', g = ' num2str(g)])
  else
    plot(UV(:,1),UV(:,2),'m.')
  ##  xlim([0 w]); ylim([0 h]);
    line([0 0],[0 h],'color','k','linewidth',2);
    line([w w],[0 h],'color','k','linewidth',2);
    line([0 w],[0 0],'color','k','linewidth',2);
    line([0 w],[h h],'color','k','linewidth',2);
    xlabel('u'); ylabel('v');
    title(['al=' num2str(rad2deg(al)) ', bet=' num2str(rad2deg(bet)) ', g=' num2str(rad2deg(g))])
    axis equal;
  end
end

椭圆是由单独的代码设置的，所以最好不设置它。任意旋转的示例（黑框 - 图像尺寸，蓝轴 - OZ，绿色 - OX，覆盆子 - OY）：

为了以防万一，我尝试了不使用四元数的旋转，但是通过将旋转矩阵乘以 X、Y 和 Z 周围的矩阵。但是，结果是相同的。我倾向于认为问题可能在于获取 u 和 v 坐标。