OpenCV中cornerSubPixel（）亚像素求精原理

采用的方法为最小二乘法：

首先我们要构建以下方程：

我们讨论角点的情况：

q是我们要求的角点

p0和p1为q周围的点

（q-pi）为一个向量

Gi为pi处的梯度

所以满足一下公式

Gi*(q-pi)=0

有以下两种情况：

（1）p0处的梯度为0,虽然（q-pi）不为0

（2）p1处（q-pi）和p1处的梯度垂直，因此乘积为0.

Gi*(q-pi)=0

我们写成最小二乘的形式：

Gi*q = Gi*pi

根据最小二乘解：

同理可得：

 

代码:

//  最大迭代次数为100次，误差精度为eps*eps，也就是0.1*0.1。
 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。计算出的权重分布如下图：
*/

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;
            }
        }
    }

/*
① 代码中CI2为本次迭代获取的亚像素角点位置，CI为上次迭代获取的亚像素角点位置，CT是初始的整数角点位置。
　　② 每次迭代结束计算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;
    }