title: Calculate camera world position with OpenCV Python
body: <p>I want to calculate my camera's position in world coordinates. This should be fairly easy, but I don't get the results I expect. I believe I've read everything on this topic, but my code isn't working. Here's what I do:</p>
  
  <p>I have a camera looking at an area.</p>
  
  <p>1) I drew a map of the area.</p>
  
  <p>2) I calculated the homography by matching 4 image points to 4 points on my map using <code>cv2.getPerspectiveTransform</code></p>
  
  <p>3) The H homography transforms every world coordinate to camera coordinate; this is working properly</p>
  
  <p>4) To calculate the camera matrix I followed <a href="https://dsp.stackexchange.com/questions/2736/step-by-step-camera-pose-estimation-for-visual-tracking-and-planar-markers">this</a>:</p>
  
  <pre><code>translation = np.zeros((3,1)) 
  translation[:,0] = homography[:,2]
  
  rotation = np.zeros((3,3))
  rotation[:,0] = homography[:,0]
  rotation[:,1] = homography[:,1]
  rotation[:,2] = np.cross(homography[0:3,0],homography[0:3,1])
  
  cameraMatrix = np.zeros((3,4))
  cameraMatrix[:,0:3] = rotation
  cameraMatrix[:,3] = homography[:,2]
  
  cameraMatrix = cameraMatrix/cameraMatrix[2][3] #normalize the matrix
  </code></pre>
  
  <p>5) According to <a href="http://en.wikipedia.org/wiki/Camera_resectioning" rel="noreferrer">this</a>, the camera's position should be calculated like this:</p>
  
  <pre><code>x,y,z = np.dot(-np.transpose(rotation),translation)
  </code></pre>
  
  <p>The coordinates I'm getting are totally wrong. The problem should be somewhere in step 4 or 5 I guess. What's wrong with my method?</p>