git.maemo.org Git - opencv/blob - cv/src/cvcornersubpix.cpp

   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. */