Matlab could not find explicit solution for PnP problem

Jacob Rowe

2020 5 24

0 个回答

3 次查看（30 天）

0 个投票

I am trying to solve the projection equation for 4 unknowns psi,tx,ty and tz. This problem is similar to the paper at http://cmp.felk.cvut.cz/~kukelova/webthesis/publications/Kukelova-Bujnak-Pajdla-ACCV-2010.pdf where they found a closed form solution. However, I cannot get matlab to solve for these unknowns. How can I obtain an expression for these unkowns through this method or something similar. When I have values for all of the other variables Matlab can produce the correct results for the unknowns.

syms tx ty tz A11 A12 A13 A21 A22 A23 X11 X12 X13 X21 X22 X23 psi theta phi
% First point
A1 = skew([A11;A12;A13]);
C1 = [X11;X12;X13;1]; 
Rz = [cos(psi), -sin(psi),   0;
      sin(psi),  cos(psi),   0;
         0,         0,       1];
     
Ry = [cos(theta),  0,   sin(theta);
          0,       1,       0;
     -sin(theta),  0,   cos(theta)];
Rx = [1,      0,          0;
      0,   cos(phi),  -sin(phi);
      0,   sin(phi),   cos(phi)];
R = Rz*Ry*Rx;
T = [tx;ty;tz];
B = [R',-R'*T];
PnP1 = A1*B*C1;
% Second point
A2 = skew([A21;A22;A23]);
C2 = [X21;X22;X23;1];
PnP2 = A2*B*C2;
% Solve equations
syms PnP_eqn
PnP_eqn = [PnP1(:,:)==0; PnP2(:,:)==0];
[psi, tx, ty, tz] = solve(PnP_eqn);
function [skew_out] = skew(A)
skew_out = [ 0 ,  -A(3),  A(2);
            A(3),   0,   -A(1);
           -A(2),  A(1),   0];
       
end