怎么用opencv去找不完整棋盘格的内角点

#include "test.h"

void FindChessCorners2(cv::Mat &chess_img_L, cv::Mat &chess_img_R, int &chessrow,int &chesscol,std::vectorcv::Point2f &corners_L,std::vectorcv::Point2f &corners_R)
{
MeasureKernel Kernel;
GetCorners_Left(chess_img_L, Kernel.s_kernel, Kernel.b_kernel, corners_L);
GetCorners_Right(chess_img_R, Kernel.s_kernel, Kernel.b_kernel, corners_R); 
}

void GetKernels(MeasureKernel &Kernel)
{
int i, loc_flag;
for (i = 0; i < CORNER_PIN_NUM; i++)
{
loc_flag = i;
switch (loc_flag){
case 0:{
Kernel.s_kernel.at(0) = cv::Mat_(CORNER_SKERNEL_SIZE, CORNER_SKERNEL_SIZE);//对角模板的一，三象限 左下和右上白色
for (int i = 0; i < Kernel.s_kernel.at(0).rows; i++){
for (int j = 0; j < Kernel.s_kernel.at(0).cols; j++){
if ((i < CORNER_SKERNEL_SIZE/2 && j > CORNER_SKERNEL_SIZE/2 ) &&( i > CORNER_SKERNEL_SIZE/2 && j < CORNER_SKERNEL_SIZE/2) )
Kernel.s_kernel.at(0).at(i, j) = -1;
else
Kernel.s_kernel.at(0).at(i, j) = 1;
}
}
Kernel.b_kernel.at(0) = cv::Mat_(CORNER_BKERNEL_SIZE, CORNER_BKERNEL_SIZE);
for (int i = 0; i < Kernel.b_kernel.at(0).rows; i++){
for (int j = 0; j < Kernel.b_kernel.at(0).cols; j++){
if ((i < CORNER_BKERNEL_SIZE/2 && j > CORNER_BKERNEL_SIZE/2 )&& (i > CORNER_BKERNEL_SIZE/2 && j < CORNER_BKERNEL_SIZE/2) )
Kernel.b_kernel.at(0).at(i, j) = -1;
else
Kernel.b_kernel.at(0).at(i, j) = 1;
}
}
break;
}
case 1:{
Kernel.s_kernel.at(1) = cv::Mat_(CORNER_SKERNEL_SIZE, CORNER_SKERNEL_SIZE);//对角模板的二，四象限 左上和右下白色
for (int i = 0; i < Kernel.s_kernel.at(1).rows; i++){
for (int j = 0; j < Kernel.s_kernel.at(1).cols; j++){
if ((i >= CORNER_SKERNEL_SIZE/2 && j > CORNER_SKERNEL_SIZE/2)&&(i <= CORNER_SKERNEL_SIZE/2 && j < CORNER_SKERNEL_SIZE/2))
Kernel.s_kernel.at(1).at(i, j) = -1;
else
Kernel.s_kernel.at(1).at(i, j) = 1;
}
}
Kernel.b_kernel.at(1) = cv::Mat_(CORNER_BKERNEL_SIZE, CORNER_BKERNEL_SIZE);
for (int i = 0; i < Kernel.b_kernel.at(1).rows; i++){
for (int j = 0; j < Kernel.b_kernel.at(1).cols; j++){
if ((i >= CORNER_BKERNEL_SIZE/2 && j > CORNER_BKERNEL_SIZE/2)&&(i <= CORNER_BKERNEL_SIZE/2 && j < CORNER_BKERNEL_SIZE/2))
Kernel.b_kernel.at(1).at(i, j) = -1;
else
Kernel.b_kernel.at(1).at(i, j) = 1;
}
}
break;
}

    }
}
}
int GetCorners_Left(cv::Mat &chess_img_L, std::vectorcv::Mat &s_kernel, std::vectorcv::Mat &b_kernel, std::vectorcv::Point2f &chess_corners)
{
GetOneCorner(chess_img_L, s_kernel.at(0), b_kernel.at(0), chess_corners[0]);//左图像内角点一三象限 左下和右上白色

GetOneCorner(chess_img_L, s_kernel.at(1), b_kernel.at(1), chess_corners[1]);//左图像内角点二四象限   左上和右下白色 两者都是内角点
/*if((chess_corners_L.at(0).x <= 0) || (chess_corners_L.at(0).x > (chess_img_L.cols/2-1)) || (chess_corners_L.at(0).y <= 0) || (chess_corners_L.at(0).y > (chess_img_L.rows/2-1)))
return 1;
if((chess_corners_L.at(1).x <= 0) || (chess_corners_L.at(1).x > (chess_img_L.cols/2-1)) || (chess_corners_L.at(1).y < (chess_img_L.rows/2-1)) || (chess_corners_L.at(1).y > (chess_img_L.rows-1)))
return 2;*/
return 0;
}

int GetCorners_Right(cv::Mat &chess_img_R, std::vectorcv::Mat &s_kernel, std::vectorcv::Mat &b_kernel, std::vectorcv::Point2f &chess_corners)
{
GetOneCorner(chess_img_R, s_kernel.at(0), b_kernel.at(0), chess_corners[2]);//右图像内角点一三象限 左下和右上白色

GetOneCorner(chess_img_R, s_kernel.at(1), b_kernel.at(1), chess_corners[3]);//右图像内角点二四象限   左上和右下白色   两者都是内角点

/*if((chess_corners_R.at(0).x <= 0) || (chess_corners_R.at(0).x > (chess_img_R.cols/2-1)) || (chess_corners_R.at(0).y <= 0) || (chess_corners_R.at(0).y > (chess_img_R.rows/2-1)))
    return 1;
if((chess_corners_R.at(1).x <= 0) || (chess_corners_R.at(1).x > (chess_img_R.cols/2-1)) || (chess_corners_R.at(1).y < (chess_img_R.rows/2-1)) || (chess_corners_R.at(1).y > (chess_img_R.rows-1)))
    return 2;*/
return 0;
}
bool GetOneCorner(cv::Mat chess_img_L, cv::Mat s_kernel, cv::Mat b_kernel, cv::Point2f &maxLoc)

{

int x,y;
float scale;
double maxVal1 = 0;//初始化
double maxVal2 = 0;//初始化
cv::Point maxLoc1;
cv::Point maxLoc2;
cv::Mat s_gray;
cv::Mat dst1;
cv::Mat dst2;
cv::Mat result1;

scale = float(chess_img_L.cols) / float(CORNER_SCALED_WIDTH); 
cv::resize(chess_img_L, s_gray, cv::Size(), 1.0f/scale, 1.0f/scale, CV_INTER_LINEAR);  //调整图片大小 
s_gray.convertTo(s_gray, CV_32FC1);                     //精度化操作，转换为32位浮点型单通道矩阵
cv::filter2D(s_gray, dst1, s_gray.depth(), s_kernel);   //模板卷积运算
cv::minMaxLoc(dst1, NULL, &maxVal1, NULL, &maxLoc1);    //找不需要返回值的最大，最小值元素  
x = int(maxLoc1.x * scale - CORNER_RECT_SIZE/2 +0.5);  
x = x < 0? 0 : x;
x = x > (chess_img_L.cols-(CORNER_RECT_SIZE+1))? (chess_img_L.cols-(CORNER_RECT_SIZE+1)): x;
y = int(maxLoc1.y * scale - CORNER_RECT_SIZE/2 + 0.5);//还原原来的纵坐标
y = y < 0? 0 : y;
y = y > (chess_img_L.rows-(CORNER_RECT_SIZE+1))? (chess_img_L.rows-(CORNER_RECT_SIZE+1)): y;    
result1 = chess_img_L(cv::Rect(x, y, CORNER_RECT_SIZE+1, CORNER_RECT_SIZE+1)).clone();  
result1.convertTo(result1, CV_32FC1);                   //转换为32位浮点型单通道矩阵
cv::filter2D(result1, dst2, result1.depth(), b_kernel); //第二次的模板运算
cv::minMaxLoc(dst2, NULL, &maxVal2, NULL, &maxLoc2);    //找不需要返回值的最大，最小值元素  
maxLoc.x = float(x+maxLoc2.x);//还原x坐标
maxLoc.y = float(y+maxLoc2.y);//还原y坐标
return true;
}