subpixelcorner.cpp

Go to the documentation of this file.

#include "subpixelcorner.h"
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;

namespace aruco {

SubPixelCorner::SubPixelCorner() {
    _winSize = 15;
    _apertureSize = 3;
    _term.maxCount = 10;
    _term.epsilon = 0.1;
    _term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS;
    enable = true;
}

void SubPixelCorner::checkTerm() {
    switch (_term.type) {
    case CV_TERMCRIT_ITER:
        _term.epsilon = 0.f;
        _term.maxCount;
        break;
    case CV_TERMCRIT_EPS:
        _term.maxCount = _term.COUNT;
        break;
    case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
        break;
    default:
        _term.maxCount = _term.COUNT;
        _term.epsilon = 0.1;
        _term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS;
        break;
    }

    eps = std::max(_term.epsilon, 0.0);
    eps = eps * eps;

    _max_iters = std::max(_term.maxCount, 1);
    int max1 = TermCriteria::MAX_ITER;
    _max_iters = std::min(_max_iters, max1);
}

double SubPixelCorner::pointDist(cv::Point2f estimate_corner, cv::Point2f curr_corner) {
    double dist = ((curr_corner.x - estimate_corner.x) * (curr_corner.x - estimate_corner.x)) +
                  ((curr_corner.y - estimate_corner.y) * (curr_corner.y - estimate_corner.y));
    return dist;
}


void SubPixelCorner::generateMask() {

    double coeff = 1. / (_winSize * _winSize);
    float *maskX = (float *)calloc(1, (_winSize * sizeof(float)));
    float *maskY = (float *)calloc(1, (_winSize * sizeof(float)));
    mask.create(_winSize, _winSize, CV_32FC(1));
    /* calculate mask */
    int k = 0;
    for (int i = -_winSize / 2, k = 0; i <= _winSize / 2; i++, k++) {
        maskX[k] = (float)exp(-i * i * coeff);
    }

    maskY = maskX;

    for (int i = 0; i < _winSize; i++) {
        float *mask_ptr = mask.ptr< float >(i);
        for (int j = 0; j < _winSize; j++) {
            mask_ptr[j] = maskX[j] * maskY[i];
        }
    }
}

void SubPixelCorner::RefineCorner(cv::Mat image, std::vector< cv::Point2f > &corners) {

    if (enable == false)
        return;
    checkTerm();

    generateMask();
    // loop over all the corner points
    for (int k = 0; k < corners.size(); k++) {
        cv::Point2f curr_corner;
        // initial estimate
        cv::Point2f estimate_corner = corners[k];

        // cerr << 'SSS" << corners[k].x <<":" << corners[k].y << endl;

        if (estimate_corner.x < 0 || estimate_corner.y < 0 || estimate_corner.y > image.rows || estimate_corner.y > image.cols)
            continue;
        int iter = 0;
        double dist = TermCriteria::EPS;
        // loop till termination criteria is met
        do {
            iter = iter + 1;
            curr_corner = estimate_corner;

            /*
    Point cx;
    cx.x=floor(curr_corner.x);
    cx.y=floor(curr_corner.y);
    double dx=curr_corner.x-cx.x;
    double dy=curr_corner.y-cx.y;
    float vIx[2];
    float vIy[2];

    vIx[0] = dx;
    vIx[1] = 1 - dx;
    vIy[0] = dy;
    vIy[1] = 1 - dy;

    int x1=std::max((int)(cx.x-_winSize-_apertureSize/2),0);
    int y1=std::max((int)(cx.y-_winSize-_apertureSize/2),0);

    xmin = x1<0?0:x1;
    xmax = x1+_winSize<image.cols?x1+_winSize:image.cols-1;
    ymin = y1<0?0:y1;
    ymax = y1+_winSize<image.rows?y1+_winSize:image.rows-1;

    Rect roi=Rect(xmin,ymin,xmax-xmin,ymax-ymin);
    */

            Mat local;
            cv::getRectSubPix(image, Size(_winSize + 2 * (_apertureSize / 2), _winSize + 2 * (_apertureSize / 2)), curr_corner, local);



            cv::Mat Dx, Dy;
            // extracing image ROI about the corner point
            // Mat local=image(roi);
            // computing the gradients over the neighborhood about corner point
            cv::Sobel(local, Dx, CV_32FC(1), 1, 0, _apertureSize, 1, 0);
            cv::Sobel(local, Dy, CV_32FC(1), 0, 1, _apertureSize, 1, 0);

            // parameters requried for estimations
            double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0;
            int lx = 0, ly = 0;
            for (int i = _apertureSize / 2; i <= _winSize; i++) {

                float *dx_ptr = Dx.ptr< float >(i);
                float *dy_ptr = Dy.ptr< float >(i);
                ly = i - _winSize / 2 - _apertureSize / 2;

                float *mask_ptr = mask.ptr< float >(ly + _winSize / 2);

                for (int j = _apertureSize / 2; j <= _winSize; j++) {

                    lx = j - _winSize / 2 - _apertureSize / 2;
                    // cerr << lx+_winSize/2 << ":" ;
                    double val = mask_ptr[lx + _winSize / 2];
                    double dxx = dx_ptr[j] * dx_ptr[j] * val;
                    double dyy = dy_ptr[j] * dy_ptr[j] * val;
                    double dxy = dx_ptr[j] * dy_ptr[j] * val;

                    A = A + dxx;
                    B = B + dxy;
                    E = E + dyy;
                    C = C + dxx * lx + dxy * ly;
                    F = F + dxy * lx + dyy * ly;
                }
            }

            // computing denominator
            double det = (A * E - B * B);
            if (fabs(det) > DBL_EPSILON * DBL_EPSILON) {
                det = 1.0 / det;
                // translating back to original corner and adding new estimates
                estimate_corner.x = curr_corner.x + ((C * E) - (B * F)) * det;
                estimate_corner.y = curr_corner.y + ((A * F) - (C * D)) * det;
            } else {
                estimate_corner.x = curr_corner.x;
                estimate_corner.y = curr_corner.y;
            }

            dist = pointDist(estimate_corner, curr_corner);


        } while (iter < _max_iters && dist > eps);

        // double dist=pointDist(corners[k],estimate_corner);
        if (fabs(corners[k].x - estimate_corner.x) > _winSize || fabs(corners[k].y - estimate_corner.y) > _winSize) {
            estimate_corner.x = corners[k].x;
            estimate_corner.y = corners[k].y;
        }
        corners[k].x = estimate_corner.x;
        corners[k].y = estimate_corner.y;
        // cerr << "EEE" << corners[k].x <<":" << corners[k].y << endl;
    }
}
}