Реализация повернутой проективной камеры

Я пытаюсь построить проекцию точек в пространстве на изображение. Моя проективная камера должна поворачиваться по тангажу, крену и рысканию. Я также знаю, что для проективная матрица имеет вид P = K*[R|t], где K = [fx,0,cx; 0,fy,cy; 0,0,1] - матрица внутренних параметров камеры, R - матрица поворота (ее я получаю из кватерниона трех последовательных поворотов по тангажу, крену и рысканию), а t = [1,0,0,-xc; 0,1,0,-yc; 0,0,1,-zc] - матрица сдвига в точку C = (xc,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.