diff --git a/modules/calib/include/opencv2/calib.hpp b/modules/calib/include/opencv2/calib.hpp index edf7c2779d..26185f020c 100644 --- a/modules/calib/include/opencv2/calib.hpp +++ b/modules/calib/include/opencv2/calib.hpp @@ -422,6 +422,11 @@ enum { CALIB_CB_SYMMETRIC_GRID = 1, #define CALIB_NINTRINSIC 18 //!< Maximal size of camera internal parameters (initrinsics) vector +enum CameraModel { + CALIB_MODEL_PINHOLE = 0, //!< Pinhole camera model + CALIB_MODEL_FISHEYE = 1, //!< Fisheye camera model +}; + enum { CALIB_USE_INTRINSIC_GUESS = 0x00001, //!< Use user provided intrinsics as initial point for optimization. CALIB_FIX_ASPECT_RATIO = 0x00002, //!< Use with CALIB_USE_INTRINSIC_GUESS. The ratio fx/fy stays the same as in the input cameraMatrix. CALIB_FIX_PRINCIPAL_POINT = 0x00004, //!< The principal point (cx, cy) stays the same as in the input camera matrix. Image center is used as principal point, if CALIB_USE_INTRINSIC_GUESS is not set. @@ -1136,6 +1141,97 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); +/** @brief Calibrates a camera pair set up. This function finds the extrinsic parameters between the two cameras. + +@param objectPoints1 Vector of vectors of the calibration pattern points for camera 1. +A similar structure as objectPoints in @ref calibrateCamera and for each pattern view, +both cameras do not need to see the same object points. objectPoints1.size(), imagePoints1.size() +nees to be equal,as well as objectPoints1[i].size(), imagePoints1[i].size() need to be equal for each i. +@param objectPoints2 Vector of vectors of the calibration pattern points for camera 2. +A similar structure as objectPoints1. objectPoints2.size(), and imagePoints2.size() nees to be equal, +as well as objectPoints2[i].size(), imagePoints2[i].size() need to be equal for each i. +However, objectPoints1[i].size() and objectPoints2[i].size() are not required to be equal. +@param imagePoints1 Vector of vectors of the projections of the calibration pattern points, +observed by the first camera. The same structure as in @ref calibrateCamera. +@param imagePoints2 Vector of vectors of the projections of the calibration pattern points, +observed by the second camera. The same structure as in @ref calibrateCamera. +@param cameraMatrix1 Input/output camera intrinsic matrix for the first camera, the same as in +@ref calibrateCamera. Furthermore, for the stereo case, additional flags may be used, see below. +@param distCoeffs1 Input/output vector of distortion coefficients, the same as in +@ref calibrateCamera. +@param cameraModel1 Flag reflecting the type of model for camera 1 (pinhole / fisheye): +- @ref CALIB_MODEL_PINHOLE pinhole camera model +- @ref CALIB_MODEL_FISHEYE fisheye camera model +@param cameraMatrix2 Input/output second camera intrinsic matrix for the second camera. +See description for cameraMatrix1. +@param distCoeffs2 Input/output lens distortion coefficients for the second camera. See +description for distCoeffs1. +@param cameraModel2 Flag reflecting the type of model for camera 2 (pinhole / fisheye). +See description for cameraModel1. +@param R Output rotation matrix. Together with the translation vector T, this matrix brings +points given in the first camera's coordinate system to points in the second camera's +coordinate system. In more technical terms, the tuple of R and T performs a change of basis +from the first camera's coordinate system to the second camera's coordinate system. Due to its +duality, this tuple is equivalent to the position of the first camera with respect to the +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 the 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_USE_EXTRINSIC_GUESS R and T contain valid initial values that are optimized further. +@param criteria Termination criteria for the iterative optimization algorithm. + +The function estimates the transformation between two cameras similar to stereo pair calibration. +The principle follows closely to @ref stereoCalibrate. To understand the problem of estimating the +relative pose between a camera pair, please refer to the description there. The difference for +this function is that, camera intrinsics are not optimized and two cameras are not required +to have overlapping fields of view as long as they are observing the same calibration target +and the absolute positions of each object point are known. +![](pics/register_pair.png) +The above illustration shows an example where such a case may become relevant. +Additionally, it supports a camera pair with the mixed model (pinhole / fisheye). +Similarly to #calibrateCamera, the function minimizes the total re-projection error for all the +points in all the available views from both cameras. +@return the final value of the re-projection error. + +@sa calibrateCamera, stereoCalibrate + */ +CV_EXPORTS_AS(registerCamerasExtended) double registerCameras( InputArrayOfArrays objectPoints1, + InputArrayOfArrays objectPoints2, + InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputArray cameraMatrix1, InputArray distCoeffs1, + CameraModel cameraModel1, + InputArray cameraMatrix2, InputArray distCoeffs2, + CameraModel cameraModel2, + InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, + OutputArray perViewErrors, + int flags = 0, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) ); + +/// @overload +CV_EXPORTS_W double registerCameras( InputArrayOfArrays objectPoints1, + InputArrayOfArrays objectPoints2, + InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputArray cameraMatrix1, InputArray distCoeffs1, + CameraModel cameraModel1, + InputArray cameraMatrix2, InputArray distCoeffs2, + CameraModel cameraModel2, + InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F, + OutputArray perViewErrors, + int flags = 0, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) ); + /** @brief Estimates intrinsics and extrinsics (camera pose) for multi-camera system a.k.a multiview calibraton. @param[in] objPoints Calibration pattern object points. Expected shape: NUM_FRAMES x NUM_POINTS x 3. Supported data type: CV_32F. diff --git a/modules/calib/src/calibration.cpp b/modules/calib/src/calibration.cpp index 0aa9be30de..4afea2f871 100644 --- a/modules/calib/src/calibration.cpp +++ b/modules/calib/src/calibration.cpp @@ -1182,6 +1182,399 @@ static double stereoCalibrateImpl( return std::sqrt(reprojErr/(pointsTotal*2)); } +static double registerCamerasImpl( + const Mat& _objectPoints1, const Mat& _objectPoints2, + const Mat& _imagePoints1, const Mat& _imagePoints2, + const Mat& _npoints1, const Mat& _npoints2, + const Mat& _cameraMatrix1, const Mat& _distCoeffs1, CameraModel cameraModel1, + const Mat& _cameraMatrix2, const Mat& _distCoeffs2, CameraModel cameraModel2, + Mat matR, Mat matT, + Mat matE, Mat matF, + Mat rvecs, Mat tvecs, + Mat perViewErr, int flags, + TermCriteria termCrit ) +{ + Matx33d A[2]; + std::vector tdists(2); + int pointsTotal[2], maxPoints = 0, nparams; + pointsTotal[0] = 0; + pointsTotal[1] = 0; + + CV_Assert( _imagePoints1.type() == _imagePoints2.type() && + _imagePoints1.depth() == _objectPoints1.depth() && + _imagePoints2.depth() == _objectPoints2.depth() ); + + CV_Assert( (_npoints1.cols == 1 || _npoints1.rows == 1) && + _npoints1.type() == CV_32S ); + CV_Assert( (_npoints2.cols == 1 || _npoints2.rows == 1) && + _npoints2.type() == CV_32S ); + + Mat _npoints[2]; + _npoints[0] = _npoints1; + _npoints[1] = _npoints2; + + int nimages = (int)_npoints[0].total(); + for(int i = 0; i < nimages; i++ ) + { + for (int k = 0; k < 2; k++) { + int ni = _npoints[k].at(i); + maxPoints = std::max(maxPoints, ni); + pointsTotal[k] += ni; + } + } + + Mat objectPoints[2]; + Mat imagePoints[2]; + _objectPoints1.convertTo(objectPoints[0], CV_64F); + _objectPoints2.convertTo(objectPoints[1], CV_64F); + objectPoints[0] = objectPoints[0].reshape(3, 1); + objectPoints[1] = objectPoints[1].reshape(3, 1); + + if( !rvecs.empty() ) + { + int cn = rvecs.channels(); + int depth = rvecs.depth(); + CV_Assert(depth == CV_32F || depth == CV_64F); + if(((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(); + CV_Assert(depth == CV_32F || depth == CV_64F); + if(((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" ); + } + + CameraModel cameraModels[2] = {cameraModel1, cameraModel2}; + for(int k = 0; k < 2; k++ ) + { + const Mat& points = k == 0 ? _imagePoints1 : _imagePoints2; + const Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; + const Mat& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; + + int depth = points.depth(); + int cn = points.channels(); + CV_Assert( (depth == CV_32F || depth == CV_64F) && + ((points.rows == pointsTotal[k] && points.cols*cn == 2) || + (points.rows == 1 && points.cols == pointsTotal[k] && cn == 2))); + + points.convertTo(imagePoints[k], CV_64F); + imagePoints[k] = imagePoints[k].reshape(2, 1); + + cameraMatrix.convertTo(A[k], CV_64F); + distCoeffs.convertTo(tdists[k], CV_64F); + } + + // we optimize for the inter-camera R(3),t(3) + // Param mapping is: + // - from 0 next 6: stereo pair Rt, from 6+i*6 next 6: Rt for each ith camera of nimages, + nparams = 6*(nimages+1); + std::vector param(nparams, 0.); + + // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component) + std::vector rtsort(nimages*6); + /* + Compute initial estimate of pose + For each image, compute: + R(om) is the rotation matrix of om + om(R) is the rotation vector of R + R_ref = R(om_right) * R(om_left)' + T_ref_list = [T_ref_list; T_right - R_ref * T_left] + om_ref_list = {om_ref_list; om(R_ref)] + om = median(om_ref_list) + T = median(T_ref_list) + */ + int pos[2]; + pos[0] = 0; + pos[1] = 0; + for(int i = 0; i < nimages; i++ ) + { + Matx33d R[2]; + Vec3d rv, T[2]; + for(int k = 0; k < 2; k++ ) + { + int ni = _npoints[k].at(i); + + Mat objpt_i = objectPoints[k].colRange(pos[k], pos[k] + ni); + Mat imgpt_ik = imagePoints[k].colRange(pos[k], pos[k] + ni); + if (cameraModels[k] == CALIB_MODEL_PINHOLE) + solvePnP(objpt_i, imgpt_ik, A[k], tdists[k], rv, T[k], false, SOLVEPNP_ITERATIVE ); + else if (cameraModels[k] == CALIB_MODEL_FISHEYE) + fisheye::solvePnP(objpt_i, imgpt_ik, A[k], tdists[k], rv, T[k], false, SOLVEPNP_ITERATIVE ); + else + CV_Error(CV_StsBadArg, cv::format("Camera type %d is not supported", cameraModels[k])); + + Rodrigues(rv, R[k]); + + if( k == 0 ) + { + // save initial om_left and T_left + param[(i+1)*6 + 0] = rv[0]; + param[(i+1)*6 + 1] = rv[1]; + param[(i+1)*6 + 2] = rv[2]; + param[(i+1)*6 + 3] = T[0][0]; + param[(i+1)*6 + 4] = T[0][1]; + param[(i+1)*6 + 5] = T[0][2]; + } + pos[k] += ni; + } + + R[0] = R[1]*R[0].t(); + T[1] -= R[0]*T[0]; + + Rodrigues(R[0], rv); + + rtsort[i + nimages*0] = rv[0]; + rtsort[i + nimages*1] = rv[1]; + rtsort[i + nimages*2] = rv[2]; + rtsort[i + nimages*3] = T[1][0]; + rtsort[i + nimages*4] = T[1][1]; + rtsort[i + nimages*5] = T[1][2]; + } + + if(flags & CALIB_USE_EXTRINSIC_GUESS) + { + Vec3d R, T; + matT.convertTo(T, CV_64F); + + if( matR.rows == 3 && matR.cols == 3 ) + Rodrigues(matR, R); + else + { + CV_Assert(matR.total() == 3); + matR.convertTo(R, CV_64F); + } + + param[0] = R[0]; + param[1] = R[1]; + param[2] = R[2]; + param[3] = T[0]; + param[4] = T[1]; + param[5] = T[2]; + } + else + { + // find the medians and save the first 6 parameters + for(int i = 0; i < 6; i++ ) + { + size_t idx = i*nimages; + std::nth_element(rtsort.begin() + idx, + rtsort.begin() + idx + nimages/2, + rtsort.begin() + idx + nimages); + double h = rtsort[idx + nimages/2]; + param[i] = (nimages % 2 == 0) ? (h + rtsort[idx + nimages/2 - 1]) * 0.5 : h; + } + } + + // Preallocated place for callback calculations + Mat errBuf( maxPoints*2, 1, CV_64F ); + Mat JeBuf( maxPoints*2, 6, CV_64F ); + Mat J_LRBuf( maxPoints*2, 6, CV_64F ); + + // auto lmcallback = [&, nimages, cameraModels, _npoints] + auto lmcallback = [&, nimages] + (InputOutputArray _param, OutputArray JtErr_, OutputArray JtJ_, double& errnorm) + { + Mat_ param_m = _param.getMat(); + Vec3d om_LR(param_m(0), param_m(1), param_m(2)); + Vec3d T_LR(param_m(3), param_m(4), param_m(5)); + Vec3d om[2], T[2]; + Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2; + double reprojErr = 0; + + int ptPos[2]; + ptPos[0] = 0; + ptPos[1] = 0; + for(int i = 0; i < nimages; i++ ) + { + int idx = (i+1)*6; + om[0] = Vec3d(param_m(idx + 0), param_m(idx + 1), param_m(idx + 2)); + T[0] = Vec3d(param_m(idx + 3), param_m(idx + 4), param_m(idx + 5)); + + if( JtJ_.needed() || JtErr_.needed() ) + composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1], dr3dr1, noArray(), + dr3dr2, noArray(), noArray(), dt3dt1, dt3dr2, dt3dt2 ); + else + composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1] ); + + for(int k = 0; k < 2; k++ ) + { + int ni = _npoints[k].at(i); + Mat objpt_i = objectPoints[k](Range::all(), Range(ptPos[k], ptPos[k] + ni)); + Mat err = errBuf (Range(0, ni*2), Range::all()); + Mat Je = JeBuf (Range(0, ni*2), Range::all()); + Mat J_LR = J_LRBuf(Range(0, ni*2), Range::all()); + + Mat tmpImagePoints = err.reshape(2, 1); + Mat dpdrot = Je.colRange(0, 3); + Mat dpdt = Je.colRange(3, 6); + + Mat imgpt_ik = imagePoints[k](Range::all(), Range(ptPos[k], ptPos[k] + ni)); + + if (cameraModels[k] == CALIB_MODEL_PINHOLE) { + if( JtJ_.needed() || JtErr_.needed() ) + projectPoints(objpt_i, om[k], T[k], A[k], tdists[k], + tmpImagePoints, dpdrot, dpdt, noArray(), noArray(), noArray(), noArray(), 0.); + else + projectPoints(objpt_i, om[k], T[k], A[k], tdists[k], tmpImagePoints); + } else if (cameraModels[k] == CALIB_MODEL_FISHEYE) { + if( JtJ_.needed() || JtErr_.needed() ) { + Mat jacobian; // of size num_points*2 x 15 (2 + 2+ 4 + 3 + 3 + 1 ; // f, c, k, om, T, alpha) + fisheye::projectPoints(objpt_i, tmpImagePoints, om[k], T[k], A[k], tdists[k], 0, jacobian); + jacobian.colRange(8, 11).copyTo(dpdrot); + jacobian.colRange(11,14).copyTo(dpdt); + } else + fisheye::projectPoints(objpt_i, tmpImagePoints, om[k], T[k], A[k], tdists[k]); + } + + subtract( tmpImagePoints, imgpt_ik, tmpImagePoints ); + + if( JtJ_.needed() ) + { + Mat JtErr = JtErr_.getMat(); + Mat JtJ = JtJ_.getMat(); + int eofs = (i+1)*6; + assert( JtJ_.needed() && JtErr_.needed() ); + + if( k == 1 ) + { + // d(err_{x|y}R) ~ de3 + // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2} + for(int p = 0; p < ni*2; p++ ) + { + Matx13d de3dr3, de3dt3, de3dr2, de3dt2, de3dr1, de3dt1; + for(int j = 0; j < 3; j++) + de3dr3(j) = Je.at(p, j); + + for(int j = 0; j < 3; j++) + de3dt3(j) = Je.at(p, 3+j); + + for(int j = 0; j < 3; j++) + de3dr2(j) = J_LR.at(p, j); + + for(int j = 0; j < 3; j++) + de3dt2(j) = J_LR.at(p, 3+j); + + de3dr1 = de3dr3 * dr3dr1; + de3dt1 = de3dt3 * dt3dt1; + de3dr2 = de3dr3 * dr3dr2 + de3dt3 * dt3dr2; + de3dt2 = de3dt3 * dt3dt2; + + for(int j = 0; j < 3; j++) + Je.at(p, j) = de3dr1(j); + + for(int j = 0; j < 3; j++) + Je.at(p, 3+j) = de3dt1(j); + + for(int j = 0; j < 3; j++) + J_LR.at(p, j) = de3dr2(j); + + for(int j = 0; j < 3; j++) + J_LR.at(p, 3+j) = de3dt2(j); + } + + JtJ(Rect(0, 0, 6, 6)) += J_LR.t()*J_LR; + JtJ(Rect(eofs, 0, 6, 6)) = J_LR.t()*Je; + JtErr.rowRange(0, 6) += J_LR.t()*err; + } + + JtJ(Rect(eofs, eofs, 6, 6)) += Je.t()*Je; + JtErr.rowRange(eofs, eofs + 6) += Je.t()*err; + } + + double viewErr = norm(err, NORM_L2SQR); + if(!perViewErr.empty()) + perViewErr.at(i, k) = std::sqrt(viewErr/ni); + reprojErr += viewErr; + ptPos[k] += ni; + } + } + + errnorm = reprojErr; + return true; + }; + + double reprojErr = 0; + LevMarq solver(param, lmcallback, + LevMarq::Settings() + .setMaxIterations((unsigned int)termCrit.maxCount) + .setStepNormTolerance(termCrit.epsilon) + .setSmallEnergyTolerance(termCrit.epsilon * termCrit.epsilon)); + // geodesic not supported for normal callbacks + LevMarq::Report r = solver.optimize(); + + // If solver failed, then the last calculated perViewErr can be wrong & should be recalculated + if (!r.found && !perViewErr.empty()) + { + lmcallback(param, noArray(), noArray(), reprojErr); + } + + reprojErr = r.energy; + + // Extract optimized params from the param vector + + Vec3d om_LR(param[0], param[1], param[2]); + Vec3d T_LR(param[3], param[4], param[5]); + Matx33d R_LR; + Rodrigues( om_LR, R_LR ); + if( matR.rows == 1 || matR.cols == 1 ) + om_LR.convertTo(matR, matR.depth()); + else + R_LR.convertTo(matR, matR.depth()); + T_LR.convertTo(matT, matT.depth()); + + if( !matE.empty() || !matF.empty() ) + { + Matx33d Tx(0, -T_LR[2], T_LR[1], + T_LR[2], 0, -T_LR[0], + -T_LR[1], T_LR[0], 0); + Matx33d E = Tx*R_LR; + if( !matE.empty() ) + E.convertTo(matE, matE.depth()); + if( !matF.empty()) + { + Matx33d iA0 = A[0].inv(), iA1 = A[1].inv(); + Matx33d F = iA1.t() * E * iA0; + F.convertTo(matF, matF.depth(), fabs(F(2,2)) > 0 ? 1./F(2,2) : 1.); + } + } + + 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[0] + pointsTotal[1])); +} + static void collectCalibrationData( InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, @@ -1705,6 +2098,140 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints, return err; } +double registerCameras( InputArrayOfArrays _objectPoints1, + InputArrayOfArrays _objectPoints2, + InputArrayOfArrays _imagePoints1, + InputArrayOfArrays _imagePoints2, + InputArray _cameraMatrix1, InputArray _distCoeffs1, CameraModel cameraModel1, + InputArray _cameraMatrix2, InputArray _distCoeffs2, CameraModel cameraModel2, + InputOutputArray _Rmat, InputOutputArray _Tmat, + OutputArray _Emat, OutputArray _Fmat, + OutputArray _perViewErrors, int flags, + TermCriteria criteria) +{ + return registerCameras(_objectPoints1, _objectPoints2, _imagePoints1, _imagePoints2, _cameraMatrix1, _distCoeffs1, cameraModel1, + _cameraMatrix2, _distCoeffs2, cameraModel2, _Rmat, _Tmat, _Emat, _Fmat, + noArray(), noArray(), _perViewErrors, flags, criteria); +} + +double registerCameras( InputArrayOfArrays _objectPoints1, + InputArrayOfArrays _objectPoints2, + InputArrayOfArrays _imagePoints1, + InputArrayOfArrays _imagePoints2, + InputArray _cameraMatrix1, InputArray _distCoeffs1, CameraModel cameraModel1, + InputArray _cameraMatrix2, InputArray _distCoeffs2, CameraModel cameraModel2, + InputOutputArray _Rmat, InputOutputArray _Tmat, + OutputArray _Emat, OutputArray _Fmat, + OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, + OutputArray _perViewErrors, int flags, + TermCriteria criteria) +{ + // Only support fisheye camera model and pinhole camera model + CV_Assert(cameraModel1 == CALIB_MODEL_FISHEYE || cameraModel1 == CALIB_MODEL_PINHOLE); + CV_Assert(cameraModel2 == CALIB_MODEL_FISHEYE || cameraModel2 == CALIB_MODEL_PINHOLE); + + // Two cameras should contain the same number of frames + CV_Assert(_objectPoints1.total() == _objectPoints2.total()); + + int rtype = CV_64F; + Mat cameraMatrix1 = _cameraMatrix1.getMat(); + Mat cameraMatrix2 = _cameraMatrix2.getMat(); + Mat distCoeffs1 = _distCoeffs1.getMat(); + Mat distCoeffs2 = _distCoeffs2.getMat(); + cameraMatrix1 = prepareCameraMatrix(cameraMatrix1, rtype, flags); + cameraMatrix2 = prepareCameraMatrix(cameraMatrix2, rtype, flags); + + int paramNum1 = int(_distCoeffs1.getMat().total()), paramNum2 = int(_distCoeffs2.getMat().total()); + distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype, paramNum1); + distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype, paramNum2); + + if(!(flags & CALIB_USE_EXTRINSIC_GUESS)) + { + _Rmat.create(3, 3, rtype); + _Tmat.create(3, 1, rtype); + } + + int nimages = int(_objectPoints1.total()); + CV_Assert( nimages > 0 ); + + Mat objPt1, objPt2, imgPt1, imgPt2, npoints1, npoints2, rvecLM, tvecLM; + + collectCalibrationData( _objectPoints1, _imagePoints1, noArray(), + objPt1, imgPt1, noArray(), npoints1 ); + collectCalibrationData( _objectPoints2, _imagePoints2, noArray(), + objPt2, imgPt2, noArray(), npoints2 ); + + Mat matR = _Rmat.getMat(), matT = _Tmat.getMat(); + + 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 ) + { + _Emat.create(3, 3, rtype); + matE = _Emat.getMat(); + } + if( F_needed ) + { + _Fmat.create(3, 3, rtype); + 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 ) + { + _perViewErrors.create(nimages, 2, CV_64F); + matErr = _perViewErrors.getMat(); + } + + double err = registerCamerasImpl(objPt1, objPt2, imgPt1, imgPt2, + npoints1, npoints2, + cameraMatrix1, distCoeffs1, cameraModel1, + cameraMatrix2, distCoeffs2, cameraModel2, + matR, matT, matE, matF, rvecLM, tvecLM, + matErr, flags, criteria); + + 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; +} } /* End of file. */ diff --git a/modules/calib/test/test_cameracalibration.cpp b/modules/calib/test/test_cameracalibration.cpp index fb276c547e..52f1983236 100644 --- a/modules/calib/test/test_cameracalibration.cpp +++ b/modules/calib/test/test_cameracalibration.cpp @@ -1769,6 +1769,475 @@ void CV_StereoCalibrationTest_CPP::correct( const Mat& F, /////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////// Register Cameras //////////////////////////////////////////////// + +class CV_CameraRegistrationTest : public cvtest::BaseTest +{ +public: + CV_CameraRegistrationTest(); + ~CV_CameraRegistrationTest(); + void clear(); +protected: + // covers of tested functions + virtual double registerCameraPair( const vector >& objectPoints1, + const vector >& objectPoints2, + const vector >& imagePoints1, + const vector >& imagePoints2, + Mat& cameraMatrix1, Mat& distCoeffs1, CameraModel cameraModel1, + Mat& cameraMatrix2, Mat& distCoeffs2, CameraModel cameraModel2, + Mat& R, Mat& T, + Mat& E, Mat& F, + std::vector& rotationMatrices, std::vector& translationVectors, + vector& perViewErrors1, vector& perViewErrors2, + TermCriteria criteria, int flags ) = 0; + + virtual double 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 ) = 0; + + int compare(double* val, double* refVal, int len, + double eps, const char* paramName); + + void run(int); +}; + + +CV_CameraRegistrationTest::CV_CameraRegistrationTest() +{ +} + + +CV_CameraRegistrationTest::~CV_CameraRegistrationTest() +{ + clear(); +} + +void CV_CameraRegistrationTest::clear() +{ + cvtest::BaseTest::clear(); +} + +int CV_CameraRegistrationTest::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 ); +} + +void CV_CameraRegistrationTest::run( int ) +{ + const int ntests = 1; + const double maxReprojErr = 2; + const double maxDiffBtwRmsErrors = 1e-4; + const double maxDiffBtwEstErrors = 1e-10; + FILE* f = 0; + + for(int testcase = 1; testcase <= ntests; testcase++) + { + cv::String filepath; + char buf[1000]; + filepath = cv::format("%scv/stereo/case%d/stereo_calib.txt", ts->get_data_path().c_str(), testcase ); + f = fopen(filepath.c_str(), "rt"); + Size patternSize; + vector imglist; + + if( !f || !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &patternSize.width, &patternSize.height) != 2 ) + { + ts->printf( cvtest::TS::LOG, "The file %s can not be opened or has invalid content\n", filepath.c_str() ); + ts->set_failed_test_info( f ? cvtest::TS::FAIL_INVALID_TEST_DATA : cvtest::TS::FAIL_MISSING_TEST_DATA ); + if (f) + fclose(f); + return; + } + + for(;;) + { + if( !fgets( buf, sizeof(buf)-3, f )) + break; + size_t len = strlen(buf); + while( len > 0 && isspace(buf[len-1])) + buf[--len] = '\0'; + if( buf[0] == '#') + continue; + filepath = cv::format("%scv/stereo/case%d/%s", ts->get_data_path().c_str(), testcase, buf ); + imglist.push_back(string(filepath)); + } + fclose(f); + + if( imglist.size() == 0 || imglist.size() % 2 != 0 ) + { + ts->printf( cvtest::TS::LOG, "The number of images is 0 or an odd number in the case #%d\n", testcase ); + ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); + return; + } + + int nframes = (int)(imglist.size()/2); + int npoints = patternSize.width*patternSize.height; + vector > objpt(nframes); + vector > imgpt1(nframes); + vector > imgpt2(nframes); + Size imgsize; + + for( int i = 0; i < nframes; i++ ) + { + Mat left = imread(imglist[i*2]); + Mat right = imread(imglist[i*2+1]); + 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 ); + ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA ); + return; + } + imgsize = left.size(); + bool found1 = findChessboardCorners(left, patternSize, imgpt1[i]); + bool found2 = findChessboardCorners(right, patternSize, imgpt2[i]); + 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 ); + ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT ); + return; + } + for( int j = 0; j < npoints; j++ ) + objpt[i].push_back(Point3f((float)(j%patternSize.width), (float)(j/patternSize.width), 0.f)); + } + + vector rotMats1(nframes); + vector transVecs1(nframes); + vector rotMats2(nframes); + vector transVecs2(nframes); + vector rmsErrorPerView1(nframes); + vector rmsErrorPerView2(nframes); + vector rmsErrorPerViewFromReprojectedImgPts1(nframes); + vector rmsErrorPerViewFromReprojectedImgPts2(nframes); + + Mat M1 = Mat::eye(3,3,CV_64F), M2 = Mat::eye(3,3,CV_64F), D1(5,1,CV_64F), D2(5,1,CV_64F); + Mat R_stereo, T_stereo, R_register, T_register; + Mat E_stereo, F_stereo, E_register, F_register; + M1.at(0,2) = M2.at(0,2)=(imgsize.width-1)*0.5; + M1.at(1,2) = M2.at(1,2)=(imgsize.height-1)*0.5; + D1 = Scalar::all(0); + D2 = Scalar::all(0 + ); + // Initialize the intrinsics + calibrateStereoCamera(objpt, imgpt1, imgpt2, M1, D1, M2, D2, imgsize, R_stereo, T_stereo, E_stereo, F_stereo, + rotMats1, transVecs1, rmsErrorPerView1, rmsErrorPerView2, + TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 1e-6), + CALIB_SAME_FOCAL_LENGTH + //+ CV_CALIB_FIX_ASPECT_RATIO + + CALIB_FIX_PRINCIPAL_POINT + + CALIB_ZERO_TANGENT_DIST + + CALIB_FIX_K3 + + CALIB_FIX_K4 + CALIB_FIX_K5 //+ CV_CALIB_FIX_K6 + ); + + // Use the fixed intrinsics to esimtate with two different methods + double rmsErrorFromStereoCalibStereo = calibrateStereoCamera(objpt, imgpt1, imgpt2, M1, D1, M2, D2, imgsize, R_stereo, T_stereo, E_stereo, F_stereo, + rotMats1, transVecs1, rmsErrorPerView1, rmsErrorPerView2, + TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 1e-6), + CALIB_FIX_INTRINSIC + ); + + double rmsErrorFromStereoCalibRegister = registerCameraPair(objpt, objpt, imgpt1, imgpt2, M1, D1, CALIB_MODEL_PINHOLE, M2, D2, CALIB_MODEL_PINHOLE, R_register, T_register, E_register, F_register, + rotMats2, transVecs2, rmsErrorPerView1, rmsErrorPerView2, + TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 1e-6), + 0 + ); + + + /* rmsErrorFromStereoCalibRegister /= nframes*npoints; */ + if (rmsErrorFromStereoCalibRegister > maxReprojErr) + { + ts->printf(cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n", + rmsErrorFromStereoCalibRegister, testcase); + ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); + return; + } + + double rmsErrorFromReprojectedImgPts = 0.0f; + if (rotMats1.empty() || transVecs1.empty()) + { + rmsErrorPerViewFromReprojectedImgPts1 = rmsErrorPerView1; + rmsErrorPerViewFromReprojectedImgPts2 = rmsErrorPerView2; + rmsErrorFromReprojectedImgPts = rmsErrorFromStereoCalibRegister; + } + else + { + size_t totalPoints = 0; + double totalErr[2] = { 0, 0 }; + for (size_t i = 0; i < objpt.size(); ++i) { + RotMat r1 = rotMats1[i]; + Vec3d t1 = transVecs1[i]; + + RotMat r2 = Mat(R_register * r1); + Mat T2t = R_register * t1; + Vec3d t2 = Mat(T2t + T_register); + + 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); + + size_t n = objpt[i].size(); + totalErr[0] += viewErr[0]; + totalErr[1] += viewErr[1]; + totalPoints += n; + + rmsErrorPerViewFromReprojectedImgPts1[i] = sqrt(viewErr[0] / n); + rmsErrorPerViewFromReprojectedImgPts2[i] = sqrt(viewErr[1] / n); + } + rmsErrorFromReprojectedImgPts = std::sqrt((totalErr[0] + totalErr[1]) / (2 * totalPoints)); + } + + if (abs(rmsErrorFromStereoCalibRegister - 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", + rmsErrorFromStereoCalibRegister, rmsErrorFromReprojectedImgPts, + (rmsErrorFromStereoCalibRegister - rmsErrorFromReprojectedImgPts), testcase); + ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); + return; + } + + /* ----- Compare per view rms re-projection errors ----- */ + CV_Assert(rmsErrorPerView1.size() == (size_t)nframes); + CV_Assert(rmsErrorPerViewFromReprojectedImgPts1.size() == (size_t)nframes); + 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"); + int code2 = compare(&rmsErrorPerView2[0], &rmsErrorPerViewFromReprojectedImgPts2[0], nframes, + 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); + 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); + ts->set_failed_test_info(code2); + return; + } + + /* ----- compare the result from stereoCalibrate and registerCameras ----- */ + if (abs(rmsErrorFromStereoCalibStereo - rmsErrorFromStereoCalibRegister) > maxDiffBtwRmsErrors) + { + ts->printf(cvtest::TS::LOG, + "The difference of the average reprojection error from the register camera and stero calibration is too large (|%g - %g| = %g), testcase %d\n", + rmsErrorFromStereoCalibStereo, rmsErrorFromStereoCalibRegister, + (rmsErrorFromStereoCalibStereo - rmsErrorFromStereoCalibRegister), testcase); + ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); + return; + } + + int code3 = compare(&R_stereo.at(0), &R_register.at(0), 9, maxDiffBtwEstErrors, "R vector"); + int code4 = compare(&T_stereo.at(0), &T_register.at(0), 3, maxDiffBtwEstErrors, "T vector"); + int code5 = compare(&E_stereo.at(0), &E_register.at(0), 9, maxDiffBtwEstErrors, "E vector"); + int code6 = compare(&F_stereo.at(0), &F_register.at(0), 9, maxDiffBtwEstErrors, "F vector"); + + if (code3 < 0) + { + ts->printf(cvtest::TS::LOG, + "The estimated R does not match, testcase %d\n", + testcase); + ts->set_failed_test_info(code3); + return; + } + if (code4 < 0) + { + ts->printf(cvtest::TS::LOG, + "The estimated T does not match, testcase %d\n", + testcase); + ts->set_failed_test_info(code4); + return; + } + if (code5 < 0) + { + ts->printf(cvtest::TS::LOG, + "The estimated E does not match, testcase %d\n", + testcase); + ts->set_failed_test_info(code5); + return; + } + if (code6 < 0) + { + ts->printf(cvtest::TS::LOG, + "The estimated F does not match, testcase %d\n", + testcase); + ts->set_failed_test_info(code6); + return; + } + } +} + +//-------------------------------- CV_CameraRegistrationTest_CPP ------------------------------ + +class CV_CameraRegistrationTest_CPP : public CV_CameraRegistrationTest +{ +public: + CV_CameraRegistrationTest_CPP() {} +protected: + virtual double registerCameraPair( const vector >& objectPoints1, + const vector >& objectPoints2, + const vector >& imagePoints1, + const vector >& imagePoints2, + Mat& cameraMatrix1, Mat& distCoeffs1, CameraModel cameraModel1, + Mat& cameraMatrix2, Mat& distCoeffs2, CameraModel cameraModel2, + Mat& R, Mat& T, + Mat& E, Mat& F, + std::vector& rotationMatrices, std::vector& translationVectors, + vector& perViewErrors1, vector& perViewErrors2, + TermCriteria criteria, int flags ); + + virtual double 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 ); +}; + +double CV_CameraRegistrationTest_CPP::registerCameraPair( const vector >& objectPoints1, + const vector >& objectPoints2, + const vector >& imagePoints1, + const vector >& imagePoints2, + Mat& cameraMatrix1, Mat& distCoeffs1, CameraModel cameraModel1, + Mat& cameraMatrix2, Mat& distCoeffs2, CameraModel cameraModel2, + Mat& R, Mat& T, + Mat& E, Mat& F, + std::vector& rotationMatrices, std::vector& translationVectors, + vector& perViewErrors1, vector& perViewErrors2, + TermCriteria criteria, int flags ) +{ + + vector rvecs, tvecs; + Mat perViewErrorsMat; + double avgErr = registerCameras(objectPoints1, objectPoints2, imagePoints1, imagePoints2, + cameraMatrix1, distCoeffs1, cameraModel1, + cameraMatrix2, distCoeffs2, cameraModel2, + R, T, E, F, + rvecs, tvecs, perViewErrorsMat, + flags, criteria); + + size_t numImgs = imagePoints1.size(); + + if (perViewErrors1.size() != numImgs) + { + perViewErrors1.resize(numImgs); + } + if (perViewErrors2.size() != numImgs) + { + perViewErrors2.resize(numImgs); + } + + for (int i = 0; i < (int)numImgs; i++) + { + perViewErrors1[i] = perViewErrorsMat.at(i, 0); + perViewErrors2[i] = perViewErrorsMat.at(i, 1); + } + + if (rotationMatrices.size() != numImgs) + { + rotationMatrices.resize(numImgs); + } + if (translationVectors.size() != numImgs) + { + translationVectors.resize(numImgs); + } + + for( size_t i = 0; i < numImgs; i++ ) + { + Mat r9; + cv::Rodrigues( rvecs[i], r9 ); + r9.convertTo(rotationMatrices[i], CV_64F); + tvecs[i].convertTo(translationVectors[i], CV_64F); + } + return avgErr; +} + +double CV_CameraRegistrationTest_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 ) +{ + 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 ); + + size_t numImgs = imagePoints1.size(); + + if (perViewErrors1.size() != numImgs) + { + perViewErrors1.resize(numImgs); + } + if (perViewErrors2.size() != numImgs) + { + perViewErrors2.resize(numImgs); + } + + for (int i = 0; i < (int)numImgs; i++) + { + perViewErrors1[i] = perViewErrorsMat.at(i, 0); + perViewErrors2[i] = perViewErrorsMat.at(i, 1); + } + + if (rotationMatrices.size() != numImgs) + { + rotationMatrices.resize(numImgs); + } + if (translationVectors.size() != numImgs) + { + translationVectors.resize(numImgs); + } + + for( size_t i = 0; i < numImgs; i++ ) + { + Mat r9; + cv::Rodrigues( rvecs[i], r9 ); + r9.convertTo(rotationMatrices[i], CV_64F); + tvecs[i].convertTo(translationVectors[i], CV_64F); + } + return avgErr; +} + +TEST(Calib3d_CameraRegistration_CPP, regression) { CV_CameraRegistrationTest_CPP test; test.safe_run(); } +/////////////////////////////////////////////////////////////////////////////////////////////////// + TEST(Calib3d_CalibrateCamera_CPP, regression) { CV_CameraCalibrationTest_CPP test; test.safe_run(); } TEST(Calib3d_CalibrationMatrixValues_CPP, accuracy) { CV_CalibrationMatrixValuesTest_CPP test; test.safe_run(); } TEST(Calib3d_ProjectPoints_CPP, regression) { CV_ProjectPointsTest_CPP test; test.safe_run(); } diff --git a/modules/calib/test/test_fisheye.cpp b/modules/calib/test/test_fisheye.cpp index 4f8ec5dafd..8ed2310f26 100644 --- a/modules/calib/test/test_fisheye.cpp +++ b/modules/calib/test/test_fisheye.cpp @@ -655,4 +655,118 @@ TEST_F(fisheyeTest, multiview_calibration) EXPECT_MAT_NEAR(distortions[1], D2_correct, 5e-2); } +TEST_F(fisheyeTest, cameraRegistrationWithPerViewTransformations) +{ + const int n_images = 34; + + const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY"); + + std::vector > leftPoints(n_images); + std::vector > rightPoints(n_images); + std::vector > objectPoints(n_images); + + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> leftPoints[i]; + fs_left.release(); + + cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); + CV_Assert(fs_right.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_right[cv::format("image_%d", i )] >> rightPoints[i]; + fs_right.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + cv::Matx33d K1, K2, theR; + cv::Vec3d theT; + cv::Vec4d D1, D2; + + int flag = 0; + flag |= cv::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::CALIB_CHECK_COND; + flag |= cv::CALIB_FIX_SKEW; + + cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, + K1, D1, K2, D2, imageSize, theR, theT,flag, cv::TermCriteria(3, 12, 0)); + + cv::Mat E, F, perViewErrors; + std::vector rvecs, tvecs; + flag = 0; + double rmsErrorRegisterCamera = cv::registerCameras(objectPoints, objectPoints, leftPoints, rightPoints, + K1, D1, CALIB_MODEL_FISHEYE, + K2, D2, CALIB_MODEL_FISHEYE, + theR, theT, E, F, rvecs, tvecs, perViewErrors, flag, + cv::TermCriteria(3, 12, 0)); + 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++ ) + { + cv::Matx33d viewRotMat1, viewRotMat2; + cv::Vec3d viewT1, viewT2; + cv::Mat rVec; + cv::Rodrigues( rvecs[i], rVec ); + rVec.convertTo(viewRotMat1, CV_64F); + tvecs[i].convertTo(viewT1, CV_64F); + + viewRotMat2 = theR * viewRotMat1; + cv::Vec3d T2t = theR * viewT1; + viewT2 = T2t + theT; + + cv::Vec3d viewRotVec1, viewRotVec2; + cv::Rodrigues(viewRotMat1, viewRotVec1); + cv::Rodrigues(viewRotMat2, viewRotVec2); + + double alpha1 = K1(0, 1) / K1(0, 0); + double alpha2 = K2(0, 1) / K2(0, 0); + cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[0], viewRotVec1, viewT1, K1, D1, alpha1); + cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[1], viewRotVec2, viewT2, K2, D2, alpha2); + + double viewMSError[2] = { + cv::norm(leftPoints[i], reprojectedImgPts[0], cv::NORM_L2SQR), + cv::norm(rightPoints[i], reprojectedImgPts[1], cv::NORM_L2SQR) + }; + + size_t n = objectPoints[i].size(); + totalMSError[0] += viewMSError[0]; + totalMSError[1] += viewMSError[1]; + totalPoints += n; + } + + double rmsErrorFromReprojectedImgPts = std::sqrt((totalMSError[0] + totalMSError[1]) / (2 * totalPoints)); + + cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, + -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, + -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); + + 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); + + EXPECT_MAT_NEAR(theR, R_correct, 1e-6); + EXPECT_MAT_NEAR(theT, T_correct, 1e-6); + + EXPECT_MAT_NEAR(K1, K1_correct, 1e-4); + EXPECT_MAT_NEAR(K2, K2_correct, 1e-4); + + EXPECT_MAT_NEAR(D1, D1_correct, 1e-5); + EXPECT_MAT_NEAR(D2, D2_correct, 1e-5); + + EXPECT_NEAR(rmsErrorRegisterCamera, rmsErrorFromReprojectedImgPts, 1e-4); +} + }} // namespace