title: OpenCV: 3D Pose estimation of color markers using StereoCamera system
body: <p>I have a stereo camera system and correctly calibrate it using both, <code>cv::calibrateCamera</code> and <code>cv::stereoCalibrate</code>. My <code>reprojection error</code> seems to be okay: </p>
  
  <ul>
  <li>Cam0: 0.401427  </li>
  <li>Cam1: 0.388200  </li>
  <li>Stereo: 0.399642</li>
  </ul>
  
  <p><img src="https://i.stack.imgur.com/gz9j8.jpg" alt="A snapshot of the ongoing calibration process"></p>
  
  <p>I check my calibration by calling <code>cv::stereoRectify</code> and transforming my images using <code>cv::initUndistortRectifyMap</code> and <code>cv::remap</code>. The result is shown below (Something strange I noticed is when displaying the rectified images there are usually artifacts in form of a deformed copy of the original image on one or sometimes even both images):</p>
  
  <p><img src="https://i.stack.imgur.com/voV0T.jpg" alt="Rectification"></p>
  
  <p>I also correctly estimate the position of my markers in pixel coordinates using <code>cv::findContours</code> on a thresholded HSV image.</p>
  
  <p><img src="https://i.stack.imgur.com/H1ckW.jpg" alt="enter image description here"></p>
  
  <p>Unfortunately, when I now try to <code>cv::triangulatePoints</code> my results are very poor compared to my estimated coordinates, especially in x-direction:</p>
  
  <pre><code>P1 = {   58 (±1),  150 (±1), -90xx (±2xxx)  } (bottom)
  P2 = {  115 (±1),  -20 (±1), -90xx (±2xxx)  } (right)
  P3 = { 1155 (±6),  575 (±3), 60xxx (±20xxx) } (top-left)
  </code></pre>
  
  <p>Those are the results in mm in camera coordinates. Both cameras are positioned approximately 550 mm away from the checkerboard and the square size is 13 mm. Apparently, my results are not even close to what I expect (negative and huge z-coordinates).</p>
  
  <p>So my questions are:</p>
  
  <ol>
  <li>I followed the <code>stereo_calib.cpp</code> sample quite closely and I seem to at least visually obtain good result (see reprojection error). Why are my triangulation results so poor?</li>
  <li>How can I transform my results to the real-world coordinate system so I can actually check my results quantitatively? Do I have to do it manually as shown <a href="https://stackoverflow.com/questions/12299870/computing-x-y-coordinate-3d-from-image-point">over here</a>, or is there some OpenCV functions for that matter?</li>
  </ol>
  
  <hr>
  
  <p>Here is my code:</p>
  
  <pre><code>std::vector&lt;std::vector&lt;cv::Point2f&gt; &gt; imagePoints[2];
  std::vector&lt;std::vector&lt;cv::Point3f&gt; &gt; objectPoints;
  
  imagePoints[0].resize(s-&gt;nrFrames);
  imagePoints[1].resize(s-&gt;nrFrames);
  objectPoints.resize( s-&gt;nrFrames );
  
  // [Obtain image points..]
  // cv::findChessboardCorners, cv::cornerSubPix
  
  // Calc obj points
  for( int i = 0; i &lt; s-&gt;nrFrames; i++ )
      for( int j = 0; j &lt; s-&gt;boardSize.height; j++ )
          for( int k = 0; k &lt; s-&gt;boardSize.width; k++ )
              objectPoints[i].push_back( Point3f( j * s-&gt;squareSize, k * s-&gt;squareSize, 0 ) );
  
  
  // Mono calibration
  cv::Mat cameraMatrix[2], distCoeffs[2];
  cameraMatrix[0] = cv::Mat::eye(3, 3, CV_64F);
  cameraMatrix[1] = cv::Mat::eye(3, 3, CV_64F);
  
  std::vector&lt;cv::Mat&gt; tmp0, tmp1;
  
  double err0 = cv::calibrateCamera( objectPoints, imagePoints[0], cv::Size( 656, 492 ),
      cameraMatrix[0], distCoeffs[0], tmp0, tmp1,    
      CV_CALIB_FIX_PRINCIPAL_POINT + CV_CALIB_FIX_ASPECT_RATIO );
  std::cout &lt;&lt; "Cam0 reproj err: " &lt;&lt; err0 &lt;&lt; std::endl;
  
  double err1 = cv::calibrateCamera( objectPoints, imagePoints[1], cv::Size( 656, 492 ),
      cameraMatrix[1], distCoeffs[1], tmp0, tmp1,
      CV_CALIB_FIX_PRINCIPAL_POINT + CV_CALIB_FIX_ASPECT_RATIO );
  std::cout &lt;&lt; "Cam1 reproj err: " &lt;&lt; err1 &lt;&lt; std::endl;
  
  // Stereo calibration
  cv::Mat R, T, E, F;
  
  double err2 = cv::stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
      cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1],
      cv::Size( 656, 492 ), R, T, E, F,
      cv::TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
      CV_CALIB_USE_INTRINSIC_GUESS + // because of mono calibration
      CV_CALIB_SAME_FOCAL_LENGTH +
      CV_CALIB_RATIONAL_MODEL +
      CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5);
  std::cout &lt;&lt; "Stereo reproj err: " &lt;&lt; err2 &lt;&lt; std::endl;
  
  // StereoRectify
  cv::Mat R0, R1, P0, P1, Q;
  Rect validRoi[2];
  cv::stereoRectify( cameraMatrix[0], distCoeffs[0],
              cameraMatrix[1], distCoeffs[1],
              cv::Size( 656, 492 ), R, T, R0, R1, P0, P1, Q,
              CALIB_ZERO_DISPARITY, 1, cv::Size( 656, 492 ), &amp;validRoi[0], &amp;validRoi[1]);
  
  
  // [Track marker..]
  // cv::cvtColor, cv::inRange, cv::morphologyEx, cv::findContours, cv::fitEllipse, *calc ellipsoid centers*
  
  // Triangulation
  unsigned int N = centers[0].size();
  
  
  cv::Mat pnts3D;
  cv::triangulatePoints( P0, P1, centers[0], centers[1], pnts3D );
  
  
  cv::Mat t = pnts3D.t();
  cv::Mat pnts3DT = cv::Mat( N, 1, CV_32FC4, t.data );
  
  cv::Mat resultPoints; 
  cv::convertPointsFromHomogeneous( pnts3DT, resultPoints );
  </code></pre>
  
  <p>Finally, <code>resultPoints</code> is supposed to contain column vectors of my 3D positions in camera coordinates.</p>
  
  <p><em>Edit: I removed some unnecessary conversions to shorten the code</em></p>
  
  <p><em>Edit2: The results I get using @marols suggested solution for triangulation</em></p>
  
  <p><img src="https://i.stack.imgur.com/Iz9AS.jpg" alt="Qualitative and quantitative triangulation results"></p>
  
  <pre><code>P1 = { 111,  47, 526 } (bottom-right)
  P2 = {  -2,   2, 577 } (left)
  P3 = {  64, -46, 616 } (top)
  </code></pre>