title: RGB-D pose estimation with OpenCv and python
body: <p>I'm currently trying to solve the RGBD SLAM problem, but am experiencing some issues estimating poses via RANSAC. I have correctly transformed the points from 2d to 3d via:</p>
  
  <pre><code>def transform3d(x, y, depth):
      Z = depth[x][y] / scalingFactor
      X = (x - centerX) * Z / focalX
      Y = (y - centerY) * Z / focalY
      return (X,Y,Z)
  
  def transform(matches, depth1, depth2, kp1, kp2):
      points_3d, points_2d = [], []
      temp = np.zeros((1, 2))
      for mat in matches:
          img1_idx = mat.queryIdx
          img2_idx = mat.trainIdx
          (y1, x1) = kp1[img1_idx].pt
          (y2, x2) = kp2[img2_idx].pt
          if depth[x1][y1] == 0:
              continue
          points_2d.append(kp2[img2_idx].pt)
          points_3d.append(np.array(transform3d(x1, y1, depth)))
  
      return (np.array(points_3d, np.float32), np.array(points_2d, np.float32))
  </code></pre>
  
  <p>afterwards I call the calibrateCamera function to retrieve the distortion param</p>
  
  <pre><code>mtx = np.array([[focalX, 0, centerX], [0, focalY, centerY], [0, 0, 1]], np.float32)
  
  cv2.calibrateCamera(np.array([points_3d]), np.array([points_2d]), rgb1.shape[::-1], None, None, flags=1)
  </code></pre>
  
  <p>and did RANSAC, to obtain the rotation and translation matrix:</p>
  
  <pre><code>cv2.solvePnPRansac(np.array([points_3d]), np.array([points_2d]), mtx, dist)
  </code></pre>
  
  <p>For the above I went through OpenCVs tutorial for estimating poses.</p>
  
  <p>I have also followed this article <a href="http://ksimek.github.io/2012/08/22/extrinsic/" rel="nofollow">http://ksimek.github.io/2012/08/22/extrinsic/</a> and tried to express the pose 
  by doing</p>
  
  <pre><code>R = cv2.Rodrigues(rvecs)[0].T
  pose = -R*tvecs
  </code></pre>
  
  <p>my poses are definitely wrong! yet I have no idea where the issue lies.</p>
  
  <p>I have also cross-checked my code with this C++ implementation of RGBD SLAM <a href="http://www.cnblogs.com/gaoxiang12/p/4659805.html" rel="nofollow">http://www.cnblogs.com/gaoxiang12/p/4659805.html</a></p>
  
  <p>Please help! I really want to get my robot moving :)</p>