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

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