OpenCV Evaluation: 3D Pose of Color Markers Using the StereoCamera System

Question

I have a stereo camera system and correctly calibrate it using both cv::calibrateCamera and cv::stereoCalibrate . My reprojection error looks fine:

• Cam0: 0.401427
• Cam1: 0.388200
• Stereo: 0.399642

I check my calibration by calling cv::stereoRectify and changing my images with cv::initUndistortRectifyMap and cv::remap . The result is shown below (something strange that I noticed, when displaying straightened images, there are usually artifacts in the form of a distorted copy of the original image on one, and sometimes both images):

I also correctly evaluate the position of my markers in pixel coordinates using cv::findContours on the HSV threshold image.

Unfortunately, when I try cv::triangulatePoints , my results are very poor compared to my estimated coordinates, especially in the x direction:

 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) 

These are the results in mm in camera coordinates. Both cameras are located approximately 550 mm from the chessboard, and the square size is 13 mm. Apparently, my results are not even close to the expected ones (negative and huge z-coordinates).

So my questions are:

• I followed the stereo_calib.cpp tag stereo_calib.cpp , and I apparently at least visually got a good result (see repetition error). Why are my triangulation results so poor?
• How can I convert my results to a real coordinate system so that I can really verify my results quantitatively? Do I need to do this manually, as shown here , or are there some OpenCV features in this regard?

---

Here is my code:

 std::vector<std::vector<cv::Point2f> > imagePoints[2]; std::vector<std::vector<cv::Point3f> > objectPoints; imagePoints[0].resize(s->nrFrames); imagePoints[1].resize(s->nrFrames); objectPoints.resize( s->nrFrames ); // [Obtain image points..] // cv::findChessboardCorners, cv::cornerSubPix // Calc obj points for( int i = 0; i < s->nrFrames; i++ ) for( int j = 0; j < s->boardSize.height; j++ ) for( int k = 0; k < s->boardSize.width; k++ ) objectPoints[i].push_back( Point3f( j * s->squareSize, k * s->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<cv::Mat> 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 << "Cam0 reproj err: " << err0 << 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 << "Cam1 reproj err: " << err1 << 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 << "Stereo reproj err: " << err2 << 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 ), &validRoi[0], &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 ); 

Finally, resultPoints should contain the column vectors of my three-dimensional positions in camera coordinates.

Edit: I removed unnecessary conversions to shorten the code

Edit2: the results I get with the proposed @marols triangulation solution

 P1 = { 111, 47, 526 } (bottom-right) P2 = { -2, 2, 577 } (left) P3 = { 64, -46, 616 } (top) 

Answer 1

My answer will focus on proposing another solution for triangulation points. In the case of stereo vision, you can use the Q matrix returned by stereo rectification as follows:

 std::vector<cv::Vec3f> surfacePoints, realSurfacePoints; unsigned int N = centers[0].size(); for(int i=0;i<N;i++) { double d, disparity; // since you have stereo vision system in which cameras lays next to // each other on OX axis, disparity is measured along OX axis d = T.at<double>(0,0); disparity = centers[0][i].x - centers[1][i].x; surfacePoints.push_back(cv::Vec3f(centers[0][i].x, centers[0][i].y, disparity)); } cv::perspectiveTransform(surfacePoints, realSurfacePoints, Q); 

Apply the following code snippet to your code, I could make some mistakes, but you need to create a cv :: Vec3f array, each of which has the following structure: (point.x, point.y, mismatch between points in the second image) and pass it into a promising conversion method (see docs for more details). If you want to learn more about how the Q matrix is ​​created (basically it is a “backward” projection from an image to a point in the real world), see the book “OpenCV Training” on page 435.

In the stereo vision system that I developed, the described method works perfectly and gives the correct results with even larger calibration errors (for example, 1.2).

Answer 2

To project into a real-world coordinate system, you need a projection camera matrix. This can be done like this:

 cv::Mat KR = CalibMatrix * R; cv::Mat eyeC = cv::Mat::eye(3,4,CV_64F); eyeC.at<double>(0,3) = -T.at<double>(0); eyeC.at<double>(1,3) = -T.at<double>(1); eyeC.at<double>(2,3) = -T.at<double>(2); CameraMatrix = cv::Mat(3,4,CV_64F); CameraMatrix.at<double>(0,0) = KR.at<double>(0,0) * eyeC.at<double>(0,0) + KR.at<double>(0,1) * eyeC.at<double>(1,0) + KR.at<double>(0,2) * eyeC.at<double>(2,0); CameraMatrix.at<double>(0,1) = KR.at<double>(0,0) * eyeC.at<double>(0,1) + KR.at<double>(0,1) * eyeC.at<double>(1,1) + KR.at<double>(0,2) * eyeC.at<double>(2,1); CameraMatrix.at<double>(0,2) = KR.at<double>(0,0) * eyeC.at<double>(0,2) + KR.at<double>(0,1) * eyeC.at<double>(1,2) + KR.at<double>(0,2) * eyeC.at<double>(2,2); CameraMatrix.at<double>(0,3) = KR.at<double>(0,0) * eyeC.at<double>(0,3) + KR.at<double>(0,1) * eyeC.at<double>(1,3) + KR.at<double>(0,2) * eyeC.at<double>(2,3); CameraMatrix.at<double>(1,0) = KR.at<double>(1,0) * eyeC.at<double>(0,0) + KR.at<double>(1,1) * eyeC.at<double>(1,0) + KR.at<double>(1,2) * eyeC.at<double>(2,0); CameraMatrix.at<double>(1,1) = KR.at<double>(1,0) * eyeC.at<double>(0,1) + KR.at<double>(1,1) * eyeC.at<double>(1,1) + KR.at<double>(1,2) * eyeC.at<double>(2,1); CameraMatrix.at<double>(1,2) = KR.at<double>(1,0) * eyeC.at<double>(0,2) + KR.at<double>(1,1) * eyeC.at<double>(1,2) + KR.at<double>(1,2) * eyeC.at<double>(2,2); CameraMatrix.at<double>(1,3) = KR.at<double>(1,0) * eyeC.at<double>(0,3) + KR.at<double>(1,1) * eyeC.at<double>(1,3) + KR.at<double>(1,2) * eyeC.at<double>(2,3); CameraMatrix.at<double>(2,0) = KR.at<double>(2,0) * eyeC.at<double>(0,0) + KR.at<double>(2,1) * eyeC.at<double>(1,0) + KR.at<double>(2,2) * eyeC.at<double>(2,0); CameraMatrix.at<double>(2,1) = KR.at<double>(2,0) * eyeC.at<double>(0,1) + KR.at<double>(2,1) * eyeC.at<double>(1,1) + KR.at<double>(2,2) * eyeC.at<double>(2,1); CameraMatrix.at<double>(2,2) = KR.at<double>(2,0) * eyeC.at<double>(0,2) + KR.at<double>(2,1) * eyeC.at<double>(1,2) + KR.at<double>(2,2) * eyeC.at<double>(2,2); CameraMatrix.at<double>(2,3) = KR.at<double>(2,0) * eyeC.at<double>(0,3) + KR.at<double>(2,1) * eyeC.at<double>(1,3) + KR.at<double>(2,2) * eyeC.at<double>(2,3);