+++ /dev/null
-#include <opencv2/calib3d.hpp>
-#include "linalg.hpp"
-#include "cvCalibrationFork.hpp"
-
-using namespace cv;
-
-static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
- const std::vector<uchar>& 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<Point3d>(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<CvMat> 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<double>(s) = std::sqrt(JtJinv.at<double>(j,j)*sigma2);
- j++;
- }
- else
- stdDevsM.at<double>(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<Point2f>();
- }
-
- Point3f* objPtData = objPtMat.ptr<Point3f>();
- Point2f* imgPtData1 = imgPtMat1.ptr<Point2f>();
-
- 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<int>(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<Mat>
- 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<aruco::CharucoBoard> &_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<uchar>& cols,
- const std::vector<uchar>& 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_<double> nonzero_param = cvarrToMat(JtJW);
-
- subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(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;
-}
+++ /dev/null
-#include "linalg.hpp"
-
-#ifdef USE_LAPACK
-
-typedef int integer;
-#include <lapacke.h>
-
-#include <cassert>
-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<uchar> 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<uchar> 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<float>(), &lda, w.ptr<float>(),
- vt.data ? vt.ptr<float>() : (float*)&v1, &ldv,
- u.data ? u.ptr<float>() : (float*)&u1, &ldu,
- (float*)(buffer + work_ofs), &lwork,
- (integer*)(buffer + iwork_ofs), &info );
- }
- else
- {
- dgesdd_(mode, &n, &m, _a.ptr<double>(), &lda, w.ptr<double>(),
- vt.data ? vt.ptr<double>() : &v1, &ldv,
- u.data ? u.ptr<double>() : &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<typename T1, typename T2, typename T3> 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<typename T> 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<double> 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<uchar> _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<float>()[0] >= FLT_EPSILON ?
- w.ptr<float>()[n-1]/w.ptr<float>()[0] : 0) :
- (w.ptr<double>()[0] >= DBL_EPSILON ?
- w.ptr<double>()[n-1]/w.ptr<double>()[0] : 0);
- }
- return 0;
-}
-
-#endif //USE_LAPACK
projects / platform / upstream / opencv.git / commitdiff