diff --git a/modules/calib/include/opencv2/calib.hpp b/modules/calib/include/opencv2/calib.hpp index 08c72150803037dcc3773a1472f2018c78b4f19d..d9a9055416a126c17c2d1c137e4a9ad2e1d6dbc9 100644 --- a/modules/calib/include/opencv2/calib.hpp +++ b/modules/calib/include/opencv2/calib.hpp @@ -994,6 +994,15 @@ second camera coordinate system. @param T Output translation vector, see description above. @param E Output essential matrix. @param F Output fundamental matrix. +@param rvecs Output vector of rotation vectors ( @ref Rodrigues ) estimated for each pattern view in the +coordinate system of the first camera of the stereo pair (e.g. std::vector). More in detail, each +i-th rotation vector together with the corresponding i-th translation vector (see the next output parameter +description) brings the calibration pattern from the object coordinate space (in which object points are +specified) to the camera coordinate space of the first camera of the stereo pair. In more technical terms, +the tuple of the i-th rotation and translation vector performs a change of basis from object coordinate space +to camera coordinate space of the first camera of the stereo pair. +@param tvecs Output vector of translation vectors estimated for each pattern view, see parameter description +of previous output parameter ( rvecs ). @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view. @param flags Different flags that may be zero or a combination of the following values: - @ref CALIB_FIX_INTRINSIC Fix cameraMatrix? and distCoeffs? so that only R, T, E, and F @@ -1091,6 +1100,7 @@ CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate( InputArrayOfArray InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, Size imageSize, InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) ); @@ -1103,6 +1113,14 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, int flags = CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) ); +/// @overload +CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, + InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, + InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, + Size imageSize, InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F, + OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); /** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$ @@ -1635,6 +1653,15 @@ similar to D1 . @param imageSize Size of the image used only to initialize camera intrinsic matrix. @param R Output rotation matrix between the 1st and the 2nd camera coordinate systems. @param T Output translation vector between the coordinate systems of the cameras. +@param rvecs Output vector of rotation vectors ( @ref Rodrigues ) estimated for each pattern view in the +coordinate system of the first camera of the stereo pair (e.g. std::vector). More in detail, each +i-th rotation vector together with the corresponding i-th translation vector (see the next output parameter +description) brings the calibration pattern from the object coordinate space (in which object points are +specified) to the camera coordinate space of the first camera of the stereo pair. In more technical terms, +the tuple of the i-th rotation and translation vector performs a change of basis from object coordinate space +to camera coordinate space of the first camera of the stereo pair. +@param tvecs Output vector of translation vectors estimated for each pattern view, see parameter description +of previous output parameter ( rvecs ). @param flags Different flags that may be zero or a combination of the following values: - @ref fisheye::CALIB_FIX_INTRINSIC Fix K1, K2? and D1, D2? so that only R, T matrices are estimated. @@ -1650,9 +1677,15 @@ zero. @param criteria Termination criteria for the iterative optimization algorithm. */ CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, - InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, - OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC, - TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); + InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, + OutputArray R, OutputArray T, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = fisheye::CALIB_FIX_INTRINSIC, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); + +/// @overload +CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, + OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); //! @} calib3d_fisheye } //end namespace fisheye diff --git a/modules/calib/src/calibration.cpp b/modules/calib/src/calibration.cpp index 930dd4e6f2cdf3adbcfcc61557019a8a4d01f907..e0c09c31be103a379765308da128c4e186bd2804 100644 --- a/modules/calib/src/calibration.cpp +++ b/modules/calib/src/calibration.cpp @@ -563,7 +563,8 @@ static double calibrateCameraInternal( const Mat& objectPoints, //std::cout << "single camera calib. param after LM: " << param0.t() << "\n"; - // If solver failed, then the last calculated perViewErr can be wrong & should be recalculated + // If solver failed or last LM iteration was not successful, + // then the last calculated perViewErr can be wrong & should be recalculated Mat JtErr, JtJ, JtJinv, JtJN; double reprojErr = 0; if (!stdDevs.empty()) @@ -651,13 +652,14 @@ static double stereoCalibrateImpl( Mat& _cameraMatrix2, Mat& _distCoeffs2, Size imageSize, Mat matR, Mat matT, Mat matE, Mat matF, + Mat rvecs, Mat tvecs, Mat perViewErr, int flags, TermCriteria termCrit ) { int NINTRINSIC = CALIB_NINTRINSIC; - double dk[2][14]={{0}}; - Mat Dist[2]; + // initial camera intrinsicss + Vec distInitial[2]; Matx33d A[2]; int pointsTotal = 0, maxPoints = 0, nparams; bool recomputeIntrinsics = false; @@ -667,7 +669,7 @@ static double stereoCalibrateImpl( _imagePoints1.depth() == _objectPoints.depth() ); CV_Assert( (_npoints.cols == 1 || _npoints.rows == 1) && - _npoints.type() == CV_32S ); + _npoints.type() == CV_32S ); int nimages = (int)_npoints.total(); for(int i = 0; i < nimages; i++ ) @@ -682,6 +684,27 @@ static double stereoCalibrateImpl( _objectPoints.convertTo(objectPoints, CV_64F); objectPoints = objectPoints.reshape(3, 1); + if( !rvecs.empty() ) + { + int cn = rvecs.channels(); + int depth = rvecs.depth(); + if( (depth != CV_32F && depth != CV_64F) || + ((rvecs.rows != nimages || (rvecs.cols*cn != 3 && rvecs.cols*cn != 9)) && + (rvecs.rows != 1 || rvecs.cols != nimages || cn != 3)) ) + CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel " + "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" ); + } + if( !tvecs.empty() ) + { + int cn = tvecs.channels(); + int depth = tvecs.depth(); + if( (depth != CV_32F && depth != CV_64F) || + ((tvecs.rows != nimages || tvecs.cols*cn != 3) && + (tvecs.rows != 1 || tvecs.cols != nimages || cn != 3)) ) + CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel " + "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" ); + } + for(int k = 0; k < 2; k++ ) { const Mat& points = k == 0 ? _imagePoints1 : _imagePoints2; @@ -694,29 +717,29 @@ static double stereoCalibrateImpl( ((points.rows == pointsTotal && points.cols*cn == 2) || (points.rows == 1 && points.cols == pointsTotal && cn == 2))); - A[k] = Matx33d(1, 0, 0, 0, 1, 0, 0, 0, 1); - Dist[k] = Mat(1,14,CV_64F,dk[k]); + A[k] = Matx33d::eye(); points.convertTo(imagePoints[k], CV_64F); imagePoints[k] = imagePoints[k].reshape(2, 1); - if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS| - CALIB_FIX_ASPECT_RATIO|CALIB_FIX_FOCAL_LENGTH) ) + if( flags & ( CALIB_FIX_INTRINSIC | CALIB_USE_INTRINSIC_GUESS | + CALIB_FIX_ASPECT_RATIO | CALIB_FIX_FOCAL_LENGTH ) ) cameraMatrix.convertTo(A[k], CV_64F); - if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS| - CALIB_FIX_K1|CALIB_FIX_K2|CALIB_FIX_K3|CALIB_FIX_K4|CALIB_FIX_K5|CALIB_FIX_K6| - CALIB_FIX_TANGENT_DIST) ) + if( flags & ( CALIB_FIX_INTRINSIC | CALIB_USE_INTRINSIC_GUESS | + CALIB_FIX_K1 | CALIB_FIX_K2 | CALIB_FIX_K3 | CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6 | + CALIB_FIX_TANGENT_DIST) ) { - Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), dk[k] ); + Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), distInitial[k].val); distCoeffs.convertTo(tdist, CV_64F); } - if( !(flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS))) + if( !(flags & (CALIB_FIX_INTRINSIC | CALIB_USE_INTRINSIC_GUESS))) { - Mat matA(A[k], false); + Mat mIntr(A[k], /* copyData = */ false); + Mat mDist(distInitial[k], /* copyData = */ false); calibrateCameraInternal(objectPoints, imagePoints[k], - _npoints, imageSize, 0, matA, Dist[k], + _npoints, imageSize, 0, mIntr, mDist, Mat(), Mat(), Mat(), Mat(), Mat(), flags, termCrit); } } @@ -732,7 +755,7 @@ static double stereoCalibrateImpl( if( flags & CALIB_FIX_ASPECT_RATIO ) { for(int k = 0; k < 2; k++ ) - aspectRatio[k] = A[k](0, 0)/A[k](1, 1); + aspectRatio[k] = A[k](0, 0) / A[k](1, 1); } recomputeIntrinsics = (flags & CALIB_FIX_INTRINSIC) == 0; @@ -817,7 +840,7 @@ static double stereoCalibrateImpl( for(int k = 0; k < 2; k++ ) { Mat imgpt_ik = imagePoints[k].colRange(pos, pos + ni); - solvePnP(objpt_i, imgpt_ik, A[k], Dist[k], rv, T[k], false, SOLVEPNP_ITERATIVE ); + solvePnP(objpt_i, imgpt_ik, A[k], distInitial[k], rv, T[k], false, SOLVEPNP_ITERATIVE ); Rodrigues(rv, R[k]); if( k == 0 ) @@ -883,11 +906,11 @@ static double stereoCalibrateImpl( { size_t idx = (nimages+1)*6 + k*NINTRINSIC; if( flags & CALIB_ZERO_TANGENT_DIST ) - dk[k][2] = dk[k][3] = 0; + distInitial[k][2] = distInitial[k][3] = 0; param[idx + 0] = A[k](0, 0); param[idx + 1] = A[k](1, 1); param[idx + 2] = A[k](0, 2); param[idx + 3] = A[k](1, 2); for (int i = 0; i < 14; i++) { - param[idx + 4 + i] = dk[k][i]; + param[idx + 4 + i] = distInitial[k][i]; } } } @@ -942,7 +965,7 @@ static double stereoCalibrateImpl( { intrin[k] = A[k]; for(int j = 0; j < 14; j++) - distCoeffs[k][j] = dk[k][j]; + distCoeffs[k][j] = distInitial[k][j]; } } @@ -1105,7 +1128,12 @@ static double stereoCalibrateImpl( Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; Mat& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; A[k].convertTo(cameraMatrix, cameraMatrix.depth()); - Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F,distCoeffs.channels()), Dist[k].data ); + + std::vector vdist(14); + for(int j = 0; j < 14; j++) + vdist[j] = param[idx + 4 + j]; + + Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), vdist.data()); tdist.convertTo(distCoeffs, distCoeffs.depth()); } } @@ -1126,6 +1154,33 @@ static double stereoCalibrateImpl( } } + Mat r1d = rvecs.empty() ? Mat() : rvecs.reshape(1, nimages); + Mat t1d = tvecs.empty() ? Mat() : tvecs.reshape(1, nimages); + for(int i = 0; i < nimages; i++ ) + { + int idx = (i + 1) * 6; + + if( !rvecs.empty() ) + { + Vec3d srcR(param[idx + 0], param[idx + 1], param[idx + 2]); + if( rvecs.rows * rvecs.cols * rvecs.channels() == nimages * 9 ) + { + Matx33d rod; + Rodrigues(srcR, rod); + rod.convertTo(r1d.row(i).reshape(1, 3), rvecs.depth()); + } + else if (rvecs.rows * rvecs.cols * rvecs.channels() == nimages * 3 ) + { + Mat(Mat(srcR).t()).convertTo(r1d.row(i), rvecs.depth()); + } + } + if( !tvecs.empty() ) + { + Vec3d srcT(param[idx + 3], param[idx + 4], param[idx + 5]); + Mat(Mat(srcT).t()).convertTo(t1d.row(i), tvecs.depth()); + } + } + return std::sqrt(reprojErr/(pointsTotal*2)); } @@ -1265,23 +1320,17 @@ Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, return cameraMatrix; } -static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype, int outputSize = 14) +static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype, int outputSize) { + Size sz = distCoeffs0.size(); + int n = sz.area(); + if (n > 0) + CV_Assert(sz.width == 1 || sz.height == 1); CV_Assert((int)distCoeffs0.total() <= outputSize); - Mat distCoeffs = Mat::zeros(distCoeffs0.cols == 1 ? Size(1, outputSize) : Size(outputSize, 1), rtype); - if( distCoeffs0.size() == Size(1, 4) || - distCoeffs0.size() == Size(1, 5) || - distCoeffs0.size() == Size(1, 8) || - distCoeffs0.size() == Size(1, 12) || - distCoeffs0.size() == Size(1, 14) || - distCoeffs0.size() == Size(4, 1) || - distCoeffs0.size() == Size(5, 1) || - distCoeffs0.size() == Size(8, 1) || - distCoeffs0.size() == Size(12, 1) || - distCoeffs0.size() == Size(14, 1) ) + Mat distCoeffs = Mat::zeros(sz.width == 1 ? Size(1, outputSize) : Size(outputSize, 1), rtype); + if( n == 4 || n == 5 || n == 8 || n == 12 || n == 14 ) { - Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows)); - distCoeffs0.convertTo(dstCoeffs, rtype); + distCoeffs0.convertTo(distCoeffs(Rect(Point(), sz)), rtype); } return distCoeffs; } @@ -1352,7 +1401,7 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints, (flags & CALIB_THIN_PRISM_MODEL) && !(flags & CALIB_TILTED_MODEL) ? prepareDistCoeffs(distCoeffs, rtype, 12) : - prepareDistCoeffs(distCoeffs, rtype); + prepareDistCoeffs(distCoeffs, rtype, 14); if( !(flags & CALIB_RATIONAL_MODEL) && (!(flags & CALIB_THIN_PRISM_MODEL)) && (!(flags & CALIB_TILTED_MODEL))) @@ -1534,7 +1583,23 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints, InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2, Size imageSize, InputOutputArray _Rmat, InputOutputArray _Tmat, OutputArray _Emat, OutputArray _Fmat, - OutputArray _perViewErrors, int flags , + OutputArray _perViewErrors, int flags, + TermCriteria criteria) +{ + return stereoCalibrate(_objectPoints, _imagePoints1, _imagePoints2, _cameraMatrix1, _distCoeffs1, + _cameraMatrix2, _distCoeffs2, imageSize, _Rmat, _Tmat, _Emat, _Fmat, + noArray(), noArray(), _perViewErrors, flags, criteria); +} + +double stereoCalibrate( InputArrayOfArrays _objectPoints, + InputArrayOfArrays _imagePoints1, + InputArrayOfArrays _imagePoints2, + InputOutputArray _cameraMatrix1, InputOutputArray _distCoeffs1, + InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2, + Size imageSize, InputOutputArray _Rmat, InputOutputArray _Tmat, + OutputArray _Emat, OutputArray _Fmat, + OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, + OutputArray _perViewErrors, int flags, TermCriteria criteria) { int rtype = CV_64F; @@ -1544,8 +1609,8 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints, Mat distCoeffs2 = _distCoeffs2.getMat(); cameraMatrix1 = prepareCameraMatrix(cameraMatrix1, rtype, flags); cameraMatrix2 = prepareCameraMatrix(cameraMatrix2, rtype, flags); - distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype); - distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype); + distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype, 14); + distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype, 14); if( !(flags & CALIB_RATIONAL_MODEL) && (!(flags & CALIB_THIN_PRISM_MODEL)) && @@ -1561,13 +1626,18 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints, _Tmat.create(3, 1, rtype); } - Mat objPt, imgPt, imgPt2, npoints; + int nimages = int(_objectPoints.total()); + CV_Assert( nimages > 0 ); + + Mat objPt, imgPt, imgPt2, npoints, rvecLM, tvecLM; collectCalibrationData( _objectPoints, _imagePoints1, _imagePoints2, objPt, imgPt, imgPt2, npoints ); Mat matR = _Rmat.getMat(), matT = _Tmat.getMat(); - bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed(); + bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(); + bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(); + bool errors_needed = _perViewErrors.needed(); Mat matE, matF, matErr; if( E_needed ) @@ -1581,22 +1651,59 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints, matF = _Fmat.getMat(); } + bool rvecs_mat_vec = _rvecs.isMatVector(); + bool tvecs_mat_vec = _tvecs.isMatVector(); + + if( rvecs_needed ) + { + _rvecs.create(nimages, 1, CV_64FC3); + + if( rvecs_mat_vec ) + rvecLM.create(nimages, 3, CV_64F); + else + rvecLM = _rvecs.getMat(); + } + if( tvecs_needed ) + { + _tvecs.create(nimages, 1, CV_64FC3); + + if( tvecs_mat_vec ) + tvecLM.create(nimages, 3, CV_64F); + else + tvecLM = _tvecs.getMat(); + } + if( errors_needed ) { - int nimages = int(_objectPoints.total()); _perViewErrors.create(nimages, 2, CV_64F); matErr = _perViewErrors.getMat(); } double err = stereoCalibrateImpl(objPt, imgPt, imgPt2, npoints, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, - matR, matT, matE, matF, + matR, matT, matE, matF, rvecLM, tvecLM, matErr, flags, criteria); cameraMatrix1.copyTo(_cameraMatrix1); cameraMatrix2.copyTo(_cameraMatrix2); distCoeffs1.copyTo(_distCoeffs1); distCoeffs2.copyTo(_distCoeffs2); + for(int i = 0; i < nimages; i++ ) + { + if( rvecs_needed && rvecs_mat_vec ) + { + _rvecs.create(3, 1, CV_64F, i, true); + Mat rv = _rvecs.getMat(i); + Mat(rvecLM.row(i).t()).copyTo(rv); + } + if( tvecs_needed && tvecs_mat_vec ) + { + _tvecs.create(3, 1, CV_64F, i, true); + Mat tv = _tvecs.getMat(i); + Mat(tvecLM.row(i).t()).copyTo(tv); + } + } + return err; } diff --git a/modules/calib/src/fisheye.cpp b/modules/calib/src/fisheye.cpp index cb715cd7296d3e6982ee89ba9d8a254d3983067f..b74f7b3e54c8d6cbf14b362066d1e7f5eaaa15fa 100644 --- a/modules/calib/src/fisheye.cpp +++ b/modules/calib/src/fisheye.cpp @@ -887,6 +887,15 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, OutputArray R, OutputArray T, int flags, TermCriteria criteria) +{ + return cv::fisheye::stereoCalibrate(objectPoints, imagePoints1, imagePoints2, K1, D1, K2, D2, imageSize, R, T, + noArray(), noArray(), flags, criteria); +} + +double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, + OutputArray R, OutputArray T, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, + int flags, TermCriteria criteria) { CV_INSTRUMENT_REGION(); @@ -1102,12 +1111,12 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO rms = sqrt(rms); _K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0], - 0, intrinsicLeft.f[1], intrinsicLeft.c[1], - 0, 0, 1); + 0, intrinsicLeft.f[1], intrinsicLeft.c[1], + 0, 0, 1); _K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0], - 0, intrinsicRight.f[1], intrinsicRight.c[1], - 0, 0, 1); + 0, intrinsicRight.f[1], intrinsicRight.c[1], + 0, 0, 1); Mat _R; Rodrigues(omcur, _R); @@ -1118,6 +1127,27 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO if (D2.needed()) Mat(intrinsicRight.k).convertTo(D2, D2.empty() ? CV_64FC1 : D2.type()); if (R.needed()) _R.convertTo(R, R.empty() ? CV_64FC1 : R.type()); if (T.needed()) Mat(Tcur).convertTo(T, T.empty() ? CV_64FC1 : T.type()); + if (rvecs.isMatVector()) + { + if(rvecs.empty()) + rvecs.create(n_images, 1, CV_64FC3); + + if(tvecs.empty()) + tvecs.create(n_images, 1, CV_64FC3); + + for(int i = 0; i < n_images; i++ ) + { + rvecs.create(3, 1, CV_64F, i, true); + tvecs.create(3, 1, CV_64F, i, true); + rvecs1[i].copyTo(rvecs.getMat(i)); + tvecs1[i].copyTo(tvecs.getMat(i)); + } + } + else + { + if (rvecs.needed()) cv::Mat(rvecs1).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type()); + if (tvecs.needed()) cv::Mat(tvecs1).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type()); + } return rms; } diff --git a/modules/calib/test/test_cameracalibration.cpp b/modules/calib/test/test_cameracalibration.cpp index 74f591059e6118613496cee55c2623d16d84f1a2..cec501b811bfb9f93a8190f39d608249bbb743eb 100644 --- a/modules/calib/test/test_cameracalibration.cpp +++ b/modules/calib/test/test_cameracalibration.cpp @@ -1256,10 +1256,8 @@ protected: virtual void correct( const Mat& F, const Mat &points1, const Mat &points2, Mat &newPoints1, Mat &newPoints2 ) = 0; -#if 0 // not ported: #22519 int compare(double* val, double* refVal, int len, double eps, const char* paramName); -#endif void run(int); }; @@ -1326,13 +1324,11 @@ bool CV_StereoCalibrationTest::checkPandROI( int test_case_idx, const Mat& M, co return true; } -#if 0 // not ported: #22519 int CV_StereoCalibrationTest::compare(double* val, double* ref_val, int len, double eps, const char* param_name ) { return cvtest::cmpEps2_64f( ts, val, ref_val, len, eps, param_name ); } -#endif void CV_StereoCalibrationTest::run( int ) { @@ -1340,9 +1336,7 @@ void CV_StereoCalibrationTest::run( int ) const double maxReprojErr = 2; const double maxScanlineDistErr_c = 3; const double maxScanlineDistErr_uc = 4; -#if 0 // not ported: #22519 const double maxDiffBtwRmsErrors = 1e-4; -#endif FILE* f = 0; for(int testcase = 1; testcase <= ntests; testcase++) @@ -1399,7 +1393,7 @@ void CV_StereoCalibrationTest::run( int ) if(left.empty() || right.empty()) { ts->printf( cvtest::TS::LOG, "Can not load images %s and %s, testcase %d\n", - imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase ); + imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase ); ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA ); return; } @@ -1409,8 +1403,8 @@ void CV_StereoCalibrationTest::run( int ) if(!found1 || !found2) { ts->printf( cvtest::TS::LOG, "The function could not detect boards (%d x %d) on the images %s and %s, testcase %d\n", - patternSize.width, patternSize.height, - imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase ); + patternSize.width, patternSize.height, + imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase ); ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT ); return; } @@ -1444,17 +1438,16 @@ void CV_StereoCalibrationTest::run( int ) + CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5 //+ CV_CALIB_FIX_K6 ); -#if 1 // not ported: #22519 - rmsErrorFromStereoCalib /= nframes*npoints; -#endif + + /* rmsErrorFromStereoCalib /= nframes*npoints; */ if (rmsErrorFromStereoCalib > maxReprojErr) { ts->printf(cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n", - rmsErrorFromStereoCalib, testcase); + rmsErrorFromStereoCalib, testcase); ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); return; } -#if 0 // not ported: #22519 + double rmsErrorFromReprojectedImgPts = 0.0f; if (rotMats1.empty() || transVecs1.empty()) { @@ -1464,9 +1457,8 @@ void CV_StereoCalibrationTest::run( int ) } else { - vector reprojectedImgPts[2] = {vector(nframes), vector(nframes)}; size_t totalPoints = 0; - double totalErr[2] = { 0, 0 }, viewErr[2]; + double totalErr[2] = { 0, 0 }; for (size_t i = 0; i < objpt.size(); ++i) { RotMat r1 = rotMats1[i]; Vec3d t1 = transVecs1[i]; @@ -1475,9 +1467,12 @@ void CV_StereoCalibrationTest::run( int ) Mat T2t = R * t1; Vec3d t2 = Mat(T2t + T); + vector reprojectedImgPts[2] = { vector(nframes), + vector(nframes) }; projectPoints(objpt[i], r1, t1, M1, D1, reprojectedImgPts[0]); projectPoints(objpt[i], r2, t2, M2, D2, reprojectedImgPts[1]); + double viewErr[2]; viewErr[0] = cv::norm(imgpt1[i], reprojectedImgPts[0], cv::NORM_L2SQR); viewErr[1] = cv::norm(imgpt2[i], reprojectedImgPts[1], cv::NORM_L2SQR); @@ -1490,16 +1485,15 @@ void CV_StereoCalibrationTest::run( int ) rmsErrorPerViewFromReprojectedImgPts2[i] = sqrt(viewErr[1] / n); } rmsErrorFromReprojectedImgPts = std::sqrt((totalErr[0] + totalErr[1]) / (2 * totalPoints)); - } if (abs(rmsErrorFromStereoCalib - rmsErrorFromReprojectedImgPts) > maxDiffBtwRmsErrors) { ts->printf(cvtest::TS::LOG, - "The difference of the average reprojection error from the calibration function and from the " - "reprojected image points is too big (|%g - %g| = %g), testcase %d\n", - rmsErrorFromStereoCalib, rmsErrorFromReprojectedImgPts, - (rmsErrorFromStereoCalib - rmsErrorFromReprojectedImgPts), testcase); + "The difference of the average reprojection error from the calibration function and from the " + "reprojected image points is too big (|%g - %g| = %g), testcase %d\n", + rmsErrorFromStereoCalib, rmsErrorFromReprojectedImgPts, + (rmsErrorFromStereoCalib - rmsErrorFromReprojectedImgPts), testcase); ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); return; } @@ -1510,28 +1504,27 @@ void CV_StereoCalibrationTest::run( int ) CV_Assert(rmsErrorPerView2.size() == (size_t)nframes); CV_Assert(rmsErrorPerViewFromReprojectedImgPts2.size() == (size_t)nframes); int code1 = compare(&rmsErrorPerView1[0], &rmsErrorPerViewFromReprojectedImgPts1[0], nframes, - maxDiffBtwRmsErrors, "per view errors vector"); + maxDiffBtwRmsErrors, "per view errors vector"); int code2 = compare(&rmsErrorPerView2[0], &rmsErrorPerViewFromReprojectedImgPts2[0], nframes, - maxDiffBtwRmsErrors, "per view errors vector"); + maxDiffBtwRmsErrors, "per view errors vector"); if (code1 < 0) { ts->printf(cvtest::TS::LOG, - "Some of the per view rms reprojection errors differ between calibration function and reprojected " - "points, for the first camera, testcase %d\n", - testcase); + "Some of the per view rms reprojection errors differ between calibration function and reprojected " + "points, for the first camera, testcase %d\n", + testcase); ts->set_failed_test_info(code1); return; } if (code2 < 0) { ts->printf(cvtest::TS::LOG, - "Some of the per view rms reprojection errors differ between calibration function and reprojected " - "points, for the second camera, testcase %d\n", - testcase); + "Some of the per view rms reprojection errors differ between calibration function and reprojected " + "points, for the second camera, testcase %d\n", + testcase); ts->set_failed_test_info(code2); return; } -#endif Mat R1, R2, P1, P2, Q; Rect roi1, roi2; @@ -1771,33 +1764,23 @@ protected: }; double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector >& objectPoints, - const vector >& imagePoints1, - const vector >& imagePoints2, - Mat& cameraMatrix1, Mat& distCoeffs1, - Mat& cameraMatrix2, Mat& distCoeffs2, - Size imageSize, Mat& R, Mat& T, - Mat& E, Mat& F, - std::vector& rotationMatrices, std::vector& translationVectors, - vector& perViewErrors1, vector& perViewErrors2, - TermCriteria criteria, int flags ) + const vector >& imagePoints1, + const vector >& imagePoints2, + Mat& cameraMatrix1, Mat& distCoeffs1, + Mat& cameraMatrix2, Mat& distCoeffs2, + Size imageSize, Mat& R, Mat& T, + Mat& E, Mat& F, + std::vector& rotationMatrices, std::vector& translationVectors, + vector& perViewErrors1, vector& perViewErrors2, + TermCriteria criteria, int flags ) { -#if 1 // not ported: #22519 - CV_UNUSED(rotationMatrices); - CV_UNUSED(translationVectors); - CV_UNUSED(perViewErrors1); - CV_UNUSED(perViewErrors2); - return stereoCalibrate( objectPoints, imagePoints1, imagePoints2, - cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, - imageSize, R, T, E, F, flags, criteria ); -#else vector rvecs, tvecs; Mat perViewErrorsMat; - double avgErr = stereoCalibrate( objectPoints, imagePoints1, imagePoints2, - cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, - imageSize, R, T, E, F, - rvecs, tvecs, perViewErrorsMat, - flags, criteria ); + cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, + imageSize, R, T, E, F, + rvecs, tvecs, perViewErrorsMat, + flags, criteria ); size_t numImgs = imagePoints1.size(); @@ -1810,10 +1793,10 @@ double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector((int)i, 0); - perViewErrors2[i] = perViewErrorsMat.at((int)i, 1); + perViewErrors1[i] = perViewErrorsMat.at(i, 0); + perViewErrors2[i] = perViewErrorsMat.at(i, 1); } if (rotationMatrices.size() != numImgs) @@ -1825,7 +1808,7 @@ double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector reprojectedImgPts[2] = {std::vector(n_images), std::vector(n_images)}; + std::vector reprojectedImgPts[2] = { std::vector(n_images), + std::vector(n_images) }; size_t totalPoints = 0; double totalMSError[2] = { 0, 0 }; for( size_t i = 0; i < n_images; i++ ) @@ -969,12 +970,12 @@ TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations) -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412, - 0, 562.849402029712, 380.555455380889, - 0, 0, 1); + 0, 562.849402029712, 380.555455380889, + 0, 0, 1); - cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359, - 0, 561.90171021422, 380.401340535339, - 0, 0, 1); + cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359, + 0, 561.90171021422, 380.401340535339, + 0, 0, 1); cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); @@ -990,7 +991,7 @@ TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations) EXPECT_NEAR(rmsErrorStereoCalib, rmsErrorFromReprojectedImgPts, 1e-4); } -#endif + TEST_F(fisheyeTest, estimateNewCameraMatrixForUndistortRectify) {