1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // Intel License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000, Intel Corporation, all rights reserved. 14 // Third party copyrights are property of their respective owners. 15 // 16 // Redistribution and use in source and binary forms, with or without modification, 17 // are permitted provided that the following conditions are met: 18 // 19 // * Redistribution's of source code must retain the above copyright notice, 20 // this list of conditions and the following disclaimer. 21 // 22 // * Redistribution's in binary form must reproduce the above copyright notice, 23 // this list of conditions and the following disclaimer in the documentation 24 // and/or other materials provided with the distribution. 25 // 26 // * The name of Intel Corporation may not be used to endorse or promote products 27 // derived from this software without specific prior written permission. 28 // 29 // This software is provided by the copyright holders and contributors "as is" and 30 // any express or implied warranties, including, but not limited to, the implied 31 // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 // In no event shall the Intel Corporation or contributors be liable for any direct, 33 // indirect, incidental, special, exemplary, or consequential damages 34 // (including, but not limited to, procurement of substitute goods or services; 35 // loss of use, data, or profits; or business interruption) however caused 36 // and on any theory of liability, whether in contract, strict liability, 37 // or tort (including negligence or otherwise) arising in any way out of 38 // the use of this software, even if advised of the possibility of such damage. 39 // 40 //M*/ 41 #include "_cv.h" 42 43 CV_IMPL void 44 cvFindCornerSubPix( const void* srcarr, CvPoint2D32f* corners, 45 int count, CvSize win, CvSize zeroZone, 46 CvTermCriteria criteria ) 47 { 48 float* buffer = 0; 49 50 CV_FUNCNAME( "cvFindCornerSubPix" ); 51 52 __BEGIN__; 53 54 const int MAX_ITERS = 100; 55 const float drv_x[] = { -1.f, 0.f, 1.f }; 56 const float drv_y[] = { 0.f, 0.5f, 0.f }; 57 float *maskX; 58 float *maskY; 59 float *mask; 60 float *src_buffer; 61 float *gx_buffer; 62 float *gy_buffer; 63 int win_w = win.width * 2 + 1, win_h = win.height * 2 + 1; 64 int win_rect_size = (win_w + 4) * (win_h + 4); 65 double coeff; 66 CvSize size, src_buf_size; 67 int i, j, k, pt_i; 68 int max_iters, buffer_size; 69 double eps; 70 71 CvMat stub, *src = (CvMat*)srcarr; 72 CV_CALL( src = cvGetMat( srcarr, &stub )); 73 74 if( CV_MAT_TYPE( src->type ) != CV_8UC1 ) 75 CV_ERROR( CV_StsBadMask, "" ); 76 77 if( !corners ) 78 CV_ERROR( CV_StsNullPtr, "" ); 79 80 if( count < 0 ) 81 CV_ERROR( CV_StsBadSize, "" ); 82 83 if( count == 0 ) 84 EXIT; 85 86 if( win.width <= 0 || win.height <= 0 ) 87 CV_ERROR( CV_StsBadSize, "" ); 88 89 size = cvGetMatSize( src ); 90 91 if( size.width < win_w + 4 || size.height < win_h + 4 ) 92 CV_ERROR( CV_StsBadSize, "" ); 93 94 /* initialize variables, controlling loop termination */ 95 switch( criteria.type ) 96 { 97 case CV_TERMCRIT_ITER: 98 eps = 0.f; 99 max_iters = criteria.max_iter; 100 break; 101 case CV_TERMCRIT_EPS: 102 eps = criteria.epsilon; 103 max_iters = MAX_ITERS; 104 break; 105 case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS: 106 eps = criteria.epsilon; 107 max_iters = criteria.max_iter; 108 break; 109 default: 110 assert( 0 ); 111 CV_ERROR( CV_StsBadFlag, "" ); 112 } 113 114 eps = MAX( eps, 0 ); 115 eps *= eps; /* use square of error in comparsion operations. */ 116 117 max_iters = MAX( max_iters, 1 ); 118 max_iters = MIN( max_iters, MAX_ITERS ); 119 120 /* setup buffer */ 121 buffer_size = (win_rect_size * 5 + win_w + win_h + 32) * sizeof(float); 122 buffer = (float*)cvAlloc( buffer_size ); 123 124 /* assign pointers */ 125 maskX = buffer; 126 maskY = maskX + win_w + 4; 127 mask = maskY + win_h + 4; 128 src_buffer = mask + win_w * win_h; 129 gx_buffer = src_buffer + win_rect_size; 130 gy_buffer = gx_buffer + win_rect_size; 131 132 coeff = 1. / (win.width * win.width); 133 134 /* calculate mask */ 135 for( i = -win.width, k = 0; i <= win.width; i++, k++ ) 136 { 137 maskX[k] = (float)exp( -i * i * coeff ); 138 } 139 140 if( win.width == win.height ) 141 { 142 maskY = maskX; 143 } 144 else 145 { 146 coeff = 1. / (win.height * win.height); 147 for( i = -win.height, k = 0; i <= win.height; i++, k++ ) 148 { 149 maskY[k] = (float) exp( -i * i * coeff ); 150 } 151 } 152 153 for( i = 0; i < win_h; i++ ) 154 { 155 for( j = 0; j < win_w; j++ ) 156 { 157 mask[i * win_w + j] = maskX[j] * maskY[i]; 158 } 159 } 160 161 162 /* make zero_zone */ 163 if( zeroZone.width >= 0 && zeroZone.height >= 0 && 164 zeroZone.width * 2 + 1 < win_w && zeroZone.height * 2 + 1 < win_h ) 165 { 166 for( i = win.height - zeroZone.height; i <= win.height + zeroZone.height; i++ ) 167 { 168 for( j = win.width - zeroZone.width; j <= win.width + zeroZone.width; j++ ) 169 { 170 mask[i * win_w + j] = 0; 171 } 172 } 173 } 174 175 /* set sizes of image rectangles, used in convolutions */ 176 src_buf_size.width = win_w + 2; 177 src_buf_size.height = win_h + 2; 178 179 /* do optimization loop for all the points */ 180 for( pt_i = 0; pt_i < count; pt_i++ ) 181 { 182 CvPoint2D32f cT = corners[pt_i], cI = cT; 183 int iter = 0; 184 double err; 185 186 do 187 { 188 CvPoint2D32f cI2; 189 double a, b, c, bb1, bb2; 190 191 IPPI_CALL( icvGetRectSubPix_8u32f_C1R( (uchar*)src->data.ptr, src->step, size, 192 src_buffer, (win_w + 2) * sizeof( src_buffer[0] ), 193 cvSize( win_w + 2, win_h + 2 ), cI )); 194 195 /* calc derivatives */ 196 icvSepConvSmall3_32f( src_buffer, src_buf_size.width * sizeof(src_buffer[0]), 197 gx_buffer, win_w * sizeof(gx_buffer[0]), 198 src_buf_size, drv_x, drv_y, buffer ); 199 200 icvSepConvSmall3_32f( src_buffer, src_buf_size.width * sizeof(src_buffer[0]), 201 gy_buffer, win_w * sizeof(gy_buffer[0]), 202 src_buf_size, drv_y, drv_x, buffer ); 203 204 a = b = c = bb1 = bb2 = 0; 205 206 /* process gradient */ 207 for( i = 0, k = 0; i < win_h; i++ ) 208 { 209 double py = i - win.height; 210 211 for( j = 0; j < win_w; j++, k++ ) 212 { 213 double m = mask[k]; 214 double tgx = gx_buffer[k]; 215 double tgy = gy_buffer[k]; 216 double gxx = tgx * tgx * m; 217 double gxy = tgx * tgy * m; 218 double gyy = tgy * tgy * m; 219 double px = j - win.width; 220 221 a += gxx; 222 b += gxy; 223 c += gyy; 224 225 bb1 += gxx * px + gxy * py; 226 bb2 += gxy * px + gyy * py; 227 } 228 } 229 230 { 231 double A[4]; 232 double InvA[4]; 233 CvMat matA, matInvA; 234 235 A[0] = a; 236 A[1] = A[2] = b; 237 A[3] = c; 238 239 cvInitMatHeader( &matA, 2, 2, CV_64F, A ); 240 cvInitMatHeader( &matInvA, 2, 2, CV_64FC1, InvA ); 241 242 cvInvert( &matA, &matInvA, CV_SVD ); 243 cI2.x = (float)(cI.x + InvA[0]*bb1 + InvA[1]*bb2); 244 cI2.y = (float)(cI.y + InvA[2]*bb1 + InvA[3]*bb2); 245 } 246 247 err = (cI2.x - cI.x) * (cI2.x - cI.x) + (cI2.y - cI.y) * (cI2.y - cI.y); 248 cI = cI2; 249 } 250 while( ++iter < max_iters && err > eps ); 251 252 /* if new point is too far from initial, it means poor convergence. 253 leave initial point as the result */ 254 if( fabs( cI.x - cT.x ) > win.width || fabs( cI.y - cT.y ) > win.height ) 255 { 256 cI = cT; 257 } 258 259 corners[pt_i] = cI; /* store result */ 260 } 261 262 __CLEANUP__; 263 __END__; 264 265 cvFree( &buffer ); 266 } 267 268 /* End of file. */ 269
