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 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
22 //
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
26 //
27 // * The name of the copyright holders may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42 #include "precomp.hpp"
43
cornerSubPix(InputArray _image,InputOutputArray _corners,Size win,Size zeroZone,TermCriteria criteria)44 void cv::cornerSubPix( InputArray _image, InputOutputArray _corners,
45 Size win, Size zeroZone, TermCriteria criteria )
46 {
47 CV_INSTRUMENT_REGION();
48
49 const int MAX_ITERS = 100;
50 int win_w = win.width * 2 + 1, win_h = win.height * 2 + 1;
51 int i, j, k;
52 int max_iters = (criteria.type & CV_TERMCRIT_ITER) ? MIN(MAX(criteria.maxCount, 1), MAX_ITERS) : MAX_ITERS;
53 double eps = (criteria.type & CV_TERMCRIT_EPS) ? MAX(criteria.epsilon, 0.) : 0;
54 eps *= eps; // use square of error in comparison operations
55
56 cv::Mat src = _image.getMat(), cornersmat = _corners.getMat();
57 int count = cornersmat.checkVector(2, CV_32F);
58 CV_Assert( count >= 0 );
59 Point2f* corners = cornersmat.ptr<Point2f>();
60
61 if( count == 0 )
62 return;
63
64 CV_Assert( win.width > 0 && win.height > 0 );
65 CV_Assert( src.cols >= win.width*2 + 5 && src.rows >= win.height*2 + 5 );
66 CV_Assert( src.channels() == 1 );
67
68 Mat maskm(win_h, win_w, CV_32F), subpix_buf(win_h+2, win_w+2, CV_32F);
69 float* mask = maskm.ptr<float>();
70
71 for( i = 0; i < win_h; i++ )
72 {
73 float y = (float)(i - win.height)/win.height;
74 float vy = std::exp(-y*y);
75 for( j = 0; j < win_w; j++ )
76 {
77 float x = (float)(j - win.width)/win.width;
78 mask[i * win_w + j] = (float)(vy*std::exp(-x*x));
79 }
80 }
81
82 // make zero_zone
83 if( zeroZone.width >= 0 && zeroZone.height >= 0 &&
84 zeroZone.width * 2 + 1 < win_w && zeroZone.height * 2 + 1 < win_h )
85 {
86 for( i = win.height - zeroZone.height; i <= win.height + zeroZone.height; i++ )
87 {
88 for( j = win.width - zeroZone.width; j <= win.width + zeroZone.width; j++ )
89 {
90 mask[i * win_w + j] = 0;
91 }
92 }
93 }
94
95 // do optimization loop for all the points
96 for( int pt_i = 0; pt_i < count; pt_i++ )
97 {
98 Point2f cT = corners[pt_i], cI = cT;
99 int iter = 0;
100 double err = 0;
101
102 do
103 {
104 Point2f cI2;
105 double a = 0, b = 0, c = 0, bb1 = 0, bb2 = 0;
106
107 getRectSubPix(src, Size(win_w+2, win_h+2), cI, subpix_buf, subpix_buf.type());
108 const float* subpix = &subpix_buf.at<float>(1,1);
109
110 // process gradient
111 for( i = 0, k = 0; i < win_h; i++, subpix += win_w + 2 )
112 {
113 double py = i - win.height;
114
115 for( j = 0; j < win_w; j++, k++ )
116 {
117 double m = mask[k];
118 double tgx = subpix[j+1] - subpix[j-1];
119 double tgy = subpix[j+win_w+2] - subpix[j-win_w-2];
120 double gxx = tgx * tgx * m;
121 double gxy = tgx * tgy * m;
122 double gyy = tgy * tgy * m;
123 double px = j - win.width;
124
125 a += gxx;
126 b += gxy;
127 c += gyy;
128
129 bb1 += gxx * px + gxy * py;
130 bb2 += gxy * px + gyy * py;
131 }
132 }
133
134 double det=a*c-b*b;
135 if( fabs( det ) <= DBL_EPSILON*DBL_EPSILON )
136 break;
137
138 // 2x2 matrix inversion
139 double scale=1.0/det;
140 cI2.x = (float)(cI.x + c*scale*bb1 - b*scale*bb2);
141 cI2.y = (float)(cI.y - b*scale*bb1 + a*scale*bb2);
142 err = (cI2.x - cI.x) * (cI2.x - cI.x) + (cI2.y - cI.y) * (cI2.y - cI.y);
143 cI = cI2;
144 if( cI.x < 0 || cI.x >= src.cols || cI.y < 0 || cI.y >= src.rows )
145 break;
146 }
147 while( ++iter < max_iters && err > eps );
148
149 // if new point is too far from initial, it means poor convergence.
150 // leave initial point as the result
151 if( fabs( cI.x - cT.x ) > win.width || fabs( cI.y - cT.y ) > win.height )
152 cI = cT;
153
154 corners[pt_i] = cI;
155 }
156 }
157
158
159 CV_IMPL void
cvFindCornerSubPix(const void * srcarr,CvPoint2D32f * _corners,int count,CvSize win,CvSize zeroZone,CvTermCriteria criteria)160 cvFindCornerSubPix( const void* srcarr, CvPoint2D32f* _corners,
161 int count, CvSize win, CvSize zeroZone,
162 CvTermCriteria criteria )
163 {
164 if(!_corners || count <= 0)
165 return;
166
167 cv::Mat src = cv::cvarrToMat(srcarr), corners(count, 1, CV_32FC2, _corners);
168 cv::cornerSubPix(src, corners, win, zeroZone, criteria);
169 }
170
171 /* End of file. */
172