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
43CV_IMPL void
44cvFindCornerSubPix( 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