title: Stereo Vision setup <- Estimate Head Pose Angles
body: <p>I'm looking for a solution to estimate head poses from a stereo camera setup.</p>
  <p>I calibrated my cameras via opencv and estimated all important matrices to estimate the disparity of my tracked face points to get the 3D and 2D coordinates to solve the PnP-Problem via opencv.</p>
  <p>The idea was to track the landamrks on both images Left and Right (I used the unsdistorted images via <code>cv.remap(image_L, stereoMap_L)</code>).
  On this images I estimated the landmarks, see image.</p>
  <p><a href="https://i.stack.imgur.com/kXzeV.jpg" rel="nofollow noreferrer">Landmarks left and right</a></p>
  <p>With the estimated landmarks I estimated the disparity of all landmarks.</p>
  <p>For Disparity I used this function:</p>
  <pre><code>def disparity(points_L, points_R):
     dispar = landmarks_left[:,0] - landmarks_right[:,0]
  return dispar
  </code></pre>
  <p>I followd this paper to estimate all important matrices and points for my setup.
  <a href="https://www.researchgate.net/publication/326167621_Three-Dimensional_Head_Pose_Estimation_Using_a_Stereo_Camera_Arrangement" rel="nofollow noreferrer">Three-Dimensional Head Pose Estimation Using a Stereo Camera Arrangement</a></p>
  <pre><code>face_2d_L = np.array(face_2d_L, dtype=np.float64) #landmarks left 2D
  face_2d_R = np.array(face_2d_R, dtype=np.float64) # landmarks right 2D
  dispar = disparity(face_2d_L[:,0], face_2d_R[:,0])
  
  face_3d_L[:,2] = dispar   
  Q_matrix = np.load('Q_matrix.npy') # estimated via stereo camera calibration   
  face_3d_Left = face_3d_L @ Q_matrix.T
  face_3d_Left = (face_3d_Left[:,0:3].T / face_3d_Left[:,3]).T
  
  cam_matrix_L = np.load('cam_matrix_L.npy')
  cam_matrix_R = np.load('cam_matrix_R.npy')
      
  dist_matrix_L = np.load('distorsion_left.npy')
  dist_matrix_R = np.load('distorsion_right.npy')
      
  success, rot_vec_L, trans_vec_L = cv2.solvePnP(face_3d_Left, face_2d_L, cam_matrix_L, dist_matrix_L)
      
  </code></pre>
  <p>To solve the PnP-Problem I used the 3D estimated points from the landmarks and the disaprity, the 2D landmarks, left cam matrix and the distorsion matrix from the stereo calibration.</p>
  <p>Then i thought I'm more or less done and have all import matrices to estimate the rotation vectors.</p>
  <pre><code>rmat_L, jac_L = cv2.Rodrigues(rot_vec_L)
  angles_L, mtxR_L, mtxQ_L, Qx_L, Qy_L, Qz_L = cv2.RQDecomp3x3(rmat_L)
  </code></pre>
  <p>Unfortunately I think that I made some mistakes or understand something wrong.
  Perhaps you can help to understand this topic a bit better.</p>
  <p>Thank you!</p>