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
cvFindCornerSubPix(const void * srcarr,CvPoint2D32f * corners,int count,CvSize win,CvSize zeroZone,CvTermCriteria criteria)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