我正在尝试将空间中的点投影到图像上。我的投影相机需要在俯仰、滚动和偏航中旋转。我也知道对于 ，投影矩阵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 坐标。
