From 76d03fccb9b4cae4734558046c422649de1afc8a Mon Sep 17 00:00:00 2001 From: alegarda Date: Wed, 20 Feb 2013 15:11:47 +0100 Subject: [PATCH] Added Thin Prism Distortion Model Only the code. --- .../calib3d/include/opencv2/calib3d/calib3d.hpp | 5 ++ modules/calib3d/src/calibration.cpp | 66 ++++++++++++++++------ modules/imgproc/src/undistort.cpp | 30 ++++++---- 3 files changed, 71 insertions(+), 30 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index 49451df..122247c 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -234,6 +234,9 @@ CVAPI(void) cvDrawChessboardCorners( CvArr* image, CvSize pattern_size, #define CV_CALIB_FIX_K5 4096 #define CV_CALIB_FIX_K6 8192 #define CV_CALIB_RATIONAL_MODEL 16384 +#define CV_CALIB_THIN_PRISM_MODEL 32768 +#define CV_CALIB_FIX_S1_S2_S3_S4 65536 + /* Finds intrinsic and extrinsic camera parameters from a few views of known calibration pattern */ @@ -534,6 +537,8 @@ enum CALIB_FIX_K5 = CV_CALIB_FIX_K5, CALIB_FIX_K6 = CV_CALIB_FIX_K6, CALIB_RATIONAL_MODEL = CV_CALIB_RATIONAL_MODEL, + CALIB_THIN_PRISM_MODEL = CV_CALIB_THIN_PRISM_MODEL, + CALIB_FIX_S1_S2_S3_S4=CV_CALIB_FIX_S1_S2_S3_S4, // only for stereo CALIB_FIX_INTRINSIC = CV_CALIB_FIX_INTRINSIC, CALIB_SAME_FOCAL_LENGTH = CV_CALIB_SAME_FOCAL_LENGTH, diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index e483935..b3ed13b 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -762,7 +762,7 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) } -static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8 or 8x1 floating-point vector"; +static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12 or 12x1 floating-point vector"; CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, const CvMat* r_vec, @@ -781,7 +781,7 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, int calc_derivatives; const CvPoint3D64f* M; CvPoint2D64f* m; - double r[3], R[9], dRdr[27], t[3], a[9], k[8] = {0,0,0,0,0,0,0,0}, fx, fy, cx, cy; + double r[3], R[9], dRdr[27], t[3], a[9], k[12] = {0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy; CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k; CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr ); double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0; @@ -884,7 +884,8 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, (distCoeffs->rows != 1 && distCoeffs->cols != 1) || (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 && distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 && - distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8) ) + distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 && + distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12) ) CV_Error( CV_StsBadArg, cvDistCoeffErr ); _k = cvMat( distCoeffs->rows, distCoeffs->cols, @@ -966,8 +967,8 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, { if( !CV_IS_MAT(dpdk) || (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) || - dpdk->rows != count*2 || (dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) ) - CV_Error( CV_StsBadArg, "dp/df must be 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" ); + dpdk->rows != count*2 || (dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) ) + CV_Error( CV_StsBadArg, "dp/df must be 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" ); if( !distCoeffs ) CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" ); @@ -1004,8 +1005,8 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, a3 = r2 + 2*y*y; cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6; icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6); - xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2; - yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1; + xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4; + yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4; m[i].x = xd*fx + cx; m[i].y = yd*fy + cy; @@ -1063,6 +1064,18 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, dpdk_p[7] = fx*x*icdist2*cdist*(-icdist2)*icdist2*r6; dpdk_p[dpdk_step+7] = fy*y*cdist*(-icdist2)*icdist2*r6; } + + if( _dpdk->cols > 8 ) + { + dpdk_p[8] = fx*r2; //s1 + dpdk_p[9] = fx*r4; //s2 + dpdk_p[10] = 0;//s3 + dpdk_p[11] = 0;//s4 + dpdk_p[dpdk_step+8] = 0; //s1 + dpdk_p[dpdk_step+9] = 0; //s2 + dpdk_p[dpdk_step+10] = fy*r2; //s3 + dpdk_p[dpdk_step+11] = fy*r4; //s4 + } } } dpdk_p += dpdk_step*2; @@ -1078,9 +1091,9 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt); double da1dt = 2*(x*dydt[j] + y*dxdt[j]); double dmxdt = fx*(dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt + - k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j])); + k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j])+k[8]*dr2dt+2*r2*k[9]*dr2dt); double dmydt = fy*(dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt + - k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt); + k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt+k[10]*dr2dt+2*r2*k[11]*dr2dt); dpdt_p[j] = dmxdt; dpdt_p[dpdt_step+j] = dmydt; } @@ -1116,9 +1129,9 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints, double dicdist2_dr = -icdist2*icdist2*(k[5]*dr2dr + 2*k[6]*r2*dr2dr + 3*k[7]*r4*dr2dr); double da1dr = 2*(x*dydr + y*dxdr); double dmxdr = fx*(dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr + - k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr)); + k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr)+(k[8]*dr2dr+2*r2*k[9]*dr2dr)); double dmydr = fy*(dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr + - k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr); + k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr+(k[10]*dr2dr+2*r2*k[11]*dr2dr)); dpdr_p[j] = dmxdr; dpdr_p[dpdr_step+j] = dmydr; } @@ -1458,12 +1471,12 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs, CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit ) { - const int NINTRINSIC = 12; + const int NINTRINSIC = 16; Ptr matM, _m, _Ji, _Je, _err; CvLevMarq solver; double reprojErr = 0; - double A[9], k[8] = {0,0,0,0,0,0,0,0}; + double A[9], k[12] = {0,0,0,0,0,0,0,0,0,0,0,0}; CvMat matA = cvMat(3, 3, CV_64F, A), _k; int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn; double aspectRatio = 0.; @@ -1480,7 +1493,10 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, (npoints->rows != 1 && npoints->cols != 1) ) CV_Error( CV_StsUnsupportedFormat, "the array of point counters must be 1-dimensional integer vector" ); - + //when the thin prism model is used the distortion coefficients matrix must have 12 parameters + if((flags & CV_CALIB_THIN_PRISM_MODEL) && (distCoeffs->cols*distCoeffs->rows != 12)) + CV_Error( CV_StsBadArg, "Thin prism model must have 12 parameters in the distortion matrix" ); + nimages = npoints->rows*npoints->cols; npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type); @@ -1517,7 +1533,8 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, (distCoeffs->cols != 1 && distCoeffs->rows != 1) || (distCoeffs->cols*distCoeffs->rows != 4 && distCoeffs->cols*distCoeffs->rows != 5 && - distCoeffs->cols*distCoeffs->rows != 8) ) + distCoeffs->cols*distCoeffs->rows != 8 && + distCoeffs->cols*distCoeffs->rows != 12) ) CV_Error( CV_StsBadArg, cvDistCoeffErr ); for( i = 0; i < nimages; i++ ) @@ -1613,6 +1630,7 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, param[0] = A[0]; param[1] = A[4]; param[2] = A[2]; param[3] = A[5]; param[4] = k[0]; param[5] = k[1]; param[6] = k[2]; param[7] = k[3]; param[8] = k[4]; param[9] = k[5]; param[10] = k[6]; param[11] = k[7]; + param[12] = k[8]; param[13] = k[9]; param[14] = k[10]; param[15] = k[11]; if( flags & CV_CALIB_FIX_FOCAL_LENGTH ) mask[0] = mask[1] = 0; @@ -1637,6 +1655,16 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, mask[10] = 0; if( flags & CV_CALIB_FIX_K6 ) mask[11] = 0; + if(!(flags & CV_CALIB_THIN_PRISM_MODEL)) + flags |= CALIB_FIX_S1_S2_S3_S4; + + if(flags & CALIB_FIX_S1_S2_S3_S4) + { + mask[12] = 0; + mask[13] = 0; + mask[14] = 0; + mask[15] = 0; + } } // 2. initialize extrinsic parameters @@ -1672,6 +1700,7 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints, A[0] = param[0]; A[4] = param[1]; A[2] = param[2]; A[5] = param[3]; k[0] = param[4]; k[1] = param[5]; k[2] = param[6]; k[3] = param[7]; k[4] = param[8]; k[5] = param[9]; k[6] = param[10]; k[7] = param[11]; + k[8] = param[12];k[9] = param[13];k[10] = param[14];k[11] = param[15]; if( !proceed ) break; @@ -3222,7 +3251,8 @@ static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype) distCoeffs0.size() == Size(1, 8) || distCoeffs0.size() == Size(4, 1) || distCoeffs0.size() == Size(5, 1) || - distCoeffs0.size() == Size(8, 1) ) + distCoeffs0.size() == Size(8, 1) || + distCoeffs0.size() == Size(12, 1) ) { Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows)); distCoeffs0.convertTo(dstCoeffs, rtype); @@ -3407,7 +3437,7 @@ double cv::calibrateCamera( InputArrayOfArrays _objectPoints, cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype); Mat distCoeffs = _distCoeffs.getMat(); distCoeffs = prepareDistCoeffs(distCoeffs, rtype); - if( !(flags & CALIB_RATIONAL_MODEL) ) + if( !(flags & CALIB_RATIONAL_MODEL) &&(!(flags & CALIB_THIN_PRISM_MODEL))) distCoeffs = distCoeffs.rows == 1 ? distCoeffs.colRange(0, 5) : distCoeffs.rowRange(0, 5); int i; @@ -3483,7 +3513,7 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype); distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype); - if( !(flags & CALIB_RATIONAL_MODEL) ) + if( !(flags & CALIB_RATIONAL_MODEL) &&(!(flags & CALIB_THIN_PRISM_MODEL))) { distCoeffs1 = distCoeffs1.rows == 1 ? distCoeffs1.colRange(0, 5) : distCoeffs1.rowRange(0, 5); distCoeffs2 = distCoeffs2.rows == 1 ? distCoeffs2.colRange(0, 5) : distCoeffs2.rowRange(0, 5); diff --git a/modules/imgproc/src/undistort.cpp b/modules/imgproc/src/undistort.cpp index fc13b50..25d9548 100644 --- a/modules/imgproc/src/undistort.cpp +++ b/modules/imgproc/src/undistort.cpp @@ -94,7 +94,7 @@ void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoef distCoeffs = Mat_(distCoeffs); else { - distCoeffs.create(8, 1, CV_64F); + distCoeffs.create(12, 1, CV_64F); distCoeffs = 0.; } @@ -108,7 +108,8 @@ void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoef CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) || distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) || - distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1)); + distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1) || + distCoeffs.size() == Size(1, 12) || distCoeffs.size() == Size(12, 1); if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() ) distCoeffs = distCoeffs.t(); @@ -121,6 +122,10 @@ void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoef double k4 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[5] : 0.; double k5 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[6] : 0.; double k6 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[7] : 0.; + double s1 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? ((double*)distCoeffs.data)[8] : 0.; + double s2 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? ((double*)distCoeffs.data)[9] : 0.; + double s3 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? ((double*)distCoeffs.data)[10] : 0.; + double s4 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? ((double*)distCoeffs.data)[11] : 0.; for( int i = 0; i < size.height; i++ ) { @@ -136,8 +141,8 @@ void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoef double x2 = x*x, y2 = y*y; double r2 = x2 + y2, _2xy = 2*x*y; double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2); - double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0; - double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0; + double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+s2*r2*r2) + u0; + double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+s4*r2*r2) + v0; if( m1type == CV_16SC2 ) { int iu = saturate_cast(u*INTER_TAB_SIZE); @@ -260,7 +265,7 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr const CvMat* _distCoeffs, const CvMat* matR, const CvMat* matP ) { - double A[3][3], RR[3][3], k[8]={0,0,0,0,0,0,0,0}, fx, fy, ifx, ify, cx, cy; + double A[3][3], RR[3][3], k[12]={0,0,0,0,0,0,0,0,0,0,0}, fx, fy, ifx, ify, cx, cy; CvMat matA=cvMat(3, 3, CV_64F, A), _Dk; CvMat _RR=cvMat(3, 3, CV_64F, RR); const CvPoint2D32f* srcf; @@ -289,7 +294,8 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) && (_distCoeffs->rows*_distCoeffs->cols == 4 || _distCoeffs->rows*_distCoeffs->cols == 5 || - _distCoeffs->rows*_distCoeffs->cols == 8)); + _distCoeffs->rows*_distCoeffs->cols == 8 || + _distCoeffs->rows*_distCoeffs->cols == 12)); _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k); @@ -355,8 +361,8 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr { double r2 = x*x + y*y; double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2); - double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x); - double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y; + double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2; + double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2; x = (x0 - deltaX)*icdist; y = (y0 - deltaY)*icdist; } @@ -493,7 +499,7 @@ float cv::initWideAngleProjMap( InputArray _cameraMatrix0, InputArray _distCoeff OutputArray _map1, OutputArray _map2, int projType, double _alpha ) { Mat cameraMatrix0 = _cameraMatrix0.getMat(), distCoeffs0 = _distCoeffs0.getMat(); - double k[8] = {0,0,0,0,0,0,0,0}, M[9]={0,0,0,0,0,0,0,0,0}; + double k[12] = {0,0,0,0,0,0,0,0,0,0,0}, M[9]={0,0,0,0,0,0,0,0,0}; Mat distCoeffs(distCoeffs0.rows, distCoeffs0.cols, CV_MAKETYPE(CV_64F,distCoeffs0.channels()), k); Mat cameraMatrix(3,3,CV_64F,M); Point2f scenter((float)cameraMatrix.at(0,2), (float)cameraMatrix.at(1,2)); @@ -531,7 +537,7 @@ float cv::initWideAngleProjMap( InputArray _cameraMatrix0, InputArray _distCoeff dcenter.y = (dsize.height - 1)*0.5f; Mat mapxy(dsize, CV_32FC2); - double k1 = k[0], k2 = k[1], k3 = k[2], p1 = k[3], p2 = k[4], k4 = k[5], k5 = k[6], k6 = k[7]; + double k1 = k[0], k2 = k[1], k3 = k[2], p1 = k[3], p2 = k[4], k4 = k[5], k5 = k[6], k6 = k[7], s1 = k[8], s2 = k[9], s3 = k[10], s4 = k[11]; double fx = cameraMatrix.at(0,0), fy = cameraMatrix.at(1,1), cx = scenter.x, cy = scenter.y; for( int y = 0; y < dsize.height; y++ ) @@ -549,8 +555,8 @@ float cv::initWideAngleProjMap( InputArray _cameraMatrix0, InputArray _distCoeff double x2 = q.x*q.x, y2 = q.y*q.y; double r2 = x2 + y2, _2xy = 2*q.x*q.y; double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2/(1 + ((k6*r2 + k5)*r2 + k4)*r2); - double u = fx*(q.x*kr + p1*_2xy + p2*(r2 + 2*x2)) + cx; - double v = fy*(q.y*kr + p1*(r2 + 2*y2) + p2*_2xy) + cy; + double u = fx*(q.x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+ s2*r2*r2) + cx; + double v = fy*(q.y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+ s4*r2*r2) + cy; mxy[x] = Point2f((float)u, (float)v); } -- 2.7.4