void rgbDctDenoising(const Mat &src, Mat &dst, const double sigma, const int psize) { CV_Assert( src.type() == CV_MAKE_TYPE(CV_32F, 3) ); cv::Matx33f mt(cvInvSqrt(3.0f), cvInvSqrt(3.0f), cvInvSqrt(3.0f), cvInvSqrt(2.0f), 0.0f, -cvInvSqrt(2.0f), cvInvSqrt(6.0f), -2.0f*cvInvSqrt(6.0f), cvInvSqrt(6.0f)); cv::transform(src, dst, mt); std::vector <Mat> mv; split(dst, mv); for (size_t i = 0; i < mv.size(); ++i) grayDctDenoising(mv[i], mv[i], sigma, psize); merge(mv, dst); cv::transform( dst, dst, mt.inv() ); }
static CvStatus icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints, float focalLength, CvTermCriteria criteria, float* rotation, float* translation ) { int i, j, k; int count = 0, converged = 0; float inorm, jnorm, invInorm, invJnorm, invScale, scale = 0, inv_Z = 0; float diff = (float)criteria.epsilon; /* Check bad arguments */ if( imagePoints == NULL ) return CV_NULLPTR_ERR; if( pObject == NULL ) return CV_NULLPTR_ERR; if( focalLength <= 0 ) return CV_BADFACTOR_ERR; if( !rotation ) return CV_NULLPTR_ERR; if( !translation ) return CV_NULLPTR_ERR; if( (criteria.type == 0) || (criteria.type > (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS))) return CV_BADFLAG_ERR; if( (criteria.type & CV_TERMCRIT_EPS) && criteria.epsilon < 0 ) return CV_BADFACTOR_ERR; if( (criteria.type & CV_TERMCRIT_ITER) && criteria.max_iter <= 0 ) return CV_BADFACTOR_ERR; /* init variables */ float inv_focalLength = 1 / focalLength; int N = pObject->N; float *objectVectors = pObject->obj_vecs; float *invMatrix = pObject->inv_matr; float *imgVectors = pObject->img_vecs; while( !converged ) { if( count == 0 ) { /* subtract out origin to get image vectors */ for( i = 0; i < N; i++ ) { imgVectors[i] = imagePoints[i + 1].x - imagePoints[0].x; imgVectors[N + i] = imagePoints[i + 1].y - imagePoints[0].y; } } else { diff = 0; /* Compute new SOP (scaled orthograthic projection) image from pose */ for( i = 0; i < N; i++ ) { /* objectVector * k */ float old; float tmp = objectVectors[i] * rotation[6] /*[2][0]*/ + objectVectors[N + i] * rotation[7] /*[2][1]*/ + objectVectors[2 * N + i] * rotation[8] /*[2][2]*/; tmp *= inv_Z; tmp += 1; old = imgVectors[i]; imgVectors[i] = imagePoints[i + 1].x * tmp - imagePoints[0].x; diff = MAX( diff, (float) fabs( imgVectors[i] - old )); old = imgVectors[N + i]; imgVectors[N + i] = imagePoints[i + 1].y * tmp - imagePoints[0].y; diff = MAX( diff, (float) fabs( imgVectors[N + i] - old )); } } /* calculate I and J vectors */ for( i = 0; i < 2; i++ ) { for( j = 0; j < 3; j++ ) { rotation[3*i+j] /*[i][j]*/ = 0; for( k = 0; k < N; k++ ) { rotation[3*i+j] /*[i][j]*/ += invMatrix[j * N + k] * imgVectors[i * N + k]; } } } inorm = rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ + rotation[1] /*[0][1]*/ * rotation[1] /*[0][1]*/ + rotation[2] /*[0][2]*/ * rotation[2] /*[0][2]*/; jnorm = rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ + rotation[4] /*[1][1]*/ * rotation[4] /*[1][1]*/ + rotation[5] /*[1][2]*/ * rotation[5] /*[1][2]*/; invInorm = cvInvSqrt( inorm ); invJnorm = cvInvSqrt( jnorm ); inorm *= invInorm; jnorm *= invJnorm; rotation[0] /*[0][0]*/ *= invInorm; rotation[1] /*[0][1]*/ *= invInorm; rotation[2] /*[0][2]*/ *= invInorm; rotation[3] /*[1][0]*/ *= invJnorm; rotation[4] /*[1][1]*/ *= invJnorm; rotation[5] /*[1][2]*/ *= invJnorm; /* row2 = row0 x row1 (cross product) */ rotation[6] /*->m[2][0]*/ = rotation[1] /*->m[0][1]*/ * rotation[5] /*->m[1][2]*/ - rotation[2] /*->m[0][2]*/ * rotation[4] /*->m[1][1]*/; rotation[7] /*->m[2][1]*/ = rotation[2] /*->m[0][2]*/ * rotation[3] /*->m[1][0]*/ - rotation[0] /*->m[0][0]*/ * rotation[5] /*->m[1][2]*/; rotation[8] /*->m[2][2]*/ = rotation[0] /*->m[0][0]*/ * rotation[4] /*->m[1][1]*/ - rotation[1] /*->m[0][1]*/ * rotation[3] /*->m[1][0]*/; scale = (inorm + jnorm) / 2.0f; inv_Z = scale * inv_focalLength; count++; converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon)); converged |= ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter)); } invScale = 1 / scale; translation[0] = imagePoints[0].x * invScale; translation[1] = imagePoints[0].y * invScale; translation[2] = 1 / inv_Z; return CV_NO_ERR; }
/* for now this function works bad with singular cases You can see in the code, that when some troubles with matrices or some variables occur - box filled with zero values is returned. However in general function works fine. */ static CvStatus icvFitEllipse_32f( CvSeq* points, CvBox2D* box ) { CvStatus status = CV_OK; float u[6]; CvMatr32f D = 0; float S[36]; /* S = D' * D */ float C[36]; float INVQ[36]; /* transposed eigenvectors */ float INVEIGV[36]; /* auxulary matrices */ float TMP1[36]; float TMP2[36]; int i, index = -1; float eigenvalues[6]; float a, b, c, d, e, f; float offx, offy; float *matr; int n = points->total; CvSeqReader reader; int is_float = CV_SEQ_ELTYPE(points) == CV_32FC2; CvMat _S, _EIGVECS, _EIGVALS; /* create matrix D of input points */ D = icvCreateMatrix_32f( 6, n ); offx = offy = 0; cvStartReadSeq( points, &reader ); /* shift all points to zero */ for( i = 0; i < n; i++ ) { if( !is_float ) { offx += (float)((CvPoint*)reader.ptr)->x; offy += (float)((CvPoint*)reader.ptr)->y; } else { offx += ((CvPoint2D32f*)reader.ptr)->x; offy += ((CvPoint2D32f*)reader.ptr)->y; } CV_NEXT_SEQ_ELEM( points->elem_size, reader ); } c = 1.f / n; offx *= c; offy *= c; /* fill matrix rows as (x*x, x*y, y*y, x, y, 1 ) */ matr = D; for( i = 0; i < n; i++ ) { float x, y; if( !is_float ) { x = (float)((CvPoint*)reader.ptr)->x - offx; y = (float)((CvPoint*)reader.ptr)->y - offy; } else { x = ((CvPoint2D32f*)reader.ptr)->x - offx; y = ((CvPoint2D32f*)reader.ptr)->y - offy; } CV_NEXT_SEQ_ELEM( points->elem_size, reader ); matr[0] = x * x; matr[1] = x * y; matr[2] = y * y; matr[3] = x; matr[4] = y; matr[5] = 1.f; matr += 6; } /* compute S */ icvMulTransMatrixR_32f( D, 6, n, S ); /* fill matrix C */ icvSetZero_32f( C, 6, 6 ); C[2] = 2.f; //icvSetElement_32f( C, 6, 6, 0, 2, 2.f ); C[7] = -1.f; //icvSetElement_32f( C, 6, 6, 1, 1, -1.f ); C[12] = 2.f; //icvSetElement_32f( C, 6, 6, 2, 0, 2.f ); /* find eigenvalues */ //status1 = icvJacobiEigens_32f( S, INVEIGV, eigenvalues, 6, 0.f ); //assert( status1 == CV_OK ); _S = cvMat( 6, 6, CV_32F, S ); _EIGVECS = cvMat( 6, 6, CV_32F, INVEIGV ); _EIGVALS = cvMat( 6, 1, CV_32F, eigenvalues ); cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 ); //avoid troubles with small negative values for( i = 0; i < 6; i++ ) eigenvalues[i] = (float)fabs(eigenvalues[i]); cvbSqrt( eigenvalues, eigenvalues, 6 ); cvbInvSqrt( eigenvalues, eigenvalues, 6 ); for( i = 0; i < 6; i++ ) icvScaleVector_32f( &INVEIGV[i * 6], &INVEIGV[i * 6], 6, eigenvalues[i] ); // INVQ = transp(INVEIGV) * INVEIGV icvMulTransMatrixR_32f( INVEIGV, 6, 6, INVQ ); /* create matrix INVQ*C*INVQ */ icvMulMatrix_32f( INVQ, 6, 6, C, 6, 6, TMP1 ); icvMulMatrix_32f( TMP1, 6, 6, INVQ, 6, 6, TMP2 ); /* find its eigenvalues and vectors */ //status1 = icvJacobiEigens_32f( TMP2, INVEIGV, eigenvalues, 6, 0.f ); //assert( status1 == CV_OK ); _S = cvMat( 6, 6, CV_32F, TMP2 ); cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 ); /* search for positive eigenvalue */ for( i = 0; i < 3; i++ ) { if( eigenvalues[i] > 0 ) { index = i; break; } } /* only 3 eigenvalues must be not zero and only one of them must be positive if it is not true - return zero result */ if( index == -1 ) { box->center.x = box->center.y = box->size.width = box->size.height = box->angle = 0.f; goto error; } /* now find truthful eigenvector */ icvTransformVector_32f( INVQ, &INVEIGV[index * 6], u, 6, 6 ); /* extract vector components */ a = u[0]; b = u[1]; c = u[2]; d = u[3]; e = u[4]; f = u[5]; { /* extract ellipse axes from above values */ /* 1) find center of ellipse it satisfy equation | a b/2 | * | x0 | + | d/2 | = |0 | | b/2 c | | y0 | | e/2 | |0 | */ float x0, y0; float idet = 1.f / (a * c - b * b * 0.25f); /* we must normalize (a b c d e f ) to fit (4ac-b^2=1) */ float scale = cvSqrt( 0.25f * idet ); if (!scale) { box->center.x = box->center.y = box->size.width = box->size.height = box->angle = 0.f; goto error; } a *= scale; b *= scale; c *= scale; d *= scale; e *= scale; f *= scale; //x0 = box->center.x = (-d * c * 0.5f + e * b * 0.25f) * 4.f; //y0 = box->center.y = (-a * e * 0.5f + d * b * 0.25f) * 4.f; x0 = box->center.x = (-d * c + e * b * 0.5f) * 2.f; y0 = box->center.y = (-a * e + d * b * 0.5f) * 2.f; /* offset ellipse to (x0,y0) */ /* new f == F(x0,y0) */ f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0; if (!f) { box->center.x = box->center.y = box->size.width = box->size.height = box->angle = 0.f; goto error; } scale = -1.f / f; /* normalize to f = 1 */ a *= scale; b *= scale; c *= scale; } /* recover center */ box->center.x += offx; box->center.y += offy; /* extract axis of ellipse */ /* one more eigenvalue operation */ TMP1[0] = a; TMP1[1] = TMP1[2] = b * 0.5f; TMP1[3] = c; //status1 = icvJacobiEigens_32f( TMP1, INVEIGV, eigenvalues, 2, 0.f ); //assert( status1 == CV_OK ); _S = cvMat( 2, 2, CV_32F, TMP1 ); _EIGVECS = cvMat( 2, 2, CV_32F, INVEIGV ); _EIGVALS = cvMat( 2, 1, CV_32F, eigenvalues ); cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 ); /* exteract axis length from eigenvectors */ box->size.height = 2 * cvInvSqrt( eigenvalues[0] ); box->size.width = 2 * cvInvSqrt( eigenvalues[1] ); if ( !(box->size.height && box->size.width) ) { assert(0); } /* calc angle */ box->angle = cvFastArctan( INVEIGV[3], INVEIGV[2] ); error: if( D ) icvDeleteMatrix( D ); return status; }