Matlab解剖提取相机投影矩阵以定位和旋转

Question

我在空间中有 6 个点，已知坐标以 mm 为单位，图像中对应的 2D 像素坐标（图像大小为 640x320 像素，点坐标是从图像的左上角测量的。我还有相机的焦距为 43.456mm. 试图找到相机的位置和方向。 我在这里的matlab代码会给我相机位置-572.8052 -676.7060 548.7718 并且看起来是正确的，但我很难找到方向值（相机的偏航俯仰滚动以度为单位） 我知道旋转值应该是 60.3,5.6,-45.1 我的代码中的最后 4 行需要更新以输出相机的方向。 我真的很感谢你在这方面的帮助。 谢谢。

这是我的matlab代码：

Points_2D= [135 183 ; 188 129 ; 298 256 ; 301 43 ; 497  245; 464 110];

Points_3D= [-22.987 417.601 -126.543 ; -132.474 37.67 140.702 ; ...
            388.445 518.635 -574.784 ; 250.015 259.803 67.137 ; ...
            405.915 -25.566 -311.834 ; 568.859 164.809 -162.604  ];


M = [0;0;0;0;0;0;0;0;0;0;0]; 

A = []; 
for i = 1:size(Points_2D,1) 
    u_i = Points_2D(i,1); 
    v_i = Points_2D(i,2); 
    x_i = Points_3D(i,1); 
    y_i = Points_3D(i,2); 
    z_i = Points_3D(i,3); 
    A_vec_1 = [x_i y_i z_i 1 0 0 0 0 -u_i*x_i -u_i*y_i -u_i*z_i -u_i]; %
    A_vec_2 = [ 0 0 0 0 x_i y_i z_i 1 -v_i*x_i -v_i*y_i -v_i*z_i -v_i]; %
    A(end+1,:) = A_vec_1;
    A(end+1,:) = A_vec_2;
end 

[U,S,V] = svd(A);
M = V(:,end);
M = transpose(reshape(M,[],3));

Q = M(:,1:3); 
m_4 = M(:,4); 
Center = (-Q^-1)*m_4


k=[43.456/640 0 320 ;0 43.456/320 160;0 0 1 ];
Rotation= (Q^-1)*k;
CC=Rotation'
eul=rotm2eul(CC)

Answer 1

第一件事：6分就够了，但很可能你有一些错误。为了获得更好的性能，建议有 6 个以上的点，最好是 10-15。

您的代码似乎正确，直到：

Q = M(:,1:3); 
m_4 = M(:,4); 

所以您正在寻找相机的外在和内在参数，即rotation、translation、alpha（x 方向倾斜）、ro、beta（x 方向倾斜）、@ 987654328@，v0（相机中心翻译）。所以一共有5个内在参数，6个外在参数。

这是link，它解释了如何计算这些参数的详细信息。我有一个没有彻底测试的代码，可能有一些错误，但它在我的情况下是有效的。

从M 继续，这是您找到的 3x4 矩阵：

a1 = M(1, 1:3);
a2 = M(2, 1:3);
a3 = M(3, 1:3);

b = M(1:3,4);

% Decomposition of the parameters
eps = 1; %this can be -1 or +1, based on the value you choose, you will have two different results.
ro = eps/sqrt(sumsqr(a3));
r3 = ro*a3;
u0 = ro.^2*(dot(a1,a3))
v0 = ro.^2*(dot(a2,a3))

cos_theta = -dot(cross(a1,a3),cross(a2,a3))/ ...
         dist_cross(a1,a3)/(dist_cross(a2,a3));
theta = acos(cos_theta);
alpha = ro^2*dist_cross(a1,a3)*sin(theta)
beta = ro^2*dist_cross(a2,a3)*sin(theta)
theta_deg = theta*180/pi
r1 = 1/dist_cross(a2,a3)*cross(a2,a3);
r2 = cross(r3,r1);
R = [r1;r2;r3] % Rotation matrix 3x3

% ro*A.inv(R) = K
K = [alpha  -alpha*cot(theta) u0;
     0      beta/sin(theta)   v0;
     0              0         1  ];
T = ro*(inv(K)*b) % Translation matrix, 1x3

在哪里

function [dis] = dist_cross(mi,mj)
    dis = sqrt(sumsqr(cross(mi,mj)));
end

我不保证它是完全正确的，但它应该会有所帮助。