diff --git a/modules/ccalib/CMakeLists.txt b/modules/ccalib/CMakeLists.txt index d7763aa909a..9def98ddd1f 100644 --- a/modules/ccalib/CMakeLists.txt +++ b/modules/ccalib/CMakeLists.txt @@ -1,2 +1,5 @@ set(the_description "Custom Calibration Pattern") ocv_define_module(ccalib opencv_core opencv_imgproc opencv_calib3d opencv_features2d opencv_highgui opencv_imgcodecs WRAP python) +if(BUILD_TESTS) + add_subdirectory(test) +endif() diff --git a/modules/ccalib/include/opencv2/ccalib/omnidir.hpp b/modules/ccalib/include/opencv2/ccalib/omnidir.hpp index d3132b3fff3..69bf8ce9192 100644 --- a/modules/ccalib/include/opencv2/ccalib/omnidir.hpp +++ b/modules/ccalib/include/opencv2/ccalib/omnidir.hpp @@ -48,6 +48,13 @@ namespace cv { + /** following typedefs used in k3 support (github.com/opencv/opencv_contrib/issues/3952) +      should go to opencv2/core/matx.hpp at some point */ + typedef Matx Matx15f; + typedef Matx Matx15d; + typedef Matx Matx25f; + typedef Matx Matx25d; + typedef Vec Vec5d; namespace omnidir { //! @addtogroup ccalib @@ -62,7 +69,9 @@ namespace omnidir CALIB_FIX_P2 = 32, CALIB_FIX_XI = 64, CALIB_FIX_GAMMA = 128, - CALIB_FIX_CENTER = 256 + CALIB_FIX_CENTER = 256, + CALIB_FIX_K3 = 512 + }; enum{ @@ -270,10 +279,11 @@ namespace internal void decodeParametersStereo(InputArray parameters, OutputArray K1, OutputArray K2, OutputArray om, OutputArray T, OutputArrayOfArrays omL, OutputArrayOfArrays tL, OutputArray D1, OutputArray D2, double& xi1, double& xi2); - void estimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, Mat& errors, Vec2d& std_error, double& rms, int flags); + void estimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, + Mat& errors, Vec2d& std_error, double& rms, int flags); - void estimateUncertaintiesStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputArray parameters, Mat& errors, - Vec2d& std_error, double& rms, int flags); + void estimateUncertaintiesStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputArray parameters, Mat& errors, Vec2d& std_error, double& rms, int flags); double computeMeanReproErr(InputArrayOfArrays imagePoints, InputArrayOfArrays proImagePoints); diff --git a/modules/ccalib/src/omnidir.cpp b/modules/ccalib/src/omnidir.cpp index 5e5f8cc2bf4..f0175d898c6 100644 --- a/modules/ccalib/src/omnidir.cpp +++ b/modules/ccalib/src/omnidir.cpp @@ -69,7 +69,7 @@ namespace cv { namespace double ds; Matx12d dc; double dxi; - Matx14d dkp; // distortion k1,k2,p1,p2 + Matx15d dkp; // distortion k1,k2,p1,p2,k3 }; }} @@ -89,7 +89,7 @@ void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints CV_Assert((rvec.depth() == CV_64F || rvec.depth() == CV_32F) && rvec.total() == 3); CV_Assert((tvec.depth() == CV_64F || tvec.depth() == CV_32F) && tvec.total() == 3); CV_Assert((K.type() == CV_64F || K.type() == CV_32F) && K.size() == Size(3,3)); - CV_Assert((D.type() == CV_64F || D.type() == CV_32F) && D.total() == 4); + CV_Assert((D.type() == CV_64F || D.type() == CV_32F) && (D.total() == 4 || D.total() == 5)); imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2)); @@ -115,9 +115,6 @@ void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints s = Kc(0,1); } - Vec4d kp = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr() : *D.getMat().ptr(); - //Vec kp= (Vec)*D.getMat().ptr >(); - const Vec3d* Xw_alld = objectPoints.getMat().ptr(); const Vec3f* Xw_allf = objectPoints.getMat().ptr(); Vec2d* xpd = imagePoints.getMat().ptr(); @@ -130,19 +127,31 @@ void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints JacobianRow *Jn = 0; if (jacobian.needed()) { - int nvars = 2+2+1+4+3+3+1; // f,c,s,kp,om,T,xi + int nvars = 2+2+1+5+3+3+1; // f,c,s,kp(5),om,T,xi jacobian.create(2*int(n), nvars, CV_64F); Jn = jacobian.getMat().ptr(0); } - double k1=kp[0],k2=kp[1]; - double p1 = kp[2], p2 = kp[3]; + Mat Dm = D.getMat(); + double k1=0, k2=0, p1=0, p2=0, k3=0; + + if (Dm.depth() == CV_32F) + { + const float* d = Dm.ptr(); + k1 = d[0]; k2 = d[1]; p1 = d[2]; p2 = d[3]; + if (Dm.total() == 5) { k3 = d[4]; } + } + else + { + const double* d = Dm.ptr(); + k1 = d[0]; k2 = d[1]; p1 = d[2]; p2 = d[3]; + if (Dm.total() == 5) { k3 = d[4]; } + } for (int i = 0; i < n; i++) { // convert to camera coordinate Vec3d Xw = objectPoints.depth() == CV_32F ? (Vec3d)Xw_allf[i] : Xw_alld[i]; - Vec3d Xc = (Vec3d)(R*Xw + T); // convert to unit sphere @@ -153,11 +162,15 @@ void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints // add distortion Vec2d xd; - double r2 = xu[0]*xu[0]+xu[1]*xu[1]; + + double r2 = xu[0]*xu[0] + xu[1]*xu[1]; double r4 = r2*r2; + double r6 = r4*r2; + double radial = 1 + k1*r2 + k2*r4 + k3*r6; + + xd[0] = xu[0]*radial + 2*p1*xu[0]*xu[1] + p2*(r2+2*xu[0]*xu[0]); + xd[1] = xu[1]*radial + p1*(r2+2*xu[1]*xu[1]) + 2*p2*xu[0]*xu[1]; - xd[0] = xu[0]*(1+k1*r2+k2*r4) + 2*p1*xu[0]*xu[1] + p2*(r2+2*xu[0]*xu[0]); - xd[1] = xu[1]*(1+k1*r2+k2*r4) + p1*(r2+2*xu[1]*xu[1]) + 2*p2*xu[0]*xu[1]; // convert to pixel coordinate Vec2d final; @@ -172,9 +185,6 @@ void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints { xpd[i] = final; } - /*xpd[i][0] = f[0]*xd[0]+s*xd[1]+c[0]; - xpd[i][1] = f[1]*xd[1]+c[1];*/ - if (jacobian.needed()) { double dXcdR_a[] = {Xw[0],Xw[1],Xw[2],0,0,0,0,0,0, @@ -190,10 +200,16 @@ void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints Matx23d dxudXs(1/(Xs[2]+xi), 0, -Xs[0]/(Xs[2]+xi)/(Xs[2]+xi), 0, 1/(Xs[2]+xi), -Xs[1]/(Xs[2]+xi)/(Xs[2]+xi)); // pre-compute some reusable things - double temp1 = 2*k1*xu[0] + 4*k2*xu[0]*r2; - double temp2 = 2*k1*xu[1] + 4*k2*xu[1]*r2; - Matx22d dxddxu(k2*r4+6*p2*xu[0]+2*p1*xu[1]+xu[0]*temp1+k1*r2+1, 2*p1*xu[0]+2*p2*xu[1]+xu[0]*temp2, - 2*p1*xu[0]+2*p2*xu[1]+xu[1]*temp1, k2*r4+2*p2*xu[0]+6*p1*xu[1]+xu[1]*temp2+k1*r2+1); + double temp1 = 2*k1*xu[0] + 4*k2*xu[0]*r2 + 6*k3*xu[0]*r4; + double temp2 = 2*k1*xu[1] + 4*k2*xu[1]*r2 + 6*k3*xu[1]*r4; + + Matx22d dxddxu( + (k3*r6 + k2*r4 + 6*p2*xu[0] + 2*p1*xu[1] + xu[0]*temp1 + k1*r2 + 1), + (2*p1*xu[0] + 2*p2*xu[1] + xu[0]*temp2), + (2*p1*xu[0] + 2*p2*xu[1] + xu[1]*temp1), + (k3*r6 + k2*r4 + 2*p2*xu[0] + 6*p1*xu[1] + xu[1]*temp2 + k1*r2 + 1) + ); + Matx22d dxpddxd(f[0], s, 0, f[1]); Matx23d dxpddXc = dxpddxd * dxddxu * dxudXs * dXsdXc; @@ -211,11 +227,16 @@ void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints // derivative of xpd respect to xi Matx21d dxpddxi = dxpddxd * dxddxu * dxudxi; - Matx dxddkp(xu[0]*r2, xu[0]*r4, 2*xu[0]*xu[1], r2+2*xu[0]*xu[0], - xu[1]*r2, xu[1]*r4, r2+2*xu[1]*xu[1], 2*xu[0]*xu[1]); + r6 = r4*r2; + + Matx25d dxddkp( + xu[0]*r2, xu[0]*r4, 2*xu[0]*xu[1], r2+2*xu[0]*xu[0], xu[0]*r6, + xu[1]*r2, xu[1]*r4, r2+2*xu[1]*xu[1], 2*xu[0]*xu[1], xu[1]*r6 + ); + // derivative of xpd respect to kp - Matx dxpddkp = dxpddxd * dxddkp; + Matx25d dxpddkp = dxpddxd * dxddkp; // derivative of xpd respect to f Matx22d dxpddf(xd[0], 0, @@ -252,7 +273,7 @@ void cv::omnidir::undistortPoints( InputArray distorted, OutputArray undistorted CV_Assert(distorted.type() == CV_64FC2 || distorted.type() == CV_32FC2); CV_Assert(R.empty() || (!R.empty() && (R.size() == Size(3, 3) || R.total() * R.channels() == 3) && (R.depth() == CV_64F || R.depth() == CV_32F))); - CV_Assert((D.depth() == CV_64F || D.depth() == CV_32F) && D.total() == 4); + CV_Assert((D.depth() == CV_64F || D.depth() == CV_32F) && (D.total() == 4 || D.total() == 5)); CV_Assert(K.size() == Size(3, 3) && (K.depth() == CV_64F || K.depth() == CV_32F)); CV_Assert(xi.total() == 1 && (xi.depth() == CV_64F || xi.depth() == CV_32F)); @@ -275,9 +296,21 @@ void cv::omnidir::undistortPoints( InputArray distorted, OutputArray undistorted s = camMat(0,1); } - Vec4d kp = D.depth()==CV_32F ? (Vec4d)*D.getMat().ptr():(Vec4d)*D.getMat().ptr(); - Vec2d k = Vec2d(kp[0], kp[1]); - Vec2d p = Vec2d(kp[2], kp[3]); + Mat Dm = D.getMat(); + double k1=0, k2=0, p1=0, p2=0, k3=0; + + if (Dm.depth() == CV_32F) + { + const float* d = Dm.ptr(); + k1 = d[0]; k2 = d[1]; p1 = d[2]; p2 = d[3]; + if (Dm.total() == 5) { k3 = d[4]; } + } + else + { + const double* d = Dm.ptr(); + k1 = d[0]; k2 = d[1]; p1 = d[2]; p2 = d[3]; + if (Dm.total() == 5) { k3 = d[4]; } + } double _xi = xi.depth() == CV_32F ? (double)*xi.getMat().ptr() : *xi.getMat().ptr(); cv::Matx33d RR = cv::Matx33d::eye(); @@ -311,8 +344,10 @@ void cv::omnidir::undistortPoints( InputArray distorted, OutputArray undistorted { double r2 = pu[0]*pu[0] + pu[1]*pu[1]; double r4 = r2*r2; - pu[0] = (pp[0] - 2*p[0]*pu[0]*pu[1] - p[1]*(r2+2*pu[0]*pu[0])) / (1 + k[0]*r2 + k[1]*r4); - pu[1] = (pp[1] - 2*p[1]*pu[0]*pu[1] - p[0]*(r2+2*pu[1]*pu[1])) / (1 + k[0]*r2 + k[1]*r4); + double r6 = r4*r2; + double radial = 1.0 + k1*r2 + k2*r4 + k3*r6; + pu[0] = (pp[0] - 2*p1*pu[0]*pu[1] - p2*(r2 + 2*pu[0]*pu[0])) / radial; + pu[1] = (pp[1] - 2*p2*pu[0]*pu[1] - p1*(r2 + 2*pu[1]*pu[1])) / radial; } // project to unit sphere @@ -353,7 +388,8 @@ void cv::omnidir::initUndistortRectifyMap(InputArray K, InputArray D, InputArray map2.create( size, map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F ); CV_Assert((K.depth() == CV_32F || K.depth() == CV_64F) && (D.depth() == CV_32F || D.depth() == CV_64F)); - CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4)); + CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4 || D.total() == 5)); + CV_Assert(P.empty()|| (P.depth() == CV_32F || P.depth() == CV_64F)); CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3)); CV_Assert(R.empty() || (R.depth() == CV_32F || R.depth() == CV_64F)); @@ -379,12 +415,27 @@ void cv::omnidir::initUndistortRectifyMap(InputArray K, InputArray D, InputArray s = camMat(0,1); } - Vec4d kp = Vec4d::all(0); + double _xi = xi.depth() == CV_32F ? (double)*xi.getMat().ptr() + : *xi.getMat().ptr(); + + double k1=0, k2=0, p1=0, p2=0, k3=0; if (!D.empty()) - kp = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr(): *D.getMat().ptr(); - double _xi = xi.depth() == CV_32F ? (double)*xi.getMat().ptr() : *xi.getMat().ptr(); - Vec2d k = Vec2d(kp[0], kp[1]); - Vec2d p = Vec2d(kp[2], kp[3]); + { + Mat Dm = D.getMat(); + if (Dm.depth() == CV_32F) + { + const float* d = Dm.ptr(); + k1=d[0]; k2=d[1]; p1=d[2]; p2=d[3]; + if (Dm.total()==5) { k3=d[4]; } + } + else + { + const double* d = Dm.ptr(); + k1=d[0]; k2=d[1]; p1=d[2]; p2=d[3]; + if (Dm.total()==5) { k3=d[4]; } + } + } + cv::Matx33d RR = cv::Matx33d::eye(); if (!R.empty() && R.total() * R.channels() == 3) { @@ -430,8 +481,10 @@ void cv::omnidir::initUndistortRectifyMap(InputArray K, InputArray D, InputArray // add distortion double r2 = xu*xu + yu*yu; double r4 = r2*r2; - double xd = (1+k[0]*r2+k[1]*r4)*xu + 2*p[0]*xu*yu + p[1]*(r2+2*xu*xu); - double yd = (1+k[0]*r2+k[1]*r4)*yu + p[0]*(r2+2*yu*yu) + 2*p[1]*xu*yu; + double r6 = r4*r2; + double radial = 1.0 + k1*r2 + k2*r4 + k3*r6; + double xd = radial*xu + 2*p1*xu*yu + p2*(r2 + 2*xu*xu); + double yd = radial*yu + p1*(r2 + 2*yu*yu) + 2*p2*xu*yu; // to image pixel double u = f[0]*xd + s*yd + c[0]; double v = f[1]*yd + c[1]; @@ -471,9 +524,6 @@ void cv::omnidir::initUndistortRectifyMap(InputArray K, InputArray D, InputArray double _xt = 0.0, _yt = 0.0, _wt = 0.0; if (flags == omnidir::RECTIFY_CYLINDRICAL) { - //_xt = std::sin(theta); - //_yt = h; - //_wt = std::cos(theta); _xt = std::cos(theta); _yt = std::sin(theta); _wt = h; @@ -506,10 +556,13 @@ void cv::omnidir::initUndistortRectifyMap(InputArray K, InputArray D, InputArray double xu = Xs / (Zs + _xi), yu = Ys / (Zs + _xi); // add distortion + double r2 = xu*xu + yu*yu; double r4 = r2*r2; - double xd = (1+k[0]*r2+k[1]*r4)*xu + 2*p[0]*xu*yu + p[1]*(r2+2*xu*xu); - double yd = (1+k[0]*r2+k[1]*r4)*yu + p[0]*(r2+2*yu*yu) + 2*p[1]*xu*yu; + double r6 = r4*r2; + double radial = 1.0 + k1*r2 + k2*r4 + k3*r6; + double xd = radial*xu + 2*p1*xu*yu + p2*(r2 + 2*xu*xu); + double yd = radial*yu + p1*(r2 + 2*yu*yu) + 2*p2*xu*yu; // to image pixel double u = f[0]*xd + s*yd + c[0]; double v = f[1]*yd + c[1]; @@ -676,7 +729,8 @@ void cv::omnidir::internal::initializeCalibration(InputArrayOfArrays patternPoin Matx33d Kc(gamma, 0, u0, 0, gamma, v0, 0, 0, 1); // reproject error - cv::omnidir::projectPoints(objPoints, projedImgPoints, om, t, Kc, 1, Matx14d(0, 0, 0, 0), cv::noArray()); + cv::omnidir::projectPoints(objPoints, projedImgPoints, om, t, Kc, 1, Matx15d(0,0,0,0,0), cv::noArray()); + double reprojectError = omnidir::internal::computeMeanReproErr(imgPoints, projedImgPoints); // if this reproject error is smaller @@ -708,7 +762,17 @@ void cv::omnidir::internal::initializeCalibration(InputArrayOfArrays patternPoin for (int i = 0; i< n_img; i++) { Mat _projected; - cv::omnidir::projectPoints(patternPoints.getMat(i), _projected, v_omAll[i], v_tAll[i], _K, 1, Matx14d(0, 0, 0, 0), cv::noArray()); + cv::omnidir::projectPoints( + patternPoints.getMat(i), + _projected, + v_omAll[i], + v_tAll[i], + _K, + 1, + Matx15d (0,0,0,0,0), + cv::noArray() + ); + double _error = omnidir::internal::computeMeanReproErr(imagePoints.getMat(i), _projected); if(_error < 100) { @@ -753,7 +817,7 @@ void cv::omnidir::internal::initializeStereoCalibration(InputArrayOfArrays objec { Mat idx1, idx2; Matx33d _K1, _K2; - Matx14d _D1, _D2; + Matx15d _D1, _D2; Mat _xi1m, _xi2m; std::vector omAllTemp1, omAllTemp2, tAllTemp1, tAllTemp2; @@ -832,8 +896,8 @@ void cv::omnidir::internal::initializeStereoCalibration(InputArrayOfArrays objec } if (D1.empty()) { - D1.create(1, 4, CV_64F); - D2.create(1, 4, CV_64F); + D1.create(1, 5, CV_64F); + D2.create(1, 5, CV_64F); } Mat(_K1).copyTo(K1.getMat()); Mat(_K2).copyTo(K2.getMat()); @@ -856,9 +920,9 @@ void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, Inp int n = (int)objectPoints.total(); - Mat JTJ = Mat::zeros(10 + 6*n, 10 + 6*n, CV_64F); - JTJ_inv = Mat::zeros(10 + 6*n, 10 + 6*n, CV_64F); - JTE = Mat::zeros(10 + 6*n, 1, CV_64F); + Mat JTJ = Mat::zeros(11 + 6*n, 11 + 6*n, CV_64F); + JTJ_inv = Mat::zeros(11 + 6*n, 11 + 6*n, CV_64F); + JTE = Mat::zeros(11 + 6*n, 1, CV_64F); int nPointsAll = 0; for (int i = 0; i < n; ++i) @@ -866,13 +930,13 @@ void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, Inp nPointsAll += (int)objectPoints.getMat(i).total(); } - Mat J = Mat::zeros(2*nPointsAll, 10+6*n, CV_64F); - Mat exAll = Mat::zeros(2*nPointsAll, 10+6*n, CV_64F); + Mat J = Mat::zeros(2*nPointsAll, 11 + 6*n, CV_64F); + Mat exAll = Mat::zeros(2*nPointsAll, 11 + 6*n, CV_64F); double *para = parameters.getMat().ptr(); Matx33d K(para[6*n], para[6*n+2], para[6*n+3], 0, para[6*n+1], para[6*n+4], 0, 0, 1); - Matx14d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]); + Matx15d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9], para[6*n+10]); double xi = para[6*n+5]; for (int i = 0; i < n; i++) { @@ -889,23 +953,24 @@ void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, Inp Mat projError = imgPoints - imgProj; // The intrinsic part of Jacobian - Mat JIn(jacobian.rows, 10, CV_64F); + Mat JIn(jacobian.rows, 11, CV_64F); Mat JEx(jacobian.rows, 6, CV_64F); - jacobian.colRange(6, 16).copyTo(JIn); + jacobian.colRange(6, 17).copyTo(JIn); jacobian.colRange(0, 6).copyTo(JEx); - JTJ(Rect(6*n, 6*n, 10, 10)) = JTJ(Rect(6*n, 6*n, 10, 10)) + JIn.t()*JIn; + JTJ(Rect(6*n, 6*n, 11, 11)) = JTJ(Rect(6*n, 6*n, 11, 11)) + JIn.t()*JIn; JTJ(Rect(i*6, i*6, 6, 6)) = JEx.t() * JEx; Mat JExTIn = JEx.t() * JIn; - JExTIn.copyTo(JTJ(Rect(6*n, i*6, 10, 6))); + JExTIn.copyTo(JTJ(Rect(6*n, i*6, 11, 6))); - Mat(JIn.t()*JEx).copyTo(JTJ(Rect(i*6, 6*n, 6, 10))); + Mat(JIn.t()*JEx).copyTo(JTJ(Rect(i*6, 6*n, 6, 11))); + + JTE(Rect(0, 6*n, 1, 11)) = JTE(Rect(0, 6*n, 1, 11)) + JIn.t() * projError.reshape(1, 2*(int)projError.total()); - JTE(Rect(0, 6*n, 1, 10)) = JTE(Rect(0, 6*n,1, 10)) + JIn.t() * projError.reshape(1, 2*(int)projError.total()); JTE(Rect(0, i*6, 1, 6)) = JEx.t() * projError.reshape(1, 2*(int)projError.total()); //int nPoints = objectPoints.getMat(i).total(); @@ -915,7 +980,7 @@ void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, Inp } //JTJ = J.t()*J; //JTE = J.t()*exAll; - std::vector _idx(6*n+10, 1); + std::vector _idx(6*n+11, 1); flags2idx(flags, _idx, n); subMatrix(JTJ, JTJ, _idx, _idx); @@ -924,7 +989,7 @@ void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, Inp //SVD svd(JTJ, SVD::NO_UV); //double cond = svd.w.at(0)/svd.w.at(5); - //if (cond_JTJ.needed()) + //if (cond_JTJ.needed()) //{ // cond_JTJ.create(1, 1, CV_64F); // cond_JTJ.getMat().at(0) = cond; @@ -945,21 +1010,22 @@ void cv::omnidir::internal::computeJacobianStereo(InputArrayOfArrays objectPoint // compute Jacobian matrix by naive way int n_img = (int)objectPoints.total(); int n_points = (int)objectPoints.getMat(0).total(); - Mat J = Mat::zeros(4 * n_points * n_img, 20 + 6 * (n_img + 1), CV_64F); + Mat J = Mat::zeros(4 * n_points * n_img, 22 + 6 * (n_img + 1), CV_64F); + Mat exAll = Mat::zeros(4 * n_points * n_img, 1, CV_64F); double *para = parameters.getMat().ptr(); int offset1 = (n_img + 1) * 6; - int offset2 = offset1 + 10; + int offset2 = offset1 + 11; Matx33d K1(para[offset1], para[offset1+2], para[offset1+3], 0, para[offset1+1], para[offset1+4], 0, 0, 1); - Matx14d D1(para[offset1+6], para[offset1+7], para[offset1+8], para[offset1+9]); + Matx15d D1(para[offset1+6], para[offset1+7], para[offset1+8], para[offset1+9], para[offset1+10]); double xi1 = para[offset1+5]; Matx33d K2(para[offset2], para[offset2+2], para[offset2+3], 0, para[offset2+1], para[offset2+4], 0, 0, 1); - Matx14d D2(para[offset2+6], para[offset2+7], para[offset2+8], para[offset2+9]); + Matx15d D2(para[offset2+6], para[offset2+7], para[offset2+8], para[offset2+9], para[offset2+10]); double xi2 = para[offset2+5]; Mat om = parameters.getMat().reshape(1, 1).colRange(0, 3); @@ -986,7 +1052,7 @@ void cv::omnidir::internal::computeJacobianStereo(InputArrayOfArrays objectPoint Mat projError1 = imgPoints1i - imgProj1; //Mat JIn1(jacobian1.rows, 10, CV_64F); //Mat JEx1(jacobian1.rows, 6, CV_64F); - jacobian1.colRange(6, 16).copyTo(J(Rect(6*(n_img+1), i*n_points*4, 10, n_points*2))); + jacobian1.colRange(6, 17).copyTo(J(Rect(6*(n_img+1), i*n_points*4, 11, n_points*2))); jacobian1.colRange(0, 6).copyTo(J(Rect(6+i*6, i*n_points*4, 6, n_points*2))); projError1.reshape(1, 2*n_points).copyTo(exAll.rowRange(i*4*n_points, (i*4+2)*n_points)); @@ -1005,10 +1071,11 @@ void cv::omnidir::internal::computeJacobianStereo(InputArrayOfArrays objectPoint dxrdT.copyTo(J(Rect(3, (i*4+2)*n_points, 3, n_points*2))); dxrdom1.copyTo(J(Rect(6+i*6, (i*4+2)*n_points, 3, n_points*2))); dxrdT1.copyTo(J(Rect(6+i*6+3, (i*4+2)*n_points, 3, n_points*2))); - jacobian2.colRange(6, 16).copyTo(J(Rect(6*(n_img+1)+10, (4*i+2)*n_points, 10, n_points*2))); + jacobian2.colRange(6, 17).copyTo(J(Rect(6*(n_img+1) + 11, (4*i+2)*n_points, 11, n_points*2))); + } - std::vector _idx(6*(n_img+1)+20, 1); + std::vector _idx(6*(n_img+1)+22, 1); flags2idxStereo(flags, _idx, n_img); Mat JTJ = J.t()*J; @@ -1073,7 +1140,12 @@ double cv::omnidir::calibrate(InputArrayOfArrays patternPoints, InputArrayOfArra (patternPoints.type() == CV_32FC3 && imagePoints.type() == CV_32FC2)); CV_Assert(patternPoints.getMat(0).channels() == 3 && imagePoints.getMat(0).channels() == 2); CV_Assert((!K.empty() && K.size() == Size(3,3)) || K.empty()); - CV_Assert((!D.empty() && D.total() == 4) || D.empty()); + CV_Assert((!D.empty() && (D.total() == 4 || D.total() == 5)) || D.empty()); + const bool useK3 = (!D.empty() && D.total() == 5); + int flags_eff = flags; + if (!useK3) + flags_eff |= omnidir::CALIB_FIX_K3; + CV_Assert((!xi.empty() && xi.total() == 1) || xi.empty()); CV_Assert((!omAll.empty() && omAll.depth() == patternPoints.depth()) || omAll.empty()); CV_Assert((!tAll.empty() && tAll.depth() == patternPoints.depth()) || tAll.empty()); @@ -1096,7 +1168,7 @@ double cv::omnidir::calibrate(InputArrayOfArrays patternPoints, InputArrayOfArra // initialization std::vector _omAll, _tAll; Matx33d _K; - Matx14d _D; + Mat _D(1, 5, CV_64F); Mat _idx; cv::omnidir::internal::initializeCalibration(_patternPoints, _imagePoints, size, _omAll, _tAll, _K, _xi, _idx); std::vector _patternPointsTmp = _patternPoints; @@ -1112,9 +1184,9 @@ double cv::omnidir::calibrate(InputArrayOfArrays patternPoints, InputArrayOfArra } int n = (int)_patternPoints.size(); - Mat finalParam(1, 10 + 6*n, CV_64F); - Mat currentParam(1, 10 + 6*n, CV_64F); - cv::omnidir::internal::encodeParameters(_K, _omAll, _tAll, Mat::zeros(1,4,CV_64F), _xi, currentParam); + Mat finalParam(1, 11 + 6*n, CV_64F); + Mat currentParam(1, 11 + 6*n, CV_64F); + cv::omnidir::internal::encodeParameters(_K, _omAll, _tAll, Mat::zeros(1,5,CV_64F), _xi, currentParam); // optimization const double alpha_smooth = 0.01; @@ -1129,12 +1201,12 @@ double cv::omnidir::calibrate(InputArrayOfArrays patternPoints, InputArrayOfArra double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0); Mat JTJ_inv, JTError; double epsilon = 0.01 * std::pow(0.9, (double)iter/10); - cv::omnidir::internal::computeJacobian(_patternPoints, _imagePoints, currentParam, JTJ_inv, JTError, flags, epsilon); + cv::omnidir::internal::computeJacobian(_patternPoints, _imagePoints, currentParam, JTJ_inv, JTError, flags_eff, epsilon); // Gauss - Newton Mat G = alpha_smooth2*JTJ_inv * JTError; - omnidir::internal::fillFixed(G, flags, n); + omnidir::internal::fillFixed(G, flags_eff, n); finalParam = currentParam + G.t(); @@ -1147,8 +1219,6 @@ double cv::omnidir::calibrate(InputArrayOfArrays patternPoints, InputArrayOfArra } cv::omnidir::internal::decodeParameters(currentParam, _K, _omAll, _tAll, _D, _xi); - //double repr = internal::computeMeanReproErr(_patternPoints, _imagePoints, _K, _D, _xi, _omAll, _tAll); - if (omAll.needed()) { omAll.create((int)_omAll.size(), 1, CV_64FC3); @@ -1183,11 +1253,20 @@ double cv::omnidir::calibrate(InputArrayOfArrays patternPoints, InputArrayOfArra } if (D.empty()) { - D.create(1, 4, CV_64F); + D.create(1, 4, CV_64F); } + Mat Dm = D.getMat(); + CV_Assert(Dm.total() == 4 || Dm.total() == 5); Mat(_K).convertTo(K.getMat(), K.empty()? CV_64F : K.type()); - Mat(_D).convertTo(D.getMat(), D.empty() ? CV_64F: D.type()); + if (Dm.total() == 4) + { + _D.colRange(0, 4).copyTo(Dm); + } + else + { + _D.copyTo(Dm); + } if (xi.empty()) { @@ -1206,7 +1285,8 @@ double cv::omnidir::calibrate(InputArrayOfArrays patternPoints, InputArrayOfArra Vec2d std_error; double rms; Mat errors; - cv::omnidir::internal::estimateUncertainties(_patternPoints, _imagePoints, finalParam, errors, std_error, rms, flags); + cv::omnidir::internal::estimateUncertainties(_patternPoints, _imagePoints, finalParam, errors, std_error, rms, flags_eff); + return rms; } @@ -1220,6 +1300,9 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input CV_Assert(((flags & CALIB_USE_GUESS) && !K1.empty() && !D1.empty() && !K2.empty() && !D2.empty()) || !(flags & CALIB_USE_GUESS)); + const bool useK3 = (!D1.empty() && D1.total() == 5) || (!D2.empty() && D2.total() == 5); + int flags_eff = flags; + if (!useK3 ) flags_eff |= omnidir::CALIB_FIX_K3; int depth = objectPoints.depth(); std::vector _objectPoints, _imagePoints1, _imagePoints2, @@ -1238,7 +1321,7 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input } Matx33d _K1, _K2; - Matx14d _D1, _D2; + Matx15d _D1, _D2; double _xi1, _xi2; @@ -1247,7 +1330,7 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input // initializaition Mat _idx; - internal::initializeStereoCalibration(_objectPoints, _imagePoints1, _imagePoints2, imageSize1, imageSize2, _om, _T, _omL, _TL, _K1, _D1, _K2, _D2, _xi1, _xi2, flags, _idx); + internal::initializeStereoCalibration(_objectPoints, _imagePoints1, _imagePoints2, imageSize1, imageSize2, _om, _T, _omL, _TL, _K1, _D1, _K2, _D2, _xi1, _xi2, flags_eff, _idx); if(idx.needed()) { idx.create(1, (int)_idx.total(), CV_32S); @@ -1261,8 +1344,8 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input } int n = (int)_objectPointsFilt.size(); - Mat finalParam(1, 10 + 6*n, CV_64F); - Mat currentParam(1, 10 + 6*n, CV_64F); + Mat finalParam(1, 22 + 6*(n + 1), CV_64F); + Mat currentParam(1, 22 + 6*(n + 1), CV_64F); //double repr1 = internal::computeMeanReproErrStereo(_objectPoints, _imagePoints1, _imagePoints2, _K1, _K2, _D1, _D2, _xi1, _xi2, _om, // _T, _omL, _TL); @@ -1281,13 +1364,12 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input Mat JTJ_inv, JTError; double epsilon = 0.01 * std::pow(0.9, (double)iter/10); - cv::omnidir::internal::computeJacobianStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt, currentParam, - JTJ_inv, JTError, flags, epsilon); + cv::omnidir::internal::computeJacobianStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt, currentParam, JTJ_inv, JTError, flags_eff, epsilon); // Gauss - Newton Mat G = alpha_smooth2*JTJ_inv * JTError; - omnidir::internal::fillFixedStereo(G, flags, n); + omnidir::internal::fillFixedStereo(G, flags_eff, n); finalParam = currentParam + G.t(); @@ -1306,10 +1388,20 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input if (K1.empty()) { K1.create(3, 3, CV_64F); - D1.create(1, 4, CV_64F); + } + if (K2.empty()) + { K2.create(3, 3, CV_64F); - D2.create(1, 4, CV_64F); } + if (D1.empty()) + { + D1.create(1, useK3 ? 5 : 4, CV_64F); + } + if (D2.empty()) + { + D2.create(1, useK3 ? 5 : 4, CV_64F); + } + if (om.empty()) { om.create(3, 1, CV_64F); @@ -1322,9 +1414,21 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input } Mat(_K1).convertTo(K1.getMat(), K1.empty() ? CV_64F : K1.type()); - Mat(_D1).convertTo(D1.getMat(), D1.empty() ? CV_64F : D1.type()); + + { + Mat Dm = D1.getMat(); + CV_Assert(Dm.total() == 4 || Dm.total() == 5); + if (Dm.total() == 4) Mat(_D1).colRange(0, 4).copyTo(Dm); + else Mat(_D1).copyTo(Dm); + } + Mat(_K2).convertTo(K2.getMat(), K2.empty() ? CV_64F : K2.type()); - Mat(_D2).convertTo(D2.getMat(), D2.empty() ? CV_64F : D2.type()); + { + Mat Dm = D2.getMat(); + CV_Assert(Dm.total() == 4 || Dm.total() == 5); + if (Dm.total() == 4) Mat(_D2).colRange(0, 4).copyTo(Dm); + else Mat(_D2).copyTo(Dm); + } Mat(_om).convertTo(om.getMat(), om.empty() ? CV_64F: om.type()); Mat(_T).convertTo(T.getMat(), T.empty() ? CV_64F: T.type()); @@ -1375,8 +1479,8 @@ double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, Input double rms; Mat errors; - cv::omnidir::internal::estimateUncertaintiesStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt, - finalParam, errors, std_error, rms, flags); + cv::omnidir::internal::estimateUncertaintiesStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt, finalParam, errors, std_error, rms, flags_eff); + return rms; } @@ -1387,8 +1491,8 @@ void cv::omnidir::stereoReconstruct(InputArray image1, InputArray image2, InputA { CV_Assert(!K1.empty() && K1.size() == Size(3,3) && (K1.type() == CV_64F || K1.type() == CV_32F)); CV_Assert(!K2.empty() && K2.size() == Size(3,3) && (K2.type() == CV_64F || K2.type() == CV_32F)); - CV_Assert(!D1.empty() && D1.total() == 4 && (D1.type() == CV_64F || D1.type() == CV_32F)); - CV_Assert(!D2.empty() && D2.total() == 4 && (D2.type() == CV_64F || D2.type() == CV_32F)); + CV_Assert(!D1.empty() && (D1.total() == 4 || D1.total() == 5) && (D1.type() == CV_64F || D1.type() == CV_32F)); + CV_Assert(!D2.empty() && (D2.total() == 4 || D2.total() == 5) && (D2.type() == CV_64F || D2.type() == CV_32F)); CV_Assert(!R.empty() && (R.size() == Size(3,3) || R.total() == 3) && (R.type() == CV_64F || R.type() == CV_32F)); CV_Assert(!T.empty() && T.total() == 3 && (T.type() == CV_64F || T.type() == CV_32F)); CV_Assert(!image1.empty() && (image1.type() == CV_8U || image1.type() == CV_8UC3)); @@ -1541,13 +1645,17 @@ void cv::omnidir::stereoReconstruct(InputArray image1, InputArray image2, InputA void cv::omnidir::internal::encodeParameters(InputArray K, InputArrayOfArrays omAll, InputArrayOfArrays tAll, InputArray distoaration, double xi, OutputArray parameters) { CV_Assert(K.type() == CV_64F && K.size() == Size(3,3)); - CV_Assert(distoaration.total() == 4 && distoaration.type() == CV_64F); + CV_Assert(distoaration.type() == CV_64F && (distoaration.total() == 4 || distoaration.total() == 5)); int n = (int)omAll.total(); Mat _omAll = omAll.getMat(), _tAll = tAll.getMat(); Matx33d _K = K.getMat(); - Vec4d _D = (Vec4d)distoaration.getMat(); - parameters.create(1, 10+6*n,CV_64F); + Mat Dm = distoaration.getMat(); + double k1=0, k2=0, p1=0, p2=0, k3=0; + const double* d = Dm.ptr(); + k1 = d[0]; k2 = d[1]; p1 = d[2]; p2 = d[3]; + if (Dm.total() == 5) { k3 = d[4]; } + parameters.create(1, 11 + 6*n, CV_64F); Mat _params = parameters.getMat(); for (int i = 0; i < n; i++) { @@ -1561,10 +1669,11 @@ void cv::omnidir::internal::encodeParameters(InputArray K, InputArrayOfArrays om _params.at(0, 6*n+3) = _K(0,2); _params.at(0, 6*n+4) = _K(1,2); _params.at(0, 6*n+5) = xi; - _params.at(0, 6*n+6) = _D[0]; - _params.at(0, 6*n+7) = _D[1]; - _params.at(0, 6*n+8) = _D[2]; - _params.at(0, 6*n+9) = _D[3]; + _params.at(0, 6*n+6) = k1; + _params.at(0, 6*n+7) = k2; + _params.at(0, 6*n+8) = p1; + _params.at(0, 6*n+9) = p2; + _params.at(0, 6*n+10) = k3; } void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, InputArray om, InputArray T, InputArrayOfArrays omL, InputArrayOfArrays tL, @@ -1575,11 +1684,12 @@ void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, CV_Assert(!om.empty() && om.type() == CV_64F && om.total() == 3); CV_Assert(!T.empty() && T.type() == CV_64F && T.total() == 3); CV_Assert(omL.total() == tL.total() && omL.type() == CV_64FC3 && tL.type() == CV_64FC3); - CV_Assert(D1.type() == CV_64F && D1.total() == 4 && D2.type() == CV_64F && D2.total() == 4); + CV_Assert(D1.type() == CV_64F && (D1.total() == 4 || D1.total() == 5)); + CV_Assert(D2.type() == CV_64F && (D2.total() == 4 || D2.total() == 5)); int n = (int)omL.total(); // om, T, omL, tL, intrinsic left, intrinsic right - parameters.create(1, 20 + 6 * (n + 1), CV_64F); + parameters.create(1, 22 + 6 * (n + 1), CV_64F); Mat _params = parameters.getMat(); @@ -1593,29 +1703,51 @@ void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, Matx33d _K1 = K1.getMat(); Matx33d _K2 = K2.getMat(); - Vec4d _D1 = D1.getMat(); - Vec4d _D2 = D2.getMat(); - _params.at(0, 6*(n+1)) = _K1(0,0); - _params.at(0, 6*(n+1)+1) = _K1(1,1); - _params.at(0, 6*(n+1)+2) = _K1(0,1); - _params.at(0, 6*(n+1)+3) = _K1(0,2); - _params.at(0, 6*(n+1)+4) = _K1(1,2); - _params.at(0, 6*(n+1)+5) = xi1; - _params.at(0, 6*(n+1)+6) = _D1[0]; - _params.at(0, 6*(n+1)+7) = _D1[1]; - _params.at(0, 6*(n+1)+8) = _D1[2]; - _params.at(0, 6*(n+1)+9) = _D1[3]; - - _params.at(0, 6*(n+1)+10) = _K2(0,0); - _params.at(0, 6*(n+1)+11) = _K2(1,1); - _params.at(0, 6*(n+1)+12) = _K2(0,1); - _params.at(0, 6*(n+1)+13) = _K2(0,2); - _params.at(0, 6*(n+1)+14) = _K2(1,2); - _params.at(0, 6*(n+1)+15) = xi2; - _params.at(0, 6*(n+1)+16) = _D2[0]; - _params.at(0, 6*(n+1)+17) = _D2[1]; - _params.at(0, 6*(n+1)+18) = _D2[2]; - _params.at(0, 6*(n+1)+19) = _D2[3]; + double k1_1=0, k2_1=0, p1_1=0, p2_1=0, k3_1=0; + double k1_2=0, k2_2=0, p1_2=0, p2_2=0, k3_2=0; + + { + const Mat Dm = D1.getMat(); + const double* d = Dm.ptr(); + k1_1 = d[0]; k2_1 = d[1]; p1_1 = d[2]; p2_1 = d[3]; + if (Dm.total() == 5) k3_1 = d[4]; + } + { + const Mat Dm = D2.getMat(); + const double* d = Dm.ptr(); + k1_2 = d[0]; k2_2 = d[1]; p1_2 = d[2]; p2_2 = d[3]; + if (Dm.total() == 5) k3_2 = d[4]; + } + + int base1 = 6*(n+1); + + // --- Left intrinsics (11 params) + _params.at(0, base1+0) = _K1(0,0); // fx1 + _params.at(0, base1+1) = _K1(1,1); // fy1 + _params.at(0, base1+2) = _K1(0,1); // s1 + _params.at(0, base1+3) = _K1(0,2); // cx1 + _params.at(0, base1+4) = _K1(1,2); // cy1 + _params.at(0, base1+5) = xi1; // xi1 + _params.at(0, base1+6) = k1_1; + _params.at(0, base1+7) = k2_1; + _params.at(0, base1+8) = p1_1; + _params.at(0, base1+9) = p2_1; + _params.at(0, base1+10) = k3_1; + + int base2 = base1 + 11; + + // --- Right intrinsics (11 params) + _params.at(0, base2+0) = _K2(0,0); // fx2 + _params.at(0, base2+1) = _K2(1,1); // fy2 + _params.at(0, base2+2) = _K2(0,1); // s2 + _params.at(0, base2+3) = _K2(0,2); // cx2 + _params.at(0, base2+4) = _K2(1,2); // cy2 + _params.at(0, base2+5) = xi2; // xi2 + _params.at(0, base2+6) = k1_2; + _params.at(0, base2+7) = k2_2; + _params.at(0, base2+8) = p1_2; + _params.at(0, base2+9) = p2_2; + _params.at(0, base2+10) = k3_2; } @@ -1624,20 +1756,25 @@ void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, if(K.empty()) K.create(3,3,CV_64F); Matx33d _K; - int n = (int)(parameters.total()-10)/6; + int n = (int)(parameters.total() - 11) / 6; if(omAll.empty()) omAll.create(1, n, CV_64FC3); if(tAll.empty()) tAll.create(1, n, CV_64FC3); if(distoration.empty()) - distoration.create(1, 4, CV_64F); - Matx14d _D = distoration.getMat(); + distoration.create(1, 5, CV_64F); Mat param = parameters.getMat(); double *para = param.ptr(); - _K = Matx33d(para[6*n], para[6*n+2], para[6*n+3], - 0, para[6*n+1], para[6*n+4], - 0, 0, 1); - _D = Matx14d(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]); + _K = Matx33d( + para[6*n], para[6*n+2], para[6*n+3], + 0, para[6*n+1], para[6*n+4], + 0, 0, 1 + ); + + Matx15d _D( + para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9], para[6*n+10] + ); + xi = para[6*n+5]; std::vector _omAll(n), _tAll(n); for (int i = 0; i < n; i++) @@ -1645,7 +1782,16 @@ void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, _omAll[i] = Vec3d(param.colRange(i*6, i*6+3)); _tAll[i] = Vec3d(param.colRange(i*6+3, i*6+6)); } - Mat(_D).convertTo(distoration, CV_64F); + Mat Dm = distoration.getMat(); + CV_Assert(Dm.total() == 4 || Dm.total() == 5); + if (Dm.total() == 4) + { + Mat(_D).colRange(0,4).copyTo(Dm); + } + else + { + Mat(_D).copyTo(Dm); + } Mat(_K).convertTo(K, CV_64F); if (omAll.kind() == _InputArray::STD_VECTOR_MAT) @@ -1675,16 +1821,16 @@ void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, if(T.empty()) T.create(3, 1, CV_64F); - int n = ((int)parameters.total() - 20) / 6 - 1; + int n = ((int)parameters.total() - 22) / 6 - 1; if(omL.empty()) omL.create(1, n, CV_64FC3); if(tL.empty()) tL.create(1, n, CV_64FC3); - if(D1.empty()) - D1.create(1, 4, CV_64F); - if(D2.empty()) - D2.create(1, 4, CV_64F); + if (D1.empty()) + D1.create(1, 5, CV_64F); + if (D2.empty()) + D2.create(1, 5, CV_64F); Mat param = parameters.getMat().reshape(1, 1); param.colRange(0, 3).reshape(1, 3).copyTo(om.getMat()); @@ -1703,19 +1849,33 @@ void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, 0, para[offset1+1], para[offset1+4], 0, 0, 1); xi1 = para[offset1+5]; - Matx14d _D1(para[offset1+6], para[offset1+7], para[offset1+8], para[offset1+9]); - int offset2 = (n + 1)*6 + 10; + Matx15d _D1( + para[offset1+6], para[offset1+7], para[offset1+8], para[offset1+9], para[offset1+10] + ); + int offset2 = (n + 1)*6 + 11; Matx33d _K2(para[offset2], para[offset2+2], para[offset2+3], 0, para[offset2+1], para[offset2+4], 0, 0, 1); xi2 = para[offset2+5]; - Matx14d _D2(para[offset2+6], para[offset2+7], para[offset2+8], para[offset2+9]); + Matx15d _D2( + para[offset2+6], para[offset2+7], para[offset2+8], para[offset2+9], para[offset2+10] + ); Mat(_K1).convertTo(K1, CV_64F); - Mat(_D1).convertTo(D1, CV_64F); Mat(_K2).convertTo(K2, CV_64F); - Mat(_D2).convertTo(D2, CV_64F); + { + Mat Dm = D1.getMat(); + CV_Assert(Dm.total() == 4 || Dm.total() == 5); + if (Dm.total() == 4) { Mat(_D1).colRange(0, 4).copyTo(Dm); } + else { Mat(_D1).copyTo(Dm); } + } + { + Mat Dm = D2.getMat(); + CV_Assert(Dm.total() == 4 || Dm.total() == 5); + if (Dm.total() == 4) { Mat(_D2).colRange(0, 4).copyTo(Dm); } + else { Mat(_D2).copyTo(Dm); } + } if(omL.kind() == _InputArray::STD_VECTOR_MAT) { for(int i = 0; i < n; ++i) @@ -1752,7 +1912,7 @@ void cv::omnidir::internal::estimateUncertainties(InputArrayOfArrays objectPoint Matx33d K(para[6*n], para[6*n+2], para[6*n+3], 0, para[6*n+1], para[6*n+4], 0, 0, 1); - Matx14d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]); + Matx15d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9], para[6*n+10]); double xi = para[6*n+5]; int nPointsAccu = 0; @@ -1812,7 +1972,7 @@ void cv::omnidir::internal::estimateUncertaintiesStereo(InputArrayOfArrays objec CV_Assert(!imagePoints1.empty() && imagePoints1.type() == CV_64FC2 && imagePoints1.total() == objectPoints.total()); CV_Assert(!imagePoints2.empty() && imagePoints2.type() == CV_64FC2 && imagePoints1.total() == imagePoints2.total()); int n_img = (int)objectPoints.total(); - CV_Assert((int)parameters.total() == (6*(n_img+1)+20)); + CV_Assert((int)parameters.total() == (6*(n_img+1)+22)); Mat _K1, _K2, _D1, _D2; Vec3d _om, _T; @@ -2034,8 +2194,13 @@ void cv::omnidir::internal::subMatrix(const Mat& src, Mat& dst, const std::vecto void cv::omnidir::internal::flags2idx(int flags, std::vector& idx, int n) { - idx = std::vector(6*n+10,1); + idx = std::vector(6*n+11, 1); int _flags = flags; + if (_flags & omnidir::CALIB_FIX_K3) + { + idx[6*n + 10] = 0; + _flags &= ~omnidir::CALIB_FIX_K3; + } if(_flags >= omnidir::CALIB_FIX_CENTER) { idx[6*n+3] = 0; @@ -2081,10 +2246,16 @@ void cv::omnidir::internal::flags2idx(int flags, std::vector& idx, int n) void cv::omnidir::internal::flags2idxStereo(int flags, std::vector& idx, int n) { - idx = std::vector(6*(n+1)+20, 1); + idx = std::vector(6*(n+1)+22, 1); int _flags = flags; int offset1 = 6*(n+1); - int offset2 = offset1 + 10; + int offset2 = offset1 + 11; + if (_flags & omnidir::CALIB_FIX_K3) + { + idx[offset1 + 10] = 0; + idx[offset2 + 10] = 0; + _flags &= ~omnidir::CALIB_FIX_K3; + } if(_flags >= omnidir::CALIB_FIX_CENTER) { idx[offset1+3] = 0; @@ -2142,11 +2313,11 @@ void cv::omnidir::internal::flags2idxStereo(int flags, std::vector& idx, in void cv::omnidir::internal::fillFixed(Mat&G, int flags, int n) { Mat tmp = G.clone(); - std::vector idx(6*n + 10, 1); + std::vector idx(6*n + 11, 1); flags2idx(flags, idx, n); G.release(); - G.create(6*n +10, 1, CV_64F); - G = cv::Mat::zeros(6*n +10, 1, CV_64F); + G.create(6*n + 11, 1, CV_64F); + G = cv::Mat::zeros(6*n + 11, 1, CV_64F); for (int i = 0,j=0; i < (int)idx.size(); i++) { if (idx[i]) @@ -2159,11 +2330,11 @@ void cv::omnidir::internal::fillFixed(Mat&G, int flags, int n) void cv::omnidir::internal::fillFixedStereo(Mat& G, int flags, int n) { Mat tmp = G.clone(); - std::vector idx(6*(n+1)+20, 1); + std::vector idx(6*(n+1)+22, 1); flags2idxStereo(flags, idx, n); G.release(); - G.create(6 * (n+1) + 20, 1, CV_64F); - G = cv::Mat::zeros(6* (n + 1) + 20, 1, CV_64F); + G.create(6 * (n+1) + 22, 1, CV_64F); + G = cv::Mat::zeros(6* (n + 1) + 22, 1, CV_64F); for (int i = 0,j=0; i < (int)idx.size(); i++) { if (idx[i]) diff --git a/modules/ccalib/test/CMakeLists.txt b/modules/ccalib/test/CMakeLists.txt new file mode 100644 index 00000000000..31b06da9d1c --- /dev/null +++ b/modules/ccalib/test/CMakeLists.txt @@ -0,0 +1,2 @@ +set(the_description "OpenCV ccalib accuracy tests (contrib)") +ocv_add_accuracy_tests() diff --git a/modules/ccalib/test/test_main.cpp b/modules/ccalib/test/test_main.cpp new file mode 100644 index 00000000000..cfd82a5bb01 --- /dev/null +++ b/modules/ccalib/test/test_main.cpp @@ -0,0 +1,2 @@ +#include +CV_TEST_MAIN("ccalib") diff --git a/modules/ccalib/test/test_omnidir_k3.cpp b/modules/ccalib/test/test_omnidir_k3.cpp new file mode 100644 index 00000000000..168121c94a3 --- /dev/null +++ b/modules/ccalib/test/test_omnidir_k3.cpp @@ -0,0 +1,1221 @@ +#include "test_precomp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace opencv_test { + +using namespace cv; +using std::vector; +using std::string; + +static std::string getTestDataRoot() +{ + std::string root = cvtest::TS::ptr()->get_data_path(); + if (!root.empty() && root.back() != '/') + { + root.push_back('/'); + } + const char suffix[] = "ccalib/"; + constexpr size_t sufLen = sizeof(suffix) - 1; // 7 + if (root.size() >= sufLen && root.compare(root.size() - sufLen, sufLen, suffix) == 0) + { + root.erase(root.size() - sufLen); + } + return root; +} + +static std::string getDataPath() +{ + return getTestDataRoot() + "cv/stereo/case1/"; +} + +static vector getLeftImageFilenames() +{ + const string dir = getDataPath(); + vector filenames; + cv::glob(dir + "left*.png", filenames, false); + std::sort(filenames.begin(), filenames.end()); + + if (filenames.size() < 5) + { + throw cvtest::SkipTestException( + "opencv_extra stereo/case1 data not found (need >= 5 left*.png). " + "Expected: /cv/stereo/case1/left*.png"); + } + return filenames; +} + +static vector getRightImageFilenames() +{ + const string dir = getDataPath(); + vector filenames; + cv::glob(dir + "right*.png", filenames, false); + std::sort(filenames.begin(), filenames.end()); + + if (filenames.size() < 5) + { + throw cvtest::SkipTestException( + "opencv_extra stereo/case1 data not found (need >= 5 right*.png). " + "Expected: /cv/stereo/case1/right*.png"); + } + return filenames; +} + +// Extract a numeric "index" from a filename like ".../left01.png" or ".../right12.png". +// We just take the last contiguous digit run in the stem. +static int extractLastNumberFromPath(const cv::String& path) +{ + // basename without extension + string s(path.c_str()); + // Remove directory + size_t slash = s.find_last_of("/\\"); + string base = (slash == string::npos) ? s : s.substr(slash + 1); + // Remove extension + size_t dot = base.find_last_of('.'); + string stem = (dot == string::npos) ? base : base.substr(0, dot); + + // Find last digit run + int end = (int)stem.size() - 1; + while (end >= 0 && !std::isdigit((unsigned char)stem[(size_t)end])) end--; + if (end < 0) return -1; + + int start = end; + while (start >= 0 && std::isdigit((unsigned char)stem[(size_t)start])) start--; + start++; + + try { + return std::stoi(stem.substr((size_t)start, (size_t)(end - start + 1))); + } catch (...) { + return -1; + } +} + +// Builds a map: index -> filepath for all files matching pattern. +static std::map indexFiles(const vector& filenames) +{ + std::map m; + for (const auto& filename : filenames) + { + int idx = extractLastNumberFromPath(filename); + if (idx < 0) continue; + // If duplicates exist, keep first (or overwrite; doesn't matter much) + if (!m.count(idx)) m[idx] = filename; + } + return m; +} + +static vector makeChessboard3D(Size board, float square) +{ + vector obj; + obj.reserve((size_t)board.area()); + for (int y = 0; y < board.height; ++y) + for (int x = 0; x < board.width; ++x) + obj.emplace_back((float)x * square, (float)y * square, 0.f); + return obj; +} + +static bool detectCorners(const Mat& gray, Size board, vector& corners) +{ + bool ok = findChessboardCorners(gray, board, corners,CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE); + if (!ok) + { + return false; + } + cornerSubPix(gray, corners,Size(11,11), Size(-1,-1),TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 50, 1e-4)); + return true; +} + +// Compute reprojection RMS using omnidir::projectPoints. +// If idx is non-empty, only the used views are evaluated (calibrate may reject some). +static double computeReprojRms(const vector>& objectPoints, + const vector>& imagePoints, + const Mat& K, double xi, const Mat& D, + const vector& rvecs, const vector& tvecs, + const Mat& idx) +{ + CV_Assert(objectPoints.size() == imagePoints.size()); + CV_Assert(rvecs.size() == tvecs.size()); + const bool useIdx = !idx.empty(); + const size_t nUsed = useIdx ? (size_t)idx.total() : objectPoints.size(); + CV_Assert(rvecs.size() == nUsed); + Mat idxRow; + if (useIdx) + { + idxRow = idx.reshape(1, 1); + if (idxRow.type() != CV_32S) + { + idxRow.convertTo(idxRow, CV_32S); + } + } + double sse = 0.0; + size_t n = 0; + for (size_t i = 0; i < nUsed; ++i) + { + const int view = useIdx ? idxRow.at((int)i) : (int)i; + vector proj; + cv::omnidir::projectPoints(objectPoints[(size_t)view], proj, rvecs[i], tvecs[i], K, xi, D); + CV_Assert(proj.size() == imagePoints[(size_t)view].size()); + for (size_t j = 0; j < proj.size(); ++j) + { + const double dx = (double)proj[j].x - (double)imagePoints[(size_t)view][j].x; + const double dy = (double)proj[j].y - (double)imagePoints[(size_t)view][j].y; + sse += dx*dx + dy*dy; + } + n += proj.size(); + } + return (n > 0) ? std::sqrt(sse / (double)n) : 0.0; +} + +static void loadRealData(const vector& filenames, + Size board, float square, + vector>& objectPoints, + vector>& imagePoints, + Size& imageSize) +{ + objectPoints.clear(); + imagePoints.clear(); + imageSize = Size(); + const auto obj = makeChessboard3D(board, square); + for (const auto& filename : filenames) + { + Mat img = imread(filename, IMREAD_GRAYSCALE); + if (img.empty()) + { + continue; + } + if (imageSize.empty()) + { + imageSize = img.size(); + } + vector corners; + if (!detectCorners(img, board, corners)) + { + continue; + } + objectPoints.push_back(obj); + imagePoints.push_back(corners); + } + if (objectPoints.size() < 5) + { + throw cvtest::SkipTestException("Not enough valid chessboard detections in: " + getDataPath()); + } +} + +// Loads stereo chessboard observations from opencv_extra testdata (case1). +// Pairs left/right images by the numeric index in the filename, detects corners in both, +// and fills objectPoints + imagePointsL/R for stereoCalibrate. +static void loadStereoRealDataPairs(const vector& leftFilenames, + const vector& rightFilenames, + Size board, float square, + vector>& objectPoints, + vector>& imagePointsL, + vector>& imagePointsR, + Size& imageSizeL, + Size& imageSizeR) +{ + auto L = indexFiles(leftFilenames); + auto R = indexFiles(rightFilenames); + + // Intersect indices + vector common; + common.reserve(std::min(L.size(), R.size())); + for (const auto& kv : L) + { + if (R.count(kv.first)) + { + common.push_back(kv.first); + } + } + if (common.size() < 5) + { + throw cvtest::SkipTestException("Not enough left/right pairs with matching indices."); + } + std::sort(common.begin(), common.end()); + objectPoints.clear(); + imagePointsL.clear(); + imagePointsR.clear(); + imageSizeL = Size(); + imageSizeR = Size(); + const auto obj = makeChessboard3D(board, square); + for (int id : common) + { + Mat imgL = imread(L[id], IMREAD_GRAYSCALE); + Mat imgR = imread(R[id], IMREAD_GRAYSCALE); + if (imgL.empty() || imgR.empty()) + { + continue; + } + if (imageSizeL.empty()) + { + imageSizeL = imgL.size(); + } + if (imageSizeR.empty()) + { + imageSizeR = imgR.size(); + } + // If your dataset is guaranteed same size, assert: + if (imgL.size() != imageSizeL || imgR.size() != imageSizeR) + { + continue; + } + vector cL, cR; + if (!detectCorners(imgL, board, cL)) continue; + if (!detectCorners(imgR, board, cR)) continue; + + objectPoints.push_back(obj); + imagePointsL.push_back(std::move(cL)); + imagePointsR.push_back(std::move(cR)); + } + if (objectPoints.size() < 5) + throw cvtest::SkipTestException("Not enough valid stereo chessboard detections."); +} + +// Synthetic data generator (deterministic, k3 observable) +static void synthesizeOmniData(int nViews, + const vector& obj, + const Size& imageSize, + const Matx33d& Ktrue, double xiTrue, + const Mat& Dtrue, + vector>& objectPoints, + vector>& imagePoints, + double noiseSigmaPx = 0.25, + uint64 seed = 0xBEEF) +{ + RNG rng((uint64)seed); + objectPoints.clear(); + imagePoints.clear(); + objectPoints.reserve((size_t)nViews); + imagePoints.reserve((size_t)nViews); + + const double cx = Ktrue(0,2); + const double cy = Ktrue(1,2); + const int margin = 8; // pixels + + int tries = 0; + const int maxTries = 20000; + + while ((int)imagePoints.size() < nViews && tries++ < maxTries) + { + // Make the board large in the image -> k3 becomes observable + // Smaller z => larger projection radius (but keep it in front) + Vec3d r(rng.uniform(-0.35, 0.35), + rng.uniform(-0.35, 0.35), + rng.uniform(-0.35, 0.35)); + + Vec3d t(rng.uniform(-0.25, 0.25), + rng.uniform(-0.25, 0.25), + rng.uniform(0.55, 0.95)); + + Mat rvec(r), tvec(t); + vector img; + cv::omnidir::projectPoints(obj, img, rvec, tvec, Ktrue, xiTrue, Dtrue); + // Validate: finite + inside image + bool ok = true; + double maxR = 0.0; + for (const auto& p : img) + { + if (!std::isfinite(p.x) || !std::isfinite(p.y)) + { + ok = false; break; + } + if (p.x < margin || p.y < margin || p.x > imageSize.width - 1 - margin || p.y > imageSize.height - 1 - margin) + { + ok = false; break; + } + const double dx = (double)p.x - cx; + const double dy = (double)p.y - cy; + maxR = std::max(maxR, std::sqrt(dx*dx + dy*dy)); + } + // Enforce radius coverage: need points close to edges + // (tune threshold if needed) + const double desiredR = 0.38 * std::min(imageSize.width, imageSize.height); + if (!ok || maxR < desiredR) + { + continue; + } + // Add pixel noise + for (auto& p : img) + { + p.x += (float)rng.gaussian(noiseSigmaPx); + p.y += (float)rng.gaussian(noiseSigmaPx); + } + objectPoints.push_back(obj); + imagePoints.push_back(std::move(img)); + } + if ((int)imagePoints.size() < nViews) + { + CV_Error(cv::Error::StsError, + "synthesizeOmniData: Could not generate enough valid views (k3 observable). " + "Try increasing maxTries or relaxing constraints."); + } +} + +// Synthetic stereo generator (deterministic). +// Produces matched left/right observations for stereoCalibrate() with k3 observable +// (forces large radius coverage) + optional Gaussian noise. +static void synthesizeStereoOmniData(int nViews, + const std::vector& obj, + const cv::Size& imageSize, + const cv::Matx33d& K1true, double xi1True, const cv::Mat& D1true, + const cv::Matx33d& K2true, double xi2True, const cv::Mat& D2true, + const cv::Mat& omRig, const cv::Mat& TRig, // 3x1 om, 3x1 T (rig: cam2 = rig * cam1) + std::vector>& objectPoints, + std::vector>& imagePointsL, + std::vector>& imagePointsR, + double noiseSigmaPx = 0.25, + uint64 seed = 0xBEEF) +{ + CV_Assert(omRig.total() == 3 && TRig.total() == 3); + CV_Assert(D1true.total() == 5 && D2true.total() == 5); + + cv::RNG rng((uint64)seed); + + objectPoints.clear(); + imagePointsL.clear(); + imagePointsR.clear(); + + objectPoints.reserve((size_t)nViews); + imagePointsL.reserve((size_t)nViews); + imagePointsR.reserve((size_t)nViews); + + const double cx1 = K1true(0,2), cy1 = K1true(1,2); + const double cx2 = K2true(0,2), cy2 = K2true(1,2); + + const int margin = 8; + int tries = 0; + const int maxTries = 40000; + + // Precompute rig rotation matrix + cv::Mat Rrig; + cv::Rodrigues(omRig, Rrig); + Rrig.convertTo(Rrig, CV_64F); + cv::Mat Trig64 = TRig.reshape(1,3); + Trig64.convertTo(Trig64, CV_64F); + + while ((int)imagePointsL.size() < nViews && tries++ < maxTries) + { + // Random board pose in LEFT camera coordinates + cv::Vec3d r(rng.uniform(-0.35, 0.35), + rng.uniform(-0.35, 0.35), + rng.uniform(-0.35, 0.35)); + cv::Vec3d t(rng.uniform(-0.25, 0.25), + rng.uniform(-0.25, 0.25), + rng.uniform(0.55, 0.95)); + + cv::Mat rvecL(r), tvecL(t); + + // Derive RIGHT pose: Xc2 = Rrig * Xc1 + Trig + cv::Mat RL; + cv::Rodrigues(rvecL, RL); + cv::Mat RR = Rrig * RL; + cv::Mat tR = Rrig * tvecL.reshape(1,3) + Trig64; + + cv::Mat rvecR; + cv::Rodrigues(RR, rvecR); + + std::vector imgL, imgR; + cv::omnidir::projectPoints(obj, imgL, rvecL, tvecL, K1true, xi1True, D1true); + cv::omnidir::projectPoints(obj, imgR, rvecR, tR, K2true, xi2True, D2true); + + bool ok = true; + double maxR1 = 0.0, maxR2 = 0.0; + + for (size_t i = 0; i < imgL.size(); ++i) + { + const auto& pL = imgL[i]; + const auto& pR = imgR[i]; + + if (!std::isfinite(pL.x) || !std::isfinite(pL.y) || + !std::isfinite(pR.x) || !std::isfinite(pR.y)) + { ok = false; break; } + + if (pL.x < margin || pL.y < margin || + pL.x > imageSize.width - 1 - margin || + pL.y > imageSize.height - 1 - margin) + { ok = false; break; } + + if (pR.x < margin || pR.y < margin || + pR.x > imageSize.width - 1 - margin || + pR.y > imageSize.height - 1 - margin) + { ok = false; break; } + + const double dx1 = (double)pL.x - cx1, dy1 = (double)pL.y - cy1; + const double dx2 = (double)pR.x - cx2, dy2 = (double)pR.y - cy2; + maxR1 = std::max(maxR1, std::sqrt(dx1*dx1 + dy1*dy1)); + maxR2 = std::max(maxR2, std::sqrt(dx2*dx2 + dy2*dy2)); + } + + // Need strong radius coverage (k3 observability) in BOTH cameras + const double desiredR = 0.38 * std::min(imageSize.width, imageSize.height); + if (!ok || maxR1 < desiredR || maxR2 < desiredR) + continue; + + // Add noise (deterministic) + for (auto& p : imgL) { p.x += (float)rng.gaussian(noiseSigmaPx); p.y += (float)rng.gaussian(noiseSigmaPx); } + for (auto& p : imgR) { p.x += (float)rng.gaussian(noiseSigmaPx); p.y += (float)rng.gaussian(noiseSigmaPx); } + + objectPoints.push_back(obj); + imagePointsL.push_back(std::move(imgL)); + imagePointsR.push_back(std::move(imgR)); + } + + if ((int)imagePointsL.size() < nViews) + { + CV_Error(cv::Error::StsError, + "synthesizeStereoOmniData: Could not generate enough valid stereo views (k3 observable)."); + } +} + +// Real-data sanity test for the 5-parameter omnidir distortion model (k1,k2,p1,p2,k3): +// - Runs omnidir::calibrate on opencv_extra cv/stereo/case1 (left*.png) detections. +// - Checks returned parameters are finite and within broad sanity ranges. +// - Verifies undistort/rectify maps can be built with D(1x5). +// Cross-checks the reported RMS by reprojecting points with omnidir::projectPoints. +TEST(Omnidir_K3_Hard, RealData_5params_K3_Free_StabilityAndSanity) +{ + const Size board(9,6); + const float square = 0.04f; + std::vector> objPts; + std::vector> imgPts; + Size imageSize; + const vector filenames = getLeftImageFilenames(); + loadRealData(filenames, board, square, objPts, imgPts, imageSize); + // 5-parameter model (k3 included) + Mat K = Mat::eye(3,3,CV_64F); + Mat xi(1,1,CV_64F); xi.at(0,0) = 1.0; + Mat D5(1,5,CV_64F, Scalar(0)); + std::vector rvecs, tvecs; + Mat idx; + const int flags = omnidir::CALIB_FIX_SKEW; // keep it comparable to your other tests + TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, 300, 1e-9); + double rms = omnidir::calibrate(objPts, imgPts, imageSize, K, xi, D5, rvecs, tvecs, flags, criteria, idx); + // Must have used some views + ASSERT_GT((int)idx.total(), 0); + ASSERT_EQ((int)D5.total(), 5); + // maps should be constructible with 5-param distortion (k3 included) + Mat xiMat(1,1,CV_64F); + xiMat.at(0,0) = xi.at(0,0); + Mat map1, map2; + omnidir::initUndistortRectifyMap(K, D5, xiMat, Matx33d::eye(), K, imageSize, CV_32FC1, map1, map2, omnidir::RECTIFY_PERSPECTIVE); + EXPECT_FALSE(map1.empty()); + EXPECT_FALSE(map2.empty()); + // RMS sanity (broad but meaningful) + EXPECT_TRUE(std::isfinite(rms)); + EXPECT_GT(rms, 0.0); + EXPECT_LT(rms, 5.0); + // Intrinsics sanity + const double fx = K.at(0,0); + const double fy = K.at(1,1); + const double cx = K.at(0,2); + const double cy = K.at(1,2); + const double xiv = xi.at(0,0); + + EXPECT_TRUE(std::isfinite(fx)); + EXPECT_TRUE(std::isfinite(fy)); + EXPECT_TRUE(std::isfinite(cx)); + EXPECT_TRUE(std::isfinite(cy)); + EXPECT_TRUE(std::isfinite(xiv)); + + EXPECT_GT(fx, 0.0); + EXPECT_GT(fy, 0.0); + EXPECT_GE(cx, 0.0); + EXPECT_GE(cy, 0.0); + EXPECT_LT(cx, imageSize.width); + EXPECT_LT(cy, imageSize.height); + + // xi should not explode + EXPECT_LT(std::abs(xiv), 10.0); + + // Distortion sanity (all finite, not insane) + for (int i = 0; i < 5; ++i) + { + EXPECT_TRUE(std::isfinite(D5.at(0,i))); + } + // External reprojection RMS cross-check (optional but good) + const double reproj = computeReprojRms(objPts, imgPts, K, xiv, D5, rvecs, tvecs, idx); + EXPECT_TRUE(std::isfinite(reproj)); + EXPECT_GT(reproj, 0.0); + EXPECT_LT(reproj, 5.0); + + // Reproj RMS should be close-ish to internal RMS (loose tolerance) + EXPECT_LE(std::abs(reproj - rms), std::max(0.05, 0.02 * rms)); +} + +// Real-data regression (4-parameter legacy model): +// Calibrates with 4 distortion coefficients on opencv_extra stereo/case1 and compares K/xi/D4 +// against pre-recorded "golden" values to catch unintended behavior changes (backward stability). +TEST(Omnidir_K3_Hard, RealData_GoldenCoefficients_4params) +{ + const Size board(9,6); + const float square = 0.04f; + vector> objPts; + vector> imgPts; + Size imageSize; + const vector filenames = getLeftImageFilenames(); + loadRealData(filenames, board, square, objPts, imgPts, imageSize); + Mat K = Mat::eye(3,3,CV_64F); + Mat xi = Mat(1,1,CV_64F); xi.at(0,0) = 1.0; + Mat D4 = Mat(1,4,CV_64F, Scalar(0)); + vector r, t; + Mat idx; + const int flags = omnidir::CALIB_FIX_SKEW; + TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, 250, 1e-8); + double rms = omnidir::calibrate(objPts, imgPts, imageSize, K, xi, D4, r, t, flags, criteria, idx); + + ASSERT_GT((int)idx.total(), 0); + ASSERT_EQ((int)D4.total(), 4); + + // --- GOLDEN VALUES (FILL THESE FROM BASELINE RUN) --- + + const double GOLD_fx = 978.1963596766802; + const double GOLD_fy = 978.1142764489673; + const double GOLD_cx = 342.3670276970017; + const double GOLD_cy = 235.6861159367739; + const double GOLD_xi = 0.8234519365247485; + const double GOLD_d0 = -0.1760250991330968; + const double GOLD_d1 = -0.8070188169070457; + const double GOLD_d2 = 0.003591801582976898; + const double GOLD_d3 = -0.000695840037846224; + + // Tolerances: keep stable across platforms + const double TOL_f = 8.0; // pixels + const double TOL_c = 8.0; // pixels + const double TOL_xi = 0.08; // unitless + const double TOL_d = 0.05; // distortion coeffs + + EXPECT_NEAR(K.at(0,0), GOLD_fx, TOL_f); + EXPECT_NEAR(K.at(1,1), GOLD_fy, TOL_f); + EXPECT_NEAR(K.at(0,2), GOLD_cx, TOL_c); + EXPECT_NEAR(K.at(1,2), GOLD_cy, TOL_c); + EXPECT_NEAR(xi.at(0,0), GOLD_xi, TOL_xi); + EXPECT_NEAR(D4.at(0,0), GOLD_d0, TOL_d); + EXPECT_NEAR(D4.at(0,1), GOLD_d1, TOL_d); + EXPECT_NEAR(D4.at(0,2), GOLD_d2, TOL_d); + EXPECT_NEAR(D4.at(0,3), GOLD_d3, TOL_d); + + // Sanity: RMS reasonable and finite. + EXPECT_TRUE(std::isfinite(rms)); + EXPECT_LT(rms, 5.0); +} + +// Backward compatibility check when k3==0: +// Verifies that using D4 and D5=[D4,k3=0] produces identical: +// - projections (projectPoints), +// - undistorted points (undistortPoints), +// - undistort/rectify maps (initUndistortRectifyMap), +// within tight tolerances. +TEST(Omnidir_K3_Hard, BackwardCompat_4coeffs_equals_5coeffs_k3_zero_project_and_maps) +{ + const Matx33d K(520, 0, 320, + 0, 520, 240, + 0, 0, 1); + const double xi = 1.2; + + const Size sz(640, 480); + Mat xiMat(1, 1, CV_64F); + xiMat.at(0, 0) = xi; + + Mat D4(1, 4, CV_64F); + D4.at(0, 0) = -0.10; + D4.at(0, 1) = 0.01; + D4.at(0, 2) = 0.001; + D4.at(0, 3) = -0.0005; + + Mat D5(1, 5, CV_64F); + for (int i = 0; i < 4; ++i) D5.at(0, i) = D4.at(0, i); + D5.at(0, 4) = 0.0; // k3 = 0 + + // --- projectPoints equality --- + vector obj = makeChessboard3D(Size(9, 6), 0.04f); + Mat rvec = (Mat_(3, 1) << 0.2, -0.1, 0.05); + Mat tvec = (Mat_(3, 1) << 0.40, -0.25, 1.80); + + vector p4, p5; + cv::omnidir::projectPoints(obj, p4, rvec, tvec, K, xi, D4); + cv::omnidir::projectPoints(obj, p5, rvec, tvec, K, xi, D5); + + ASSERT_EQ(p4.size(), p5.size()); + + double maxAbs = 0.0; + for (size_t i = 0; i < p4.size(); ++i) + { + maxAbs = std::max(maxAbs, std::abs((double)p4[i].x - (double)p5[i].x)); + maxAbs = std::max(maxAbs, std::abs((double)p4[i].y - (double)p5[i].y)); + } + EXPECT_LT(maxAbs, 1e-8); + + // --- undistortPoints equality (k3=0) --- + // Use distorted points generated with the legacy (D4) model. + vector distorted; + cv::omnidir::projectPoints(obj, distorted, rvec, tvec, K, xi, D4); + + Mat und4, und5; + cv::omnidir::undistortPoints(distorted, und4, K, D4, xiMat, Matx33d::eye()); + cv::omnidir::undistortPoints(distorted, und5, K, D5, xiMat, Matx33d::eye()); + + ASSERT_FALSE(und4.empty()); + ASSERT_FALSE(und5.empty()); + ASSERT_EQ(und4.total(), und5.total()); + ASSERT_EQ(und4.type(), und5.type()); + + EXPECT_LT(cv::norm(und4, und5, cv::NORM_INF), 1e-8); + + // --- initUndistortRectifyMap equality --- + Mat m1_4, m2_4, m1_5, m2_5; + cv::omnidir::initUndistortRectifyMap(K, D4, xiMat, Matx33d::eye(), K, sz, CV_32FC1, m1_4, m2_4, cv::omnidir::RECTIFY_PERSPECTIVE); + cv::omnidir::initUndistortRectifyMap(K, D5, xiMat, Matx33d::eye(), K, sz, CV_32FC1, m1_5, m2_5, cv::omnidir::RECTIFY_PERSPECTIVE); + + ASSERT_FALSE(m1_4.empty()); + ASSERT_FALSE(m2_4.empty()); + ASSERT_FALSE(m1_5.empty()); + ASSERT_FALSE(m2_5.empty()); + + EXPECT_LT(cv::norm(m1_4, m1_5, cv::NORM_INF), 1e-6); + EXPECT_LT(cv::norm(m2_4, m2_5, cv::NORM_INF), 1e-6); +} + +// Deterministic synthetic validation of k3 parameter: +// Generates image points using known K/xi/Dtrue with non-zero k3, then calibrates with k3 fixed vs free. +// Ensures free-k3 improves reprojection RMS and recovers k3 close to the ground-truth value. +TEST(Omnidir_K3_Hard, Synthetic_Recovers_K3_and_Improves_Fit_vs_4params) +{ + const Size imageSize(640, 480); + const Size board(9, 6); + const float square = 0.04f; + const int nViews = 20; + + const Matx33d Ktrue(520, 0, 320, + 0, 520, 240, + 0, 0, 1); + const double xiTrue = 1.15; + + Mat Dtrue(1, 5, CV_64F); + Dtrue.at(0,0) = -0.10; // k1 + Dtrue.at(0,1) = 0.01; // k2 + Dtrue.at(0,2) = 0.001; // p1 + Dtrue.at(0,3) = -0.0005; // p2 + Dtrue.at(0,4) = -0.2; // k3 + + vector obj = makeChessboard3D(board, square); + vector> objectPoints; + vector> imagePoints; + + synthesizeOmniData(nViews, obj, imageSize, Ktrue, xiTrue, Dtrue, + objectPoints, imagePoints, + /*noiseSigmaPx=*/0.02, /*seed=*/0xCAFE); + + TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, 300, 1e-9); + + const int flags_common = + cv::omnidir::CALIB_USE_GUESS | + cv::omnidir::CALIB_FIX_XI | + cv::omnidir::CALIB_FIX_K1 | + cv::omnidir::CALIB_FIX_K2 | + cv::omnidir::CALIB_FIX_P1 | + cv::omnidir::CALIB_FIX_P2 | + cv::omnidir::CALIB_FIX_SKEW | + cv::omnidir::CALIB_FIX_CENTER | + cv::omnidir::CALIB_FIX_GAMMA; + + // Run A: k3 FIXED to 0 + Mat K_A = Mat(Ktrue).clone(); + Mat xi_A(1,1,CV_64F); xi_A.at(0,0) = xiTrue; + + Mat D_A = Dtrue.clone(); + D_A.at(0,4) = 0.0; + + vector rA, tA; + Mat idxA; + + const int flags_A = flags_common | cv::omnidir::CALIB_FIX_K3; + double rms_A = cv::omnidir::calibrate(objectPoints, imagePoints, imageSize, K_A, xi_A, D_A, rA, tA, flags_A, criteria, idxA); + + // Run B: k3 FREE (start from 0 and must recover) + Mat K_B = Mat(Ktrue).clone(); + Mat xi_B(1,1,CV_64F); xi_B.at(0,0) = xiTrue; + Mat D_B = Dtrue.clone(); + D_B.at(0,4) = 0.0; + vector rB, tB; + Mat idxB; + const int flags_B = flags_common; // no FIX_K3 + double rms_B = cv::omnidir::calibrate(objectPoints, imagePoints, imageSize, K_B, xi_B, D_B, rB, tB, flags_B, criteria, idxB); + ASSERT_GT((int)idxA.total(), 0); + ASSERT_GT((int)idxB.total(), 0); + + // 1) internal RMS: B should not be worse than A (small slack) + EXPECT_LE(rms_B, rms_A + 1e-12); + + // 2) external reprojection RMS: B must be better than A (strong check) + const double reproj_A = computeReprojRms(objectPoints, imagePoints, K_A, xiTrue, D_A, rA, tA, idxA); + const double reproj_B = computeReprojRms(objectPoints, imagePoints, K_B, xiTrue, D_B, rB, tB, idxB); + EXPECT_LT(reproj_B, reproj_A - 1e-4); + + // k3 sanity + recovery + Mat dB = D_B.reshape(1, 1); + ASSERT_EQ((int)dB.total(), 5); + + const double k3_est = dB.at(0,4); + EXPECT_TRUE(std::isfinite(k3_est)); + EXPECT_LT(std::abs(k3_est), 5.0); + EXPECT_NEAR(k3_est, Dtrue.at(0,4), 0.2); + EXPECT_GT(std::abs(k3_est), 1e-4); +} + +// Stereo backward compatibility (real data): +// D5 with k3 fixed to 0 should behave similarly to the legacy D4 model. +// We check that calibration succeeds and yields comparable RMS and (k1,k2,p1,p2). +TEST(Omnidir_K3_Hard, Stereo_RealData_BackwardCompat_D4_equals_D5_k3_zero) +{ + const Size board(9,6); + const float square = 0.04f; + vector> objPts; + vector> imgL, imgR; + Size szL, szR; + const vector leftFilenames = getLeftImageFilenames(); + const vector rightFilenames = getRightImageFilenames(); + loadStereoRealDataPairs(leftFilenames, rightFilenames, board, square, objPts, imgL, imgR, szL, szR); + // --- Run A: legacy D4 model --- + Mat K1_A = Mat::eye(3,3,CV_64F); + Mat K2_A = Mat::eye(3,3,CV_64F); + Mat xi1_A(1,1,CV_64F); xi1_A.at(0,0) = 1.0; + Mat xi2_A(1,1,CV_64F); xi2_A.at(0,0) = 1.0; + + Mat D1_A(1,4,CV_64F, Scalar(0)); + Mat D2_A(1,4,CV_64F, Scalar(0)); + + Mat om_A, T_A; + vector rvecsL_A, tvecsL_A; + Mat idx_A; + + const int flagsA = cv::omnidir::CALIB_FIX_SKEW; + TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, 250, 1e-8); + + double rms_A = cv::omnidir::stereoCalibrate(objPts, imgL, imgR, szL, szR, + K1_A, xi1_A, D1_A, + K2_A, xi2_A, D2_A, + om_A, T_A, rvecsL_A, tvecsL_A, + flagsA, criteria, idx_A); + + ASSERT_GT((int)idx_A.total(), 0); + ASSERT_EQ((int)D1_A.total(), 4); + ASSERT_EQ((int)D2_A.total(), 4); + + // --- Run B: D5 model but k3 FIXED to 0 --- + Mat K1_B = Mat::eye(3,3,CV_64F); + Mat K2_B = Mat::eye(3,3,CV_64F); + Mat xi1_B(1,1,CV_64F); xi1_B.at(0,0) = 1.0; + Mat xi2_B(1,1,CV_64F); xi2_B.at(0,0) = 1.0; + + Mat D1_B(1,5,CV_64F, Scalar(0)); + Mat D2_B(1,5,CV_64F, Scalar(0)); + D1_B.at(0,4) = 0.0; // k3 fixed to 0 + D2_B.at(0,4) = 0.0; + + Mat om_B, T_B; + vector rvecsL_B, tvecsL_B; + Mat idx_B; + + const int flagsB = cv::omnidir::CALIB_FIX_SKEW | cv::omnidir::CALIB_FIX_K3; + + double rms_B = cv::omnidir::stereoCalibrate(objPts, imgL, imgR, szL, szR, + K1_B, xi1_B, D1_B, + K2_B, xi2_B, D2_B, + om_B, T_B, rvecsL_B, tvecsL_B, + flagsB, criteria, idx_B); + + ASSERT_GT((int)idx_B.total(), 0); + ASSERT_EQ((int)D1_B.total(), 5); + ASSERT_EQ((int)D2_B.total(), 5); + + // --- Expectations (loose but meaningful) --- + EXPECT_TRUE(std::isfinite(rms_A)); + EXPECT_TRUE(std::isfinite(rms_B)); + EXPECT_LT(rms_A, 10.0); + EXPECT_LT(rms_B, 10.0); + + // RMS should be close-ish (since model is effectively identical) + EXPECT_LT(std::abs(rms_A - rms_B), 0.5); + + // k3 must remain exactly (or nearly) zero because FIX_K3 is set + EXPECT_NEAR(D1_B.at(0,4), 0.0, 1e-12); + EXPECT_NEAR(D2_B.at(0,4), 0.0, 1e-12); + + // Distortion first 4 coefficients should be comparable (loose tolerance) + const double TOL_D = 0.3; + for (int i = 0; i < 4; ++i) + { + EXPECT_LT(std::abs(D1_A.at(0,i) - D1_B.at(0,i)), TOL_D); + EXPECT_LT(std::abs(D2_A.at(0,i) - D2_B.at(0,i)), TOL_D); + } +} + +// Stereo real-data sanity with 5-coefficient distortion (includes k3, free): +// Verifies calibration produces finite parameters, reasonable RMS, and that rectification maps can be created. +TEST(Omnidir_K3_Hard, Stereo_RealData_5params_K3_Free_SanityAndRectifyMaps) +{ + const Size board(9,6); + const float square = 0.04f; + + vector> objPts; + vector> imgL, imgR; + Size szL, szR; + const vector leftFilenames = getLeftImageFilenames(); + const vector rightFilenames = getRightImageFilenames(); + loadStereoRealDataPairs(leftFilenames, rightFilenames, board, square, objPts, imgL, imgR, szL, szR); + Mat K1 = Mat::eye(3,3,CV_64F); + Mat K2 = Mat::eye(3,3,CV_64F); + Mat xi1(1,1,CV_64F); xi1.at(0,0) = 1.0; + Mat xi2(1,1,CV_64F); xi2.at(0,0) = 1.0; + + Mat D1(1,5,CV_64F, Scalar(0)); + Mat D2(1,5,CV_64F, Scalar(0)); + + Mat om, T; + vector rvecsL, tvecsL; + Mat idx; + + const int flags = cv::omnidir::CALIB_FIX_SKEW; + TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, 300, 1e-9); + + double rms = cv::omnidir::stereoCalibrate(objPts, imgL, imgR, szL, szR, + K1, xi1, D1, + K2, xi2, D2, + om, T, rvecsL, tvecsL, + flags, criteria, idx); + + ASSERT_GT((int)idx.total(), 0); + ASSERT_EQ((int)D1.total(), 5); + ASSERT_EQ((int)D2.total(), 5); + + EXPECT_TRUE(std::isfinite(rms)); + EXPECT_GT(rms, 0.0); + EXPECT_LT(rms, 10.0); + + // Check finiteness (do NOT assert small magnitude for k3 on real data) + auto finiteMat = [](const Mat& m){ + Mat mm = m.reshape(1,1); + for (int i = 0; i < (int)mm.total(); ++i) + if (!std::isfinite(mm.at(0,i))) return false; + return true; + }; + + EXPECT_TRUE(finiteMat(K1)); + EXPECT_TRUE(finiteMat(K2)); + EXPECT_TRUE(std::isfinite(xi1.at(0,0))); + EXPECT_TRUE(std::isfinite(xi2.at(0,0))); + + for (int i = 0; i < 5; ++i) + { + EXPECT_TRUE(std::isfinite(D1.at(0,i))); + EXPECT_TRUE(std::isfinite(D2.at(0,i))); + } + + // Rectify rotations + Mat R1, R2; + cv::omnidir::stereoRectify(om, T, R1, R2); + EXPECT_FALSE(R1.empty()); + EXPECT_FALSE(R2.empty()); + + // Build undistort/rectify maps for both cameras using D5 + Mat map1L, map2L, map1R, map2R; + cv::omnidir::initUndistortRectifyMap(K1, D1, xi1, R1, K1, szL, + CV_32FC1, map1L, map2L, + cv::omnidir::RECTIFY_PERSPECTIVE); + cv::omnidir::initUndistortRectifyMap(K2, D2, xi2, R2, K2, szR, + CV_32FC1, map1R, map2R, + cv::omnidir::RECTIFY_PERSPECTIVE); + + EXPECT_FALSE(map1L.empty()); + EXPECT_FALSE(map2L.empty()); + EXPECT_FALSE(map1R.empty()); + EXPECT_FALSE(map2R.empty()); +} + +// Synthetic stereo k3 check: +// Generate data with non-zero k3, then stereoCalibrate with k3 fixed vs free. +// Expect free-k3 to fit no worse and recover k3 towards ground truth. +TEST(Omnidir_K3_Hard, Stereo_Synthetic_Recovers_K3_and_Improves_Fit) +{ + const cv::Size imageSize(640, 480); + const cv::Size board(9, 6); + const float square = 0.04f; + const int nViews = 20; + + // Ground-truth intrinsics + const cv::Matx33d K1true(520, 0, 320, + 0, 520, 240, + 0, 0, 1); + const cv::Matx33d K2true(525, 0, 320, + 0, 525, 240, + 0, 0, 1); + + const double xi1True = 1.10; + const double xi2True = 1.15; + + // Ground-truth distortion (k3 non-zero) + cv::Mat D1true(1,5,CV_64F), D2true(1,5,CV_64F); + D1true.at(0,0) = -0.10; D1true.at(0,1) = 0.01; + D1true.at(0,2) = 0.001; D1true.at(0,3) = -0.0005; + D1true.at(0,4) = -0.20; // k3 + + D2true.at(0,0) = -0.08; D2true.at(0,1) = 0.015; + D2true.at(0,2) = 0.0005;D2true.at(0,3) = -0.0002; + D2true.at(0,4) = -0.18; // k3 + + // Stereo rig transform (cam2 w.r.t cam1) + cv::Mat omRig = (cv::Mat_(3,1) << 0.01, -0.02, 0.015); + cv::Mat TRig = (cv::Mat_(3,1) << 0.10, 0.00, 0.00); // baseline ~10cm + + const auto obj = makeChessboard3D(board, square); + + std::vector> objectPoints; + std::vector> imgL, imgR; + + synthesizeStereoOmniData(nViews, obj, imageSize, + K1true, xi1True, D1true, + K2true, xi2True, D2true, + omRig, TRig, + objectPoints, imgL, imgR, + /*noiseSigmaPx=*/0.02, /*seed=*/0xCAFE); + + cv::TermCriteria criteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 300, 1e-9); + const int flags_common = + cv::omnidir::CALIB_USE_GUESS | + cv::omnidir::CALIB_FIX_XI | + cv::omnidir::CALIB_FIX_K1 | + cv::omnidir::CALIB_FIX_K2 | + cv::omnidir::CALIB_FIX_P1 | + cv::omnidir::CALIB_FIX_P2 | + cv::omnidir::CALIB_FIX_SKEW | + cv::omnidir::CALIB_FIX_CENTER | + cv::omnidir::CALIB_FIX_GAMMA; + + // --- Run A: k3 FIXED to 0 --- + cv::Mat K1_A = cv::Mat(K1true).clone(); + cv::Mat K2_A = cv::Mat(K2true).clone(); + cv::Mat xi1_A(1,1,CV_64F); xi1_A.at(0,0) = xi1True; + cv::Mat xi2_A(1,1,CV_64F); xi2_A.at(0,0) = xi2True; + + cv::Mat D1_A = D1true.clone(); D1_A.at(0,4) = 0.0; + cv::Mat D2_A = D2true.clone(); D2_A.at(0,4) = 0.0; + + cv::Mat om_A, T_A; + std::vector rA, tA; + cv::Mat idxA; + + const int flags_A = flags_common | cv::omnidir::CALIB_FIX_K3; + double rms_A = cv::omnidir::stereoCalibrate(objectPoints, imgL, imgR, imageSize, imageSize, + K1_A, xi1_A, D1_A, + K2_A, xi2_A, D2_A, + om_A, T_A, rA, tA, + flags_A, criteria, idxA); + + // --- Run B: k3 FREE (start from 0) --- + cv::Mat K1_B = cv::Mat(K1true).clone(); + cv::Mat K2_B = cv::Mat(K2true).clone(); + cv::Mat xi1_B(1,1,CV_64F); xi1_B.at(0,0) = xi1True; + cv::Mat xi2_B(1,1,CV_64F); xi2_B.at(0,0) = xi2True; + + cv::Mat D1_B = D1true.clone(); D1_B.at(0,4) = 0.0; + cv::Mat D2_B = D2true.clone(); D2_B.at(0,4) = 0.0; + + cv::Mat om_B, T_B; + std::vector rB, tB; + cv::Mat idxB; + + const int flags_B = flags_common; // k3 free + double rms_B = cv::omnidir::stereoCalibrate(objectPoints, imgL, imgR, imageSize, imageSize, + K1_B, xi1_B, D1_B, + K2_B, xi2_B, D2_B, + om_B, T_B, rB, tB, + flags_B, criteria, idxB); + + ASSERT_GT((int)idxA.total(), 0); + ASSERT_GT((int)idxB.total(), 0); + + // Free-k3 should not be worse (small slack) + EXPECT_LE(rms_B, rms_A + 1e-12); + + // k3 should move away from 0 and towards ground truth (loose tolerances) + EXPECT_GT(std::abs(D1_B.at(0,4)), 1e-4); + EXPECT_GT(std::abs(D2_B.at(0,4)), 1e-4); + EXPECT_NEAR(D1_B.at(0,4), D1true.at(0,4), 0.25); + EXPECT_NEAR(D2_B.at(0,4), D2true.at(0,4), 0.25); +} + +// Stereo backward compatibility at the mapping level: +// When k3 == 0, using D4 and using D5=[D4,0] should yield identical +// undistort/rectify maps (up to small floating differences). +// Why: This directly validates the k3 integration inside map generation for stereo flows. +TEST(Omnidir_K3_Hard, Stereo_BackwardCompat_D4_equals_D5_k3_zero_rectify_maps) +{ + const cv::Matx33d K(520, 0, 320, + 0, 520, 240, + 0, 0, 1); + const double xi = 1.2; + const cv::Size sz(640, 480); + + cv::Mat xiMat(1,1,CV_64F); xiMat.at(0,0) = xi; + + cv::Mat D4(1,4,CV_64F); + D4.at(0,0) = -0.10; + D4.at(0,1) = 0.01; + D4.at(0,2) = 0.001; + D4.at(0,3) = -0.0005; + + cv::Mat D5(1,5,CV_64F, cv::Scalar(0)); + for (int i = 0; i < 4; ++i) D5.at(0,i) = D4.at(0,i); + D5.at(0,4) = 0.0; + + // Use some plausible stereo rectify rotations (not necessarily from real calibration) + cv::Mat om = (cv::Mat_(3,1) << 0.01, -0.02, 0.015); + cv::Mat T = (cv::Mat_(3,1) << 0.10, 0.00, 0.00); + + cv::Mat R1, R2; + cv::omnidir::stereoRectify(om, T, R1, R2); + + cv::Mat m1L_4, m2L_4, m1L_5, m2L_5; + cv::Mat m1R_4, m2R_4, m1R_5, m2R_5; + + cv::omnidir::initUndistortRectifyMap(K, D4, xiMat, R1, K, sz, CV_32FC1, m1L_4, m2L_4, cv::omnidir::RECTIFY_PERSPECTIVE); + cv::omnidir::initUndistortRectifyMap(K, D5, xiMat, R1, K, sz, CV_32FC1, m1L_5, m2L_5, cv::omnidir::RECTIFY_PERSPECTIVE); + + cv::omnidir::initUndistortRectifyMap(K, D4, xiMat, R2, K, sz, CV_32FC1, m1R_4, m2R_4, cv::omnidir::RECTIFY_PERSPECTIVE); + cv::omnidir::initUndistortRectifyMap(K, D5, xiMat, R2, K, sz, CV_32FC1, m1R_5, m2R_5, cv::omnidir::RECTIFY_PERSPECTIVE); + + ASSERT_FALSE(m1L_4.empty()); ASSERT_FALSE(m2L_4.empty()); + ASSERT_FALSE(m1L_5.empty()); ASSERT_FALSE(m2L_5.empty()); + ASSERT_FALSE(m1R_4.empty()); ASSERT_FALSE(m2R_4.empty()); + ASSERT_FALSE(m1R_5.empty()); ASSERT_FALSE(m2R_5.empty()); + + EXPECT_LT(cv::norm(m1L_4, m1L_5, cv::NORM_INF), 1e-6); + EXPECT_LT(cv::norm(m2L_4, m2L_5, cv::NORM_INF), 1e-6); + EXPECT_LT(cv::norm(m1R_4, m1R_5, cv::NORM_INF), 1e-6); + EXPECT_LT(cv::norm(m2R_4, m2R_5, cv::NORM_INF), 1e-6); +} + +// Stereo real-data regression (legacy D4: k1,k2,p1,p2): +// Runs omnidir::stereoCalibrate on opencv_extra cv/stereo/case1 and compares K/xi/D/om/T/RMS +// against recorded golden values to detect unintended changes (backward compatibility). +TEST(Omnidir_K3_Hard, Stereo_RealData_GoldenCoefficients_4params) +{ + const cv::Size board(9,6); + const float square = 0.04f; + + std::vector> objPts; + std::vector> imgL, imgR; + cv::Size szL, szR; + const vector leftFilenames = getLeftImageFilenames(); + const vector rightFilenames = getRightImageFilenames(); + loadStereoRealDataPairs(leftFilenames, rightFilenames, board, square, objPts, imgL, imgR, szL, szR); + cv::Mat K1 = cv::Mat::eye(3,3,CV_64F); + cv::Mat K2 = cv::Mat::eye(3,3,CV_64F); + cv::Mat xi1(1,1,CV_64F); xi1.at(0,0) = 1.0; + cv::Mat xi2(1,1,CV_64F); xi2.at(0,0) = 1.0; + + cv::Mat D1(1,4,CV_64F, cv::Scalar(0)); + cv::Mat D2(1,4,CV_64F, cv::Scalar(0)); + + cv::Mat om, T; + std::vector rvecsL, tvecsL; + cv::Mat idx; + + const int flags = cv::omnidir::CALIB_FIX_SKEW; + cv::TermCriteria criteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 300, 1e-9); + + const double rms = cv::omnidir::stereoCalibrate( + objPts, imgL, imgR, szL, szR, + K1, xi1, D1, + K2, xi2, D2, + om, T, rvecsL, tvecsL, + flags, criteria, idx); + + ASSERT_GT((int)idx.total(), 0); + ASSERT_EQ((int)D1.total(), 4); + ASSERT_EQ((int)D2.total(), 4); + ASSERT_TRUE(std::isfinite(rms)); + + // ---- GOLDEN VALUES (fill from baseline run) ---- + // Run once with prints and paste here: + // std::cout << "K1=\n" << K1 << "\nxi1=" << xi1 << "\nD1=" << D1 << "\n"; + // std::cout << "K2=\n" << K2 << "\nxi2=" << xi2 << "\nD2=" << D2 << "\n"; + // std::cout << "om=\n" << om << "\nT=\n" << T << "\nRMS=" << rms << "\n"; + + const double GOLD_rms = 0.444818; + const double GOLD_K1_fx = 1401.192283542995; + const double GOLD_K1_fy = 1400.812329013437; + const double GOLD_K1_cx = 342.3254062702503; + const double GOLD_K1_cy = 235.2417898096324; + const double GOLD_xi1 = 1.614168743335463; + const double GOLD_D1_0 = 0.2235385579731314; + const double GOLD_D1_1 = -4.476668984126793; + const double GOLD_D1_2 = 0.005133958683733851; + const double GOLD_D1_3 = -0.0009732332635782253; + const double GOLD_K2_fx = 550.0137369584638; + const double GOLD_K2_fy = 549.4958410910596; + const double GOLD_K2_cx = 328.2231527771337; + const double GOLD_K2_cy = 248.8295934261545; + const double GOLD_xi2 = 0.01924461672982071; + const double GOLD_D2_0 = -0.2803838371535953; + const double GOLD_D2_1 = 0.09113955760046222; + const double GOLD_D2_2 = -0.0004255779307760857; + const double GOLD_D2_3 = 0.001086841676649092; + const double GOLD_om_0 = 0.004178062404200768; + const double GOLD_om_1 = 0.003073878685088316; + const double GOLD_om_2 = -0.003814585954328772; + const double GOLD_T_0 = -0.13351577905041; + const double GOLD_T_1 = 0.001543343941214281; + const double GOLD_T_2 = -4.292093079023809e-05; + // ---- Tolerances (start here; relax if needed for CI stability) ---- + const double TOL_f = 25.0; // pixels + const double TOL_c = 25.0; // pixels + const double TOL_xi = 0.25; // unitless + const double TOL_d = 0.40; // distortion + const double TOL_om = 0.35; // radians-ish (rotation vector magnitude scale) + const double TOL_T = 0.25; // translation in "board units" scale (depends on square) + const double TOL_rms = 0.8; + + EXPECT_NEAR(rms, GOLD_rms, TOL_rms); + + EXPECT_NEAR(K1.at(0,0), GOLD_K1_fx, TOL_f); + EXPECT_NEAR(K1.at(1,1), GOLD_K1_fy, TOL_f); + EXPECT_NEAR(K1.at(0,2), GOLD_K1_cx, TOL_c); + EXPECT_NEAR(K1.at(1,2), GOLD_K1_cy, TOL_c); + + EXPECT_NEAR(K2.at(0,0), GOLD_K2_fx, TOL_f); + EXPECT_NEAR(K2.at(1,1), GOLD_K2_fy, TOL_f); + EXPECT_NEAR(K2.at(0,2), GOLD_K2_cx, TOL_c); + EXPECT_NEAR(K2.at(1,2), GOLD_K2_cy, TOL_c); + + EXPECT_NEAR(xi1.at(0,0), GOLD_xi1, TOL_xi); + EXPECT_NEAR(xi2.at(0,0), GOLD_xi2, TOL_xi); + + EXPECT_NEAR(D1.at(0,0), GOLD_D1_0, TOL_d); + EXPECT_NEAR(D1.at(0,1), GOLD_D1_1, TOL_d); + EXPECT_NEAR(D1.at(0,2), GOLD_D1_2, TOL_d); + EXPECT_NEAR(D1.at(0,3), GOLD_D1_3, TOL_d); + + EXPECT_NEAR(D2.at(0,0), GOLD_D2_0, TOL_d); + EXPECT_NEAR(D2.at(0,1), GOLD_D2_1, TOL_d); + EXPECT_NEAR(D2.at(0,2), GOLD_D2_2, TOL_d); + EXPECT_NEAR(D2.at(0,3), GOLD_D2_3, TOL_d); + + EXPECT_NEAR(om.at(0,0), GOLD_om_0, TOL_om); + EXPECT_NEAR(om.at(1,0), GOLD_om_1, TOL_om); + EXPECT_NEAR(om.at(2,0), GOLD_om_2, TOL_om); + + EXPECT_NEAR(T.at(0,0), GOLD_T_0, TOL_T); + EXPECT_NEAR(T.at(1,0), GOLD_T_1, TOL_T); + EXPECT_NEAR(T.at(2,0), GOLD_T_2, TOL_T); +} +} // namespace opencv_test \ No newline at end of file diff --git a/modules/ccalib/test/test_precomp.hpp b/modules/ccalib/test/test_precomp.hpp new file mode 100644 index 00000000000..90c938bd58f --- /dev/null +++ b/modules/ccalib/test/test_precomp.hpp @@ -0,0 +1,17 @@ +#ifndef OPENCV_TEST_CCALIB_PRECOMP_HPP +#define OPENCV_TEST_CCALIB_PRECOMP_HPP + +#include + +// module headers +#include + +// common core headers (safe + useful in tests) +#include +#include + +#include +#include +#include + +#endif // OPENCV_TEST_CCALIB_PRECOMP_HPP