From d5603caa0b3011e05e31b216b010a9165ec0ac29 Mon Sep 17 00:00:00 2001 From: Vladislav Sovrasov Date: Thu, 18 Aug 2016 12:37:04 +0300 Subject: [PATCH] Delete direct lapack calls, minor fixes in UI --- apps/interactive-calibration/CMakeLists.txt | 9 - apps/interactive-calibration/calibCommon.hpp | 2 +- .../cvCalibrationFork.cpp | 824 ------------------ .../cvCalibrationFork.hpp | 56 -- apps/interactive-calibration/linalg.cpp | 491 ----------- apps/interactive-calibration/linalg.hpp | 13 - apps/interactive-calibration/main.cpp | 26 +- 7 files changed, 16 insertions(+), 1405 deletions(-) delete mode 100644 apps/interactive-calibration/cvCalibrationFork.cpp delete mode 100644 apps/interactive-calibration/cvCalibrationFork.hpp delete mode 100644 apps/interactive-calibration/linalg.cpp delete mode 100644 apps/interactive-calibration/linalg.hpp diff --git a/apps/interactive-calibration/CMakeLists.txt b/apps/interactive-calibration/CMakeLists.txt index 8174c40734..74a1e169e9 100644 --- a/apps/interactive-calibration/CMakeLists.txt +++ b/apps/interactive-calibration/CMakeLists.txt @@ -5,15 +5,6 @@ if(NOT OCV_DEPENDENCIES_FOUND) return() endif() -find_package(LAPACK) -if(LAPACK_FOUND) - find_file(LAPACK_HEADER "lapacke.h") - if(LAPACK_HEADER) - add_definitions(-DUSE_LAPACK) - link_libraries(${LAPACK_LIBRARIES}) - endif() -endif() - project(interactive-calibration) set(the_target opencv_interactive-calibration) diff --git a/apps/interactive-calibration/calibCommon.hpp b/apps/interactive-calibration/calibCommon.hpp index 4580a302c4..31c74b440a 100644 --- a/apps/interactive-calibration/calibCommon.hpp +++ b/apps/interactive-calibration/calibCommon.hpp @@ -23,7 +23,7 @@ namespace calib static const std::string consoleHelp = "Hot keys:\nesc - exit application\n" "s - save current data to .xml file\n" "r - delete last frame\n" - "u - enable/disable applying undistortion" + "u - enable/disable applying undistortion\n" "d - delete all frames\n" "v - switch visualization"; diff --git a/apps/interactive-calibration/cvCalibrationFork.cpp b/apps/interactive-calibration/cvCalibrationFork.cpp deleted file mode 100644 index c603dbeb87..0000000000 --- a/apps/interactive-calibration/cvCalibrationFork.cpp +++ /dev/null @@ -1,824 +0,0 @@ -#include -#include "linalg.hpp" -#include "cvCalibrationFork.hpp" - -using namespace cv; - -static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector& cols, - const std::vector& rows); -static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector"; - -static void cvEvaluateJtJ2(CvMat* _JtJ, - const CvMat* camera_matrix, - const CvMat* distortion_coeffs, - const CvMat* object_points, - const CvMat* param, - const CvMat* npoints, - int flags, int NINTRINSIC, double aspectRatio) -{ - int i, pos, ni, total = 0, npstep = 0, maxPoints = 0; - - npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type); - int nimages = npoints->rows*npoints->cols; - for( i = 0; i < nimages; i++ ) - { - ni = npoints->data.i[i*npstep]; - if( ni < 4 ) - { - CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i)); - } - maxPoints = MAX( maxPoints, ni ); - total += ni; - } - - Mat _Ji( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0)); - Mat _Je( maxPoints*2, 6, CV_64FC1 ); - Mat _err( maxPoints*2, 1, CV_64FC1 ); - Mat _m( 1, total, CV_64FC2 ); - const Mat matM = cvarrToMat(object_points); - - cvZero(_JtJ); - for(i = 0, pos = 0; i < nimages; i++, pos += ni ) - { - CvMat _ri, _ti; - ni = npoints->data.i[i*npstep]; - - cvGetRows( param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 ); - cvGetRows( param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 ); - - CvMat _Mi(matM.colRange(pos, pos + ni)); - CvMat _mi(_m.colRange(pos, pos + ni)); - - _Je.resize(ni*2); _Ji.resize(ni*2); _err.resize(ni*2); - CvMat _dpdr(_Je.colRange(0, 3)); - CvMat _dpdt(_Je.colRange(3, 6)); - CvMat _dpdf(_Ji.colRange(0, 2)); - CvMat _dpdc(_Ji.colRange(2, 4)); - CvMat _dpdk(_Ji.colRange(4, NINTRINSIC)); - CvMat _mp(_err.reshape(2, 1)); - - cvProjectPoints2( &_Mi, &_ri, &_ti, camera_matrix, distortion_coeffs, &_mp, &_dpdr, &_dpdt, - (flags & CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf, - (flags & CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk, - (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0); - cvSub( &_mp, &_mi, &_mp ); - Mat JtJ(cvarrToMat(_JtJ)); - // see HZ: (A6.14) for details on the structure of the Jacobian - JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)) += _Ji.t() * _Ji; - JtJ(Rect(NINTRINSIC + i * 6, NINTRINSIC + i * 6, 6, 6)) = _Je.t() * _Je; - JtJ(Rect(NINTRINSIC + i * 6, 0, 6, NINTRINSIC)) = _Ji.t() * _Je; - } -} - -double cvfork::cvCalibrateCamera2( const CvMat* objectPoints, - const CvMat* imagePoints, const CvMat* npoints, - CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs, - CvMat* rvecs, CvMat* tvecs, CvMat* stdDevs, CvMat* perViewErrors, int flags, CvTermCriteria termCrit ) -{ - { - const int NINTRINSIC = CV_CALIB_NINTRINSIC; - double reprojErr = 0; - - Matx33d A; - double k[14] = {0}; - CvMat matA = cvMat(3, 3, CV_64F, A.val), _k; - int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn; - double aspectRatio = 0.; - - // 0. check the parameters & allocate buffers - if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) || - !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) ) - CV_Error( CV_StsBadArg, "One of required vector arguments is not a valid matrix" ); - - if( imageSize.width <= 0 || imageSize.height <= 0 ) - CV_Error( CV_StsOutOfRange, "image width and height must be positive" ); - - if( CV_MAT_TYPE(npoints->type) != CV_32SC1 || - (npoints->rows != 1 && npoints->cols != 1) ) - CV_Error( CV_StsUnsupportedFormat, - "the array of point counters must be 1-dimensional integer vector" ); - if(flags & CV_CALIB_TILTED_MODEL) - { - //when the tilted sensor model is used the distortion coefficients matrix must have 14 parameters - if (distCoeffs->cols*distCoeffs->rows != 14) - CV_Error( CV_StsBadArg, "The tilted sensor model must have 14 parameters in the distortion matrix" ); - } - else - { - //when the thin prism model is used the distortion coefficients matrix must have 12 parameters - if(flags & CV_CALIB_THIN_PRISM_MODEL) - if (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); - - if( rvecs ) - { - cn = CV_MAT_CN(rvecs->type); - if( !CV_IS_MAT(rvecs) || - (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) || - ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) && - (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) ) - CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel " - "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" ); - } - - if( tvecs ) - { - cn = CV_MAT_CN(tvecs->type); - if( !CV_IS_MAT(tvecs) || - (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) || - ((tvecs->rows != nimages || tvecs->cols*cn != 3) && - (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) ) - CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel " - "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" ); - } - - if( stdDevs ) - { - cn = CV_MAT_CN(stdDevs->type); - if( !CV_IS_MAT(stdDevs) || - (CV_MAT_DEPTH(stdDevs->type) != CV_32F && CV_MAT_DEPTH(stdDevs->type) != CV_64F) || - ((stdDevs->rows != (nimages*6 + NINTRINSIC) || stdDevs->cols*cn != 1) && - (stdDevs->rows != 1 || stdDevs->cols != (nimages*6 + NINTRINSIC) || cn != 1)) ) - CV_Error( CV_StsBadArg, "the output array of standard deviations vectors must be 1-channel " - "1x(n*6 + NINTRINSIC) or (n*6 + NINTRINSIC)x1 array, where n is the number of views" ); - } - - if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 && - CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) || - cameraMatrix->rows != 3 || cameraMatrix->cols != 3 ) - CV_Error( CV_StsBadArg, - "Intrinsic parameters must be 3x3 floating-point matrix" ); - - if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 && - CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) || - (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 != 12 && - distCoeffs->cols*distCoeffs->rows != 14) ) - CV_Error( CV_StsBadArg, cvDistCoeffErr ); - - for( i = 0; i < nimages; i++ ) - { - ni = npoints->data.i[i*npstep]; - if( ni < 4 ) - { - CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i)); - } - maxPoints = MAX( maxPoints, ni ); - total += ni; - } - - Mat matM( 1, total, CV_64FC3 ); - Mat _m( 1, total, CV_64FC2 ); - - if(CV_MAT_CN(objectPoints->type) == 3) { - cvarrToMat(objectPoints).convertTo(matM, CV_64F); - } else { - convertPointsHomogeneous(cvarrToMat(objectPoints), matM); - } - - if(CV_MAT_CN(imagePoints->type) == 2) { - cvarrToMat(imagePoints).convertTo(_m, CV_64F); - } else { - convertPointsHomogeneous(cvarrToMat(imagePoints), _m); - } - - nparams = NINTRINSIC + nimages*6; - Mat _Ji( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0)); - Mat _Je( maxPoints*2, 6, CV_64FC1 ); - Mat _err( maxPoints*2, 1, CV_64FC1 ); - - _k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k); - if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 8 ) - { - if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 5 ) - flags |= CALIB_FIX_K3; - flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6; - } - const double minValidAspectRatio = 0.01; - const double maxValidAspectRatio = 100.0; - - // 1. initialize intrinsic parameters & LM solver - if( flags & CALIB_USE_INTRINSIC_GUESS ) - { - cvConvert( cameraMatrix, &matA ); - if( A(0, 0) <= 0 || A(1, 1) <= 0 ) - CV_Error( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" ); - if( A(0, 2) < 0 || A(0, 2) >= imageSize.width || - A(1, 2) < 0 || A(1, 2) >= imageSize.height ) - CV_Error( CV_StsOutOfRange, "Principal point must be within the image" ); - if( fabs(A(0, 1)) > 1e-5 ) - CV_Error( CV_StsOutOfRange, "Non-zero skew is not supported by the function" ); - if( fabs(A(1, 0)) > 1e-5 || fabs(A(2, 0)) > 1e-5 || - fabs(A(2, 1)) > 1e-5 || fabs(A(2,2)-1) > 1e-5 ) - CV_Error( CV_StsOutOfRange, - "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" ); - A(0, 1) = A(1, 0) = A(2, 0) = A(2, 1) = 0.; - A(2, 2) = 1.; - - if( flags & CALIB_FIX_ASPECT_RATIO ) - { - aspectRatio = A(0, 0)/A(1, 1); - - if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio ) - CV_Error( CV_StsOutOfRange, - "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" ); - } - cvConvert( distCoeffs, &_k ); - } - else - { - Scalar mean, sdv; - meanStdDev(matM, mean, sdv); - if( fabs(mean[2]) > 1e-5 || fabs(sdv[2]) > 1e-5 ) - CV_Error( CV_StsBadArg, - "For non-planar calibration rigs the initial intrinsic matrix must be specified" ); - for( i = 0; i < total; i++ ) - matM.at(i).z = 0.; - - if( flags & CALIB_FIX_ASPECT_RATIO ) - { - aspectRatio = cvmGet(cameraMatrix,0,0); - aspectRatio /= cvmGet(cameraMatrix,1,1); - if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio ) - CV_Error( CV_StsOutOfRange, - "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" ); - } - CvMat _matM(matM), m(_m); - cvInitIntrinsicParams2D( &_matM, &m, npoints, imageSize, &matA, aspectRatio ); - } - - //CvLevMarq solver( nparams, 0, termCrit ); - cvfork::CvLevMarqFork solver( nparams, 0, termCrit ); - Mat allErrors(1, total, CV_64FC2); - - if(flags & CALIB_USE_LU) { - solver.solveMethod = DECOMP_LU; - } - else if(flags & CALIB_USE_QR) - solver.solveMethod = DECOMP_QR; - - { - double* param = solver.param->data.db; - uchar* mask = solver.mask->data.ptr; - - param[0] = A(0, 0); param[1] = A(1, 1); param[2] = A(0, 2); param[3] = A(1, 2); - std::copy(k, k + 14, param + 4); - - if( flags & CV_CALIB_FIX_FOCAL_LENGTH ) - mask[0] = mask[1] = 0; - if( flags & CV_CALIB_FIX_PRINCIPAL_POINT ) - mask[2] = mask[3] = 0; - if( flags & CV_CALIB_ZERO_TANGENT_DIST ) - { - param[6] = param[7] = 0; - mask[6] = mask[7] = 0; - } - if( !(flags & CALIB_RATIONAL_MODEL) ) - flags |= CALIB_FIX_K4 + CALIB_FIX_K5 + CALIB_FIX_K6; - if( !(flags & CV_CALIB_THIN_PRISM_MODEL)) - flags |= CALIB_FIX_S1_S2_S3_S4; - if( !(flags & CV_CALIB_TILTED_MODEL)) - flags |= CALIB_FIX_TAUX_TAUY; - - mask[ 4] = !(flags & CALIB_FIX_K1); - mask[ 5] = !(flags & CALIB_FIX_K2); - mask[ 8] = !(flags & CALIB_FIX_K3); - mask[ 9] = !(flags & CALIB_FIX_K4); - mask[10] = !(flags & CALIB_FIX_K5); - mask[11] = !(flags & CALIB_FIX_K6); - - if(flags & CALIB_FIX_S1_S2_S3_S4) - { - mask[12] = 0; - mask[13] = 0; - mask[14] = 0; - mask[15] = 0; - } - if(flags & CALIB_FIX_TAUX_TAUY) - { - mask[16] = 0; - mask[17] = 0; - } - } - - // 2. initialize extrinsic parameters - for( i = 0, pos = 0; i < nimages; i++, pos += ni ) - { - CvMat _ri, _ti; - ni = npoints->data.i[i*npstep]; - - cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 ); - cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 ); - - CvMat _Mi(matM.colRange(pos, pos + ni)); - CvMat _mi(_m.colRange(pos, pos + ni)); - - cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti ); - } - - // 3. run the optimization - for(;;) - { - const CvMat* _param = 0; - CvMat *_JtJ = 0, *_JtErr = 0; - double* _errNorm = 0; - bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ); - double *param = solver.param->data.db, *pparam = solver.prevParam->data.db; - - if( flags & CALIB_FIX_ASPECT_RATIO ) - { - param[0] = param[1]*aspectRatio; - pparam[0] = pparam[1]*aspectRatio; - } - - A(0, 0) = param[0]; A(1, 1) = param[1]; A(0, 2) = param[2]; A(1, 2) = param[3]; - std::copy(param + 4, param + 4 + 14, k); - - if( !proceed ) { - //do errors estimation - if(stdDevs) { - Ptr JtJ(cvCreateMat(nparams, nparams, CV_64F)); - CvMat cvMatM(matM); - cvEvaluateJtJ2(JtJ, &matA, &_k, &cvMatM, solver.param, npoints, flags, NINTRINSIC, aspectRatio); - - Mat mask = cvarrToMat(solver.mask); - int nparams_nz = countNonZero(mask); - Mat JtJinv, JtJN; - JtJN.create(nparams_nz, nparams_nz, CV_64F); - subMatrix(cvarrToMat(JtJ), JtJN, mask, mask); - completeSymm(JtJN, false); - #ifndef USE_LAPACK - cv::invert(JtJN, JtJinv, DECOMP_SVD); - #else - cvfork::invert(JtJN, JtJinv, DECOMP_SVD); - #endif - double sigma2 = norm(allErrors, NORM_L2SQR) / (total - nparams_nz); - Mat stdDevsM = cvarrToMat(stdDevs); - int j = 0; - for (int s = 0; s < nparams; s++) - if(mask.data[s]) { - stdDevsM.at(s) = std::sqrt(JtJinv.at(j,j)*sigma2); - j++; - } - else - stdDevsM.at(s) = 0; - } - break; - } - - reprojErr = 0; - - for( i = 0, pos = 0; i < nimages; i++, pos += ni ) - { - CvMat _ri, _ti; - ni = npoints->data.i[i*npstep]; - - cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 ); - cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 ); - - CvMat _Mi(matM.colRange(pos, pos + ni)); - CvMat _mi(_m.colRange(pos, pos + ni)); - CvMat _me(allErrors.colRange(pos, pos + ni)); - - _Je.resize(ni*2); _Ji.resize(ni*2); _err.resize(ni*2); - CvMat _dpdr(_Je.colRange(0, 3)); - CvMat _dpdt(_Je.colRange(3, 6)); - CvMat _dpdf(_Ji.colRange(0, 2)); - CvMat _dpdc(_Ji.colRange(2, 4)); - CvMat _dpdk(_Ji.colRange(4, NINTRINSIC)); - CvMat _mp(_err.reshape(2, 1)); - - if( solver.state == CvLevMarq::CALC_J ) - { - cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt, - (flags & CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf, - (flags & CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk, - (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0); - } - else - cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp ); - - cvSub( &_mp, &_mi, &_mp ); - - if( solver.state == CvLevMarq::CALC_J ) - { - Mat JtJ(cvarrToMat(_JtJ)), JtErr(cvarrToMat(_JtErr)); - - // see HZ: (A6.14) for details on the structure of the Jacobian - JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)) += _Ji.t() * _Ji; - JtJ(Rect(NINTRINSIC + i * 6, NINTRINSIC + i * 6, 6, 6)) = _Je.t() * _Je; - JtJ(Rect(NINTRINSIC + i * 6, 0, 6, NINTRINSIC)) = _Ji.t() * _Je; - - JtErr.rowRange(0, NINTRINSIC) += _Ji.t() * _err; - JtErr.rowRange(NINTRINSIC + i * 6, NINTRINSIC + (i + 1) * 6) = _Je.t() * _err; - - } - if (stdDevs || perViewErrors) - cvCopy(&_mp, &_me); - reprojErr += norm(_err, NORM_L2SQR); - } - - if( _errNorm ) - *_errNorm = reprojErr; - } - - // 4. store the results - cvConvert( &matA, cameraMatrix ); - cvConvert( &_k, distCoeffs ); - - for( i = 0, pos = 0; i < nimages; i++) - { - CvMat src, dst; - if( perViewErrors ) - { - ni = npoints->data.i[i*npstep]; - perViewErrors->data.db[i] = std::sqrt(cv::norm(allErrors.colRange(pos, pos + ni), NORM_L2SQR) / ni); - pos+=ni; - } - - if( rvecs ) - { - src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 ); - if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 ) - { - dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type), - rvecs->data.ptr + rvecs->step*i ); - cvRodrigues2( &src, &matA ); - cvConvert( &matA, &dst ); - } - else - { - dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ? - rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) : - rvecs->data.ptr + rvecs->step*i ); - cvConvert( &src, &dst ); - } - } - if( tvecs ) - { - src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 ); - dst = cvMat( 3, 1, CV_MAT_DEPTH(tvecs->type), tvecs->rows == 1 ? - tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) : - tvecs->data.ptr + tvecs->step*i ); - cvConvert( &src, &dst ); - } - } - - return std::sqrt(reprojErr/total); - } -} - - -static Mat prepareCameraMatrix(Mat& cameraMatrix0, int rtype) -{ - Mat cameraMatrix = Mat::eye(3, 3, rtype); - if( cameraMatrix0.size() == cameraMatrix.size() ) - cameraMatrix0.convertTo(cameraMatrix, rtype); - return cameraMatrix; -} - -static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype) -{ - Mat distCoeffs = Mat::zeros(distCoeffs0.cols == 1 ? Size(1, 14) : Size(14, 1), rtype); - if( distCoeffs0.size() == Size(1, 4) || - distCoeffs0.size() == Size(1, 5) || - distCoeffs0.size() == Size(1, 8) || - distCoeffs0.size() == Size(1, 12) || - distCoeffs0.size() == Size(1, 14) || - distCoeffs0.size() == Size(4, 1) || - distCoeffs0.size() == Size(5, 1) || - distCoeffs0.size() == Size(8, 1) || - distCoeffs0.size() == Size(12, 1) || - distCoeffs0.size() == Size(14, 1) ) - { - Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows)); - distCoeffs0.convertTo(dstCoeffs, rtype); - } - return distCoeffs; -} - -static void collectCalibrationData( InputArrayOfArrays objectPoints, - InputArrayOfArrays imagePoints1, - InputArrayOfArrays imagePoints2, - Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2, - Mat& npoints ) -{ - int nimages = (int)objectPoints.total(); - int i, j = 0, ni = 0, total = 0; - CV_Assert(nimages > 0 && nimages == (int)imagePoints1.total() && - (!imgPtMat2 || nimages == (int)imagePoints2.total())); - - for( i = 0; i < nimages; i++ ) - { - ni = objectPoints.getMat(i).checkVector(3, CV_32F); - if( ni <= 0 ) - CV_Error(CV_StsUnsupportedFormat, "objectPoints should contain vector of vectors of points of type Point3f"); - int ni1 = imagePoints1.getMat(i).checkVector(2, CV_32F); - if( ni1 <= 0 ) - CV_Error(CV_StsUnsupportedFormat, "imagePoints1 should contain vector of vectors of points of type Point2f"); - CV_Assert( ni == ni1 ); - - total += ni; - } - - npoints.create(1, (int)nimages, CV_32S); - objPtMat.create(1, (int)total, CV_32FC3); - imgPtMat1.create(1, (int)total, CV_32FC2); - Point2f* imgPtData2 = 0; - - if( imgPtMat2 ) - { - imgPtMat2->create(1, (int)total, CV_32FC2); - imgPtData2 = imgPtMat2->ptr(); - } - - Point3f* objPtData = objPtMat.ptr(); - Point2f* imgPtData1 = imgPtMat1.ptr(); - - for( i = 0; i < nimages; i++, j += ni ) - { - Mat objpt = objectPoints.getMat(i); - Mat imgpt1 = imagePoints1.getMat(i); - ni = objpt.checkVector(3, CV_32F); - npoints.at(i) = ni; - memcpy( objPtData + j, objpt.ptr(), ni*sizeof(objPtData[0]) ); - memcpy( imgPtData1 + j, imgpt1.ptr(), ni*sizeof(imgPtData1[0]) ); - - if( imgPtData2 ) - { - Mat imgpt2 = imagePoints2.getMat(i); - int ni2 = imgpt2.checkVector(2, CV_32F); - CV_Assert( ni == ni2 ); - memcpy( imgPtData2 + j, imgpt2.ptr(), ni*sizeof(imgPtData2[0]) ); - } - } -} - -double cvfork::calibrateCamera(InputArrayOfArrays _objectPoints, - InputArrayOfArrays _imagePoints, - Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs, - OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, OutputArray _stdDeviations, OutputArray _perViewErrors, int flags, TermCriteria criteria ) -{ - int rtype = CV_64F; - Mat cameraMatrix = _cameraMatrix.getMat(); - cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype); - Mat distCoeffs = _distCoeffs.getMat(); - distCoeffs = prepareDistCoeffs(distCoeffs, rtype); - if( !(flags & CALIB_RATIONAL_MODEL) && - (!(flags & CALIB_THIN_PRISM_MODEL)) && - (!(flags & CALIB_TILTED_MODEL))) - distCoeffs = distCoeffs.rows == 1 ? distCoeffs.colRange(0, 5) : distCoeffs.rowRange(0, 5); - - int nimages = int(_objectPoints.total()); - CV_Assert( nimages > 0 ); - Mat objPt, imgPt, npoints, rvecM, tvecM, stdDeviationsM, errorsM; - - bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(), - stddev_needed = _stdDeviations.needed(), errors_needed = _perViewErrors.needed(); - - bool rvecs_mat_vec = _rvecs.isMatVector(); - bool tvecs_mat_vec = _tvecs.isMatVector(); - - if( rvecs_needed ) { - _rvecs.create(nimages, 1, CV_64FC3); - - if(rvecs_mat_vec) - rvecM.create(nimages, 3, CV_64F); - else - rvecM = _rvecs.getMat(); - } - - if( tvecs_needed ) { - _tvecs.create(nimages, 1, CV_64FC3); - - if(tvecs_mat_vec) - tvecM.create(nimages, 3, CV_64F); - else - tvecM = _tvecs.getMat(); - } - - if( stddev_needed ) { - _stdDeviations.create(nimages*6 + CV_CALIB_NINTRINSIC, 1, CV_64F); - stdDeviationsM = _stdDeviations.getMat(); - } - - if( errors_needed) { - _perViewErrors.create(nimages, 1, CV_64F); - errorsM = _perViewErrors.getMat(); - } - - collectCalibrationData( _objectPoints, _imagePoints, noArray(), - objPt, imgPt, 0, npoints ); - CvMat c_objPt = objPt, c_imgPt = imgPt, c_npoints = npoints; - CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; - CvMat c_rvecM = rvecM, c_tvecM = tvecM, c_stdDev = stdDeviationsM, c_errors = errorsM; - - double reprojErr = cvfork::cvCalibrateCamera2(&c_objPt, &c_imgPt, &c_npoints, imageSize, - &c_cameraMatrix, &c_distCoeffs, - rvecs_needed ? &c_rvecM : NULL, - tvecs_needed ? &c_tvecM : NULL, - stddev_needed ? &c_stdDev : NULL, - errors_needed ? &c_errors : NULL, flags, criteria ); - - // overly complicated and inefficient rvec/ tvec handling to support vector - for(int i = 0; i < nimages; i++ ) - { - if( rvecs_needed && rvecs_mat_vec) - { - _rvecs.create(3, 1, CV_64F, i, true); - Mat rv = _rvecs.getMat(i); - memcpy(rv.ptr(), rvecM.ptr(i), 3*sizeof(double)); - } - if( tvecs_needed && tvecs_mat_vec) - { - _tvecs.create(3, 1, CV_64F, i, true); - Mat tv = _tvecs.getMat(i); - memcpy(tv.ptr(), tvecM.ptr(i), 3*sizeof(double)); - } - } - - cameraMatrix.copyTo(_cameraMatrix); - distCoeffs.copyTo(_distCoeffs); - - return reprojErr; -} - -double cvfork::calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds, - Ptr &_board, Size imageSize, - InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs, - OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, OutputArray _stdDeviations, OutputArray _perViewErrors, - int flags, TermCriteria criteria) { - - CV_Assert(_charucoIds.total() > 0 && (_charucoIds.total() == _charucoCorners.total())); - - // Join object points of charuco corners in a single vector for calibrateCamera() function - std::vector< std::vector< Point3f > > allObjPoints; - allObjPoints.resize(_charucoIds.total()); - for(unsigned int i = 0; i < _charucoIds.total(); i++) { - unsigned int nCorners = (unsigned int)_charucoIds.getMat(i).total(); - CV_Assert(nCorners > 0 && nCorners == _charucoCorners.getMat(i).total()); //actually nCorners must be > 3 for calibration - allObjPoints[i].reserve(nCorners); - - for(unsigned int j = 0; j < nCorners; j++) { - int pointId = _charucoIds.getMat(i).ptr< int >(0)[j]; - CV_Assert(pointId >= 0 && pointId < (int)_board->chessboardCorners.size()); - allObjPoints[i].push_back(_board->chessboardCorners[pointId]); - } - } - - return cvfork::calibrateCamera(allObjPoints, _charucoCorners, imageSize, _cameraMatrix, _distCoeffs, - _rvecs, _tvecs, _stdDeviations, _perViewErrors, flags, criteria); -} - - -static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector& cols, - const std::vector& rows) { - int nonzeros_cols = cv::countNonZero(cols); - cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1); - - for (int i = 0, j = 0; i < (int)cols.size(); i++) - { - if (cols[i]) - { - src.col(i).copyTo(tmp.col(j++)); - } - } - - int nonzeros_rows = cv::countNonZero(rows); - dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1); - for (int i = 0, j = 0; i < (int)rows.size(); i++) - { - if (rows[i]) - { - tmp.row(i).copyTo(dst.row(j++)); - } - } -} - -void cvfork::CvLevMarqFork::step() -{ - using namespace cv; - const double LOG10 = log(10.); - double lambda = exp(lambdaLg10*LOG10); - int nparams = param->rows; - - Mat _JtJ = cvarrToMat(JtJ); - Mat _mask = cvarrToMat(mask); - - int nparams_nz = countNonZero(_mask); - if(!JtJN || JtJN->rows != nparams_nz) { - // prevent re-allocation in every step - JtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F )); - JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F )); - JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F )); - } - - Mat _JtJN = cvarrToMat(JtJN); - Mat _JtErr = cvarrToMat(JtJV); - Mat_ nonzero_param = cvarrToMat(JtJW); - - subMatrix(cvarrToMat(JtErr), _JtErr, std::vector(1, 1), _mask); - subMatrix(_JtJ, _JtJN, _mask, _mask); - - if( !err ) - completeSymm( _JtJN, completeSymmFlag ); -#if 1 - _JtJN.diag() *= 1. + lambda; -#else - _JtJN.diag() += lambda; -#endif -#ifndef USE_LAPACK - cv::solve(_JtJN, _JtErr, nonzero_param, solveMethod); -#else - cvfork::solve(_JtJN, _JtErr, nonzero_param, solveMethod); -#endif - - int j = 0; - for( int i = 0; i < nparams; i++ ) - param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0); -} - -cvfork::CvLevMarqFork::CvLevMarqFork(int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag) -{ - init(nparams, nerrs, criteria0, _completeSymmFlag); -} - -cvfork::CvLevMarqFork::~CvLevMarqFork() -{ - clear(); -} - -bool cvfork::CvLevMarqFork::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm ) -{ - CV_Assert( !err ); - if( state == DONE ) - { - _param = param; - return false; - } - - if( state == STARTED ) - { - _param = param; - cvZero( JtJ ); - cvZero( JtErr ); - errNorm = 0; - _JtJ = JtJ; - _JtErr = JtErr; - _errNorm = &errNorm; - state = CALC_J; - return true; - } - - if( state == CALC_J ) - { - cvCopy( param, prevParam ); - step(); - _param = param; - prevErrNorm = errNorm; - errNorm = 0; - _errNorm = &errNorm; - state = CHECK_ERR; - return true; - } - - assert( state == CHECK_ERR ); - if( errNorm > prevErrNorm ) - { - if( ++lambdaLg10 <= 16 ) - { - step(); - _param = param; - errNorm = 0; - _errNorm = &errNorm; - state = CHECK_ERR; - return true; - } - } - - lambdaLg10 = MAX(lambdaLg10-1, -16); - if( ++iters >= criteria.max_iter || - cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon ) - { - //printf("iters %i\n", iters); - _param = param; - state = DONE; - return false; - } - - prevErrNorm = errNorm; - cvZero( JtJ ); - cvZero( JtErr ); - _param = param; - _JtJ = JtJ; - _JtErr = JtErr; - state = CALC_J; - return true; -} diff --git a/apps/interactive-calibration/cvCalibrationFork.hpp b/apps/interactive-calibration/cvCalibrationFork.hpp deleted file mode 100644 index 075f251ef5..0000000000 --- a/apps/interactive-calibration/cvCalibrationFork.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef CV_CALIBRATION_FORK_HPP -#define CV_CALIBRATION_FORK_HPP - -#include -#include -#include -#include - -namespace cvfork -{ -using namespace cv; - -#define CV_CALIB_NINTRINSIC 18 -#define CALIB_USE_QR (1 << 18) - -double calibrateCamera(InputArrayOfArrays objectPoints, - InputArrayOfArrays imagePoints, Size imageSize, - InputOutputArray cameraMatrix, InputOutputArray distCoeffs, - OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray stdDeviations, - OutputArray perViewErrors, int flags = 0, TermCriteria criteria = TermCriteria( - TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); - -double cvCalibrateCamera2( const CvMat* object_points, - const CvMat* image_points, - const CvMat* point_counts, - CvSize image_size, - CvMat* camera_matrix, - CvMat* distortion_coeffs, - CvMat* rotation_vectors CV_DEFAULT(NULL), - CvMat* translation_vectors CV_DEFAULT(NULL), - CvMat* stdDeviations_vector CV_DEFAULT(NULL), - CvMat* perViewErrors_vector CV_DEFAULT(NULL), - int flags CV_DEFAULT(0), - CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria( - CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON)) ); - -double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds, - Ptr &_board, Size imageSize, - InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs, - OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, OutputArray _stdDeviations, OutputArray _perViewErrors, - int flags = 0, TermCriteria criteria = TermCriteria( - TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); - -class CvLevMarqFork : public CvLevMarq -{ -public: - CvLevMarqFork( int nparams, int nerrs, CvTermCriteria criteria= - cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON), - bool completeSymmFlag=false ); - bool updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm ); - void step(); - ~CvLevMarqFork(); -}; -} - -#endif diff --git a/apps/interactive-calibration/linalg.cpp b/apps/interactive-calibration/linalg.cpp deleted file mode 100644 index e75b4d2d93..0000000000 --- a/apps/interactive-calibration/linalg.cpp +++ /dev/null @@ -1,491 +0,0 @@ -#include "linalg.hpp" - -#ifdef USE_LAPACK - -typedef int integer; -#include - -#include -using namespace cv; - -bool cvfork::solve(InputArray _src, const InputArray _src2arg, OutputArray _dst, int method ) - { - bool result = true; - Mat src = _src.getMat(), _src2 = _src2arg.getMat(); - int type = src.type(); - bool is_normal = (method & DECOMP_NORMAL) != 0; - - CV_Assert( type == _src2.type() && (type == CV_32F || type == CV_64F) ); - - method &= ~DECOMP_NORMAL; - CV_Assert( (method != DECOMP_LU && method != DECOMP_CHOLESKY) || - is_normal || src.rows == src.cols ); - - double rcond=-1, s1=0, work1=0, *work=0, *s=0; - float frcond=-1, fs1=0, fwork1=0, *fwork=0, *fs=0; - integer m = src.rows, m_ = m, n = src.cols, mn = std::max(m,n), - nm = std::min(m, n), nb = _src2.cols, lwork=-1, liwork=0, iwork1=0, - lda = m, ldx = mn, info=0, rank=0, *iwork=0; - int elem_size = CV_ELEM_SIZE(type); - bool copy_rhs=false; - int buf_size=0; - AutoBuffer buffer; - uchar* ptr; - char N[] = {'N', '\0'}, L[] = {'L', '\0'}; - - Mat src2 = _src2; - _dst.create( src.cols, src2.cols, src.type() ); - Mat dst = _dst.getMat(); - - if( m <= n ) - is_normal = false; - else if( is_normal ) - m_ = n; - - buf_size += (is_normal ? n*n : m*n)*elem_size; - - if( m_ != n || nb > 1 || !dst.isContinuous() ) - { - copy_rhs = true; - if( is_normal ) - buf_size += n*nb*elem_size; - else - buf_size += mn*nb*elem_size; - } - - if( method == DECOMP_SVD || method == DECOMP_EIG ) - { - integer nlvl = cvRound(std::log(std::max(std::min(m_,n)/25., 1.))/CV_LOG2) + 1; - liwork = std::min(m_,n)*(3*std::max(nlvl,(integer)0) + 11); - - if( type == CV_32F ) - sgelsd_(&m_, &n, &nb, (float*)src.data, &lda, (float*)dst.data, &ldx, - &fs1, &frcond, &rank, &fwork1, &lwork, &iwork1, &info); - else - dgelsd_(&m_, &n, &nb, (double*)src.data, &lda, (double*)dst.data, &ldx, - &s1, &rcond, &rank, &work1, &lwork, &iwork1, &info ); - buf_size += nm*elem_size + (liwork + 1)*sizeof(integer); - } - else if( method == DECOMP_QR ) - { - if( type == CV_32F ) - sgels_(N, &m_, &n, &nb, (float*)src.data, &lda, - (float*)dst.data, &ldx, &fwork1, &lwork, &info ); - else - dgels_(N, &m_, &n, &nb, (double*)src.data, &lda, - (double*)dst.data, &ldx, &work1, &lwork, &info ); - } - else if( method == DECOMP_LU ) - { - buf_size += (n+1)*sizeof(integer); - } - else if( method == DECOMP_CHOLESKY ) - ; - else - CV_Error( Error::StsBadArg, "Unknown method" ); - assert(info == 0); - - lwork = cvRound(type == CV_32F ? (double)fwork1 : work1); - buf_size += lwork*elem_size; - buffer.allocate(buf_size); - ptr = (uchar*)buffer; - - Mat at(n, m_, type, ptr); - ptr += n*m_*elem_size; - - if( method == DECOMP_CHOLESKY || method == DECOMP_EIG ) - src.copyTo(at); - else if( !is_normal ) - transpose(src, at); - else - mulTransposed(src, at, true); - - Mat xt; - if( !is_normal ) - { - if( copy_rhs ) - { - Mat temp(nb, mn, type, ptr); - ptr += nb*mn*elem_size; - Mat bt = temp.colRange(0, m); - xt = temp.colRange(0, n); - transpose(src2, bt); - } - else - { - src2.copyTo(dst); - xt = Mat(1, n, type, dst.data); - } - } - else - { - if( copy_rhs ) - { - xt = Mat(nb, n, type, ptr); - ptr += nb*n*elem_size; - } - else - xt = Mat(1, n, type, dst.data); - // (a'*b)' = b'*a - gemm( src2, src, 1, Mat(), 0, xt, GEMM_1_T ); - } - - lda = (int)(at.step ? at.step/elem_size : at.cols); - ldx = (int)(xt.step ? xt.step/elem_size : (!is_normal && copy_rhs ? mn : n)); - - if( method == DECOMP_SVD || method == DECOMP_EIG ) - { - if( type == CV_32F ) - { - fs = (float*)ptr; - ptr += nm*elem_size; - fwork = (float*)ptr; - ptr += lwork*elem_size; - iwork = (integer*)alignPtr(ptr, sizeof(integer)); - - sgelsd_(&m_, &n, &nb, (float*)at.data, &lda, (float*)xt.data, &ldx, - fs, &frcond, &rank, fwork, &lwork, iwork, &info); - } - else - { - s = (double*)ptr; - ptr += nm*elem_size; - work = (double*)ptr; - ptr += lwork*elem_size; - iwork = (integer*)alignPtr(ptr, sizeof(integer)); - - dgelsd_(&m_, &n, &nb, (double*)at.data, &lda, (double*)xt.data, &ldx, - s, &rcond, &rank, work, &lwork, iwork, &info); - } - } - else if( method == DECOMP_QR ) - { - if( type == CV_32F ) - { - fwork = (float*)ptr; - sgels_(N, &m_, &n, &nb, (float*)at.data, &lda, - (float*)xt.data, &ldx, fwork, &lwork, &info); - } - else - { - work = (double*)ptr; - dgels_(N, &m_, &n, &nb, (double*)at.data, &lda, - (double*)xt.data, &ldx, work, &lwork, &info); - } - } - else if( method == DECOMP_CHOLESKY || (method == DECOMP_LU && is_normal) ) - { - if( type == CV_32F ) - { - spotrf_(L, &n, (float*)at.data, &lda, &info); - if(info==0) - spotrs_(L, &n, &nb, (float*)at.data, &lda, (float*)xt.data, &ldx, &info); - } - else - { - dpotrf_(L, &n, (double*)at.data, &lda, &info); - if(info==0) - dpotrs_(L, &n, &nb, (double*)at.data, &lda, (double*)xt.data, &ldx, &info); - } - } - else if( method == DECOMP_LU ) - { - iwork = (integer*)alignPtr(ptr, sizeof(integer)); - if( type == CV_32F ) - sgesv_(&n, &nb, (float*)at.data, &lda, iwork, (float*)xt.data, &ldx, &info ); - else - dgesv_(&n, &nb, (double*)at.data, &lda, iwork, (double*)xt.data, &ldx, &info ); - } - else - assert(0); - result = info == 0; - - if( !result ) - dst = Scalar(0); - else if( xt.data != dst.data ) - transpose( xt, dst ); - - return result; - } - -static void _SVDcompute( const InputArray _aarr, OutputArray _w, - OutputArray _u, OutputArray _vt, int flags = 0) -{ - Mat a = _aarr.getMat(), u, vt; - integer m = a.rows, n = a.cols, mn = std::max(m, n), nm = std::min(m, n); - int type = a.type(), elem_size = (int)a.elemSize(); - bool compute_uv = _u.needed() || _vt.needed(); - - if( flags & SVD::NO_UV ) - { - _u.release(); - _vt.release(); - compute_uv = false; - } - - if( compute_uv ) - { - _u.create( (int)m, (int)((flags & SVD::FULL_UV) ? m : nm), type ); - _vt.create( (int)((flags & SVD::FULL_UV) ? n : nm), n, type ); - u = _u.getMat(); - vt = _vt.getMat(); - } - - _w.create(nm, 1, type, -1, true); - - Mat _a = a, w = _w.getMat(); - CV_Assert( w.isContinuous() ); - int work_ofs=0, iwork_ofs=0, buf_size = 0; - bool temp_a = false; - double u1=0, v1=0, work1=0; - float uf1=0, vf1=0, workf1=0; - integer lda, ldu, ldv, lwork=-1, iwork1=0, info=0; - char mode[] = {compute_uv ? 'S' : 'N', '\0'}; - - if( m != n && compute_uv && (flags & SVD::FULL_UV) ) - mode[0] = 'A'; - - if( !(flags & SVD::MODIFY_A) ) - { - if( mode[0] == 'N' || mode[0] == 'A' ) - temp_a = true; - else if( compute_uv && (a.size() == vt.size() || a.size() == u.size()) && mode[0] == 'S' ) - mode[0] = 'O'; - } - - lda = a.cols; - ldv = ldu = mn; - - if( type == CV_32F ) - { - sgesdd_(mode, &n, &m, (float*)a.data, &lda, (float*)w.data, - &vf1, &ldv, &uf1, &ldu, &workf1, &lwork, &iwork1, &info ); - lwork = cvRound(workf1); - } - else - { - dgesdd_(mode, &n, &m, (double*)a.data, &lda, (double*)w.data, - &v1, &ldv, &u1, &ldu, &work1, &lwork, &iwork1, &info ); - lwork = cvRound(work1); - } - - assert(info == 0); - if( temp_a ) - { - buf_size += n*m*elem_size; - } - work_ofs = buf_size; - buf_size += lwork*elem_size; - buf_size = alignSize(buf_size, sizeof(integer)); - iwork_ofs = buf_size; - buf_size += 8*nm*sizeof(integer); - - AutoBuffer buf(buf_size); - uchar* buffer = (uchar*)buf; - - if( temp_a ) - { - _a = Mat(a.rows, a.cols, type, buffer ); - a.copyTo(_a); - } - - if( !(flags & SVD::MODIFY_A) && !temp_a ) - { - if( compute_uv && a.size() == vt.size() ) - { - a.copyTo(vt); - _a = vt; - } - else if( compute_uv && a.size() == u.size() ) - { - a.copyTo(u); - _a = u; - } - } - - if( compute_uv ) - { - ldv = (int)(vt.step ? vt.step/elem_size : vt.cols); - ldu = (int)(u.step ? u.step/elem_size : u.cols); - } - - lda = (int)(_a.step ? _a.step/elem_size : _a.cols); - if( type == CV_32F ) - { - sgesdd_(mode, &n, &m, _a.ptr(), &lda, w.ptr(), - vt.data ? vt.ptr() : (float*)&v1, &ldv, - u.data ? u.ptr() : (float*)&u1, &ldu, - (float*)(buffer + work_ofs), &lwork, - (integer*)(buffer + iwork_ofs), &info ); - } - else - { - dgesdd_(mode, &n, &m, _a.ptr(), &lda, w.ptr(), - vt.data ? vt.ptr() : &v1, &ldv, - u.data ? u.ptr() : &u1, &ldu, - (double*)(buffer + work_ofs), &lwork, - (integer*)(buffer + iwork_ofs), &info ); - } - CV_Assert(info >= 0); - if(info != 0) - { - if( u.data ) - u = Scalar(0.); - if( vt.data ) - vt = Scalar(0.); - w = Scalar(0.); - } -} -////////////////////////////////////////////////////////// -template static void -MatrAXPY( int m, int n, const T1* x, int dx, - const T2* a, int inca, T3* y, int dy ) -{ - int i, j; - for( i = 0; i < m; i++, x += dx, y += dy ) - { - T2 s = a[i*inca]; - for( j = 0; j <= n - 4; j += 4 ) - { - T3 t0 = (T3)(y[j] + s*x[j]); - T3 t1 = (T3)(y[j+1] + s*x[j+1]); - y[j] = t0; - y[j+1] = t1; - t0 = (T3)(y[j+2] + s*x[j+2]); - t1 = (T3)(y[j+3] + s*x[j+3]); - y[j+2] = t0; - y[j+3] = t1; - } - - for( ; j < n; j++ ) - y[j] = (T3)(y[j] + s*x[j]); - } -} -template static void -SVBkSb( int m, int n, const T* w, int incw, - const T* u, int ldu, int uT, - const T* v, int ldv, int vT, - const T* b, int ldb, int nb, - T* x, int ldx, double* buffer, T eps ) -{ - double threshold = 0; - int udelta0 = uT ? ldu : 1, udelta1 = uT ? 1 : ldu; - int vdelta0 = vT ? ldv : 1, vdelta1 = vT ? 1 : ldv; - int i, j, nm = std::min(m, n); - - if( !b ) - nb = m; - - for( i = 0; i < n; i++ ) - for( j = 0; j < nb; j++ ) - x[i*ldx + j] = 0; - - for( i = 0; i < nm; i++ ) - threshold += w[i*incw]; - threshold *= eps; - - // v * inv(w) * uT * b - for( i = 0; i < nm; i++, u += udelta0, v += vdelta0 ) - { - double wi = w[i*incw]; - if( wi <= threshold ) - continue; - wi = 1/wi; - - if( nb == 1 ) - { - double s = 0; - if( b ) - for( j = 0; j < m; j++ ) - s += u[j*udelta1]*b[j*ldb]; - else - s = u[0]; - s *= wi; - - for( j = 0; j < n; j++ ) - x[j*ldx] = (T)(x[j*ldx] + s*v[j*vdelta1]); - } - else - { - if( b ) - { - for( j = 0; j < nb; j++ ) - buffer[j] = 0; - MatrAXPY( m, nb, b, ldb, u, udelta1, buffer, 0 ); - for( j = 0; j < nb; j++ ) - buffer[j] *= wi; - } - else - { - for( j = 0; j < nb; j++ ) - buffer[j] = u[j*udelta1]*wi; - } - MatrAXPY( n, nb, buffer, 0, v, vdelta1, x, ldx ); - } - } -} - -static void _backSubst( const InputArray _w, const InputArray _u, const InputArray _vt, - const InputArray _rhs, OutputArray _dst ) -{ - Mat w = _w.getMat(), u = _u.getMat(), vt = _vt.getMat(), rhs = _rhs.getMat(); - int type = w.type(), esz = (int)w.elemSize(); - int m = u.rows, n = vt.cols, nb = rhs.data ? rhs.cols : m; - AutoBuffer buffer(nb); - CV_Assert( u.data && vt.data && w.data ); - - CV_Assert( rhs.data == 0 || (rhs.type() == type && rhs.rows == m) ); - - _dst.create( n, nb, type ); - Mat dst = _dst.getMat(); - if( type == CV_32F ) - SVBkSb(m, n, (float*)w.data, 1, (float*)u.data, (int)(u.step/esz), false, - (float*)vt.data, (int)(vt.step/esz), true, (float*)rhs.data, (int)(rhs.step/esz), - nb, (float*)dst.data, (int)(dst.step/esz), buffer, 10*FLT_EPSILON ); - else if( type == CV_64F ) - SVBkSb(m, n, (double*)w.data, 1, (double*)u.data, (int)(u.step/esz), false, - (double*)vt.data, (int)(vt.step/esz), true, (double*)rhs.data, (int)(rhs.step/esz), - nb, (double*)dst.data, (int)(dst.step/esz), buffer, 2*DBL_EPSILON ); - else - CV_Error( Error::StsUnsupportedFormat, "" ); -} -/////////////////////////////////////////// - -#define Sf( y, x ) ((float*)(srcdata + y*srcstep))[x] -#define Sd( y, x ) ((double*)(srcdata + y*srcstep))[x] -#define Df( y, x ) ((float*)(dstdata + y*dststep))[x] -#define Dd( y, x ) ((double*)(dstdata + y*dststep))[x] - -double cvfork::invert( InputArray _src, OutputArray _dst, int method ) -{ - Mat src = _src.getMat(); - int type = src.type(); - - CV_Assert(type == CV_32F || type == CV_64F); - - size_t esz = CV_ELEM_SIZE(type); - int m = src.rows, n = src.cols; - - if( method == DECOMP_SVD ) - { - int nm = std::min(m, n); - - AutoBuffer _buf((m*nm + nm + nm*n)*esz + sizeof(double)); - uchar* buf = alignPtr((uchar*)_buf, (int)esz); - Mat u(m, nm, type, buf); - Mat w(nm, 1, type, u.ptr() + m*nm*esz); - Mat vt(nm, n, type, w.ptr() + nm*esz); - - _SVDcompute(src, w, u, vt); - _backSubst(w, u, vt, Mat(), _dst); - - return type == CV_32F ? - (w.ptr()[0] >= FLT_EPSILON ? - w.ptr()[n-1]/w.ptr()[0] : 0) : - (w.ptr()[0] >= DBL_EPSILON ? - w.ptr()[n-1]/w.ptr()[0] : 0); - } - return 0; -} - -#endif //USE_LAPACK diff --git a/apps/interactive-calibration/linalg.hpp b/apps/interactive-calibration/linalg.hpp deleted file mode 100644 index 4a4d2b053b..0000000000 --- a/apps/interactive-calibration/linalg.hpp +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef LINALG_HPP -#define LINALG_HPP - -#include - -namespace cvfork { - -double invert( cv::InputArray _src, cv::OutputArray _dst, int method ); -bool solve(cv::InputArray _src, cv::InputArray _src2arg, cv::OutputArray _dst, int method ); - -} - -#endif diff --git a/apps/interactive-calibration/main.cpp b/apps/interactive-calibration/main.cpp index f41bcbc371..051b24cf2c 100644 --- a/apps/interactive-calibration/main.cpp +++ b/apps/interactive-calibration/main.cpp @@ -12,7 +12,6 @@ #include "calibCommon.hpp" #include "calibPipeline.hpp" #include "frameProcessor.hpp" -#include "cvCalibrationFork.hpp" #include "calibController.hpp" #include "parametersController.hpp" #include "rotationConverters.hpp" @@ -106,7 +105,7 @@ int main(int argc, char** argv) if(!parser.has("v")) globalData->imageSize = capParams.cameraResolution; int calibrationFlags = 0; - if(intParams.fastSolving) calibrationFlags |= CALIB_USE_QR; + if(intParams.fastSolving) calibrationFlags |= cv::CALIB_USE_QR; cv::Ptr controller(new calibController(globalData, calibrationFlags, parser.get("ft"), capParams.minFramesNum)); cv::Ptr dataController(new calibDataController(globalData, capParams.maxFramesNum, @@ -131,11 +130,16 @@ int main(int argc, char** argv) cv::namedWindow(mainWindowName); cv::moveWindow(mainWindowName, 10, 10); #ifdef HAVE_QT - cv::createButton("Delete last frame", deleteButton, &dataController, cv::QT_PUSH_BUTTON); - cv::createButton("Delete all frames", deleteAllButton, &dataController, cv::QT_PUSH_BUTTON); - cv::createButton("Undistort", undistortButton, &showProcessor, cv::QT_CHECKBOX, false); - cv::createButton("Save current parameters", saveCurrentParamsButton, &dataController, cv::QT_PUSH_BUTTON); - cv::createButton("Switch visualisation mode", switchVisualizationModeButton, &showProcessor, cv::QT_PUSH_BUTTON); + cv::createButton("Delete last frame", deleteButton, &dataController, + cv::QT_PUSH_BUTTON | cv::QT_NEW_BUTTONBAR); + cv::createButton("Delete all frames", deleteAllButton, &dataController, + cv::QT_PUSH_BUTTON | cv::QT_NEW_BUTTONBAR); + cv::createButton("Undistort", undistortButton, &showProcessor, + cv::QT_CHECKBOX | cv::QT_NEW_BUTTONBAR, false); + cv::createButton("Save current parameters", saveCurrentParamsButton, &dataController, + cv::QT_PUSH_BUTTON | cv::QT_NEW_BUTTONBAR); + cv::createButton("Switch visualisation mode", switchVisualizationModeButton, &showProcessor, + cv::QT_PUSH_BUTTON | cv::QT_NEW_BUTTONBAR); #endif //HAVE_QT try { bool pipelineFinished = false; @@ -156,10 +160,10 @@ int main(int argc, char** argv) if(capParams.board != chAruco) { globalData->totalAvgErr = - cvfork::calibrateCamera(globalData->objectPoints, globalData->imagePoints, + cv::calibrateCamera(globalData->objectPoints, globalData->imagePoints, globalData->imageSize, globalData->cameraMatrix, globalData->distCoeffs, cv::noArray(), cv::noArray(), - globalData->stdDeviations, globalData->perViewErrors, + globalData->stdDeviations, cv::noArray(), globalData->perViewErrors, calibrationFlags, solverTermCrit); } else { @@ -169,10 +173,10 @@ int main(int argc, char** argv) cv::aruco::CharucoBoard::create(capParams.boardSize.width, capParams.boardSize.height, capParams.charucoSquareLenght, capParams.charucoMarkerSize, dictionary); globalData->totalAvgErr = - cvfork::calibrateCameraCharuco(globalData->allCharucoCorners, globalData->allCharucoIds, + cv::aruco::calibrateCameraCharuco(globalData->allCharucoCorners, globalData->allCharucoIds, charucoboard, globalData->imageSize, globalData->cameraMatrix, globalData->distCoeffs, - cv::noArray(), cv::noArray(), globalData->stdDeviations, + cv::noArray(), cv::noArray(), globalData->stdDeviations, cv::noArray(), globalData->perViewErrors, calibrationFlags, solverTermCrit); } dataController->updateUndistortMap();