Dissecting Extracting Camera Projection Matrix to position and rotation

edit

I have 6 points in space with known coordinates in mm and corresponding 2D pixel coordinates in the image (image size is 640x320 pixels and points coordinates have been measured from upper left of the image. also I have the focal length of the camera to be 43.456mm. trying to find the camera position and orientation(x, y,z in mm and yaw pitch roll of the camera in degrees) . My matlab code here will give me the camera location as -572.8052 -676.7060 548.7718 and seems correct but I am having a hard time finding the orientation values (yaw pitch roll of the camera in degrees) I know that the rotation values should be 60.3,5.6,-45.1 Does open CV have any tools to this? I would really really appreciate your help on this. Thanks.

Here is my matlab code:

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