Reference
opening
What we analyze is the source code of OpenCV's cornerSubPixel(), and the C++ reproduction code I directly copy and paste the reference. Additionally a Python version will be given.
Go
The cornerSubPixel() API function is used to optimize the sub-pixel precision for the initial integer corner coordinates. The modified function prototype is as follows:
void cv::cornerSubPix(InputArray _image, InputOutputArray _corners,
Size win, Size zeroZone, TermCriteria criteria)
- _image is the input single-channel image;
- _corner is the initial integer corner point extracted;
win is the radius window size for calculating the sub-pixel corner point. It should be noted that the final window size is win 2+1, for example, if you set (5, 5), the final window is (5 2+1, 5*2+1) = (11, 11); - zeroZone is the "zero zone" that is not set. In the search window, the value in the set "zero zone" will not be accumulated, and the weight is 0. If it is set to Size (-1, -1), it means that there is no such area;
- Criteria is the condition threshold, including the iteration threshold and the error precision threshold. Once one of the conditions meets the set threshold, the iteration is stopped and sub-pixel corners are obtained.
Call example:
cv::cornerSubPix(grayImg, pts, cv::Size(11,11), cv::Size(-1,-1), cv::TermCriteria(cv_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
Criteria contains two conditional thresholds, how are these two conditional thresholds set in the code. As shown below, the maximum number of iterations is set to 100, and the error precision is: eps*eps=0.1*0.1=0.01.
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
Then there is the Gaussian weight. The weight near the center of the window is high, and the weight becomes smaller as it goes to the window boundary. If "zero area" is set, the weight is set to 0. The calculated weight distribution is as follows:
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;
}
}
}
The next step is to iteratively obtain sub-pixel corner points one by one according to the above formula for each initial corner point. The code is as follows.
① In the code, CI2 is the sub-pixel corner position obtained in this iteration, CI is the sub-pixel corner position obtained in the previous iteration, and CT is the initial integer corner position.
② Calculate the Euclidean distance err between CI and CI2 at the end of each iteration. If the Euclidean distance err between the two is less than the set threshold, or the number of iterations reaches the set threshold, stop the iteration.
③ After stopping the iteration, it is necessary to judge the difference between the final sub-pixel corner position and the initial integer corner point again. If the difference is greater than half of the set window size, it means that the convergence of the least squares calculation is not good, and the calculation is discarded. The obtained sub-pixel corners still use the initial integer corners.
// 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;
}
By first entering SLAM (2) - using the least square method to find the sub-pixel coordinates, we get the final solution formula:
q = ( G i TG iwi ) − 1 ( G i TG iwi ) piq=(G_i^TG_iw_i)^{ -1}(G_i^TG_iw_i)p_iq=(GiTGiwi)−1(GiTGiwi)pi
But in fact, the original formula is used here:
G i ∗ q = G i ∗ pi G_i*q=G_i*p_iGi∗q=Gi∗pi
In addition,
G i ∗ wi ∗ q = G i ∗ wi ∗ pi G_i*w_i*q=G_i*w_i*p_iGi∗wi∗q=Gi∗wi∗pi
加上求和就是：
( ∑ i = 1 n G i ∗ w i ) q = ( ∑ i = 1 n G i ∗ w i p i ) \left( \sum_{i=1}^n{G_i*w_i} \right) q=\left( \sum_{i=1}^n{G_i*w_i}p_i \right) (i=1∑nGi∗wi)q=(i=1∑nGi∗wipi)
onto the code is this part of the above code:
a += gxx;
b += gxy;
c += gyy;
bb1 += gxx * px + gxy * py;
bb2 += gxy * px + gyy * py;
Then there is a formula for inverting a 2*2 matrix:
A − 1 = [ a bc d ] − 1 = 1 / ( ad − cb ) [ d − b − c a ] A^{-1}= \left[ \begin{array}{c} a\,\,b\\ c\,\,d\\ \end{array} \right] ^{-1}=1/\left( ad-cb \ right) \left[ \begin{array}{c} d\,\,-b\\ -c\,\, a\\ \end{array} \right]A−1=[abcd]−1=1/(ad−cb)[d−b−ca]
private equation
A − 1 = [ a bc d ] − 1 = 1 / ∣ A ∣ [ c − b − b a ] A^{-1}=\left[ \begin{array}{c} a\, . \,b\\ c\,\,d\\ \end{array} \right] ^{-1}=1/|A| \left[ \begin{array}{c}c\,\,-b\\ -b\,\, a\\\end{array}\right]A−1=[abcd]−1=1/∣A∣[c−b−ba]
double det=a*c-b*b; // 这里的det就是|A|
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;
C++ custom cornerSubPixel in reference
//获取窗口内子图像
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;
}
}