cornerSubPixel亞畫素角點檢測原始碼分析（最小二乘法）
OpenCV的中有cornerSubPixel（）這個API函式用來針對初始的整數角點座標進行亞畫素精度的優化，該函式原型如下：
C++: void cornerSubPix(InputArray image, InputOutputArray corners, Size winSize, Size zeroZone, TermCriteria criteria);
image為輸入的單通道影象; corners為提取的初始整數角點;winsize為求取亞畫素角點的視窗大小，比如設定尺寸（11,11），需要注意的是11為半徑，則視窗大小為23x23; zeroZone是設定的“零區域”，在搜尋視窗內，設定的“零區域”內的值不會被累加，權重值為0。如果設定為大小（-1，-1），則表示沒有這樣的區域; critteria是條件閾值，包括迭代次數閾值和誤差精度閾值，一旦其中一項條件滿足設定的閾值，則停止迭代，獲得亞畫素角點。
這個API通過下面示例的語句呼叫：
cv::cornerSubPix(grayImg, pts, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
首先看標準包含的兩個條件閾值在原始碼中是怎麼設定的。如下所示，預設最大迭代次數為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; // 在比較操作中使用平方誤差
然後是高斯權重的計算，如下所示，視窗中心附近權重高，越往視窗邊界權重越小如果設定的有“零區域”，則權重值設定為0。計算出的權重分佈如下圖：
Mat maskm(win_h, win_w, CV_32F), subpix_buf(win_h+2, win_w+2, CV_32F);
float* mask = maskm.ptr();
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; } } }  接下來就是針對每個初始角點，按照上述公式，逐個進行迭代求取亞畫素角點，原始碼如下。
// 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;
}
①程式碼中CI2為本次迭代獲取的亞畫素位置，CI為上次迭代獲取的亞畫素角點位置，CT是初始的整數角點位置。
②每次迭代結束計算CI與CI2之間的歐式距離err，如果兩者之間的歐式距離err小於設定的閾值，或者迭代次數達到設定的閾值，則停止迭代。
③停止迭代後，需要再次判斷最終的亞畫素角點位置和初始整數角點之間的差異，如果差值大於設定視窗尺寸的一半，則說明最小二乘計算中收斂性不好，丟棄計算得到的亞畫素角點，仍然使用初始的整數角點。
ps:要理解程式碼請看：
代入推導得到的公式即可。
原文請看https://tw.saowen.com/a/6ffb4ed611d5582399844d286fbe93f58a745ca7731b71bda7de08d613124b87