title: Camera pose from solvePnP
body: <h1>Goal</h1>
  <p>I need to retrieve the position and attitude angles of a camera (using OpenCV / Python).</p>
  <h1>Definitions</h1>
  <h2>Attitude angles are defined by:</h2>
  <p><strong>Yaw</strong> being the general orientation of the camera when it lays on an horizontal plane: toward north=0, toward east = 90°, south=180°, west=270°, etc.</p>
  <p><strong>Pitch</strong> being the &quot;nose&quot; orientation of the camera: 0° = looking horizontally at a point on the horizon, -90° = looking down vertically, +90° = looking up vertically, 45° = looking up at an angle of 45° from the horizon, etc.</p>
  <p><strong>Roll</strong> being if the camera is tilted left or right when in your hands (so it is always looking at a point on the horizon when this angle is varying): +45° = tilted 45° in a clockwise rotation when you grab the camera, thus +90° (and -90°) would be the angle needed for a portrait picture for example, etc.</p>
  <h2>World reference frame:</h2>
  <p>My world reference frame is oriented so:</p>
  <blockquote>
  <p><code>Toward east       = +X</code><br />
  <code>Toward north      = +Y</code><br />
  <code>Up toward the sky = +Z</code></p>
  </blockquote>
  <p>My world objects points are given in that reference frame.</p>
  <h2>Camera reference frame:</h2>
  <p>According to the doc, the camera reference frame is oriented like that:
  <a href="https://i.stack.imgur.com/LDuic.png" rel="noreferrer"><img src="https://i.stack.imgur.com/LDuic.png" alt="camera reference frame" /></a></p>
  <h1>What to achieve</h1>
  <p>Now, from <a href="http://docs.opencv.org/trunk/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d" rel="noreferrer"><code>cv2.solvepnp()</code></a> over a bunch of images points and their corresponding world coordinates, I have computed both <code>rvec</code> and <code>tvec</code>.<br />
  But, according to the doc: <a href="http://docs.opencv.org/trunk/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d" rel="noreferrer">http://docs.opencv.org/trunk/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d</a> , they are:</p>
  <blockquote>
  <p><em><strong>rvec</strong></em> ; Output rotation vector (see <a href="https://docs.opencv.org/trunk/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac" rel="noreferrer"><code>Rodrigues()</code></a>) that, together with <code>tvec</code>, brings points from the model coordinate system to the camera coordinate system.<br />
  <em><strong>tvec</strong></em> ; Output translation vector.</p>
  </blockquote>
  <p>these vectors are given to go <strong>to</strong> the camera reference frame.<br />
  I need to do the exact inverse operation, thus retrieving camera position and attitude relative to world coordinates.</p>
  <h2>Camera position:</h2>
  <p>So I have computed the rotation matrix from <code>rvec</code> with <code>Rodrigues()</code>:</p>
  <pre><code>rmat = cv2.Rodrigues(rvec)[0]
  </code></pre>
  <p>And if I'm right here, the camera position expressed in the world coordinates system is given by:</p>
  <pre><code>camera_position = -np.matrix(rmat).T * np.matrix(tvec)
  </code></pre>
  <p>(src: <a href="https://stackoverflow.com/questions/18637494/camera-position-in-world-coordinate-from-cvsolvepnp">Camera position in world coordinate from cv::solvePnP</a> )<br />
  This looks fairly well.</p>
  <br>
  <h2>Camera attitude (yaw, pitch and roll):</h2>
  <p>But how to retrieve corresponding attitude angles (yaw, pitch and roll as describe above) from the point of view of the camera (as if it was in your hands basically)?</p>
  <p>I have tried implementing this: <a href="http://planning.cs.uiuc.edu/node102.html#eqn:yprmat" rel="noreferrer">http://planning.cs.uiuc.edu/node102.html#eqn:yprmat</a> in a function:</p>
  <pre><code>def rotation_matrix_to_attitude_angles(R):
      import math
      import numpy as np 
      cos_beta = math.sqrt(R[2,1] * R[2,1] + R[2,2] * R[2,2])
      validity = cos_beta &lt; 1e-6
      if not validity:
          alpha = math.atan2(R[1,0], R[0,0])    # yaw   [z]
          beta  = math.atan2(-R[2,0], cos_beta) # pitch [y]
          gamma = math.atan2(R[2,1], R[2,2])    # roll  [x]
      else:
          alpha = math.atan2(R[1,0], R[0,0])    # yaw   [z]
          beta  = math.atan2(-R[2,0], cos_beta) # pitch [y]
          gamma = 0                             # roll  [x]  
      return np.array([alpha, beta, gamma])    
  </code></pre>
  <p>But results are not consistent with what I want. For example, I have a roll angle of ~ -90°, but the camera is horizontal so it should be around 0.</p>
  <p>Pitch angle is around 0 so it seems correctly determined but I don't really understand why it's around 0 as the Z-axis of the camera reference frame is horizontal, so it's has already been tilted from 90° from the vertical axis of the world reference frame. I would have expected a value of -90° or +270° here. Anyway.</p>
  <p>And yaw seems good. Mainly.</p>
  <h1>Question</h1>
  <p>Did I miss something with the roll angle?</p>