subpixelcorner.cpp
Go to the documentation of this file.
00001 #include "subpixelcorner.h"
00002 #include <opencv2/imgproc/imgproc.hpp>
00003 using namespace cv;
00004 
00005 namespace aruco {
00006 
00007 SubPixelCorner::SubPixelCorner() {
00008     _winSize = 15;
00009     _apertureSize = 3;
00010     _term.maxCount = 10;
00011     _term.epsilon = 0.1;
00012     _term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS;
00013     enable = true;
00014 }
00015 
00016 void SubPixelCorner::checkTerm() {
00017     switch (_term.type) {
00018     case CV_TERMCRIT_ITER:
00019         _term.epsilon = 0.f;
00020         _term.maxCount;
00021         break;
00022     case CV_TERMCRIT_EPS:
00023         _term.maxCount = _term.COUNT;
00024         break;
00025     case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
00026         break;
00027     default:
00028         _term.maxCount = _term.COUNT;
00029         _term.epsilon = 0.1;
00030         _term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS;
00031         break;
00032     }
00033 
00034     eps = std::max(_term.epsilon, 0.0);
00035     eps = eps * eps;
00036 
00037     _max_iters = std::max(_term.maxCount, 1);
00038     int max1 = TermCriteria::MAX_ITER;
00039     _max_iters = std::min(_max_iters, max1);
00040 }
00041 
00042 double SubPixelCorner::pointDist(cv::Point2f estimate_corner, cv::Point2f curr_corner) {
00043     double dist = ((curr_corner.x - estimate_corner.x) * (curr_corner.x - estimate_corner.x)) +
00044                   ((curr_corner.y - estimate_corner.y) * (curr_corner.y - estimate_corner.y));
00045     return dist;
00046 }
00047 
00048 
00049 void SubPixelCorner::generateMask() {
00050 
00051     double coeff = 1. / (_winSize * _winSize);
00052     float *maskX = (float *)calloc(1, (_winSize * sizeof(float)));
00053     float *maskY = (float *)calloc(1, (_winSize * sizeof(float)));
00054     mask.create(_winSize, _winSize, CV_32FC(1));
00055     /* calculate mask */
00056     int k = 0;
00057     for (int i = -_winSize / 2, k = 0; i <= _winSize / 2; i++, k++) {
00058         maskX[k] = (float)exp(-i * i * coeff);
00059     }
00060 
00061     maskY = maskX;
00062 
00063     for (int i = 0; i < _winSize; i++) {
00064         float *mask_ptr = mask.ptr< float >(i);
00065         for (int j = 0; j < _winSize; j++) {
00066             mask_ptr[j] = maskX[j] * maskY[i];
00067         }
00068     }
00069 }
00070 
00071 void SubPixelCorner::RefineCorner(cv::Mat image, std::vector< cv::Point2f > &corners) {
00072 
00073     if (enable == false)
00074         return;
00075     checkTerm();
00076 
00077     generateMask();
00078     // loop over all the corner points
00079     for (int k = 0; k < corners.size(); k++) {
00080         cv::Point2f curr_corner;
00081         // initial estimate
00082         cv::Point2f estimate_corner = corners[k];
00083 
00084         // cerr << 'SSS" << corners[k].x <<":" << corners[k].y << endl;
00085 
00086         if (estimate_corner.x < 0 || estimate_corner.y < 0 || estimate_corner.y > image.rows || estimate_corner.y > image.cols)
00087             continue;
00088         int iter = 0;
00089         double dist = TermCriteria::EPS;
00090         // loop till termination criteria is met
00091         do {
00092             iter = iter + 1;
00093             curr_corner = estimate_corner;
00094 
00095             /*
00096     Point cx;
00097     cx.x=floor(curr_corner.x);
00098     cx.y=floor(curr_corner.y);
00099     double dx=curr_corner.x-cx.x;
00100     double dy=curr_corner.y-cx.y;
00101     float vIx[2];
00102     float vIy[2];
00103 
00104     vIx[0] = dx;
00105     vIx[1] = 1 - dx;
00106     vIy[0] = dy;
00107     vIy[1] = 1 - dy;
00108 
00109     int x1=std::max((int)(cx.x-_winSize-_apertureSize/2),0);
00110     int y1=std::max((int)(cx.y-_winSize-_apertureSize/2),0);
00111 
00112     xmin = x1<0?0:x1;
00113     xmax = x1+_winSize<image.cols?x1+_winSize:image.cols-1;
00114     ymin = y1<0?0:y1;
00115     ymax = y1+_winSize<image.rows?y1+_winSize:image.rows-1;
00116 
00117     Rect roi=Rect(xmin,ymin,xmax-xmin,ymax-ymin);
00118     */
00119 
00120             Mat local;
00121             cv::getRectSubPix(image, Size(_winSize + 2 * (_apertureSize / 2), _winSize + 2 * (_apertureSize / 2)), curr_corner, local);
00122 
00123 
00124 
00125             cv::Mat Dx, Dy;
00126             // extracing image ROI about the corner point
00127             // Mat local=image(roi);
00128             // computing the gradients over the neighborhood about corner point
00129             cv::Sobel(local, Dx, CV_32FC(1), 1, 0, _apertureSize, 1, 0);
00130             cv::Sobel(local, Dy, CV_32FC(1), 0, 1, _apertureSize, 1, 0);
00131 
00132             // parameters requried for estimations
00133             double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0;
00134             int lx = 0, ly = 0;
00135             for (int i = _apertureSize / 2; i <= _winSize; i++) {
00136 
00137                 float *dx_ptr = Dx.ptr< float >(i);
00138                 float *dy_ptr = Dy.ptr< float >(i);
00139                 ly = i - _winSize / 2 - _apertureSize / 2;
00140 
00141                 float *mask_ptr = mask.ptr< float >(ly + _winSize / 2);
00142 
00143                 for (int j = _apertureSize / 2; j <= _winSize; j++) {
00144 
00145                     lx = j - _winSize / 2 - _apertureSize / 2;
00146                     // cerr << lx+_winSize/2 << ":" ;
00147                     double val = mask_ptr[lx + _winSize / 2];
00148                     double dxx = dx_ptr[j] * dx_ptr[j] * val;
00149                     double dyy = dy_ptr[j] * dy_ptr[j] * val;
00150                     double dxy = dx_ptr[j] * dy_ptr[j] * val;
00151 
00152                     A = A + dxx;
00153                     B = B + dxy;
00154                     E = E + dyy;
00155                     C = C + dxx * lx + dxy * ly;
00156                     F = F + dxy * lx + dyy * ly;
00157                 }
00158             }
00159 
00160             // computing denominator
00161             double det = (A * E - B * B);
00162             if (fabs(det) > DBL_EPSILON * DBL_EPSILON) {
00163                 det = 1.0 / det;
00164                 // translating back to original corner and adding new estimates
00165                 estimate_corner.x = curr_corner.x + ((C * E) - (B * F)) * det;
00166                 estimate_corner.y = curr_corner.y + ((A * F) - (C * D)) * det;
00167             } else {
00168                 estimate_corner.x = curr_corner.x;
00169                 estimate_corner.y = curr_corner.y;
00170             }
00171 
00172             dist = pointDist(estimate_corner, curr_corner);
00173 
00174 
00175         } while (iter < _max_iters && dist > eps);
00176 
00177         // double dist=pointDist(corners[k],estimate_corner);
00178         if (fabs(corners[k].x - estimate_corner.x) > _winSize || fabs(corners[k].y - estimate_corner.y) > _winSize) {
00179             estimate_corner.x = corners[k].x;
00180             estimate_corner.y = corners[k].y;
00181         }
00182         corners[k].x = estimate_corner.x;
00183         corners[k].y = estimate_corner.y;
00184         // cerr << "EEE" << corners[k].x <<":" << corners[k].y << endl;
00185     }
00186 }
00187 }


asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Mei├čner Pascal, Qattan Mohamad
autogenerated on Thu Jun 6 2019 21:14:12