2 #include <opencv2/imgproc/imgproc.hpp> 7 SubPixelCorner::SubPixelCorner() {
12 _term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS;
16 void SubPixelCorner::checkTerm() {
18 case CV_TERMCRIT_ITER:
23 _term.maxCount = _term.COUNT;
25 case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
28 _term.maxCount = _term.COUNT;
30 _term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS;
34 eps = std::max(_term.epsilon, 0.0);
37 _max_iters = std::max(_term.maxCount, 1);
38 int max1 = TermCriteria::MAX_ITER;
39 _max_iters = std::min(_max_iters, max1);
42 double SubPixelCorner::pointDist(cv::Point2f estimate_corner, cv::Point2f curr_corner) {
43 double dist = ((curr_corner.x - estimate_corner.x) * (curr_corner.x - estimate_corner.x)) +
44 ((curr_corner.y - estimate_corner.y) * (curr_corner.y - estimate_corner.y));
49 void SubPixelCorner::generateMask() {
51 double coeff = 1. / (_winSize * _winSize);
52 float *maskX = (
float *)calloc(1, (_winSize *
sizeof(
float)));
53 float *maskY = (
float *)calloc(1, (_winSize *
sizeof(
float)));
54 mask.create(_winSize, _winSize, CV_32FC(1));
57 for (
int i = -_winSize / 2, k = 0; i <= _winSize / 2; i++, k++) {
58 maskX[k] = (float)exp(-i * i * coeff);
63 for (
int i = 0; i < _winSize; i++) {
64 float *mask_ptr = mask.ptr<
float >(i);
65 for (
int j = 0; j < _winSize; j++) {
66 mask_ptr[j] = maskX[j] * maskY[i];
71 void SubPixelCorner::RefineCorner(cv::Mat image, std::vector< cv::Point2f > &corners) {
79 for (
int k = 0; k < corners.size(); k++) {
80 cv::Point2f curr_corner;
82 cv::Point2f estimate_corner = corners[k];
86 if (estimate_corner.x < 0 || estimate_corner.y < 0 || estimate_corner.y > image.rows || estimate_corner.y > image.cols)
89 double dist = TermCriteria::EPS;
93 curr_corner = estimate_corner;
121 cv::getRectSubPix(image, Size(_winSize + 2 * (_apertureSize / 2), _winSize + 2 * (_apertureSize / 2)), curr_corner, local);
129 cv::Sobel(local, Dx, CV_32FC(1), 1, 0, _apertureSize, 1, 0);
130 cv::Sobel(local, Dy, CV_32FC(1), 0, 1, _apertureSize, 1, 0);
133 double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0;
135 for (
int i = _apertureSize / 2; i <= _winSize; i++) {
137 float *dx_ptr = Dx.ptr<
float >(i);
138 float *dy_ptr = Dy.ptr<
float >(i);
139 ly = i - _winSize / 2 - _apertureSize / 2;
141 float *mask_ptr = mask.ptr<
float >(ly + _winSize / 2);
143 for (
int j = _apertureSize / 2; j <= _winSize; j++) {
145 lx = j - _winSize / 2 - _apertureSize / 2;
147 double val = mask_ptr[lx + _winSize / 2];
148 double dxx = dx_ptr[j] * dx_ptr[j] * val;
149 double dyy = dy_ptr[j] * dy_ptr[j] * val;
150 double dxy = dx_ptr[j] * dy_ptr[j] * val;
155 C = C + dxx * lx + dxy * ly;
156 F = F + dxy * lx + dyy * ly;
161 double det = (A * E - B * B);
162 if (fabs(det) > DBL_EPSILON * DBL_EPSILON) {
165 estimate_corner.x = curr_corner.x + ((C * E) - (B * F)) * det;
166 estimate_corner.y = curr_corner.y + ((A * F) - (C * D)) * det;
168 estimate_corner.x = curr_corner.x;
169 estimate_corner.y = curr_corner.y;
172 dist = pointDist(estimate_corner, curr_corner);
175 }
while (iter < _max_iters && dist > eps);
178 if (fabs(corners[k].x - estimate_corner.x) > _winSize || fabs(corners[k].y - estimate_corner.y) > _winSize) {
179 estimate_corner.x = corners[k].x;
180 estimate_corner.y = corners[k].y;
182 corners[k].x = estimate_corner.x;
183 corners[k].y = estimate_corner.y;