--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////\r
+//\r
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
+//\r
+// By downloading, copying, installing or using the software you agree to this license.\r
+// If you do not agree to this license, do not download, install,\r
+// copy or use the software.\r
+//\r
+//\r
+// Intel License Agreement\r
+// For Open Source Computer Vision Library\r
+//\r
+// Copyright (C) 2000, Intel Corporation, all rights reserved.\r
+// Third party copyrights are property of their respective owners.\r
+//\r
+// Redistribution and use in source and binary forms, with or without modification,\r
+// are permitted provided that the following conditions are met:\r
+//\r
+// * Redistribution's of source code must retain the above copyright notice,\r
+// this list of conditions and the following disclaimer.\r
+//\r
+// * Redistribution's in binary form must reproduce the above copyright notice,\r
+// this list of conditions and the following disclaimer in the documentation\r
+// and/or other materials provided with the distribution.\r
+//\r
+// * The name of Intel Corporation may not be used to endorse or promote products\r
+// derived from this software without specific prior written permission.\r
+//\r
+// This software is provided by the copyright holders and contributors "as is" and\r
+// any express or implied warranties, including, but not limited to, the implied\r
+// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
+// In no event shall the Intel Corporation or contributors be liable for any direct,\r
+// indirect, incidental, special, exemplary, or consequential damages\r
+// (including, but not limited to, procurement of substitute goods or services;\r
+// loss of use, data, or profits; or business interruption) however caused\r
+// and on any theory of liability, whether in contract, strict liability,\r
+// or tort (including negligence or otherwise) arising in any way out of\r
+// the use of this software, even if advised of the possibility of such damage.\r
+//\r
+//M*/\r
+#include "_cv.h"\r
+\r
+CV_IMPL void\r
+cvFindCornerSubPix( const void* srcarr, CvPoint2D32f* corners,\r
+ int count, CvSize win, CvSize zeroZone,\r
+ CvTermCriteria criteria )\r
+{\r
+ float* buffer = 0;\r
+ \r
+ CV_FUNCNAME( "cvFindCornerSubPix" );\r
+\r
+ __BEGIN__;\r
+\r
+ const int MAX_ITERS = 100;\r
+ const float drv_x[] = { -1.f, 0.f, 1.f };\r
+ const float drv_y[] = { 0.f, 0.5f, 0.f };\r
+ float *maskX;\r
+ float *maskY;\r
+ float *mask;\r
+ float *src_buffer;\r
+ float *gx_buffer;\r
+ float *gy_buffer;\r
+ int win_w = win.width * 2 + 1, win_h = win.height * 2 + 1;\r
+ int win_rect_size = (win_w + 4) * (win_h + 4);\r
+ double coeff;\r
+ CvSize size, src_buf_size;\r
+ int i, j, k, pt_i;\r
+ int max_iters, buffer_size;\r
+ double eps;\r
+\r
+ CvMat stub, *src = (CvMat*)srcarr;\r
+ CV_CALL( src = cvGetMat( srcarr, &stub ));\r
+\r
+ if( CV_MAT_TYPE( src->type ) != CV_8UC1 )\r
+ CV_ERROR( CV_StsBadMask, "" );\r
+\r
+ if( !corners )\r
+ CV_ERROR( CV_StsNullPtr, "" );\r
+\r
+ if( count < 0 )\r
+ CV_ERROR( CV_StsBadSize, "" );\r
+\r
+ if( count == 0 )\r
+ EXIT;\r
+\r
+ if( win.width <= 0 || win.height <= 0 )\r
+ CV_ERROR( CV_StsBadSize, "" );\r
+\r
+ size = cvGetMatSize( src );\r
+\r
+ if( size.width < win_w + 4 || size.height < win_h + 4 )\r
+ CV_ERROR( CV_StsBadSize, "" );\r
+\r
+ /* initialize variables, controlling loop termination */\r
+ switch( criteria.type )\r
+ {\r
+ case CV_TERMCRIT_ITER:\r
+ eps = 0.f;\r
+ max_iters = criteria.max_iter;\r
+ break;\r
+ case CV_TERMCRIT_EPS:\r
+ eps = criteria.epsilon;\r
+ max_iters = MAX_ITERS;\r
+ break;\r
+ case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:\r
+ eps = criteria.epsilon;\r
+ max_iters = criteria.max_iter;\r
+ break;\r
+ default:\r
+ assert( 0 );\r
+ CV_ERROR( CV_StsBadFlag, "" );\r
+ }\r
+\r
+ eps = MAX( eps, 0 );\r
+ eps *= eps; /* use square of error in comparsion operations. */\r
+\r
+ max_iters = MAX( max_iters, 1 );\r
+ max_iters = MIN( max_iters, MAX_ITERS );\r
+\r
+ /* setup buffer */\r
+ buffer_size = (win_rect_size * 5 + win_w + win_h + 32) * sizeof(float);\r
+ buffer = (float*)cvAlloc( buffer_size );\r
+\r
+ /* assign pointers */\r
+ maskX = buffer;\r
+ maskY = maskX + win_w + 4;\r
+ mask = maskY + win_h + 4;\r
+ src_buffer = mask + win_w * win_h;\r
+ gx_buffer = src_buffer + win_rect_size;\r
+ gy_buffer = gx_buffer + win_rect_size;\r
+\r
+ coeff = 1. / (win.width * win.width);\r
+\r
+ /* calculate mask */\r
+ for( i = -win.width, k = 0; i <= win.width; i++, k++ )\r
+ {\r
+ maskX[k] = (float)exp( -i * i * coeff );\r
+ }\r
+\r
+ if( win.width == win.height )\r
+ {\r
+ maskY = maskX;\r
+ }\r
+ else\r
+ {\r
+ coeff = 1. / (win.height * win.height);\r
+ for( i = -win.height, k = 0; i <= win.height; i++, k++ )\r
+ {\r
+ maskY[k] = (float) exp( -i * i * coeff );\r
+ }\r
+ }\r
+\r
+ for( i = 0; i < win_h; i++ )\r
+ {\r
+ for( j = 0; j < win_w; j++ )\r
+ {\r
+ mask[i * win_w + j] = maskX[j] * maskY[i];\r
+ }\r
+ }\r
+\r
+\r
+ /* make zero_zone */\r
+ if( zeroZone.width >= 0 && zeroZone.height >= 0 &&\r
+ zeroZone.width * 2 + 1 < win_w && zeroZone.height * 2 + 1 < win_h )\r
+ {\r
+ for( i = win.height - zeroZone.height; i <= win.height + zeroZone.height; i++ )\r
+ {\r
+ for( j = win.width - zeroZone.width; j <= win.width + zeroZone.width; j++ )\r
+ {\r
+ mask[i * win_w + j] = 0;\r
+ }\r
+ }\r
+ }\r
+\r
+ /* set sizes of image rectangles, used in convolutions */\r
+ src_buf_size.width = win_w + 2;\r
+ src_buf_size.height = win_h + 2;\r
+\r
+ /* do optimization loop for all the points */\r
+ for( pt_i = 0; pt_i < count; pt_i++ )\r
+ {\r
+ CvPoint2D32f cT = corners[pt_i], cI = cT;\r
+ int iter = 0;\r
+ double err;\r
+\r
+ do\r
+ {\r
+ CvPoint2D32f cI2;\r
+ double a, b, c, bb1, bb2;\r
+\r
+ IPPI_CALL( icvGetRectSubPix_8u32f_C1R( (uchar*)src->data.ptr, src->step, size,\r
+ src_buffer, (win_w + 2) * sizeof( src_buffer[0] ),\r
+ cvSize( win_w + 2, win_h + 2 ), cI ));\r
+\r
+ /* calc derivatives */\r
+ icvSepConvSmall3_32f( src_buffer, src_buf_size.width * sizeof(src_buffer[0]),\r
+ gx_buffer, win_w * sizeof(gx_buffer[0]),\r
+ src_buf_size, drv_x, drv_y, buffer );\r
+\r
+ icvSepConvSmall3_32f( src_buffer, src_buf_size.width * sizeof(src_buffer[0]),\r
+ gy_buffer, win_w * sizeof(gy_buffer[0]),\r
+ src_buf_size, drv_y, drv_x, buffer );\r
+\r
+ a = b = c = bb1 = bb2 = 0;\r
+\r
+ /* process gradient */\r
+ for( i = 0, k = 0; i < win_h; i++ )\r
+ {\r
+ double py = i - win.height;\r
+\r
+ for( j = 0; j < win_w; j++, k++ )\r
+ {\r
+ double m = mask[k];\r
+ double tgx = gx_buffer[k];\r
+ double tgy = gy_buffer[k];\r
+ double gxx = tgx * tgx * m;\r
+ double gxy = tgx * tgy * m;\r
+ double gyy = tgy * tgy * m;\r
+ double px = j - win.width;\r
+\r
+ a += gxx;\r
+ b += gxy;\r
+ c += gyy;\r
+\r
+ bb1 += gxx * px + gxy * py;\r
+ bb2 += gxy * px + gyy * py;\r
+ }\r
+ }\r
+\r
+ {\r
+ double A[4];\r
+ double InvA[4];\r
+ CvMat matA, matInvA;\r
+\r
+ A[0] = a;\r
+ A[1] = A[2] = b;\r
+ A[3] = c;\r
+\r
+ cvInitMatHeader( &matA, 2, 2, CV_64F, A );\r
+ cvInitMatHeader( &matInvA, 2, 2, CV_64FC1, InvA );\r
+\r
+ cvInvert( &matA, &matInvA, CV_SVD );\r
+ cI2.x = (float)(cI.x + InvA[0]*bb1 + InvA[1]*bb2);\r
+ cI2.y = (float)(cI.y + InvA[2]*bb1 + InvA[3]*bb2);\r
+ }\r
+\r
+ err = (cI2.x - cI.x) * (cI2.x - cI.x) + (cI2.y - cI.y) * (cI2.y - cI.y);\r
+ cI = cI2;\r
+ }\r
+ while( ++iter < max_iters && err > eps );\r
+\r
+ /* if new point is too far from initial, it means poor convergence.\r
+ leave initial point as the result */\r
+ if( fabs( cI.x - cT.x ) > win.width || fabs( cI.y - cT.y ) > win.height )\r
+ {\r
+ cI = cT;\r
+ }\r
+\r
+ corners[pt_i] = cI; /* store result */\r
+ }\r
+\r
+ __CLEANUP__;\r
+ __END__;\r
+\r
+ cvFree( &buffer );\r
+}\r
+\r
+void cv::cornerSubPix( const Mat& image, vector<Point2f>& corners,\r
+ Size winSize, Size zeroZone,\r
+ TermCriteria criteria )\r
+{\r
+ CvMat _image = image;\r
+ cvFindCornerSubPix(&_image, (CvPoint2D32f*)&corners[0], (int)corners.size(),\r
+ winSize, zeroZone, criteria );\r
+}\r
+\r
+/* End of file. */\r
projects / opencv / blobdiff