title: 2D pixel coordinates to 3D world coordinates using opencv solvePnP
body: <p>I saw a lot of similar questions, but not this exact one.</p>
  <p><strong>I know:</strong></p>
  <ul>
  <li>pixel coordinates (<strong>u</strong>,<strong>v</strong>) and depth (<strong>d</strong>) - 4 points</li>
  <li>corresponding world coordinates (<strong>x</strong>,<strong>y</strong>,<strong>z</strong>) - 4 points</li>
  <li>intrinsic matrix <strong>K</strong> of the camera (I didn't calibrate yet but used the camera default one)</li>
  <li>extrinsic matrix [<strong>R</strong>|<strong>t</strong>] (I call its 4x4 version <strong>M_ext</strong>)</li>
  <li>conversion from world coordinate points <strong>X</strong> (3D) to pixel coordinate points <strong>p</strong> (2D)</li>
  </ul>
  <p><strong>I don't know:</strong></p>
  <ul>
  <li>conversion from pixel coordinate points <strong>p</strong> (2D) + depth <strong>d</strong> to world coordinate points <strong>X</strong> (3D)</li>
  </ul>
  <p><strong>0. Get extrinsic matrix:</strong></p>
  <p>To get the extrinsic matrix I use opencvs function solvePnP:</p>
  <p><code>ret, rvec, tvec = cv.solvePnP(world_points, img_points, K, np.float32([]), cv.SOLVEPNP_IPPE)</code></p>
  <p>I get the extrinsic matrix [<strong>R</strong>|<strong>t</strong>] using the rotation and translation vector above (<code> Rt = np.hstack((cv.Rodrigues(rvec)[0], tvec))</code>).</p>
  <p>I want to apply both conversions for validation that my extrinsic matrix is correct.</p>
  <p><strong>1. World coordinate points to pixel coordinates:</strong></p>
  <pre><code>camera_coordinates = K * M_ext * world_coordinates 
  u = camera_coordinates[0] / camera_coordinates[2]
  v = camera_coordinates[1] / camera_coordinates[2]
  </code></pre>
  <p>This conversion works, my pixel coordinates <strong>u</strong>, <strong>v</strong> are correct.</p>
  <p><strong>2. Pixel coordinates + depth to world coordinate points:</strong></p>
  <pre><code>camera_coordinates = ( inv(K) * [u, v, 1] ) * d
  world_coordinates = inv(M_ext) * [camera_coordinates, 1]
  </code></pre>
  <p>This inverse conversion does not yield my world coordinate points. Does anyone see where my error is?</p>