cornersubpix.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                           License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000, Intel Corporation, all rights reserved.
00014 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
00015 // Third party copyrights are property of their respective owners.
00016 //
00017 // Redistribution and use in source and binary forms, with or without modification,
00018 // are permitted provided that the following conditions are met:
00019 //
00020 //   * Redistribution's of source code must retain the above copyright notice,
00021 //     this list of conditions and the following disclaimer.
00022 //
00023 //   * Redistribution's in binary form must reproduce the above copyright notice,
00024 //     this list of conditions and the following disclaimer in the documentation
00025 //     and/or other materials provided with the distribution.
00026 //
00027 //   * The name of the copyright holders may not be used to endorse or promote products
00028 //     derived from this software without specific prior written permission.
00029 //
00030 // This software is provided by the copyright holders and contributors "as is" and
00031 // any express or implied warranties, including, but not limited to, the implied
00032 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00033 // In no event shall the Intel Corporation or contributors be liable for any direct,
00034 // indirect, incidental, special, exemplary, or consequential damages
00035 // (including, but not limited to, procurement of substitute goods or services;
00036 // loss of use, data, or profits; or business interruption) however caused
00037 // and on any theory of liability, whether in contract, strict liability,
00038 // or tort (including negligence or otherwise) arising in any way out of
00039 // the use of this software, even if advised of the possibility of such damage.
00040 //
00041 //M*/
00042 #include "precomp.hpp"
00043 
00044 void cv::cornerSubPix( InputArray _image, InputOutputArray _corners,
00045                        Size win, Size zeroZone, TermCriteria criteria )
00046 {
00047     const int MAX_ITERS = 100;
00048     int win_w = win.width * 2 + 1, win_h = win.height * 2 + 1;
00049     int i, j, k;
00050     int max_iters = (criteria.type & CV_TERMCRIT_ITER) ? MIN(MAX(criteria.maxCount, 1), MAX_ITERS) : MAX_ITERS;
00051     double eps = (criteria.type & CV_TERMCRIT_EPS) ? MAX(criteria.epsilon, 0.) : 0;
00052     eps *= eps; // use square of error in comparsion operations
00053 
00054     cv::Mat src = _image.getMat(), cornersmat = _corners.getMat();
00055     int count = cornersmat.checkVector(2, CV_32F);
00056     CV_Assert( count >= 0 );
00057     Point2f * corners = cornersmat.ptr<Point2f >();
00058 
00059     if( count == 0 )
00060         return;
00061 
00062     CV_Assert( win.width > 0 && win.height > 0 );
00063     CV_Assert( src.cols >= win.width*2 + 5 && src.rows >= win.height*2 + 5 );
00064     CV_Assert( src.channels() == 1 );
00065 
00066     Mat maskm(win_h, win_w, CV_32F), subpix_buf(win_h+2, win_w+2, CV_32F);
00067     float* mask = maskm.ptr<float>();
00068 
00069     for( i = 0; i < win_h; i++ )
00070     {
00071         float y = (float)(i - win.height)/win.height;
00072         float vy = std::exp(-y*y);
00073         for( j = 0; j < win_w; j++ )
00074         {
00075             float x = (float)(j - win.width)/win.width;
00076             mask[i * win_w + j] = (float)(vy*std::exp(-x*x));
00077         }
00078     }
00079 
00080     // make zero_zone
00081     if( zeroZone.width >= 0 && zeroZone.height >= 0 &&
00082         zeroZone.width * 2 + 1 < win_w && zeroZone.height * 2 + 1 < win_h )
00083     {
00084         for( i = win.height - zeroZone.height; i <= win.height + zeroZone.height; i++ )
00085         {
00086             for( j = win.width - zeroZone.width; j <= win.width + zeroZone.width; j++ )
00087             {
00088                 mask[i * win_w + j] = 0;
00089             }
00090         }
00091     }
00092 
00093     // do optimization loop for all the points
00094     for( int pt_i = 0; pt_i < count; pt_i++ )
00095     {
00096         Point2f  cT = corners[pt_i], cI = cT;
00097         int iter = 0;
00098         double err = 0;
00099 
00100         do
00101         {
00102             Point2f  cI2;
00103             double a = 0, b = 0, c = 0, bb1 = 0, bb2 = 0;
00104 
00105             getRectSubPix(src, Size(win_w+2, win_h+2), cI, subpix_buf, subpix_buf.type());
00106             const float* subpix = &subpix_buf.at<float>(1,1);
00107 
00108             // process gradient
00109             for( i = 0, k = 0; i < win_h; i++, subpix += win_w + 2 )
00110             {
00111                 double py = i - win.height;
00112 
00113                 for( j = 0; j < win_w; j++, k++ )
00114                 {
00115                     double m = mask[k];
00116                     double tgx = subpix[j+1] - subpix[j-1];
00117                     double tgy = subpix[j+win_w+2] - subpix[j-win_w-2];
00118                     double gxx = tgx * tgx * m;
00119                     double gxy = tgx * tgy * m;
00120                     double gyy = tgy * tgy * m;
00121                     double px = j - win.width;
00122 
00123                     a += gxx;
00124                     b += gxy;
00125                     c += gyy;
00126 
00127                     bb1 += gxx * px + gxy * py;
00128                     bb2 += gxy * px + gyy * py;
00129                 }
00130             }
00131 
00132             double det=a*c-b*b;
00133             if( fabs( det ) <= DBL_EPSILON*DBL_EPSILON )
00134                 break;
00135 
00136             // 2x2 matrix inversion
00137             double scale=1.0/det;
00138             cI2.x = (float)(cI.x + c*scale*bb1 - b*scale*bb2);
00139             cI2.y = (float)(cI.y - b*scale*bb1 + a*scale*bb2);
00140             err = (cI2.x - cI.x) * (cI2.x - cI.x) + (cI2.y - cI.y) * (cI2.y - cI.y);
00141             cI = cI2;
00142             if( cI.x < 0 || cI.x >= src.cols || cI.y < 0 || cI.y >= src.rows )
00143                 break;
00144         }
00145         while( ++iter < max_iters && err > eps );
00146 
00147         // if new point is too far from initial, it means poor convergence.
00148         // leave initial point as the result
00149         if( fabs( cI.x - cT.x ) > win.width || fabs( cI.y - cT.y ) > win.height )
00150             cI = cT;
00151 
00152         corners[pt_i] = cI;
00153     }
00154 }
00155 
00156 
00157 CV_IMPL void
00158 cvFindCornerSubPix( const void* srcarr, CvPoint2D32f* _corners,
00159                    int count, CvSize win, CvSize zeroZone,
00160                    CvTermCriteria  criteria )
00161 {
00162     if(!_corners || count <= 0)
00163         return;
00164 
00165     cv::Mat src = cv::cvarrToMat(srcarr), corners(count, 1, CV_32FC2, _corners);
00166     cv::cornerSubPix(src, corners, win, zeroZone, criteria);
00167 }
00168 
00169 /* End of file. */
00170