title: Determine camera pose?
body: <p>I am trying to determine camera pose based on fiducial marker found in a scene.</p>
  
  <p>Fiducial: <a href="http://tinypic.com/view.php?pic=4r6k3q&amp;s=8#.VNLnWTVVK1E" rel="nofollow">http://tinypic.com/view.php?pic=4r6k3q&amp;s=8#.VNLnWTVVK1E</a></p>
  
  <p><strong>Current process:</strong></p>
  
  <ol>
  <li>Use SIFT for feature detection</li>
  <li>Use SIFT for descriptor extraction</li>
  <li>Use FLANN for matching</li>
  <li>Find the homography using CV_RANSAC</li>
  <li>Identify the corners of the fiducial</li>
  <li>Identify corners of the fiducial in the scene using perspectiveTransform()</li>
  <li>Draw lines around corners (i.e. prove that it found the fiducial in the scene</li>
  <li>Run camera calibration</li>
  <li>Load calibration results (cameraMatrix &amp; distortionCoefficients)</li>
  </ol>
  
  <p>Now I am trying to figure out the camera pose.
  I attempted to use: </p>
  
  <p>void solvePnP(const Mat&amp; objectPoints, const Mat&amp; 
      imagePoints, const Mat&amp; cameraMatrix, const Mat&amp; distCoeffs,
      Mat&amp; rvec, Mat&amp; tvec, bool useExtrinsicGuess=false)</p>
  
  <p>where:</p>
  
  <ul>
  <li>obectPoints are the fiducial corners</li>
  <li>imagePoints are the fiducial corners in the scene</li>
  <li>cameraMatrix is from calibration</li>
  <li>distCoeffs is from calibration</li>
  <li>rvec and tvec should be returned to me from this function</li>
  </ul>
  
  <p>However, when I run this, I get a core dump error, so I am not sure what I am doing incorrectly.</p>
  
  <p>I haven't found very good documentation on solvePNP() - did I misunderstand the function or input parameters?</p>
  
  <p>Appreciate your help</p>
  
  <p><strong><em>Update</em></strong>
  Here is my process:</p>
  
  <pre><code>OrbFeatureDetector detector; //Orb seems more accurate than SIFT
  vector&lt;KeyPoint&gt; keypoints1, keypoints2; 
  
  detector.detect(marker_im, keypoints1);
  detector.detect(scene_im, keypoints2);
  
  Mat display_marker_im, display_scene_im;
  drawKeypoints(marker_im, keypoints1, display_marker_im, Scalar(0,0,255));
  drawKeypoints(scene_im, keypoints2, display_scene_im, Scalar(0,0,255));
  
  SiftDescriptorExtractor extractor;
  Mat descriptors1, descriptors2;
  
  extractor.compute( marker_im, keypoints1, descriptors1 );
  extractor.compute( scene_im, keypoints2, descriptors2 );
  
  BFMatcher matcher; //BF seems to match better than FLANN
  vector&lt; DMatch &gt; matches;
  matcher.match( descriptors1, descriptors2, matches );
  
  Mat img_matches;
  drawMatches( marker_im, keypoints1, scene_im, keypoints2,
      matches, img_matches, Scalar::all(-1), Scalar::all(-1),
      vector&lt;char&gt;(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
  
  vector&lt;Point2f&gt; obj, scene;
  for (int i = 0; i &lt; matches.size(); i++) {
      obj.push_back(keypoints1[matches[i].queryIdx].pt);
      scene.push_back(keypoints2[matches[i].trainIdx].pt);
  }
  
  Mat H;
  H = findHomography(obj, scene, CV_RANSAC);
  
  //Get corners of fiducial
  vector&lt;Point2f&gt; obj_corners(4);
  obj_corners[0] = cvPoint(0,0);
  obj_corners[1] = cvPoint(marker_im.cols, 0);
  obj_corners[2] = cvPoint(marker_im.cols, marker_im.rows);
  obj_corners[3] = cvPoint(0, marker_im.rows);
  vector&lt;Point2f&gt; scene_corners(4);
  
  perspectiveTransform(obj_corners, scene_corners, H);
  
  FileStorage fs2("cal.xml", FileStorage::READ);
  
  Mat cameraMatrix, distCoeffs;
  fs2["Camera_Matrix"] &gt;&gt; cameraMatrix;
  fs2["Distortion_Coefficients"] &gt;&gt; distCoeffs;
  
  Mat rvec, tvec;
  
  //same points as object_corners, just adding z-axis (0)
  vector&lt;Point3f&gt; objp(4);
  objp[0] = cvPoint3D32f(0,0,0);
  objp[1] = cvPoint3D32f(gray.cols, 0, 0);
  objp[2] = cvPoint3D32f(gray.cols, gray.rows, 0);
  objp[3] = cvPoint3D32f(0, gray.rows, 0);
  
  solvePnPRansac(objp, scene_corners, cameraMatrix, distCoeffs, rvec, tvec );
  
  Mat rotation, viewMatrix(4, 4, CV_64F);
  Rodrigues(rvec, rotation);
  
  for(int row=0; row&lt;3; ++row)
  {
     for(int col=0; col&lt;3; ++col)
     {
        viewMatrix.at&lt;double&gt;(row, col) = rotation.at&lt;double&gt;(row, col);
     }
     viewMatrix.at&lt;double&gt;(row, 3) = tvec.at&lt;double&gt;(row, 0);
  }
  
  viewMatrix.at&lt;double&gt;(3, 3) = 1.0f;
  
  cout &lt;&lt; "rotation: " &lt;&lt; rotation &lt;&lt; endl;
  cout &lt;&lt; "viewMatrix: " &lt;&lt; viewMatrix &lt;&lt; endl;
  </code></pre>