OpenCV亞像素角點cornerSubPixel()源代碼分析

時間 2019-11-16

標籤 opencv 像素 cornersubpixel 源代碼分析简体版

原文原文鏈接

　　上一篇博客中講到了goodFeatureToTrack（）這個API函數可以獲取圖像中的強角點。可是獲取的角點座標是整數，可是一般狀況下，角點的真實位置並不必定在整數像素位置，所以爲了獲取更爲精確的角點位置座標，須要角點座標達到亞像素（subPixel）精度。git

1. 求取亞像素精度的原理

　　找到一篇講述原理很是清楚的文檔github

https://xueyayang.github.io/pdf_posts/%E4%BA%9A%E5%83%8F%E7%B4%A0%E8%A7%92%E7%82%B9%E7%9A%84%E6%B1%82%E6%B3%95.pdf，貼上來，以下：函數

2. OpenCV源代碼分析

　　OpenCV中有cornerSubPixel（）這個API函數用來針對初始的整數角點座標進行亞像素精度的優化，該函數原型以下：oop

void cv::cornerSubPix( InputArray _image, InputOutputArray _corners,
                       Size win, Size zeroZone, TermCriteria criteria )

　　_image爲輸入的單通道圖像；_corners爲提取的初始整數角點（好比用goodFeatureToTrack提取的強角點）；win爲求取亞像素角點的窗口大小，好比設置Size（11,11），須要注意的是11爲半徑，則窗口大小爲23x23；zeroZone是設置的「零區域」，在搜索窗口內，設置的「零區域」內的值不會被累加，權重值爲0。若是設置爲Size(-1,-1)，則表示沒有這樣的區域；critteria是條件閾值，包括迭代次數閾值和偏差精度閾值，一旦其中一項條件知足設置的閾值，則中止迭代，得到亞像素角點。post

　　這個API經過下面示例的語句進行調用：優化

cv::cornerSubPix(grayImg, pts, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

　　首先看criteria包含的兩個條件閾值在代碼中是怎麼設置的。以下所示，最大迭代次數爲100次，偏差精度爲eps*eps，也就是0.1*0.1。spa

    const int MAX_ITERS = 100;
    int win_w = win.width * 2 + 1, win_h = win.height * 2 + 1;
    int i, j, k;
    int max_iters = (criteria.type & CV_TERMCRIT_ITER) ? MIN(MAX(criteria.maxCount, 1), MAX_ITERS) : MAX_ITERS;
    double eps = (criteria.type & CV_TERMCRIT_EPS) ? MAX(criteria.epsilon, 0.) : 0;
    eps *= eps; // use square of error in comparsion operations

　　而後是高斯權重的計算，以下所示，窗口中心附近權重高，越往窗口邊界權重越小。若是設置的有「零區域」，則權重值設置爲0。計算出的權重分佈以下圖：3d

Mat maskm(win_h, win_w, CV_32F), subpix_buf(win_h+2, win_w+2, CV_32F);
    float* mask = maskm.ptr<float>();

    for( i = 0; i < win_h; i++ )
    {
        float y = (float)(i - win.height)/win.height;
        float vy = std::exp(-y*y);
        for( j = 0; j < win_w; j++ )
        {
            float x = (float)(j - win.width)/win.width;
            mask[i * win_w + j] = (float)(vy*std::exp(-x*x));
        }
    }

    // make zero_zone
    if( zeroZone.width >= 0 && zeroZone.height >= 0 &&
        zeroZone.width * 2 + 1 < win_w && zeroZone.height * 2 + 1 < win_h )
    {
        for( i = win.height - zeroZone.height; i <= win.height + zeroZone.height; i++ )
        {
            for( j = win.width - zeroZone.width; j <= win.width + zeroZone.width; j++ )
            {
                mask[i * win_w + j] = 0;
            }
        }
    }

　　接下來就是針對每一個初始角點，按照上述公式，逐個進行迭代求取亞像素角點，代碼以下。code

　　① 代碼中CI2爲本次迭代獲取的亞像素角點位置，CI爲上次迭代獲取的亞像素角點位置，CT是初始的整數角點位置。blog

　　② 每次迭代結束計算CI與CI2之間的歐式距離err，若是二者之間的歐式距離err小於設定的閾值，或者迭代次數達到設定的閾值，則中止迭代。

　　③中止迭代後，須要再次判斷最終的亞像素角點位置和初始整數角點之間的差別，若是差值大於設定窗口尺寸的一半，則說明最小二乘計算中收斂性很差，丟棄計算獲得的亞像素角點，仍然使用初始的整數角點。

// do optimization loop for all the points
    for( int pt_i = 0; pt_i < count; pt_i++ )
    {
        Point2f cT = corners[pt_i], cI = cT;
        int iter = 0;
        double err = 0;

        do
        {
            Point2f cI2;
            double a = 0, b = 0, c = 0, bb1 = 0, bb2 = 0;

            getRectSubPix(src, Size(win_w+2, win_h+2), cI, subpix_buf, subpix_buf.type());
            const float* subpix = &subpix_buf.at<float>(1,1);

            // process gradient
            for( i = 0, k = 0; i < win_h; i++, subpix += win_w + 2 )
            {
                double py = i - win.height;

                for( j = 0; j < win_w; j++, k++ )
                {
                    double m = mask[k];
                    double tgx = subpix[j+1] - subpix[j-1];
                    double tgy = subpix[j+win_w+2] - subpix[j-win_w-2];
                    double gxx = tgx * tgx * m;
                    double gxy = tgx * tgy * m;
                    double gyy = tgy * tgy * m;
                    double px = j - win.width;

                    a += gxx;
                    b += gxy;
                    c += gyy;

                    bb1 += gxx * px + gxy * py;
                    bb2 += gxy * px + gyy * py;
                }
            }

            double det=a*c-b*b;
            if( fabs( det ) <= DBL_EPSILON*DBL_EPSILON )
                break;

            // 2x2 matrix inversion
            double scale=1.0/det;
            cI2.x = (float)(cI.x + c*scale*bb1 - b*scale*bb2);
            cI2.y = (float)(cI.y - b*scale*bb1 + a*scale*bb2);
            err = (cI2.x - cI.x) * (cI2.x - cI.x) + (cI2.y - cI.y) * (cI2.y - cI.y);
            cI = cI2;
            if( cI.x < 0 || cI.x >= src.cols || cI.y < 0 || cI.y >= src.rows )
                break;
        }
        while( ++iter < max_iters && err > eps );

        // if new point is too far from initial, it means poor convergence.
        // leave initial point as the result
        if( fabs( cI.x - cT.x ) > win.width || fabs( cI.y - cT.y ) > win.height )
            cI = cT;

        corners[pt_i] = cI;
    }

　　本身參照OpenCV源代碼寫了一個myCornerSubPix（）接口函數以便加深理解，以下，僅供參考：

//獲取窗口內子圖像
bool getSubImg(cv::Mat srcImg, cv::Point2f currPoint, cv::Mat &subImg)
{
    int subH = subImg.rows;
    int subW = subImg.cols;
    int x = int(currPoint.x+0.5f);
    int y = int(currPoint.y+0.5f);
    int initx = x - subImg.cols / 2;
    int inity = y - subImg.rows / 2;
    if (initx < 0 || inity < 0 || (initx+subW)>=srcImg.cols || (inity+subH)>=srcImg.rows )   return false;
    cv::Rect imgROI(initx, inity, subW, subH);
    subImg = srcImg(imgROI).clone();
    return true;
}

//亞像素角點提取 void myCornerSubPix(cv::Mat srcImg, vector<cv::Point2f> &pts, cv::Size winSize, cv::Size zeroZone, cv::TermCriteria criteria)
{
　　//搜索窗口大小 int winH = winSize.width * 2 + 1;
    int winW = winSize.height * 2 + 1;
    int winCnt = winH*winW;

　　//迭代閾值限制 int MAX_ITERS = 100;
    int max_iters = (criteria.type & CV_TERMCRIT_ITER) ? MIN(MAX(criteria.maxCount, 1), MAX_ITERS) : MAX_ITERS;
    double eps = (criteria.type & CV_TERMCRIT_EPS) ? MAX(criteria.epsilon, 0.) : 0;
    eps *= eps; // use square of error in comparsion operations

    //生成高斯權重
    cv::Mat weightMask = cv::Mat(winH, winW, CV_32FC1);
    for (int i = 0; i < winH; i++)
    {
        for (int j = 0; j < winW; j++)
        {
            float wx = (float)(j - winSize.width) / winSize.width;
            float wy = (float)(i - winSize.height) / winSize.height;
            float vx = exp(-wx*wx);
            float vy = exp(-wy*wy);
            weightMask.at<float>(i, j) = (float)(vx*vy);
        }
    }
　　//遍歷全部初始角點，依次迭代 for (int k = 0; k < pts.size(); k++)
    {
        double a, b, c, bb1, bb2;
        
        cv::Mat subImg = cv::Mat::zeros(winH+2, winW+2, CV_8UC1);
        cv::Point2f currPoint = pts[k];
        cv::Point2f iterPoint = currPoint;

        int iterCnt = 0;
        double err = 0;
        //迭代
        do 
        {
            a = b = c = bb1 = bb2 = 0;
            //提取以當前點爲中心的窗口子圖像（爲了方便求sobel微分，窗口各向四個方向擴展一行（列）像素）
            if ( !getSubImg(srcImg, iterPoint, subImg)) break;
            uchar *pSubData = (uchar*)subImg.data+winW+3;
            //以下計算參考上述推導公式，窗口內累加 for (int i = 0; i < winH; i ++)
            {
                for (int j = 0; j < winW; j++)
                {
　　　　　　　　　　　　//讀取高斯權重值 double m = weightMask.at<float>(i, j);
                    //sobel算子求梯度 double sobelx = double(pSubData[i*(winW+2) + j + 1] - pSubData[i*(winW+2) + j - 1]);
                    double sobely = double(pSubData[(i+1)*(winW+2) + j] - pSubData[(i - 1)*(winW+2) + j]);
                    double gxx = sobelx*sobelx*m;
                    double gxy = sobelx*sobely*m;
                    double gyy = sobely*sobely*m;
                    a += gxx;
                    b += gxy;
                    c += gyy;
                    //鄰域像素p的位置座標 double px = j - winSize.width;
                    double py = i - winSize.height;

                    bb1 += gxx*px + gxy*py;
                    bb2 += gxy*px + gyy*py;
                }
            }
            double det = a*c - b*b;
            if (fabs(det) <= DBL_EPSILON*DBL_EPSILON)
                break;
            //求逆矩陣
            double invA = c / det;
            double invC = a / det;
            double invB = -b / det;
            //角點新位置
            cv::Point2f newPoint;
            newPoint.x = (float)(iterPoint.x + invA*bb1 + invB*bb2);
            newPoint.y = (float)(iterPoint.y + invB*bb1 + invC*bb2);
            //和上一次迭代之間的偏差
            err = (newPoint.x - iterPoint.x)*(newPoint.x - iterPoint.x) + (newPoint.y - iterPoint.y)*(newPoint.y - iterPoint.y);
            //更新角點位置
            iterPoint = newPoint;
            iterCnt++;
            if (iterPoint.x < 0 || iterPoint.x >= srcImg.cols || iterPoint.y < 0 || iterPoint.y >= srcImg.rows)
                break;
        } while (err > eps && iterCnt < max_iters);
        //判斷求得的亞像素角點與初始角點之間的差別，即：最小二乘法的收斂性 if (fabs(iterPoint.x - currPoint.x) > winSize.width || fabs(iterPoint.y - currPoint.y) > winSize.height)
            iterPoint = currPoint;
　　　　//保存算出的亞像素角點
        pts[k] = iterPoint;
    }
}