diff options
Diffstat (limited to 'cv/src/cvcornersubpix.cpp')
| -rw-r--r-- | cv/src/cvcornersubpix.cpp | 268 |
1 files changed, 268 insertions, 0 deletions
diff --git a/cv/src/cvcornersubpix.cpp b/cv/src/cvcornersubpix.cpp new file mode 100644 index 0000000..5eb282c --- /dev/null +++ b/cv/src/cvcornersubpix.cpp @@ -0,0 +1,268 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// Intel License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000, Intel Corporation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of Intel Corporation may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#include "_cv.h" + +CV_IMPL void +cvFindCornerSubPix( const void* srcarr, CvPoint2D32f* corners, + int count, CvSize win, CvSize zeroZone, + CvTermCriteria criteria ) +{ + float* buffer = 0; + + CV_FUNCNAME( "cvFindCornerSubPix" ); + + __BEGIN__; + + const int MAX_ITERS = 100; + const float drv_x[] = { -1.f, 0.f, 1.f }; + const float drv_y[] = { 0.f, 0.5f, 0.f }; + float *maskX; + float *maskY; + float *mask; + float *src_buffer; + float *gx_buffer; + float *gy_buffer; + int win_w = win.width * 2 + 1, win_h = win.height * 2 + 1; + int win_rect_size = (win_w + 4) * (win_h + 4); + double coeff; + CvSize size, src_buf_size; + int i, j, k, pt_i; + int max_iters, buffer_size; + double eps; + + CvMat stub, *src = (CvMat*)srcarr; + CV_CALL( src = cvGetMat( srcarr, &stub )); + + if( CV_MAT_TYPE( src->type ) != CV_8UC1 ) + CV_ERROR( CV_StsBadMask, "" ); + + if( !corners ) + CV_ERROR( CV_StsNullPtr, "" ); + + if( count < 0 ) + CV_ERROR( CV_StsBadSize, "" ); + + if( count == 0 ) + EXIT; + + if( win.width <= 0 || win.height <= 0 ) + CV_ERROR( CV_StsBadSize, "" ); + + size = cvGetMatSize( src ); + + if( size.width < win_w + 4 || size.height < win_h + 4 ) + CV_ERROR( CV_StsBadSize, "" ); + + /* initialize variables, controlling loop termination */ + switch( criteria.type ) + { + case CV_TERMCRIT_ITER: + eps = 0.f; + max_iters = criteria.max_iter; + break; + case CV_TERMCRIT_EPS: + eps = criteria.epsilon; + max_iters = MAX_ITERS; + break; + case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS: + eps = criteria.epsilon; + max_iters = criteria.max_iter; + break; + default: + assert( 0 ); + CV_ERROR( CV_StsBadFlag, "" ); + } + + eps = MAX( eps, 0 ); + eps *= eps; /* use square of error in comparsion operations. */ + + max_iters = MAX( max_iters, 1 ); + max_iters = MIN( max_iters, MAX_ITERS ); + + /* setup buffer */ + buffer_size = (win_rect_size * 5 + win_w + win_h + 32) * sizeof(float); + buffer = (float*)cvAlloc( buffer_size ); + + /* assign pointers */ + maskX = buffer; + maskY = maskX + win_w + 4; + mask = maskY + win_h + 4; + src_buffer = mask + win_w * win_h; + gx_buffer = src_buffer + win_rect_size; + gy_buffer = gx_buffer + win_rect_size; + + coeff = 1. / (win.width * win.width); + + /* calculate mask */ + for( i = -win.width, k = 0; i <= win.width; i++, k++ ) + { + maskX[k] = (float)exp( -i * i * coeff ); + } + + if( win.width == win.height ) + { + maskY = maskX; + } + else + { + coeff = 1. / (win.height * win.height); + for( i = -win.height, k = 0; i <= win.height; i++, k++ ) + { + maskY[k] = (float) exp( -i * i * coeff ); + } + } + + for( i = 0; i < win_h; i++ ) + { + for( j = 0; j < win_w; j++ ) + { + mask[i * win_w + j] = maskX[j] * maskY[i]; + } + } + + + /* make zero_zone */ + if( zeroZone.width >= 0 && zeroZone.height >= 0 && + zeroZone.width * 2 + 1 < win_w && zeroZone.height * 2 + 1 < win_h ) + { + for( i = win.height - zeroZone.height; i <= win.height + zeroZone.height; i++ ) + { + for( j = win.width - zeroZone.width; j <= win.width + zeroZone.width; j++ ) + { + mask[i * win_w + j] = 0; + } + } + } + + /* set sizes of image rectangles, used in convolutions */ + src_buf_size.width = win_w + 2; + src_buf_size.height = win_h + 2; + + /* do optimization loop for all the points */ + for( pt_i = 0; pt_i < count; pt_i++ ) + { + CvPoint2D32f cT = corners[pt_i], cI = cT; + int iter = 0; + double err; + + do + { + CvPoint2D32f cI2; + double a, b, c, bb1, bb2; + + IPPI_CALL( icvGetRectSubPix_8u32f_C1R( (uchar*)src->data.ptr, src->step, size, + src_buffer, (win_w + 2) * sizeof( src_buffer[0] ), + cvSize( win_w + 2, win_h + 2 ), cI )); + + /* calc derivatives */ + icvSepConvSmall3_32f( src_buffer, src_buf_size.width * sizeof(src_buffer[0]), + gx_buffer, win_w * sizeof(gx_buffer[0]), + src_buf_size, drv_x, drv_y, buffer ); + + icvSepConvSmall3_32f( src_buffer, src_buf_size.width * sizeof(src_buffer[0]), + gy_buffer, win_w * sizeof(gy_buffer[0]), + src_buf_size, drv_y, drv_x, buffer ); + + a = b = c = bb1 = bb2 = 0; + + /* process gradient */ + for( i = 0, k = 0; i < win_h; i++ ) + { + double py = i - win.height; + + for( j = 0; j < win_w; j++, k++ ) + { + double m = mask[k]; + double tgx = gx_buffer[k]; + double tgy = gy_buffer[k]; + double gxx = tgx * tgx * m; + double gxy = tgx * tgy * m; + double gyy = tgy * tgy * m; + double px = j - win.width; + + a += gxx; + b += gxy; + c += gyy; + + bb1 += gxx * px + gxy * py; + bb2 += gxy * px + gyy * py; + } + } + + { + double A[4]; + double InvA[4]; + CvMat matA, matInvA; + + A[0] = a; + A[1] = A[2] = b; + A[3] = c; + + cvInitMatHeader( &matA, 2, 2, CV_64F, A ); + cvInitMatHeader( &matInvA, 2, 2, CV_64FC1, InvA ); + + cvInvert( &matA, &matInvA, CV_SVD ); + cI2.x = (float)(cI.x + InvA[0]*bb1 + InvA[1]*bb2); + cI2.y = (float)(cI.y + InvA[2]*bb1 + InvA[3]*bb2); + } + + err = (cI2.x - cI.x) * (cI2.x - cI.x) + (cI2.y - cI.y) * (cI2.y - cI.y); + cI = cI2; + } + while( ++iter < max_iters && err > eps ); + + /* if new point is too far from initial, it means poor convergence. + leave initial point as the result */ + if( fabs( cI.x - cT.x ) > win.width || fabs( cI.y - cT.y ) > win.height ) + { + cI = cT; + } + + corners[pt_i] = cI; /* store result */ + } + + __CLEANUP__; + __END__; + + cvFree( &buffer ); +} + +/* End of file. */ |