| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| #include "_cv.h" |
| #include <float.h> |
| #include <stdio.h> |
| |
| static void |
| intersect( CvPoint2D32f pt, CvSize win_size, CvSize imgSize, |
| CvPoint* min_pt, CvPoint* max_pt ) |
| { |
| CvPoint ipt; |
| |
| ipt.x = cvFloor( pt.x ); |
| ipt.y = cvFloor( pt.y ); |
| |
| ipt.x -= win_size.width; |
| ipt.y -= win_size.height; |
| |
| win_size.width = win_size.width * 2 + 1; |
| win_size.height = win_size.height * 2 + 1; |
| |
| min_pt->x = MAX( 0, -ipt.x ); |
| min_pt->y = MAX( 0, -ipt.y ); |
| max_pt->x = MIN( win_size.width, imgSize.width - ipt.x ); |
| max_pt->y = MIN( win_size.height, imgSize.height - ipt.y ); |
| } |
| |
| |
| static int icvMinimalPyramidSize( CvSize imgSize ) |
| { |
| return cvAlign(imgSize.width,8) * imgSize.height / 3; |
| } |
| |
| |
| static void |
| icvInitPyramidalAlgorithm( const CvMat* imgA, const CvMat* imgB, |
| CvMat* pyrA, CvMat* pyrB, |
| int level, CvTermCriteria * criteria, |
| int max_iters, int flags, |
| uchar *** imgI, uchar *** imgJ, |
| int **step, CvSize** size, |
| double **scale, uchar ** buffer ) |
| { |
| CV_FUNCNAME( "icvInitPyramidalAlgorithm" ); |
| |
| __BEGIN__; |
| |
| const int ALIGN = 8; |
| int pyrBytes, bufferBytes = 0, elem_size; |
| int level1 = level + 1; |
| |
| int i; |
| CvSize imgSize, levelSize; |
| |
| *buffer = 0; |
| *imgI = *imgJ = 0; |
| *step = 0; |
| *scale = 0; |
| *size = 0; |
| |
| /* check input arguments */ |
| if( ((flags & CV_LKFLOW_PYR_A_READY) != 0 && !pyrA) || |
| ((flags & CV_LKFLOW_PYR_B_READY) != 0 && !pyrB) ) |
| CV_ERROR( CV_StsNullPtr, "Some of the precomputed pyramids are missing" ); |
| |
| if( level < 0 ) |
| CV_ERROR( CV_StsOutOfRange, "The number of pyramid layers is negative" ); |
| |
| switch( criteria->type ) |
| { |
| case CV_TERMCRIT_ITER: |
| criteria->epsilon = 0.f; |
| break; |
| case CV_TERMCRIT_EPS: |
| criteria->max_iter = max_iters; |
| break; |
| case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS: |
| break; |
| default: |
| assert( 0 ); |
| CV_ERROR( CV_StsBadArg, "Invalid termination criteria" ); |
| } |
| |
| /* compare squared values */ |
| criteria->epsilon *= criteria->epsilon; |
| |
| /* set pointers and step for every level */ |
| pyrBytes = 0; |
| |
| imgSize = cvGetSize(imgA); |
| elem_size = CV_ELEM_SIZE(imgA->type); |
| levelSize = imgSize; |
| |
| for( i = 1; i < level1; i++ ) |
| { |
| levelSize.width = (levelSize.width + 1) >> 1; |
| levelSize.height = (levelSize.height + 1) >> 1; |
| |
| int tstep = cvAlign(levelSize.width,ALIGN) * elem_size; |
| pyrBytes += tstep * levelSize.height; |
| } |
| |
| assert( pyrBytes <= imgSize.width * imgSize.height * elem_size * 4 / 3 ); |
| |
| /* buffer_size = <size for patches> + <size for pyramids> */ |
| bufferBytes = (int)((level1 >= 0) * ((pyrA->data.ptr == 0) + |
| (pyrB->data.ptr == 0)) * pyrBytes + |
| (sizeof(imgI[0][0]) * 2 + sizeof(step[0][0]) + |
| sizeof(size[0][0]) + sizeof(scale[0][0])) * level1); |
| |
| CV_CALL( *buffer = (uchar *)cvAlloc( bufferBytes )); |
| |
| *imgI = (uchar **) buffer[0]; |
| *imgJ = *imgI + level1; |
| *step = (int *) (*imgJ + level1); |
| *scale = (double *) (*step + level1); |
| *size = (CvSize *)(*scale + level1); |
| |
| imgI[0][0] = imgA->data.ptr; |
| imgJ[0][0] = imgB->data.ptr; |
| step[0][0] = imgA->step; |
| scale[0][0] = 1; |
| size[0][0] = imgSize; |
| |
| if( level > 0 ) |
| { |
| uchar *bufPtr = (uchar *) (*size + level1); |
| uchar *ptrA = pyrA->data.ptr; |
| uchar *ptrB = pyrB->data.ptr; |
| |
| if( !ptrA ) |
| { |
| ptrA = bufPtr; |
| bufPtr += pyrBytes; |
| } |
| |
| if( !ptrB ) |
| ptrB = bufPtr; |
| |
| levelSize = imgSize; |
| |
| /* build pyramids for both frames */ |
| for( i = 1; i <= level; i++ ) |
| { |
| int levelBytes; |
| CvMat prev_level, next_level; |
| |
| levelSize.width = (levelSize.width + 1) >> 1; |
| levelSize.height = (levelSize.height + 1) >> 1; |
| |
| size[0][i] = levelSize; |
| step[0][i] = cvAlign( levelSize.width, ALIGN ) * elem_size; |
| scale[0][i] = scale[0][i - 1] * 0.5; |
| |
| levelBytes = step[0][i] * levelSize.height; |
| imgI[0][i] = (uchar *) ptrA; |
| ptrA += levelBytes; |
| |
| if( !(flags & CV_LKFLOW_PYR_A_READY) ) |
| { |
| prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 ); |
| next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 ); |
| cvSetData( &prev_level, imgI[0][i-1], step[0][i-1] ); |
| cvSetData( &next_level, imgI[0][i], step[0][i] ); |
| cvPyrDown( &prev_level, &next_level ); |
| } |
| |
| imgJ[0][i] = (uchar *) ptrB; |
| ptrB += levelBytes; |
| |
| if( !(flags & CV_LKFLOW_PYR_B_READY) ) |
| { |
| prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 ); |
| next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 ); |
| cvSetData( &prev_level, imgJ[0][i-1], step[0][i-1] ); |
| cvSetData( &next_level, imgJ[0][i], step[0][i] ); |
| cvPyrDown( &prev_level, &next_level ); |
| } |
| } |
| } |
| |
| __END__; |
| } |
| |
| |
| /* compute dI/dx and dI/dy */ |
| static void |
| icvCalcIxIy_32f( const float* src, int src_step, float* dstX, float* dstY, int dst_step, |
| CvSize src_size, const float* smooth_k, float* buffer0 ) |
| { |
| int src_width = src_size.width, dst_width = src_size.width-2; |
| int x, height = src_size.height - 2; |
| float* buffer1 = buffer0 + src_width; |
| |
| src_step /= sizeof(src[0]); |
| dst_step /= sizeof(dstX[0]); |
| |
| for( ; height--; src += src_step, dstX += dst_step, dstY += dst_step ) |
| { |
| const float* src2 = src + src_step; |
| const float* src3 = src + src_step*2; |
| |
| for( x = 0; x < src_width; x++ ) |
| { |
| float t0 = (src3[x] + src[x])*smooth_k[0] + src2[x]*smooth_k[1]; |
| float t1 = src3[x] - src[x]; |
| buffer0[x] = t0; buffer1[x] = t1; |
| } |
| |
| for( x = 0; x < dst_width; x++ ) |
| { |
| float t0 = buffer0[x+2] - buffer0[x]; |
| float t1 = (buffer1[x] + buffer1[x+2])*smooth_k[0] + buffer1[x+1]*smooth_k[1]; |
| dstX[x] = t0; dstY[x] = t1; |
| } |
| } |
| } |
| |
| |
| icvOpticalFlowPyrLKInitAlloc_8u_C1R_t icvOpticalFlowPyrLKInitAlloc_8u_C1R_p = 0; |
| icvOpticalFlowPyrLKFree_8u_C1R_t icvOpticalFlowPyrLKFree_8u_C1R_p = 0; |
| icvOpticalFlowPyrLK_8u_C1R_t icvOpticalFlowPyrLK_8u_C1R_p = 0; |
| |
| |
| CV_IMPL void |
| cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB, |
| void* pyrarrA, void* pyrarrB, |
| const CvPoint2D32f * featuresA, |
| CvPoint2D32f * featuresB, |
| int count, CvSize winSize, int level, |
| char *status, float *error, |
| CvTermCriteria criteria, int flags ) |
| { |
| uchar *pyrBuffer = 0; |
| uchar *buffer = 0; |
| float* _error = 0; |
| char* _status = 0; |
| |
| void* ipp_optflow_state = 0; |
| |
| CV_FUNCNAME( "cvCalcOpticalFlowPyrLK" ); |
| |
| __BEGIN__; |
| |
| const int MAX_ITERS = 100; |
| |
| CvMat stubA, *imgA = (CvMat*)arrA; |
| CvMat stubB, *imgB = (CvMat*)arrB; |
| CvMat pstubA, *pyrA = (CvMat*)pyrarrA; |
| CvMat pstubB, *pyrB = (CvMat*)pyrarrB; |
| CvSize imgSize; |
| static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */ |
| |
| int bufferBytes = 0; |
| uchar **imgI = 0; |
| uchar **imgJ = 0; |
| int *step = 0; |
| double *scale = 0; |
| CvSize* size = 0; |
| |
| int threadCount = cvGetNumThreads(); |
| float* _patchI[CV_MAX_THREADS]; |
| float* _patchJ[CV_MAX_THREADS]; |
| float* _Ix[CV_MAX_THREADS]; |
| float* _Iy[CV_MAX_THREADS]; |
| |
| int i, l; |
| |
| CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); |
| int patchLen = patchSize.width * patchSize.height; |
| int srcPatchLen = (patchSize.width + 2)*(patchSize.height + 2); |
| |
| CV_CALL( imgA = cvGetMat( imgA, &stubA )); |
| CV_CALL( imgB = cvGetMat( imgB, &stubB )); |
| |
| if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 ) |
| CV_ERROR( CV_StsUnsupportedFormat, "" ); |
| |
| if( !CV_ARE_TYPES_EQ( imgA, imgB )) |
| CV_ERROR( CV_StsUnmatchedFormats, "" ); |
| |
| if( !CV_ARE_SIZES_EQ( imgA, imgB )) |
| CV_ERROR( CV_StsUnmatchedSizes, "" ); |
| |
| if( imgA->step != imgB->step ) |
| CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" ); |
| |
| imgSize = cvGetMatSize( imgA ); |
| |
| if( pyrA ) |
| { |
| CV_CALL( pyrA = cvGetMat( pyrA, &pstubA )); |
| |
| if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) ) |
| CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" ); |
| } |
| else |
| { |
| pyrA = &pstubA; |
| pyrA->data.ptr = 0; |
| } |
| |
| if( pyrB ) |
| { |
| CV_CALL( pyrB = cvGetMat( pyrB, &pstubB )); |
| |
| if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) ) |
| CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" ); |
| } |
| else |
| { |
| pyrB = &pstubB; |
| pyrB->data.ptr = 0; |
| } |
| |
| if( count == 0 ) |
| EXIT; |
| |
| if( !featuresA || !featuresB ) |
| CV_ERROR( CV_StsNullPtr, "Some of arrays of point coordinates are missing" ); |
| |
| if( count < 0 ) |
| CV_ERROR( CV_StsOutOfRange, "The number of tracked points is negative or zero" ); |
| |
| if( winSize.width <= 1 || winSize.height <= 1 ) |
| CV_ERROR( CV_StsBadSize, "Invalid search window size" ); |
| |
| for( i = 0; i < threadCount; i++ ) |
| _patchI[i] = _patchJ[i] = _Ix[i] = _Iy[i] = 0; |
| |
| CV_CALL( icvInitPyramidalAlgorithm( imgA, imgB, pyrA, pyrB, |
| level, &criteria, MAX_ITERS, flags, |
| &imgI, &imgJ, &step, &size, &scale, &pyrBuffer )); |
| |
| if( !status ) |
| CV_CALL( status = _status = (char*)cvAlloc( count*sizeof(_status[0]) )); |
| |
| #if 0 |
| if( icvOpticalFlowPyrLKInitAlloc_8u_C1R_p && |
| icvOpticalFlowPyrLKFree_8u_C1R_p && |
| icvOpticalFlowPyrLK_8u_C1R_p && |
| winSize.width == winSize.height && |
| icvOpticalFlowPyrLKInitAlloc_8u_C1R_p( &ipp_optflow_state, imgSize, |
| winSize.width*2+1, cvAlgHintAccurate ) >= 0 ) |
| { |
| CvPyramid ipp_pyrA, ipp_pyrB; |
| static const double rate[] = { 1, 0.5, 0.25, 0.125, 0.0625, 0.03125, 0.015625, 0.0078125, |
| 0.00390625, 0.001953125, 0.0009765625, 0.00048828125, 0.000244140625, |
| 0.0001220703125 }; |
| // initialize pyramid structures |
| assert( level < 14 ); |
| ipp_pyrA.ptr = imgI; |
| ipp_pyrB.ptr = imgJ; |
| ipp_pyrA.sz = ipp_pyrB.sz = size; |
| ipp_pyrA.rate = ipp_pyrB.rate = (double*)rate; |
| ipp_pyrA.step = ipp_pyrB.step = step; |
| ipp_pyrA.state = ipp_pyrB.state = 0; |
| ipp_pyrA.level = ipp_pyrB.level = level; |
| |
| if( !error ) |
| CV_CALL( error = _error = (float*)cvAlloc( count*sizeof(_error[0]) )); |
| |
| for( i = 0; i < count; i++ ) |
| featuresB[i] = featuresA[i]; |
| |
| if( icvOpticalFlowPyrLK_8u_C1R_p( &ipp_pyrA, &ipp_pyrB, |
| (const float*)featuresA, (float*)featuresB, status, error, count, |
| winSize.width*2 + 1, level, criteria.max_iter, |
| (float)criteria.epsilon, ipp_optflow_state ) >= 0 ) |
| { |
| for( i = 0; i < count; i++ ) |
| status[i] = status[i] == 0; |
| EXIT; |
| } |
| } |
| #endif |
| |
| /* buffer_size = <size for patches> + <size for pyramids> */ |
| bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( _patchI[0][0] ) * threadCount; |
| CV_CALL( buffer = (uchar*)cvAlloc( bufferBytes )); |
| |
| for( i = 0; i < threadCount; i++ ) |
| { |
| _patchI[i] = i == 0 ? (float*)buffer : _Iy[i-1] + patchLen; |
| _patchJ[i] = _patchI[i] + srcPatchLen; |
| _Ix[i] = _patchJ[i] + patchLen; |
| _Iy[i] = _Ix[i] + patchLen; |
| } |
| |
| memset( status, 1, count ); |
| if( error ) |
| memset( error, 0, count*sizeof(error[0]) ); |
| |
| if( !(flags & CV_LKFLOW_INITIAL_GUESSES) ) |
| memcpy( featuresB, featuresA, count*sizeof(featuresA[0])); |
| |
| /* do processing from top pyramid level (smallest image) |
| to the bottom (original image) */ |
| for( l = level; l >= 0; l-- ) |
| { |
| CvSize levelSize = size[l]; |
| int levelStep = step[l]; |
| |
| { |
| #ifdef _OPENMP |
| #pragma omp parallel for num_threads(threadCount) schedule(dynamic) |
| #endif // _OPENMP |
| /* find flow for each given point */ |
| for( i = 0; i < count; i++ ) |
| { |
| CvPoint2D32f v; |
| CvPoint minI, maxI, minJ, maxJ; |
| CvSize isz, jsz; |
| int pt_status; |
| CvPoint2D32f u; |
| CvPoint prev_minJ = { -1, -1 }, prev_maxJ = { -1, -1 }; |
| double Gxx = 0, Gxy = 0, Gyy = 0, D = 0, minEig = 0; |
| float prev_mx = 0, prev_my = 0; |
| int j, x, y; |
| int threadIdx = cvGetThreadNum(); |
| float* patchI = _patchI[threadIdx]; |
| float* patchJ = _patchJ[threadIdx]; |
| float* Ix = _Ix[threadIdx]; |
| float* Iy = _Iy[threadIdx]; |
| |
| v.x = featuresB[i].x; |
| v.y = featuresB[i].y; |
| if( l < level ) |
| { |
| v.x += v.x; |
| v.y += v.y; |
| } |
| else |
| { |
| v.x = (float)(v.x * scale[l]); |
| v.y = (float)(v.y * scale[l]); |
| } |
| |
| pt_status = status[i]; |
| if( !pt_status ) |
| continue; |
| |
| minI = maxI = minJ = maxJ = cvPoint( 0, 0 ); |
| |
| u.x = (float) (featuresA[i].x * scale[l]); |
| u.y = (float) (featuresA[i].y * scale[l]); |
| |
| intersect( u, winSize, levelSize, &minI, &maxI ); |
| isz = jsz = cvSize(maxI.x - minI.x + 2, maxI.y - minI.y + 2); |
| u.x += (minI.x - (patchSize.width - maxI.x + 1))*0.5f; |
| u.y += (minI.y - (patchSize.height - maxI.y + 1))*0.5f; |
| |
| if( isz.width < 3 || isz.height < 3 || |
| icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, levelSize, |
| patchI, isz.width*sizeof(patchI[0]), isz, u ) < 0 ) |
| { |
| /* point is outside the image. take the next */ |
| status[i] = 0; |
| continue; |
| } |
| |
| icvCalcIxIy_32f( patchI, isz.width*sizeof(patchI[0]), Ix, Iy, |
| (isz.width-2)*sizeof(patchI[0]), isz, smoothKernel, patchJ ); |
| |
| for( j = 0; j < criteria.max_iter; j++ ) |
| { |
| double bx = 0, by = 0; |
| float mx, my; |
| CvPoint2D32f _v; |
| |
| intersect( v, winSize, levelSize, &minJ, &maxJ ); |
| |
| minJ.x = MAX( minJ.x, minI.x ); |
| minJ.y = MAX( minJ.y, minI.y ); |
| |
| maxJ.x = MIN( maxJ.x, maxI.x ); |
| maxJ.y = MIN( maxJ.y, maxI.y ); |
| |
| jsz = cvSize(maxJ.x - minJ.x, maxJ.y - minJ.y); |
| |
| _v.x = v.x + (minJ.x - (patchSize.width - maxJ.x + 1))*0.5f; |
| _v.y = v.y + (minJ.y - (patchSize.height - maxJ.y + 1))*0.5f; |
| |
| if( jsz.width < 1 || jsz.height < 1 || |
| icvGetRectSubPix_8u32f_C1R( imgJ[l], levelStep, levelSize, patchJ, |
| jsz.width*sizeof(patchJ[0]), jsz, _v ) < 0 ) |
| { |
| /* point is outside image. take the next */ |
| pt_status = 0; |
| break; |
| } |
| |
| if( maxJ.x == prev_maxJ.x && maxJ.y == prev_maxJ.y && |
| minJ.x == prev_minJ.x && minJ.y == prev_minJ.y ) |
| { |
| for( y = 0; y < jsz.height; y++ ) |
| { |
| const float* pi = patchI + |
| (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; |
| const float* pj = patchJ + y*jsz.width; |
| const float* ix = Ix + |
| (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x; |
| const float* iy = Iy + (ix - Ix); |
| |
| for( x = 0; x < jsz.width; x++ ) |
| { |
| double t0 = pi[x] - pj[x]; |
| bx += t0 * ix[x]; |
| by += t0 * iy[x]; |
| } |
| } |
| } |
| else |
| { |
| Gxx = Gyy = Gxy = 0; |
| for( y = 0; y < jsz.height; y++ ) |
| { |
| const float* pi = patchI + |
| (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; |
| const float* pj = patchJ + y*jsz.width; |
| const float* ix = Ix + |
| (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x; |
| const float* iy = Iy + (ix - Ix); |
| |
| for( x = 0; x < jsz.width; x++ ) |
| { |
| double t = pi[x] - pj[x]; |
| bx += (double) (t * ix[x]); |
| by += (double) (t * iy[x]); |
| Gxx += ix[x] * ix[x]; |
| Gxy += ix[x] * iy[x]; |
| Gyy += iy[x] * iy[x]; |
| } |
| } |
| |
| D = Gxx * Gyy - Gxy * Gxy; |
| if( D < DBL_EPSILON ) |
| { |
| pt_status = 0; |
| break; |
| } |
| |
| // Adi Shavit - 2008.05 |
| if( flags & CV_LKFLOW_GET_MIN_EIGENVALS ) |
| minEig = (Gyy + Gxx - sqrt((Gxx-Gyy)*(Gxx-Gyy) + 4.*Gxy*Gxy))/(2*jsz.height*jsz.width); |
| |
| D = 1. / D; |
| |
| prev_minJ = minJ; |
| prev_maxJ = maxJ; |
| } |
| |
| mx = (float) ((Gyy * bx - Gxy * by) * D); |
| my = (float) ((Gxx * by - Gxy * bx) * D); |
| |
| v.x += mx; |
| v.y += my; |
| |
| if( mx * mx + my * my < criteria.epsilon ) |
| break; |
| |
| if( j > 0 && fabs(mx + prev_mx) < 0.01 && fabs(my + prev_my) < 0.01 ) |
| { |
| v.x -= mx*0.5f; |
| v.y -= my*0.5f; |
| break; |
| } |
| prev_mx = mx; |
| prev_my = my; |
| } |
| |
| featuresB[i] = v; |
| status[i] = (char)pt_status; |
| if( l == 0 && error && pt_status ) |
| { |
| /* calc error */ |
| double err = 0; |
| if( flags & CV_LKFLOW_GET_MIN_EIGENVALS ) |
| err = minEig; |
| else |
| { |
| for( y = 0; y < jsz.height; y++ ) |
| { |
| const float* pi = patchI + |
| (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; |
| const float* pj = patchJ + y*jsz.width; |
| |
| for( x = 0; x < jsz.width; x++ ) |
| { |
| double t = pi[x] - pj[x]; |
| err += t * t; |
| } |
| } |
| err = sqrt(err); |
| } |
| error[i] = (float)err; |
| } |
| } // end of point processing loop (i) |
| } |
| } // end of pyramid levels loop (l) |
| |
| __END__; |
| |
| if( ipp_optflow_state ) |
| icvOpticalFlowPyrLKFree_8u_C1R_p( ipp_optflow_state ); |
| |
| cvFree( &pyrBuffer ); |
| cvFree( &buffer ); |
| cvFree( &_error ); |
| cvFree( &_status ); |
| } |
| |
| |
| /* Affine tracking algorithm */ |
| |
| CV_IMPL void |
| cvCalcAffineFlowPyrLK( const void* arrA, const void* arrB, |
| void* pyrarrA, void* pyrarrB, |
| const CvPoint2D32f * featuresA, |
| CvPoint2D32f * featuresB, |
| float *matrices, int count, |
| CvSize winSize, int level, |
| char *status, float *error, |
| CvTermCriteria criteria, int flags ) |
| { |
| const int MAX_ITERS = 100; |
| |
| char* _status = 0; |
| uchar *buffer = 0; |
| uchar *pyr_buffer = 0; |
| |
| CV_FUNCNAME( "cvCalcAffineFlowPyrLK" ); |
| |
| __BEGIN__; |
| |
| CvMat stubA, *imgA = (CvMat*)arrA; |
| CvMat stubB, *imgB = (CvMat*)arrB; |
| CvMat pstubA, *pyrA = (CvMat*)pyrarrA; |
| CvMat pstubB, *pyrB = (CvMat*)pyrarrB; |
| |
| static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */ |
| |
| int bufferBytes = 0; |
| |
| uchar **imgI = 0; |
| uchar **imgJ = 0; |
| int *step = 0; |
| double *scale = 0; |
| CvSize* size = 0; |
| |
| float *patchI; |
| float *patchJ; |
| float *Ix; |
| float *Iy; |
| |
| int i, j, k, l; |
| |
| CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); |
| int patchLen = patchSize.width * patchSize.height; |
| int patchStep = patchSize.width * sizeof( patchI[0] ); |
| |
| CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 ); |
| int srcPatchLen = srcPatchSize.width * srcPatchSize.height; |
| int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] ); |
| CvSize imgSize; |
| float eps = (float)MIN(winSize.width, winSize.height); |
| |
| CV_CALL( imgA = cvGetMat( imgA, &stubA )); |
| CV_CALL( imgB = cvGetMat( imgB, &stubB )); |
| |
| if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 ) |
| CV_ERROR( CV_StsUnsupportedFormat, "" ); |
| |
| if( !CV_ARE_TYPES_EQ( imgA, imgB )) |
| CV_ERROR( CV_StsUnmatchedFormats, "" ); |
| |
| if( !CV_ARE_SIZES_EQ( imgA, imgB )) |
| CV_ERROR( CV_StsUnmatchedSizes, "" ); |
| |
| if( imgA->step != imgB->step ) |
| CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" ); |
| |
| if( !matrices ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| imgSize = cvGetMatSize( imgA ); |
| |
| if( pyrA ) |
| { |
| CV_CALL( pyrA = cvGetMat( pyrA, &pstubA )); |
| |
| if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) ) |
| CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" ); |
| } |
| else |
| { |
| pyrA = &pstubA; |
| pyrA->data.ptr = 0; |
| } |
| |
| if( pyrB ) |
| { |
| CV_CALL( pyrB = cvGetMat( pyrB, &pstubB )); |
| |
| if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) ) |
| CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" ); |
| } |
| else |
| { |
| pyrB = &pstubB; |
| pyrB->data.ptr = 0; |
| } |
| |
| if( count == 0 ) |
| EXIT; |
| |
| /* check input arguments */ |
| if( !featuresA || !featuresB || !matrices ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| if( winSize.width <= 1 || winSize.height <= 1 ) |
| CV_ERROR( CV_StsOutOfRange, "the search window is too small" ); |
| |
| if( count < 0 ) |
| CV_ERROR( CV_StsOutOfRange, "" ); |
| |
| CV_CALL( icvInitPyramidalAlgorithm( imgA, imgB, |
| pyrA, pyrB, level, &criteria, MAX_ITERS, flags, |
| &imgI, &imgJ, &step, &size, &scale, &pyr_buffer )); |
| |
| /* buffer_size = <size for patches> + <size for pyramids> */ |
| bufferBytes = (srcPatchLen + patchLen*3)*sizeof(patchI[0]) + (36*2 + 6)*sizeof(double); |
| |
| CV_CALL( buffer = (uchar*)cvAlloc(bufferBytes)); |
| |
| if( !status ) |
| CV_CALL( status = _status = (char*)cvAlloc(count) ); |
| |
| patchI = (float *) buffer; |
| patchJ = patchI + srcPatchLen; |
| Ix = patchJ + patchLen; |
| Iy = Ix + patchLen; |
| |
| if( status ) |
| memset( status, 1, count ); |
| |
| if( !(flags & CV_LKFLOW_INITIAL_GUESSES) ) |
| { |
| memcpy( featuresB, featuresA, count * sizeof( featuresA[0] )); |
| for( i = 0; i < count * 4; i += 4 ) |
| { |
| matrices[i] = matrices[i + 3] = 1.f; |
| matrices[i + 1] = matrices[i + 2] = 0.f; |
| } |
| } |
| |
| for( i = 0; i < count; i++ ) |
| { |
| featuresB[i].x = (float)(featuresB[i].x * scale[level] * 0.5); |
| featuresB[i].y = (float)(featuresB[i].y * scale[level] * 0.5); |
| } |
| |
| /* do processing from top pyramid level (smallest image) |
| to the bottom (original image) */ |
| for( l = level; l >= 0; l-- ) |
| { |
| CvSize levelSize = size[l]; |
| int levelStep = step[l]; |
| |
| /* find flow for each given point at the particular level */ |
| for( i = 0; i < count; i++ ) |
| { |
| CvPoint2D32f u; |
| float Av[6]; |
| double G[36]; |
| double meanI = 0, meanJ = 0; |
| int x, y; |
| int pt_status = status[i]; |
| CvMat mat; |
| |
| if( !pt_status ) |
| continue; |
| |
| Av[0] = matrices[i*4]; |
| Av[1] = matrices[i*4+1]; |
| Av[3] = matrices[i*4+2]; |
| Av[4] = matrices[i*4+3]; |
| |
| Av[2] = featuresB[i].x += featuresB[i].x; |
| Av[5] = featuresB[i].y += featuresB[i].y; |
| |
| u.x = (float) (featuresA[i].x * scale[l]); |
| u.y = (float) (featuresA[i].y * scale[l]); |
| |
| if( u.x < -eps || u.x >= levelSize.width+eps || |
| u.y < -eps || u.y >= levelSize.height+eps || |
| icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, |
| levelSize, patchI, srcPatchStep, srcPatchSize, u ) < 0 ) |
| { |
| /* point is outside the image. take the next */ |
| if( l == 0 ) |
| status[i] = 0; |
| continue; |
| } |
| |
| icvCalcIxIy_32f( patchI, srcPatchStep, Ix, Iy, |
| (srcPatchSize.width-2)*sizeof(patchI[0]), srcPatchSize, |
| smoothKernel, patchJ ); |
| |
| /* repack patchI (remove borders) */ |
| for( k = 0; k < patchSize.height; k++ ) |
| memcpy( patchI + k * patchSize.width, |
| patchI + (k + 1) * srcPatchSize.width + 1, patchStep ); |
| |
| memset( G, 0, sizeof( G )); |
| |
| /* calculate G matrix */ |
| for( y = -winSize.height, k = 0; y <= winSize.height; y++ ) |
| { |
| for( x = -winSize.width; x <= winSize.width; x++, k++ ) |
| { |
| double ixix = ((double) Ix[k]) * Ix[k]; |
| double ixiy = ((double) Ix[k]) * Iy[k]; |
| double iyiy = ((double) Iy[k]) * Iy[k]; |
| |
| double xx, xy, yy; |
| |
| G[0] += ixix; |
| G[1] += ixiy; |
| G[2] += x * ixix; |
| G[3] += y * ixix; |
| G[4] += x * ixiy; |
| G[5] += y * ixiy; |
| |
| // G[6] == G[1] |
| G[7] += iyiy; |
| // G[8] == G[4] |
| // G[9] == G[5] |
| G[10] += x * iyiy; |
| G[11] += y * iyiy; |
| |
| xx = x * x; |
| xy = x * y; |
| yy = y * y; |
| |
| // G[12] == G[2] |
| // G[13] == G[8] == G[4] |
| G[14] += xx * ixix; |
| G[15] += xy * ixix; |
| G[16] += xx * ixiy; |
| G[17] += xy * ixiy; |
| |
| // G[18] == G[3] |
| // G[19] == G[9] |
| // G[20] == G[15] |
| G[21] += yy * ixix; |
| // G[22] == G[17] |
| G[23] += yy * ixiy; |
| |
| // G[24] == G[4] |
| // G[25] == G[10] |
| // G[26] == G[16] |
| // G[27] == G[22] |
| G[28] += xx * iyiy; |
| G[29] += xy * iyiy; |
| |
| // G[30] == G[5] |
| // G[31] == G[11] |
| // G[32] == G[17] |
| // G[33] == G[23] |
| // G[34] == G[29] |
| G[35] += yy * iyiy; |
| |
| meanI += patchI[k]; |
| } |
| } |
| |
| meanI /= patchSize.width*patchSize.height; |
| |
| G[8] = G[4]; |
| G[9] = G[5]; |
| G[22] = G[17]; |
| |
| // fill part of G below its diagonal |
| for( y = 1; y < 6; y++ ) |
| for( x = 0; x < y; x++ ) |
| G[y * 6 + x] = G[x * 6 + y]; |
| |
| cvInitMatHeader( &mat, 6, 6, CV_64FC1, G ); |
| |
| if( cvInvert( &mat, &mat, CV_SVD ) < 1e-4 ) |
| { |
| /* bad matrix. take the next point */ |
| if( l == 0 ) |
| status[i] = 0; |
| continue; |
| } |
| |
| for( j = 0; j < criteria.max_iter; j++ ) |
| { |
| double b[6] = {0,0,0,0,0,0}, eta[6]; |
| double t0, t1, s = 0; |
| |
| if( Av[2] < -eps || Av[2] >= levelSize.width+eps || |
| Av[5] < -eps || Av[5] >= levelSize.height+eps || |
| icvGetQuadrangleSubPix_8u32f_C1R( imgJ[l], levelStep, |
| levelSize, patchJ, patchStep, patchSize, Av ) < 0 ) |
| { |
| pt_status = 0; |
| break; |
| } |
| |
| for( y = -winSize.height, k = 0, meanJ = 0; y <= winSize.height; y++ ) |
| for( x = -winSize.width; x <= winSize.width; x++, k++ ) |
| meanJ += patchJ[k]; |
| |
| meanJ = meanJ / (patchSize.width * patchSize.height) - meanI; |
| |
| for( y = -winSize.height, k = 0; y <= winSize.height; y++ ) |
| { |
| for( x = -winSize.width; x <= winSize.width; x++, k++ ) |
| { |
| double t = patchI[k] - patchJ[k] + meanJ; |
| double ixt = Ix[k] * t; |
| double iyt = Iy[k] * t; |
| |
| s += t; |
| |
| b[0] += ixt; |
| b[1] += iyt; |
| b[2] += x * ixt; |
| b[3] += y * ixt; |
| b[4] += x * iyt; |
| b[5] += y * iyt; |
| } |
| } |
| |
| icvTransformVector_64d( G, b, eta, 6, 6 ); |
| |
| Av[2] = (float)(Av[2] + Av[0] * eta[0] + Av[1] * eta[1]); |
| Av[5] = (float)(Av[5] + Av[3] * eta[0] + Av[4] * eta[1]); |
| |
| t0 = Av[0] * (1 + eta[2]) + Av[1] * eta[4]; |
| t1 = Av[0] * eta[3] + Av[1] * (1 + eta[5]); |
| Av[0] = (float)t0; |
| Av[1] = (float)t1; |
| |
| t0 = Av[3] * (1 + eta[2]) + Av[4] * eta[4]; |
| t1 = Av[3] * eta[3] + Av[4] * (1 + eta[5]); |
| Av[3] = (float)t0; |
| Av[4] = (float)t1; |
| |
| if( eta[0] * eta[0] + eta[1] * eta[1] < criteria.epsilon ) |
| break; |
| } |
| |
| if( pt_status != 0 || l == 0 ) |
| { |
| status[i] = (char)pt_status; |
| featuresB[i].x = Av[2]; |
| featuresB[i].y = Av[5]; |
| |
| matrices[i*4] = Av[0]; |
| matrices[i*4+1] = Av[1]; |
| matrices[i*4+2] = Av[3]; |
| matrices[i*4+3] = Av[4]; |
| } |
| |
| if( pt_status && l == 0 && error ) |
| { |
| /* calc error */ |
| double err = 0; |
| |
| for( y = 0, k = 0; y < patchSize.height; y++ ) |
| { |
| for( x = 0; x < patchSize.width; x++, k++ ) |
| { |
| double t = patchI[k] - patchJ[k] + meanJ; |
| err += t * t; |
| } |
| } |
| error[i] = (float)sqrt(err); |
| } |
| } |
| } |
| |
| __END__; |
| |
| cvFree( &pyr_buffer ); |
| cvFree( &buffer ); |
| cvFree( &_status ); |
| } |
| |
| |
| |
| static void |
| icvGetRTMatrix( const CvPoint2D32f* a, const CvPoint2D32f* b, |
| int count, CvMat* M, int full_affine ) |
| { |
| if( full_affine ) |
| { |
| double sa[36], sb[6]; |
| CvMat A = cvMat( 6, 6, CV_64F, sa ), B = cvMat( 6, 1, CV_64F, sb ); |
| CvMat MM = cvMat( 6, 1, CV_64F, M->data.db ); |
| |
| int i; |
| |
| memset( sa, 0, sizeof(sa) ); |
| memset( sb, 0, sizeof(sb) ); |
| |
| for( i = 0; i < count; i++ ) |
| { |
| sa[0] += a[i].x*a[i].x; |
| sa[1] += a[i].y*a[i].x; |
| sa[2] += a[i].x; |
| |
| sa[6] += a[i].x*a[i].y; |
| sa[7] += a[i].y*a[i].y; |
| sa[8] += a[i].y; |
| |
| sa[12] += a[i].x; |
| sa[13] += a[i].y; |
| sa[14] += 1; |
| |
| sb[0] += a[i].x*b[i].x; |
| sb[1] += a[i].y*b[i].x; |
| sb[2] += b[i].x; |
| sb[3] += a[i].x*b[i].y; |
| sb[4] += a[i].y*b[i].y; |
| sb[5] += b[i].y; |
| } |
| |
| sa[21] = sa[0]; |
| sa[22] = sa[1]; |
| sa[23] = sa[2]; |
| sa[27] = sa[6]; |
| sa[28] = sa[7]; |
| sa[29] = sa[8]; |
| sa[33] = sa[12]; |
| sa[34] = sa[13]; |
| sa[35] = sa[14]; |
| |
| cvSolve( &A, &B, &MM, CV_SVD ); |
| } |
| else |
| { |
| double sa[16], sb[4], m[4], *om = M->data.db; |
| CvMat A = cvMat( 4, 4, CV_64F, sa ), B = cvMat( 4, 1, CV_64F, sb ); |
| CvMat MM = cvMat( 4, 1, CV_64F, m ); |
| |
| int i; |
| |
| memset( sa, 0, sizeof(sa) ); |
| memset( sb, 0, sizeof(sb) ); |
| |
| for( i = 0; i < count; i++ ) |
| { |
| sa[0] += a[i].x*a[i].x + a[i].y*a[i].y; |
| sa[1] += 0; |
| sa[2] += a[i].x; |
| sa[3] += a[i].y; |
| |
| sa[4] += 0; |
| sa[5] += a[i].x*a[i].x + a[i].y*a[i].y; |
| sa[6] += -a[i].y; |
| sa[7] += a[i].x; |
| |
| sa[8] += a[i].x; |
| sa[9] += -a[i].y; |
| sa[10] += 1; |
| sa[11] += 0; |
| |
| sa[12] += a[i].y; |
| sa[13] += a[i].x; |
| sa[14] += 0; |
| sa[15] += 1; |
| |
| sb[0] += a[i].x*b[i].x + a[i].y*b[i].y; |
| sb[1] += a[i].x*b[i].y - a[i].y*b[i].x; |
| sb[2] += b[i].x; |
| sb[3] += b[i].y; |
| } |
| |
| cvSolve( &A, &B, &MM, CV_SVD ); |
| |
| om[0] = om[4] = m[0]; |
| om[1] = -m[1]; |
| om[3] = m[1]; |
| om[2] = m[2]; |
| om[5] = m[3]; |
| } |
| } |
| |
| |
| CV_IMPL int |
| cvEstimateRigidTransform( const CvArr* _A, const CvArr* _B, CvMat* _M, int full_affine ) |
| { |
| int result = 0; |
| |
| const int COUNT = 15; |
| const int WIDTH = 160, HEIGHT = 120; |
| const int RANSAC_MAX_ITERS = 100; |
| const int RANSAC_SIZE0 = 3; |
| const double MIN_TRIANGLE_SIDE = 20; |
| const double RANSAC_GOOD_RATIO = 0.5; |
| |
| int allocated = 1; |
| CvMat *sA = 0, *sB = 0; |
| CvPoint2D32f *pA = 0, *pB = 0; |
| int* good_idx = 0; |
| char *status = 0; |
| CvMat* gray = 0; |
| |
| CV_FUNCNAME( "cvEstimateRigidTransform" ); |
| |
| __BEGIN__; |
| |
| CvMat stubA, *A; |
| CvMat stubB, *B; |
| CvSize sz0, sz1; |
| int cn, equal_sizes; |
| int i, j, k, k1; |
| int count_x, count_y, count; |
| double scale = 1; |
| CvRNG rng = cvRNG(-1); |
| double m[6]={0}; |
| CvMat M = cvMat( 2, 3, CV_64F, m ); |
| int good_count = 0; |
| |
| CV_CALL( A = cvGetMat( _A, &stubA )); |
| CV_CALL( B = cvGetMat( _B, &stubB )); |
| |
| if( !CV_IS_MAT(_M) ) |
| CV_ERROR( _M ? CV_StsBadArg : CV_StsNullPtr, "Output parameter M is not a valid matrix" ); |
| |
| if( !CV_ARE_SIZES_EQ( A, B ) ) |
| CV_ERROR( CV_StsUnmatchedSizes, "Both input images must have the same size" ); |
| |
| if( !CV_ARE_TYPES_EQ( A, B ) ) |
| CV_ERROR( CV_StsUnmatchedFormats, "Both input images must have the same data type" ); |
| |
| if( CV_MAT_TYPE(A->type) == CV_8UC1 || CV_MAT_TYPE(A->type) == CV_8UC3 ) |
| { |
| cn = CV_MAT_CN(A->type); |
| sz0 = cvGetSize(A); |
| sz1 = cvSize(WIDTH, HEIGHT); |
| |
| scale = MAX( (double)sz1.width/sz0.width, (double)sz1.height/sz0.height ); |
| scale = MIN( scale, 1. ); |
| sz1.width = cvRound( sz0.width * scale ); |
| sz1.height = cvRound( sz0.height * scale ); |
| |
| equal_sizes = sz1.width == sz0.width && sz1.height == sz0.height; |
| |
| if( !equal_sizes || cn != 1 ) |
| { |
| CV_CALL( sA = cvCreateMat( sz1.height, sz1.width, CV_8UC1 )); |
| CV_CALL( sB = cvCreateMat( sz1.height, sz1.width, CV_8UC1 )); |
| |
| if( !equal_sizes && cn != 1 ) |
| CV_CALL( gray = cvCreateMat( sz0.height, sz0.width, CV_8UC1 )); |
| |
| if( gray ) |
| { |
| cvCvtColor( A, gray, CV_BGR2GRAY ); |
| cvResize( gray, sA, CV_INTER_AREA ); |
| cvCvtColor( B, gray, CV_BGR2GRAY ); |
| cvResize( gray, sB, CV_INTER_AREA ); |
| } |
| else if( cn == 1 ) |
| { |
| cvResize( gray, sA, CV_INTER_AREA ); |
| cvResize( gray, sB, CV_INTER_AREA ); |
| } |
| else |
| { |
| cvCvtColor( A, gray, CV_BGR2GRAY ); |
| cvResize( gray, sA, CV_INTER_AREA ); |
| cvCvtColor( B, gray, CV_BGR2GRAY ); |
| } |
| |
| cvReleaseMat( &gray ); |
| A = sA; |
| B = sB; |
| } |
| |
| count_y = COUNT; |
| count_x = cvRound((double)COUNT*sz1.width/sz1.height); |
| count = count_x * count_y; |
| |
| CV_CALL( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) )); |
| CV_CALL( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) )); |
| CV_CALL( status = (char*)cvAlloc( count*sizeof(status[0]) )); |
| |
| for( i = 0, k = 0; i < count_y; i++ ) |
| for( j = 0; j < count_x; j++, k++ ) |
| { |
| pA[k].x = (j+0.5f)*sz1.width/count_x; |
| pA[k].y = (i+0.5f)*sz1.height/count_y; |
| } |
| |
| // find the corresponding points in B |
| cvCalcOpticalFlowPyrLK( A, B, 0, 0, pA, pB, count, cvSize(10,10), 3, |
| status, 0, cvTermCriteria(CV_TERMCRIT_ITER,40,0.1), 0 ); |
| |
| // repack the remained points |
| for( i = 0, k = 0; i < count; i++ ) |
| if( status[i] ) |
| { |
| if( i > k ) |
| { |
| pA[k] = pA[i]; |
| pB[k] = pB[i]; |
| } |
| k++; |
| } |
| |
| count = k; |
| } |
| else if( CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2 ) |
| { |
| count = A->cols*A->rows; |
| |
| if( CV_IS_MAT_CONT(A->type & B->type) && CV_MAT_TYPE(A->type) == CV_32FC2 ) |
| { |
| pA = (CvPoint2D32f*)A->data.ptr; |
| pB = (CvPoint2D32f*)B->data.ptr; |
| allocated = 0; |
| } |
| else |
| { |
| CvMat _pA, _pB; |
| |
| CV_CALL( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) )); |
| CV_CALL( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) )); |
| _pA = cvMat( A->rows, A->cols, CV_32FC2, pA ); |
| _pB = cvMat( B->rows, B->cols, CV_32FC2, pB ); |
| cvConvert( A, &_pA ); |
| cvConvert( B, &_pB ); |
| } |
| } |
| else |
| CV_ERROR( CV_StsUnsupportedFormat, "Both input images must have either 8uC1 or 8uC3 type" ); |
| |
| CV_CALL( good_idx = (int*)cvAlloc( count*sizeof(good_idx[0]) )); |
| |
| if( count < RANSAC_SIZE0 ) |
| EXIT; |
| |
| // RANSAC stuff: |
| // 1. find the consensus |
| for( k = 0; k < RANSAC_MAX_ITERS; k++ ) |
| { |
| int idx[RANSAC_SIZE0]; |
| CvPoint2D32f a[3]; |
| CvPoint2D32f b[3]; |
| |
| memset( a, 0, sizeof(a) ); |
| memset( b, 0, sizeof(b) ); |
| |
| // choose random 3 non-complanar points from A & B |
| for( i = 0; i < RANSAC_SIZE0; i++ ) |
| { |
| for( k1 = 0; k1 < RANSAC_MAX_ITERS; k1++ ) |
| { |
| idx[i] = cvRandInt(&rng) % count; |
| |
| for( j = 0; j < i; j++ ) |
| { |
| if( idx[j] == idx[i] ) |
| break; |
| // check that the points are not very close one each other |
| if( fabs(pA[idx[i]].x - pA[idx[j]].x) + |
| fabs(pA[idx[i]].y - pA[idx[j]].y) < MIN_TRIANGLE_SIDE ) |
| break; |
| if( fabs(pB[idx[i]].x - pB[idx[j]].x) + |
| fabs(pB[idx[i]].y - pB[idx[j]].y) < MIN_TRIANGLE_SIDE ) |
| break; |
| } |
| |
| if( j < i ) |
| continue; |
| |
| if( i+1 == RANSAC_SIZE0 ) |
| { |
| // additional check for non-complanar vectors |
| a[0] = pA[idx[0]]; |
| a[1] = pA[idx[1]]; |
| a[2] = pA[idx[2]]; |
| |
| b[0] = pB[idx[0]]; |
| b[1] = pB[idx[1]]; |
| b[2] = pB[idx[2]]; |
| |
| if( fabs((a[1].x - a[0].x)*(a[2].y - a[0].y) - (a[1].y - a[0].y)*(a[2].x - a[0].x)) < 1 || |
| fabs((b[1].x - b[0].x)*(b[2].y - b[0].y) - (b[1].y - b[0].y)*(b[2].x - b[0].x)) < 1 ) |
| continue; |
| } |
| break; |
| } |
| |
| if( k1 >= RANSAC_MAX_ITERS ) |
| break; |
| } |
| |
| if( i < RANSAC_SIZE0 ) |
| continue; |
| |
| // estimate the transformation using 3 points |
| icvGetRTMatrix( a, b, 3, &M, full_affine ); |
| |
| for( i = 0, good_count = 0; i < count; i++ ) |
| { |
| if( fabs( m[0]*pA[i].x + m[1]*pA[i].y + m[2] - pB[i].x ) + |
| fabs( m[3]*pA[i].x + m[4]*pA[i].y + m[5] - pB[i].y ) < 8 ) |
| good_idx[good_count++] = i; |
| } |
| |
| if( good_count >= count*RANSAC_GOOD_RATIO ) |
| break; |
| } |
| |
| if( k >= RANSAC_MAX_ITERS ) |
| EXIT; |
| |
| if( good_count < count ) |
| { |
| for( i = 0; i < good_count; i++ ) |
| { |
| j = good_idx[i]; |
| pA[i] = pA[j]; |
| pB[i] = pB[j]; |
| } |
| } |
| |
| icvGetRTMatrix( pA, pB, good_count, &M, full_affine ); |
| m[2] /= scale; |
| m[5] /= scale; |
| CV_CALL( cvConvert( &M, _M )); |
| result = 1; |
| |
| __END__; |
| |
| cvReleaseMat( &sA ); |
| cvReleaseMat( &sB ); |
| cvFree( &pA ); |
| cvFree( &pB ); |
| cvFree( &status ); |
| cvFree( &good_idx ); |
| cvReleaseMat( &gray ); |
| |
| return result; |
| } |
| |
| |
| /* End of file. */ |
