title: Camera pose estimation using opencv C++, SolvePnP function
body: <p>I am trying to measure the pose of a camera and I have done the following.</p>
  
  <ol>
  <li>Mark world 3-D(Assuming z=0, since it is flat) points on corners of a square on a flat surface and assume a world coordinate system.(in cms)</li>
  </ol>
  
  <p>Have taken the top left corner of the square as my origin and given the world points in the following order(x,y)or(col,row):
  (0,0),(12.8,0),(12.8,12.8),(0,12.8)   - in cms</p>
  
  <ol start="2">
  <li><p>Detect those points in my image.(in pixels)
  The image points and world points are in the same order.</p></li>
  <li><p>I have calibrated my camera for <strong>intrinsic matrix</strong> and <strong>distortion coefficients</strong>.</p></li>
  <li><p>I use <strong>SolvePnP</strong> function to get rvec and tvec.</p></li>
  <li><p>I use <strong>Rodrigues</strong> function to get rotation matrix.</p></li>
  <li><p>To check if rvec and tvec is correct, I project back the 3-D points(z=0) using <strong>ProjectPoints</strong> into the image plane and I get the points correctly on my image with an error of 3 pixels on X- axis.</p></li>
  <li><p>Now I go ahead and calculate my camera position in the world frame using the formula:</p></li>
  </ol>
  
  <p><strong>cam_worl_pos = - inverse(R) * tvec</strong>. (This formula I have verified in many blogs and also this makes sense) </p>
  
  <ol start="8">
  <li>But my <strong>cam_worl_pos</strong> x,y, and z in cms do not seem to be correct.</li>
  </ol>
  
  <p>My doubt is, if I am able to project back the 3-D world point to image plane using rvec and tvec with (<strong>3 pixel error on X-axis and almost no error on Y axis, hope it is not too bad</strong>), then why am I not getting the camera position in world frame right.</p>
  
  <p>Also, I have a doubt on SolvPnP rvec and tvec solution, they might be one of the multiple solutions, but not the one which I want.</p>
  
  <p>How do I get the right rvec and tvec from SolvPnp or any other suggestions to get rvec and tvec would also be helpful.</p>
  
  <p><strong>EDITS</strong> : </p>
  
  <p><strong>Image Size</strong> - 720(row) * 1280(col)</p>
  
  <p><strong>camera parameters</strong></p>
  
  <pre><code>cameraMatrix_Front=[908.65   0     642.88
                       0     909.28   364.95
                       0        0        1]
  
  distCoeffs_Front=[-0.4589, 0.09462, -1.46*10^-3, 1.23*10^-3]
  </code></pre>
  
  <p><strong>OpenCV C++ code</strong>:</p>
  
  <pre><code>vector&lt;Point3f&gt; front_object_pts;
  Mat rvec_front;
  Mat tvec_front;
  Mat rotation_front;
  Mat world_position_front_cam;
  
  
  //Fill front object points(x-y-z order in cms)
  //It is square of side 12.8cms on Z=0 plane
  front_object_pts.push_back(Point3f(0, 0, 0));
  front_object_pts.push_back(Point3f(12.8, 0, 0));
  front_object_pts.push_back(Point3f(12.8,12.8,0));
  front_object_pts.push_back(Point3f(0, 12.8, 0));
  
  
  //Corresponding Image points detected in the same order as object points
  front_image_pts.push_back(points_front[0]);
  front_image_pts.push_back(points_front[1]);
  front_image_pts.push_back(points_front[2]);
  front_image_pts.push_back(points_front[3]);
  
  //Detected points in image matching the 3-D points in the same order
  //(467,368)
  //(512,369)
  //(456,417)
  //(391,416)
  
  //Get rvec and tvec using Solve PnP
  solvePnP(front_object_pts, front_image_pts, cameraMatrix_Front,
           Mat(4,1,CV_64FC1,Scalar(0)), rvec_front, tvec_front, false, CV_ITERATIVE);
  
  //Output of SolvePnP
  //tvec=[-26.951,0.6041,134.72]  (3 x 1 matrix)
  //rvec=[-1.0053,0.6691,0.3752]  (3 x 1 matrix)
  
  
  //Check rvec and tvec is correct or not by projecting the 3-D object points to image
  vector&lt;Point2f&gt;check_front_image_pts
  projectPoints(front_object_pts, rvec_front, tvec_front, 
               cameraMatrix_Front, distCoeffs_Front, check_front_image_pts);
  
  
  //Here to note that I have made **distCoefficents**, 
  //a 0 vector since my   image points are detected after radial distortion is removed
  
  //Get rotation matrix
  Rodrigues(rvec_front, rotation_front);
  
  //Get rotation matrix inverse
  Mat rotation_inverse;
  transpose(rotation_front, rotation_inverse);
  
  //Get camera position in world cordinates
  world_position_front_cam = -rotation_inverse * tvec_front;
  </code></pre>
  
  <blockquote>
    <p>//Actual location of camera(Measured manually)</p>
    
    <p>X=47cm</p>
    
    <p>Y=18cm</p>
    
    <p>Z=25cm</p>
    
    <p>//Obtained location</p>
    
    <p>X=110cm</p>
    
    <p>Y=71cm</p>
    
    <p>Z=-40cm</p>
  </blockquote>