├── FindCorners.cpp
├── FindCorners.h
├── README.md
├── cornor_detect.png
└── main.cpp


/FindCorners.cpp:
--------------------------------------------------------------------------------
  1 | #include "stdafx.h"
  2 | #include "FindCorners.h"
  3 | 
  4 | FindCorners::FindCorners()
  5 | {}
  6 | FindCorners::~FindCorners()
  7 | {}
  8 | 
  9 | FindCorners::FindCorners(Mat img)
 10 | {
 11 | 	radius.push_back(4);
 12 | 	radius.push_back(8);
 13 | 	radius.push_back(12);
 14 | 	templateProps.push_back(Point2f((float)0, (float)CV_PI / 2));
 15 | 	templateProps.push_back(Point2f((float)CV_PI / 4, (float)-CV_PI / 4));
 16 | 	templateProps.push_back(Point2f((float)0, (float)CV_PI / 2));
 17 | 	templateProps.push_back(Point2f((float)CV_PI / 4, (float)-CV_PI / 4));
 18 | 	templateProps.push_back(Point2f((float)0, (float)CV_PI / 2));
 19 | 	templateProps.push_back(Point2f((float)CV_PI / 4, (float)-CV_PI / 4));
 20 | }
 21 | 
 22 | //正态分布
 23 | float FindCorners::normpdf(float dist, float mu, float sigma){
 24 | 	return exp(-0.5*(dist - mu)*(dist - mu) / (sigma*sigma)) / (std::sqrt(2 * CV_PI)*sigma);
 25 | }
 26 | 
 27 | //**************************生成核*****************************//
 28 | //angle代表核类型：45度核和90度核
 29 | //kernelSize代表核大小（最终生成的核的大小为kernelSize*2+1）
 30 | //kernelA...kernelD是生成的核
 31 | //*************************************************************************//
 32 | void FindCorners::createkernel(float angle1, float angle2, int kernelSize, Mat &kernelA, Mat &kernelB, Mat &kernelC, Mat &kernelD){
 33 | 
 34 | 	int width = (int)kernelSize * 2 + 1;
 35 | 	int height = (int)kernelSize * 2 + 1;
 36 | 	kernelA = cv::Mat::zeros(height, width, CV_32F);
 37 | 	kernelB = cv::Mat::zeros(height, width, CV_32F);
 38 | 	kernelC = cv::Mat::zeros(height, width, CV_32F);
 39 | 	kernelD = cv::Mat::zeros(height, width, CV_32F);
 40 | 
 41 | 	for (int u = 0; u<width; ++u){
 42 | 		for (int v = 0; v<height; ++v){
 43 | 			float vec[] = { u - kernelSize, v - kernelSize };//相当于将坐标原点移动到核中心
 44 | 			float dis = std::sqrt(vec[0] * vec[0] + vec[1] * vec[1]);//相当于计算到中心的距离
 45 | 			float side1 = vec[0] * (-sin(angle1)) + vec[1] * cos(angle1);//相当于将坐标原点移动后的核进行旋转，以此产生四种核
 46 | 			float side2 = vec[0] * (-sin(angle2)) + vec[1] * cos(angle2);//X=X0*cos+Y0*sin;Y=Y0*cos-X0*sin
 47 | 			if (side1 <= -0.1&&side2 <= -0.1){
 48 | 				kernelA.ptr<float>(v)[u] = normpdf(dis, 0, kernelSize / 2);
 49 | 			}
 50 | 			if (side1 >= 0.1&&side2 >= 0.1){
 51 | 				kernelB.ptr<float>(v)[u] = normpdf(dis, 0, kernelSize / 2);
 52 | 			}
 53 | 			if (side1 <= -0.1&&side2 >= 0.1){
 54 | 				kernelC.ptr<float>(v)[u] = normpdf(dis, 0, kernelSize / 2);
 55 | 			}
 56 | 			if (side1 >= 0.1&&side2 <= -0.1){
 57 | 				kernelD.ptr<float>(v)[u] = normpdf(dis, 0, kernelSize / 2);
 58 | 			}
 59 | 		}
 60 | 	}
 61 | 	//std::cout << "kernelA:" << kernelA << endl << "kernelB:" << kernelB << endl
 62 | 	//	<< "kernelC:" << kernelC<< endl << "kernelD:" << kernelD << endl;
 63 | 	//归一化
 64 | 	kernelA = kernelA / cv::sum(kernelA)[0];
 65 | 	kernelB = kernelB / cv::sum(kernelB)[0];
 66 | 	kernelC = kernelC / cv::sum(kernelC)[0];
 67 | 	kernelD = kernelD / cv::sum(kernelD)[0];
 68 | 
 69 | }
 70 | //**************************//获取最小值*****************************//
 71 | //*************************************************************************//
 72 | void FindCorners::getMin(Mat src1, Mat src2, Mat &dst){
 73 | 	//src1和src2的大小要一样
 74 | 	//if (src1.size() != src2.size())
 75 | 	//{
 76 | 	//	cout << "The size of matrix don't match" << endl;
 77 | 	//}
 78 | 	//dst = Mat::zeros(src1.size(), src1.type());
 79 | 	//for (int i = 0; i < src1.rows; i++)
 80 | 	//{
 81 | 	//	for (int j = 0; j < src1.cols; j++)
 82 | 	//	{
 83 | 	//		dst.ptr<float>(i)[j] = src1.ptr<float>(i)[j] <= src2.ptr<float>(i)[j] ? src1.ptr<float>(i)[j] : src2.ptr<float>(i)[j];
 84 | 	//	}
 85 | 	//}
 86 | 	int rowsLeft = src1.rows;
 87 | 	int colsLeft = src1.cols;
 88 | 	int rowsRight = src2.rows;
 89 | 	int colsRight = src2.cols;
 90 | 	if (rowsLeft != rowsRight || colsLeft != colsRight)return;
 91 | 
 92 | 	int channels = src1.channels();
 93 | 
 94 | 	int nr = rowsLeft;
 95 | 	int nc = colsLeft;
 96 | 	if (src1.isContinuous()){
 97 | 		nc = nc*nr;
 98 | 		nr = 1;
 99 | 		//std::cout<<"continue"<<std::endl;
100 | 	}
101 | 	for (int i = 0; i<nr; i++){
102 | 		const float* dataLeft = src1.ptr<float>(i);
103 | 		const float* dataRight = src2.ptr<float>(i);
104 | 		float* dataResult = dst.ptr<float>(i);
105 | 		for (int j = 0; j<nc*channels; ++j){
106 | 			dataResult[j] = (dataLeft[j]<dataRight[j]) ? dataLeft[j] : dataRight[j];
107 | 		}
108 | 	}
109 | }
110 | //**************************//获取最大值*****************************//
111 | //*************************************************************************//
112 | void FindCorners::getMax(Mat src1, Mat src2, Mat &dst){
113 | 	//src1和src2的大小要一样
114 | 	//if (src1.size() != src2.size())
115 | 	//{
116 | 	//	cout << "The size of matrix don't match" << endl;
117 | 	//}
118 | 	//dst = Mat::zeros(src1.size(), src1.type());
119 | 	//for (int i = 0; i < src1.cols; i++)
120 | 	//{
121 | 	//	const float* dataLeft = src1.ptr<float>(i);
122 | 	//	const float* dataRight = src2.ptr<float>(i);
123 | 	//	float* dataResult = dst.ptr<float>(i);
124 | 	//	for (int j = 0; j < src1.rows; j++)
125 | 	//	{
126 | 	//		dataResult[j] = (dataLeft[j] >= dataRight[j]) ? dataLeft[j] : dataRight[j];
127 | 	//	}
128 | 	//}
129 | 	//(没搞明白，只是换了种写法就不行了，就只能进行一次最大值的获取了。。)
130 | 	int rowsLeft = src1.rows;
131 | 	int colsLeft = src1.cols;
132 | 	int rowsRight = src2.rows;
133 | 	int colsRight = src2.cols;
134 | 	if (rowsLeft != rowsRight || colsLeft != colsRight)return;
135 | 
136 | 	int channels = src1.channels();
137 | 
138 | 	int nr = rowsLeft;
139 | 	int nc = colsLeft;
140 | 	if (src1.isContinuous()){
141 | 		nc = nc*nr;
142 | 		nr = 1;
143 | 		//std::cout<<"continue"<<std::endl;
144 | 	}
145 | 	for (int i = 0; i<nr; i++){
146 | 		const float* dataLeft = src1.ptr<float>(i);
147 | 		const float* dataRight = src2.ptr<float>(i);
148 | 		float* dataResult = dst.ptr<float>(i);
149 | 		for (int j = 0; j<nc*channels; ++j){
150 | 			dataResult[j] = (dataLeft[j] >= dataRight[j]) ? dataLeft[j] : dataRight[j];
151 | 		}
152 | 	}
153 | }
154 | //获取梯度角度和权重
155 | void FindCorners::getImageAngleAndWeight(Mat img, Mat &imgDu, Mat &imgDv, Mat &imgAngle, Mat &imgWeight){
156 | 	Mat sobelKernel(3, 3, CV_32F);
157 | 	Mat sobelKernelTrs(3, 3, CV_32F);
158 | 	//soble滤波器算子核
159 | 	sobelKernel.col(0).setTo(cv::Scalar(-1));
160 | 	sobelKernel.col(1).setTo(cv::Scalar(0));
161 | 	sobelKernel.col(2).setTo(cv::Scalar(1));
162 | 
163 | 	sobelKernelTrs = sobelKernel.t();
164 | 
165 | 	filter2D(img, imgDu, CV_32F, sobelKernel);
166 | 	filter2D(img, imgDv, CV_32F, sobelKernelTrs);
167 | 
168 | 	if (imgDu.size() != imgDv.size())return;
169 | 
170 | 	for (int i = 0; i < imgDu.rows; i++)
171 | 	{
172 | 		float* dataDv = imgDv.ptr<float>(i);
173 | 		float* dataDu = imgDu.ptr<float>(i);
174 | 		float* dataAngle = imgAngle.ptr<float>(i);
175 | 		float* dataWeight = imgWeight.ptr<float>(i);
176 | 		for (int j = 0; j < imgDu.cols; j++)
177 | 		{
178 | 			if (dataDu[j]>0.000001)
179 | 			{
180 | 				dataAngle[j] = atan2((float)dataDv[j], (float)dataDu[j]);
181 | 				if (dataAngle[j] < 0)dataAngle[j] = dataAngle[j] + CV_PI;
182 | 				else if (dataAngle[j] > CV_PI)dataAngle[j] = dataAngle[j] - CV_PI;
183 | 			}
184 | 			dataWeight[j] = std::sqrt((float)dataDv[j] * (float)dataDv[j] + (float)dataDu[j] * (float)dataDu[j]);
185 | 		}
186 | 	}
187 | }
188 | //**************************非极大值抑制*****************************//
189 | //inputCorners是输入角点，outputCorners是非极大值抑制后的角点
190 | //threshold是设定的阈值
191 | //margin是进行非极大值抑制时检查方块与输入矩阵边界的距离，patchSize是该方块的大小
192 | //*************************************************************************//
193 | void FindCorners::nonMaximumSuppression(Mat& inputCorners, vector<Point>& outputCorners, float threshold, int margin, int patchSize)
194 | {
195 | 	if (inputCorners.size <= 0)
196 | 	{
197 | 		cout << "The imput mat is empty!" << endl; return;
198 | 	}
199 | 	for (int i = margin + patchSize; i < inputCorners.cols - (margin + patchSize); i = i + patchSize + 1)//移动检查方块，每次移动一个方块的大小
200 | 	{
201 | 		for (int j = margin + patchSize; j < inputCorners.rows - (margin + patchSize); j = j + patchSize + 1)
202 | 		{
203 | 			float maxVal = inputCorners.ptr<float>(j)[i];
204 | 			int maxX = i; int maxY = j;
205 | 			for (int m = i; m < i + patchSize +1; m++)//找出该检查方块中的局部最大值
206 | 			{
207 | 				for (int n = j; n < j + patchSize +1; n++)
208 | 				{
209 | 					float temp = inputCorners.ptr<float>(n)[m];
210 | 					if (temp>maxVal)
211 | 					{
212 | 						maxVal = temp; maxX = m; maxY = n;
213 | 					}
214 | 				}
215 | 			}
216 | 			if (maxVal < threshold)continue;//若该局部最大值小于阈值则不满足要求
217 | 			int flag = 0;
218 | 			for (int m = maxX - patchSize; m < min(maxX + patchSize, inputCorners.cols-margin); m++)//二次检查
219 | 			{
220 | 				for (int n = maxY - patchSize; n < min(maxY + patchSize, inputCorners.rows - margin); n++)
221 | 				{
222 | 					if (inputCorners.ptr<float>(n)[m]>maxVal && (m<i || m>i + patchSize || n<j || n>j + patchSize))
223 | 					{
224 | 						flag = 1; break;
225 | 					}
226 | 				}
227 | 				if (flag)break;
228 | 			}
229 | 			if (flag)continue;
230 | 			outputCorners.push_back(Point(maxX, maxY));
231 | 			std::vector<float> e1(2, 0.0);
232 | 			std::vector<float> e2(2, 0.0);
233 | 			cornersEdge1.push_back(e1);
234 | 			cornersEdge2.push_back(e2);
235 | 		}
236 | 	}
237 | }
238 | //find modes of smoothed histogram
239 | void FindCorners::findModesMeanShift(vector<float> hist, vector<float> &hist_smoothed, vector<pair<float, int>> &modes, float sigma){
240 | 	//efficient mean - shift approximation by histogram smoothing
241 | 	//compute smoothed histogram
242 | 	bool allZeros = true;
243 | 	for (int i = 0; i < hist.size(); i++)
244 | 	{
245 | 		float sum = 0;
246 | 		for (int j = -(int)round(2 * sigma); j <= (int)round(2 * sigma); j++)
247 | 		{
248 | 			int idx = 0;
249 | 			if ((i + j) < 0)idx = i + j + hist.size();
250 | 			else if ((i + j) >= 32)idx = i + j - hist.size();
251 | 			else idx = (i + j);
252 | 			sum = sum + hist[idx] * normpdf(j, 0, sigma);
253 | 		}
254 | 		hist_smoothed[i]=sum;
255 | 		if (abs(hist_smoothed[i] - hist_smoothed[0])>0.0001)allZeros = false;// check if at least one entry is non - zero
256 | 																			//(otherwise mode finding may run infinitly)
257 | 	}
258 | 	if (allZeros)return;
259 | 
260 | 	//mode finding
261 | 	//for (int i = 0; i < hist.size(); i++)
262 | 	//{
263 | 	//	int j = i;
264 | 	//	while (true)
265 | 	//	{
266 | 	//		float h0 = hist_smoothed[j];
267 | 	//		int j1 = (j - 1)<0 ? j - 1 + hist.size() : j - 1;
268 | 	//		j1 = j>hist.size() ? j - 1 - hist.size() : j - 1;
269 | 	//		int j2 = (j + 1)>hist.size() - 1 ? j + 1 - hist.size() : j + 1;
270 | 	//		j2 = (j + 1)<0 ? j + 1 + hist.size() : j + 1;
271 | 	//		float h1 = hist_smoothed[j1];
272 | 	//		float h2 = hist_smoothed[j2];
273 | 	//		if (h1 >= h0&&h1 >= h2)j = j1;
274 | 	//		else if (h2 >= h0&&h2 >= h1)j = j2;
275 | 	//		else break;
276 | 	//	}
277 | 	//	if (modes.size() == 0 || modes[i].x!=(float)j)
278 | 	//	{
279 | 
280 | 	//	}
281 | 	//}
282 | 	for (int i = 0; i<hist.size(); ++i){
283 | 		int j = i;
284 | 		int curLeft = (j - 1)<0 ? j - 1 + hist.size() : j - 1;
285 | 		int curRight = (j + 1)>hist.size() - 1 ? j + 1 - hist.size() : j + 1;
286 | 		if (hist_smoothed[curLeft]<hist_smoothed[i] && hist_smoothed[curRight]<hist_smoothed[i]){
287 | 			modes.push_back(std::make_pair(hist_smoothed[i], i));
288 | 		}
289 | 	}
290 | 	std::sort(modes.begin(), modes.end());
291 | }
292 | //estimate edge orientations
293 | void FindCorners::edgeOrientations(Mat imgAngle, Mat imgWeight, int index){
294 | 	//number of bins (histogram parameter)
295 | 	int binNum = 32;
296 | 
297 | 	//convert images to vectors
298 | 	if (imgAngle.size() != imgWeight.size())return;
299 | 	vector<float> vec_angle, vec_weight;
300 | 	for (int i = 0; i < imgAngle.cols; i++)
301 | 	{
302 | 		for (int j = 0; j < imgAngle.rows; j++)
303 | 		{
304 | 			// convert angles from normals to directions
305 | 			float angle = imgAngle.ptr<float>(j)[i] + CV_PI / 2;
306 | 			angle = angle>CV_PI ? (angle - CV_PI) : angle;
307 | 			vec_angle.push_back(angle);
308 | 
309 | 			vec_weight.push_back(imgWeight.ptr<float>(j)[i]);
310 | 		}
311 | 	}
312 | 
313 | 	//create histogram
314 | 	vector<float> angleHist(binNum, 0);
315 | 	for (int i = 0; i < vec_angle.size(); i++)
316 | 	{
317 | 		int bin = max(min((int)floor(vec_angle[i] / (CV_PI / binNum)), binNum - 1), 0);
318 | 		angleHist[bin] = angleHist[bin] + vec_weight[i];
319 | 	}
320 | 
321 | 	// find modes of smoothed histogram
322 | 	vector<float> hist_smoothed(angleHist);
323 | 	vector<std::pair<float, int> > modes;
324 | 	findModesMeanShift(angleHist, hist_smoothed, modes,1);
325 | 
326 | 	// if only one or no mode = > return invalid corner
327 | 	if (modes.size() <= 1)return;
328 | 
329 | 	//extract 2 strongest modes and compute orientation at modes
330 | 	std::pair<float, int> most1 = modes[modes.size() - 1];
331 | 	std::pair<float, int> most2 = modes[modes.size() - 2];
332 | 	float most1Angle = most1.second*CV_PI / binNum;
333 | 	float most2Angle = most2.second*CV_PI / binNum;
334 | 	float tmp = most1Angle;
335 | 	most1Angle = (most1Angle>most2Angle) ? most1Angle : most2Angle;
336 | 	most2Angle = (tmp>most2Angle) ? most2Angle : tmp;
337 | 
338 | 	// compute angle between modes
339 | 	float deltaAngle = min(most1Angle - most2Angle, most2Angle + (float)CV_PI - most1Angle);
340 | 
341 | 	// if angle too small => return invalid corner
342 | 	if (deltaAngle <= 0.3)return;
343 | 
344 | 	//set statistics: orientations
345 | 	cornersEdge1[index][0] = cos(most1Angle);
346 | 	cornersEdge1[index][1] = sin(most1Angle);
347 | 	cornersEdge2[index][0] = cos(most2Angle);
348 | 	cornersEdge2[index][1] = sin(most2Angle);
349 | }
350 | //亚像素精度找角点
351 | void FindCorners::refineCorners(vector<Point> &cornors, Mat imgDu, Mat imgDv, Mat imgAngle, Mat imgWeight, float radius){
352 | 	// image dimensions
353 | 	int width = imgDu.cols;
354 | 	int height = imgDu.rows;
355 | 	// for all corners do
356 | 	for (int i = 0; i < cornors.size(); i++)
357 | 	{
358 | 		//extract current corner location
359 | 		int cu = cornors[i].x;
360 | 		int cv = cornors[i].y;
361 | 		// estimate edge orientations
362 | 		int startX, startY, ROIwidth, ROIheight;
363 | 		startX = max(cu - radius, (float)0);
364 | 		startY = max(cv - radius, (float)0);
365 | 		ROIwidth = min(cu + radius, (float)width-1) - startX ;
366 | 		ROIheight = min(cv + radius, (float)height-1) - startY ;
367 | 
368 | 		Mat roiAngle, roiWeight;
369 | 		roiAngle = imgAngle(Rect(startX, startY, ROIwidth, ROIheight));
370 | 		roiWeight = imgWeight(Rect(startX, startY, ROIwidth, ROIheight));
371 | 		edgeOrientations(roiAngle, roiWeight,i);
372 | 
373 | 		// continue, if invalid edge orientations
374 | 		if (cornersEdge1[i][0] == 0 && cornersEdge1[i][1] == 0 || cornersEdge2[i][0] == 0 && cornersEdge2[i][1] == 0)continue;
375 | 	}
376 | }
377 | //compute corner statistics
378 | void FindCorners::cornerCorrelationScore(Mat img, Mat imgWeight, vector<Point2f> cornersEdge, float &score){
379 | 	//center
380 | 	int c[] = { imgWeight.cols / 2, imgWeight.cols / 2 };
381 | 
382 | 	//compute gradient filter kernel(bandwith = 3 px)
383 | 	Mat img_filter = Mat::ones(imgWeight.size(), imgWeight.type());
384 | 	img_filter = img_filter*-1;
385 | 	for (int i = 0; i < imgWeight.cols; i++)
386 | 	{
387 | 		for (int j = 0; j < imgWeight.rows; j++)
388 | 		{
389 | 			Point2f p1 = Point2f(i - c[0], j - c[1]);
390 | 			Point2f p2 = Point2f(p1.x*cornersEdge[0].x*cornersEdge[0].x + p1.y*cornersEdge[0].x*cornersEdge[0].y,
391 | 				p1.x*cornersEdge[0].x*cornersEdge[0].y + p1.y*cornersEdge[0].y*cornersEdge[0].y);
392 | 			Point2f p3 = Point2f(p1.x*cornersEdge[1].x*cornersEdge[1].x + p1.y*cornersEdge[1].x*cornersEdge[1].y,
393 | 				p1.x*cornersEdge[1].x*cornersEdge[1].y + p1.y*cornersEdge[1].y*cornersEdge[1].y);
394 | 			float norm1 = sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y));
395 | 			float norm2 = sqrt((p1.x - p3.x)*(p1.x - p3.x) + (p1.y - p3.y)*(p1.y - p3.y));
396 | 			if (norm1 <= 1.5 || norm2 <= 1.5)
397 | 			{
398 | 				img_filter.ptr<float>(j)[i] = 1;
399 | 			}
400 | 		}
401 | 	}
402 | 
403 | 	//normalize
404 | 	Mat mean, std, mean1, std1;
405 | 	meanStdDev(imgWeight, mean, std);
406 | 	meanStdDev(img_filter, mean1, std1);
407 | 	for (int i = 0; i < imgWeight.cols; i++)
408 | 	{
409 | 		for (int j = 0; j < imgWeight.rows; j++)
410 | 		{
411 | 			imgWeight.ptr<float>(j)[i] = (float)(imgWeight.ptr<float>(j)[i] - mean.ptr<double>(0)[0]) / (float)std.ptr<double>(0)[0];
412 | 			img_filter.ptr<float>(j)[i] = (float)(img_filter.ptr<float>(j)[i] - mean1.ptr<double>(0)[0]) / (float)std1.ptr<double>(0)[0];
413 | 		}
414 | 	}
415 | 
416 | 	//convert into vectors
417 | 	vector<float> vec_filter, vec_weight;
418 | 	for (int i = 0; i < imgWeight.cols; i++)
419 | 	{
420 | 		for (int j = 0; j < imgWeight.rows; j++)
421 | 		{
422 | 			vec_filter.push_back(img_filter.ptr<float>(j)[i]);
423 | 			vec_weight.push_back(imgWeight.ptr<float>(j)[i]);
424 | 		}
425 | 	}
426 | 
427 | 	//compute gradient score
428 | 	float sum = 0;
429 | 	for (int i = 0; i < vec_weight.size(); i++)
430 | 	{
431 | 		sum += vec_weight[i] * vec_filter[i];
432 | 	}
433 | 	sum = (float)sum / (float)(vec_weight.size() - 1);
434 | 	float score_gradient = sum >= 0 ? sum : 0;
435 | 
436 | 	//create intensity filter kernel
437 | 	Mat kernelA, kernelB, kernelC, kernelD;
438 | 	createkernel(atan2(cornersEdge[0].y, cornersEdge[0].x), atan2(cornersEdge[1].y, cornersEdge[1].x), c[0], kernelA, kernelB, kernelC, kernelD);//1.1 产生四种核
439 | 
440 | 	//checkerboard responses
441 | 	float a1, a2,b1,b2;
442 | 	a1 = kernelA.dot(img);
443 | 	a2 = kernelB.dot(img);
444 | 	b1 = kernelC.dot(img);
445 | 	b2 = kernelD.dot(img);
446 | 
447 | 	float mu = (a1 + a2 + b1 + b2) / 4;
448 | 
449 | 	float score_a = (a1 - mu) >= (a2 - mu) ? (a2 - mu) : (a1 - mu);
450 | 	float score_b = (mu - b1) >= (mu - b2) ? (mu - b2) : (mu - b1);
451 | 	float score_1 = score_a >= score_b ? score_b : score_a;
452 | 
453 | 	score_b = (b1 - mu) >= (b2 - mu) ? (b2 - mu) : (b1 - mu);
454 | 	score_a = (mu - a1) >= (mu - a2) ? (mu - a2) : (mu - a1);
455 | 	float score_2 = score_a >= score_b ? score_b : score_a;
456 | 
457 | 	float score_intensity = score_1 >= score_2 ? score_1 : score_2;
458 | 
459 | 	score = score_gradient*score_intensity;
460 | }
461 | //score corners
462 | void FindCorners::scoreCorners(Mat img, Mat imgAngle, Mat imgWeight, vector<Point> &cornors, vector<int> radius, vector<float> &score){
463 | 	//for all corners do
464 | 	for (int i = 0; i < cornors.size(); i++)
465 | 	{
466 | 		//corner location
467 | 		int u = cornors[i].x;
468 | 		int v = cornors[i].y;
469 | 		if (i == 278)
470 | 		{
471 | 			int aaa = 0;
472 | 		}
473 | 		//compute corner statistics @ radius 1
474 | 		vector<float> scores;
475 | 		for (int j = 0; j < radius.size(); j++)
476 | 		{
477 | 			scores.push_back(0);
478 | 			int r = radius[j];
479 | 			if (u > r&&u <= (img.cols - r - 1) && v>r && v <= (img.rows - r -1))
480 | 			{
481 | 				int startX, startY, ROIwidth, ROIheight;
482 | 				startX = u-r;
483 | 				startY = v-r;
484 | 				ROIwidth = 2 * r + 1;
485 | 				ROIheight = 2 * r + 1;
486 | 
487 | 				Mat sub_img = img(Rect(startX, startY, ROIwidth, ROIheight));
488 | 				Mat sub_imgWeight = imgWeight(Rect(startX, startY, ROIwidth, ROIheight));
489 | 				vector<Point2f> cornersEdge;
490 | 				cornersEdge.push_back(Point2f((float)cornersEdge1[i][0], (float)cornersEdge1[i][1]));
491 | 				cornersEdge.push_back(Point2f((float)cornersEdge2[i][0], (float)cornersEdge2[i][1]));
492 | 				cornerCorrelationScore(sub_img, sub_imgWeight, cornersEdge, scores[j]);
493 | 			}
494 | 		}
495 | 		//take highest score
496 | 		score.push_back(*max_element(begin(scores), end(scores)));
497 | 	}
498 | 	
499 | }
500 | void FindCorners::detectCorners(Mat &Src, vector<Point> &resultCornors, float scoreThreshold){
501 | 	Mat gray, imageNorm;
502 | 	gray = Mat(Src.size(), CV_8U);
503 | 	if (Src.channels()==3)
504 | 	{
505 | 		cvtColor(Src, gray, COLOR_BGR2GRAY);//变为灰度图
506 | 	}
507 | 	else gray = Src.clone();
508 | 	
509 | 	normalize(gray, imageNorm, 0, 1, cv::NORM_MINMAX, CV_32F);//对灰度图进行归一化
510 | 
511 | 	Mat imgCorners = Mat::zeros(imageNorm.size(), CV_32F);//卷积核得出的点
512 | 	for (int i = 0; i < 6; i++)
513 | 	{
514 | 		//按照论文步骤，第一步：用卷积核进行卷积的方式找出可能是棋盘格角点的点
515 | 		Mat kernelA1, kernelB1, kernelC1, kernelD1;
516 | 		createkernel(templateProps[i].x, templateProps[i].y, radius[i / 2], kernelA1, kernelB1, kernelC1, kernelD1);//1.1 产生四种核
517 | 		std::cout << "kernelA:" << kernelA1 << endl << "kernelB:" << kernelB1 << endl
518 | 			<< "kernelC:" << kernelC1 << endl << "kernelD:" << kernelD1 << endl;
519 | 
520 | 		Mat imgCornerA1(imageNorm.size(), CV_32F);
521 | 		Mat imgCornerB1(imageNorm.size(), CV_32F);
522 | 		Mat imgCornerC1(imageNorm.size(), CV_32F);
523 | 		Mat imgCornerD1(imageNorm.size(), CV_32F);
524 | 		filter2D(imageNorm, imgCornerA1, CV_32F, kernelA1);//1.2 用所产生的核对图像做卷积
525 | 		filter2D(imageNorm, imgCornerB1, CV_32F, kernelB1);
526 | 		filter2D(imageNorm, imgCornerC1, CV_32F, kernelC1);
527 | 		filter2D(imageNorm, imgCornerD1, CV_32F, kernelD1);
528 | 
529 | 		Mat imgCornerMean(imageNorm.size(), CV_32F);
530 | 		imgCornerMean = (imgCornerA1 + imgCornerB1 + imgCornerC1 + imgCornerD1) / 4;//1.3 按照公式进行计算
531 | 		Mat imgCornerA(imageNorm.size(), CV_32F);
532 | 		Mat imgCornerB(imageNorm.size(), CV_32F);
533 | 		Mat imgCorner1(imageNorm.size(), CV_32F);
534 | 		Mat imgCorner2(imageNorm.size(), CV_32F);
535 | 
536 | 		getMin(imgCornerA1 - imgCornerMean, imgCornerB1 - imgCornerMean, imgCornerA);
537 | 		getMin(imgCornerMean - imgCornerC1, imgCornerMean - imgCornerD1, imgCornerB);
538 | 		getMin(imgCornerA, imgCornerB, imgCorner1);
539 | 
540 | 		getMin(imgCornerMean - imgCornerA1, imgCornerMean - imgCornerB1, imgCornerA);
541 | 		getMin(imgCornerC1 - imgCornerMean, imgCornerD1 - imgCornerMean, imgCornerB);
542 | 		getMin(imgCornerA, imgCornerB, imgCorner2);
543 | 
544 | 		getMax(imgCorners, imgCorner1, imgCorners);
545 | 		getMax(imgCorners, imgCorner2, imgCorners);
546 | 
547 | 		//getMin(imgCornerA1, imgCornerB1, imgCornerA); getMin(imgCornerC1, imgCornerD1, imgCornerB);
548 | 		//getMin(imgCornerA - imgCornerMean, imgCornerMean - imgCornerB, imgCorner1);
549 | 		//getMin(imgCornerMean - imgCornerA, imgCornerB - imgCornerMean, imgCorner2);
550 | 		//getMax(imgCorners, imgCorner2, imgCorners);//1.4 获取每个像素点的得分
551 | 		//getMax(imgCorners, imgCorner1, imgCorners);//1.4 获取每个像素点的得分
552 | 	}
553 | 
554 | 	namedWindow("ROI");//创建窗口，显示原始图像
555 | 	imshow("ROI", imgCorners); waitKey(0);
556 | 
557 | 	nonMaximumSuppression(imgCorners, cornerPoints, 0.01, 5, 3);//1.5 非极大值抑制算法进行过滤，获取棋盘格角点初步结果
558 | 
559 | 	if (cornerPoints.size()>0)
560 | 	{
561 | 		for (int i = 0; i < cornerPoints.size(); i++)
562 | 		{
563 | 			circle(Src, cornerPoints[i], 5, CV_RGB(255, 0, 0), 2);
564 | 		}
565 | 	}
566 | 	namedWindow("src");//创建窗口，显示原始图像
567 | 	imshow("src", Src); waitKey(0);
568 | 
569 | 	//算两个方向的梯度
570 | 	Mat imageDu(gray.size(), CV_32F);
571 | 	Mat imageDv(gray.size(), CV_32F);
572 | 	Mat img_angle(gray.size(), CV_32F);
573 | 	Mat img_weight(gray.size(), CV_32F);
574 | 	//获取梯度角度和权重
575 | 	getImageAngleAndWeight(gray, imageDu, imageDv, img_angle, img_weight);
576 | 	//subpixel refinement
577 | 	refineCorners(cornerPoints, imageDu, imageDv, img_angle, img_weight, 10);
578 | 	if (cornerPoints.size()>0)
579 | 	{
580 | 		for (int i = 0; i < cornerPoints.size(); i++)
581 | 		{
582 | 			if (cornersEdge1[i][0] == 0 && cornersEdge1[i][0] == 0)
583 | 			{
584 | 				cornerPoints[i].x = 0; cornerPoints[i].y = 0;
585 | 			}
586 | 
587 | 		}
588 | 	}
589 | 	//remove corners without edges
590 | 
591 | 	//score corners
592 | 	vector<float> score;
593 | 	scoreCorners(imageNorm, img_angle, img_weight, cornerPoints, radius, score);
594 | 
595 | 	if (cornerPoints.size()>0)
596 | 	{
597 | 		for (int i = 0; i < cornerPoints.size(); i++)
598 | 		{
599 | 			if (score[i]>scoreThreshold)
600 | 			{
601 | 				circle(Src, cornerPoints[i], 5, CV_RGB(255, 0, 0), 2);
602 | 			}
603 | 			
604 | 		}
605 | 	}
606 | 	namedWindow("src");//创建窗口，显示原始图像
607 | 	imshow("src", Src); waitKey(0);
608 | 
609 | 	Point maxLoc;
610 | 	FileStorage fs2("test.xml", FileStorage::WRITE);//写XML文件
611 | 	fs2 << "img_corners_a1" << cornerPoints;
612 | }
613 | 


--------------------------------------------------------------------------------
/FindCorners.h:
--------------------------------------------------------------------------------
 1 | #ifndef DETECTOR_H
 2 | #define DETECTOR_H
 3 | 
 4 | #include "stdafx.h"
 5 | #include "opencv2/imgproc.hpp"
 6 | #include "opencv2/highgui.hpp"
 7 | #include <iostream>
 8 | #include <vector>
 9 | 
10 | using namespace cv;
11 | using namespace std;
12 | using std::vector;
13 | 
14 | class FindCorners
15 | {
16 | public:
17 | 	FindCorners();
18 | 	FindCorners(Mat img);
19 | 
20 | 	~FindCorners();
21 | 
22 | public:
23 | 	void detectCorners(Mat &Src, vector<Point> &resultCornors,float scoreThreshold);
24 | 
25 | private:
26 | 	//正态分布
27 | 	float normpdf(float dist, float mu, float sigma);
28 | 	//获取最小值
29 | 	void getMin(Mat src1, Mat src2, Mat &dst);
30 | 	//获取最大值
31 | 	void getMax(Mat src1, Mat src2, Mat &dst);
32 | 	//获取梯度角度和权重
33 | 	void getImageAngleAndWeight(Mat img, Mat &imgDu, Mat &imgDv, Mat &imgAngle, Mat &imgWeight);
34 | 	//estimate edge orientations
35 | 	void edgeOrientations(Mat imgAngle, Mat imgWeight,int index);
36 | 	//find modes of smoothed histogram
37 | 	void findModesMeanShift(vector<float> hist, vector<float> &hist_smoothed, vector<pair<float, int>> &modes, float sigma);
38 | 	//score corners
39 | 	void scoreCorners(Mat img, Mat imgAngle, Mat imgWeight, vector<Point> &cornors, vector<int> radius, vector<float> &score);
40 | 	//compute corner statistics
41 | 	void cornerCorrelationScore(Mat img, Mat imgWeight, vector<Point2f> cornersEdge, float &score);
42 | 	//亚像素精度找角点
43 | 	void refineCorners(vector<Point> &cornors, Mat imgDu, Mat imgDv, Mat imgAngle, Mat imgWeight, float radius);
44 | 	//生成核
45 | 	void createkernel(float angle1, float angle2, int kernelSize, Mat &kernelA, Mat &kernelB, Mat &kernelC, Mat &kernelD);
46 | 	//非极大值抑制
47 | 	void nonMaximumSuppression(Mat& inputCorners, vector<Point>& outputCorners, float threshold, int margin, int patchSize);
48 | 
49 | private:
50 | 	vector<Point2f> templateProps;
51 | 	vector<int> radius;
52 | 	vector<Point> cornerPoints;
53 | 	std::vector<std::vector<float> > cornersEdge1;
54 | 	std::vector<std::vector<float> > cornersEdge2;
55 | 	std::vector<cv::Point* > cornerPointsRefined;
56 | 
57 | };
58 | 
59 | #endif // DETECTOR_H
60 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # CornerDetect
2 | Realization of "Automatic Camera and Range Sensor Calibration using a single Shot" by C++;
3 | This is a key work to detect checkerboard corners and plays a role in camera calibration.
4 | ![image](https://github.com/qibao77/cornerDetect/blob/master/cornor_detect.png)
5 | # Reference
6 | Geiger A, Moosmann F, Car Ö, et al. Automatic camera and range sensor calibration using a single shot[C]//Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012: 3936-3943.
7 | 


--------------------------------------------------------------------------------
/cornor_detect.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/qibao77/cornerDetect/decc0c65284a83365a72776dc18ee4823c637a7d/cornor_detect.png


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
 1 | // CalibrationADAS.cpp : 定义控制台应用程序的入口点。
 2 | //
 3 | 
 4 | #include "stdafx.h"
 5 | #include "opencv2/imgproc.hpp"
 6 | #include "opencv2/highgui.hpp"
 7 | #include <algorithm>
 8 | #include "FindCorners.h "
 9 | #include <stdio.h>
10 | #include <iostream>
11 | #include <time.h>
12 | using namespace cv;
13 | using namespace std;
14 | using std::vector;
15 | vector<Point2i> points;
16 | 
17 | int _tmain(int argc, _TCHAR* argv[])
18 | {
19 | 	//Mat kernels;
20 | 	//FileStorage fs2("templateA1.xml", FileStorage::READ);//读XML文件
21 | 	//fs2["templateA1"] >> kernels;
22 | 	//cout << "kernels: " << kernels << endl;
23 | 
24 | 	//读入原始图像
25 | 	Mat src; //输入图像
26 | 	cout << "This is a demo for Parking slot detection." << endl;
27 | 	cout << "开始读入图像..." << endl;
28 | 	string filename = "Img\\02.png";//图像路径位置 "Img\\birdView0015.png"   calib\\_70.png
29 | 	src = imread(filename, -1);//载入测试图像
30 | 	if (src.empty())//不能读取图像
31 | 	{
32 | 		printf("Cannot read image file: %s\n", filename.c_str());
33 | 		return -1;
34 | 	}
35 | 	namedWindow("SrcImg");//创建窗口，显示原始图像
36 | 	imshow("SrcImg", src);
37 | 
38 | 	vector<Point> corners;//存储找到的角点
39 | 	FindCorners corner_detector(src);
40 | 	corner_detector.detectCorners(src, corners,0.025);
41 | 	return 0;
42 | }
43 | 
44 | 


--------------------------------------------------------------------------------