title: Camera pose estimation (OpenCV PnP)
body: <p>I am trying to get a global pose estimate from an image of four fiducials with known global positions using my webcam.</p>
  
  <p>I have checked many stackexchange questions and a few papers and I cannot seem to get a a correct solution. The position numbers I do get out are repeatable but in no way linearly proportional to camera movement. FYI I am using C++ OpenCV 2.1.</p>
  
  <p><a href="https://i.stack.imgur.com/WRa9I.png" rel="noreferrer"><strong>At this link is pictured</strong></a> my coordinate systems and the test data used below.</p>
  
  <pre><code>% Input to solvePnP():
  imagePoints =     [ 481, 831; % [x, y] format
                      520, 504;
                     1114, 828;
                     1106, 507]
  objectPoints = [0.11, 1.15, 0; % [x, y, z] format
                  0.11, 1.37, 0; 
                  0.40, 1.15, 0;
                  0.40, 1.37, 0]
  
  % camera intrinsics for Logitech C910
  cameraMat = [1913.71011, 0.00000,    1311.03556;
               0.00000,    1909.60756, 953.81594;
               0.00000,    0.00000,    1.00000]
  distCoeffs = [0, 0, 0, 0, 0]
  
  % output of solvePnP():
  tVec = [-0.3515;
           0.8928; 
           0.1997]
  
  rVec = [2.5279;
         -0.09793;
          0.2050]
  % using Rodrigues to convert back to rotation matrix:
  
  rMat = [0.9853, -0.1159,  0.1248;
         -0.0242, -0.8206, -0.5708;
          0.1686,  0.5594, -0.8114]
  </code></pre>
  
  <p>So far, <strong>can anyone see anything wrong with these numbers</strong>? I would appreciate it if someone would check them in for example MatLAB (code above is m-file friendly).</p>
  
  <p>From this point, I am unsure of how to get the global pose from rMat and tVec.
  From what I have read in <a href="https://stackoverflow.com/questions/14515200/python-opencv-solvepnp-yields-wrong-translation-vector">this question</a>, to get the pose from rMat and tVec is simply:</p>
  
  <pre><code>position = transpose(rMat) * tVec   % matrix multiplication
  </code></pre>
  
  <p>However I suspect from other sources that I have read it is not that simple.</p>
  
  <p><strong>To get the position of the camera in real world coordinates, what do I need to do?</strong>
  As I am unsure if this is an implementation problem (however most likely a theory problem) I would like for someone who has used the solvePnP function successfully in OpenCV to answer this question, although any ideas are welcome too!</p>
  
  <p>Thank you very much for your time.</p>