Matlab解剖提取相機投影矩陣的位置和旋轉

Question

我有空間6點，已知的座標在毫米和相應的二維像素座標在圖像中（圖像尺寸是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方向），u0，v0（翻譯相機中心）。所以共有5個內在函數，6個外部參數。

這是一個link，它解釋瞭如何計算這些參數的詳細信息。我有一個代碼，我沒有徹底測試，可能會有一些錯誤，但它在我的情況下工作。

從M繼續這是你找到的3×4矩陣：

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 

我不保證它是完全正確的，但它應該幫助。