基于多项式插值亚像素的实现

void SubPixelEdge(Mat & imgsrc, Mat & edge, vector<Point2f> & vPts, double &x_c, double &y_c, int thres, int parts)
{
	static Mat kernels[KERNEL_SUM];

	if (kernels[0].empty())
	{
		int k = 0;
		kernels[k++] = (Mat_<float>(3, 3) << 1, 2, 1, 0, 0, 0, -1, -2, -1);	// 270°
		kernels[k++] = (Mat_<float>(3, 3) << 2, 1, 0, 1, 0, -1, 0, -1, -2);	// 315°
		kernels[k++] = (Mat_<float>(3, 3) << 1, 0, -1, 2, 0, -2, 1, 0, -1);	// 0°
		kernels[k++] = (Mat_<float>(3, 3) << 0, -1, -2, 1, 0, -1, 2, 1, 0);	// 45°

		flip(kernels[0], kernels[k++], 0);											// 90°

		kernels[k++] = (Mat_<float>(3, 3) << -2, -1, 0, -1, 0, 1, 0, 1, 2);	// 135°

		flip(kernels[2], kernels[k++], 1);											// 180°

		kernels[k++] = (Mat_<float>(3, 3) << 0, 1, 2, -1, 0, 1, -2, -1, 0);	// 225°
	}

	// 梯度图像
	Mat gradients[KERNEL_SUM];

	EDGE_PARAM edge_param;
	edge_param.filter_count = 0;
	edge_param.thres = thres;

	for (int i = 0; i < HERNEL_HALF; i++)
	{
		std::thread f([](Mat * src, Mat * grad, Mat * ker, EDGE_PARAM * param)
		{
			filter2D(*src, *grad, CV_16S, *ker);
			*(grad + HERNEL_HALF) = -(*grad);

			param->mtx.lock();
			param->filter_count++;
			param->mtx.unlock();

		}, &imgsrc, &gradients[i], &kernels[i], &edge_param);

		f.detach();
	}

	while (edge_param.filter_count < HERNEL_HALF)
	{
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}

	// 幅值和角度矩阵合并成一个矩阵
	// 新创建的图像总是连续的, 所以可以按行来操作提高效率
	Mat amp_ang(imgsrc.rows, imgsrc.cols, CV_16SC2, Scalar::all(0));

	edge_param.filter_count = 0;
	edge_param.parts = parts;

	assert(parts >= 1 && parts < (amp_ang.rows >> 1));

	for (int i = 0; i < parts; i++)
	{
		std::thread f([](Mat * amp_ang, Mat * grad, int cur_part, EDGE_PARAM * param)
		{
			const int length = amp_ang->rows * amp_ang->cols;
			const int step = length / param->parts;

			const int start = cur_part * step;
			int end = start + step;

			if (cur_part >= param->parts - 1)
			{
				end = length;
			}

			short *amp_ang_ptr = (short *)amp_ang->data + (start << 1);
			short *grad_ptr[KERNEL_SUM] = { nullptr };

			for (int k = 0; k < KERNEL_SUM; k++)
			{
				grad_ptr[k] = (short *)grad[k].data + start;
			}

			for (int j = start; j < end; j++)
			{
				// 找出最大值来判断方向
				for (int k = 0; k < KERNEL_SUM; k++)
				{
					if (*amp_ang_ptr < *grad_ptr[k])
					{
						*amp_ang_ptr = *grad_ptr[k];
						*(amp_ang_ptr + 1) = k;
					}

					grad_ptr[k]++;
				}

				amp_ang_ptr += 2;
			}

			param->mtx.lock();
			param->filter_count++;
			param->mtx.unlock();

		}, &amp_ang, gradients, i, &edge_param);

		f.detach();
	}

	while (edge_param.filter_count < parts)
	{
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}

	// 	vector<Mat> slt;
	// 	split(amp_ang, slt);
	// 
	// 	CSmartImage show;
	// 	normalize(slt[0], show, 0, 255, CV_MINMAX);
	// 	show.convertTo(show, CV_8UC1);
	// 	show.Show();

	edge_param.filter_count = 0;
	edge_param.thres = thres;

	edge = Mat::zeros(amp_ang.rows, amp_ang.cols, CV_8UC1);
	vector<vector<Point2f>> vvtmp(parts);

	for (int i = 0; i < parts; i++)
	{
		std::thread f([](Mat * amp_ang, Mat * edge, vector<Point2f> * v, int cur_part, EDGE_PARAM * param)
		{
			static const float root2 = (float)sqrt(2.0);
			static const float a2r = (float)(CV_PI / 180.0);
			static const short angle_list[] = { 270, 315, 0, 45, 90, 135, 180, 225 };

			// 三角函数表
			float tri_list[2][KERNEL_SUM] = { 0 };
			float tri_list_root2[2][KERNEL_SUM] = { 0 };

			for (int j = 0; j < KERNEL_SUM; j++)
			{
				tri_list[0][j] = (float)(0.5f * cos(angle_list[j] * a2r));

				// 0.5前面的负号非常关键, 因为图像的y方向和直角坐标系的y方向相反
				tri_list[1][j] = (float)(-0.5f * sin(angle_list[j] * a2r));

				tri_list_root2[0][j] = tri_list[0][j] * root2;
				tri_list_root2[1][j] = tri_list[1][j] * root2;
			}

			const int thres = param->thres;

			const int end_x = amp_ang->cols - 1;
			const int rows = amp_ang->rows / param->parts;

			int start_y = rows * cur_part;
			int end_y = start_y + rows;

			if (cur_part)
			{
				start_y -= 2;
			}

			if (cur_part >= param->parts - 1)
			{
				end_y = amp_ang->rows;
			}

			v->reserve((end_y - start_y) * amp_ang->cols);

			start_y++;
			end_y--;

			for (int r = start_y; r < end_y; r++)
			{
				// 3 * 3 邻域, 所以用3个指针, 一个指针指一行
				const short *pAmpang1 = amp_ang->ptr<short>(r - 1);
				const short *pAmpang2 = amp_ang->ptr<short>(r);
				const short *pAmpang3 = amp_ang->ptr<short>(r + 1);

				BYTE *pEdge = edge->ptr<BYTE>(r);

				for (int c = 1; c < end_x; c++)
				{
					const int j = c << 1;

					if (pAmpang2[j] >= thres)
					{
						switch (pAmpang2[j + 1])
						{
						case 0:
							if (pAmpang2[j] > pAmpang1[j] && pAmpang2[j] >= pAmpang3[j])
							{
								pEdge[c] = 255;

								v->push_back(Point2f((float)c, r + tri_list[1][pAmpang2[j + 1]] * (pAmpang1[j] - pAmpang3[j]) /
									(pAmpang1[j] + pAmpang3[j] - (pAmpang2[j] << 1))));
							}
							break;

						case 4:
							if (pAmpang2[j] >= pAmpang1[j] && pAmpang2[j] > pAmpang3[j])
							{
								pEdge[c] = 255;

								v->push_back(Point2f((float)c, r - tri_list[1][pAmpang2[j + 1]] * (pAmpang1[j] - pAmpang3[j]) /
									(pAmpang1[j] + pAmpang3[j] - (pAmpang2[j] << 1))));
							}
							break;

						case 1:
							if (pAmpang2[j] > pAmpang1[j - 2] && pAmpang2[j] >= pAmpang3[j + 2])
							{
								pEdge[c] = 255;

								const float tmp = (float)(pAmpang1[j - 2] - pAmpang3[j + 2]) /
									(pAmpang1[j - 2] + pAmpang3[j + 2] - (pAmpang2[j] << 1));

								v->push_back(
									Point2f(c + tmp * tri_list_root2[0][pAmpang2[j + 1]],
										r + tmp * tri_list_root2[1][pAmpang2[j + 1]]));
							}
							break;

						case 5:
							if (pAmpang2[j] >= pAmpang1[j - 2] && pAmpang2[j] > pAmpang3[j + 2])
							{
								pEdge[c] = 255;

								const float tmp = (float)(pAmpang1[j - 2] - pAmpang3[j + 2]) /
									(pAmpang1[j - 2] + pAmpang3[j + 2] - (pAmpang2[j] << 1));

								v->push_back(
									Point2f(c - tmp * tri_list_root2[0][pAmpang2[j + 1]],
										r - tmp * tri_list_root2[1][pAmpang2[j + 1]]));
							}

							break;

						case 2:
							if (pAmpang2[j] > pAmpang2[j - 2] && pAmpang2[j] >= pAmpang2[j + 2])
							{
								pEdge[c] = 255;

								v->push_back(Point2f(c + tri_list[0][pAmpang2[j + 1]] * (pAmpang2[j - 2] - pAmpang2[j + 2]) /
									(pAmpang2[j - 2] + pAmpang2[j + 2] - (pAmpang2[j] << 1)), (float)r));
							}
							break;

						case 6:
							if (pAmpang2[j] >= pAmpang2[j - 2] && pAmpang2[j] > pAmpang2[j + 2])
							{
								pEdge[c] = 255;

								v->push_back(Point2f(c - tri_list[0][pAmpang2[j + 1]] * (pAmpang2[j - 2] - pAmpang2[j + 2]) /
									(pAmpang2[j - 2] + pAmpang2[j + 2] - (pAmpang2[j] << 1)), (float)r));
							}
							break;

						case 3:
							if (pAmpang2[j] >= pAmpang1[j + 2] && pAmpang2[j] > pAmpang3[j - 2])
							{
								pEdge[c] = 255;

								const float tmp = (float)(pAmpang3[j - 2] - pAmpang1[j + 2]) /
									(pAmpang1[j + 2] + pAmpang3[j - 2] - (pAmpang2[j] << 1));

								v->push_back(
									Point2f(c + tmp * tri_list_root2[0][pAmpang2[j + 1]],
										r + tmp * tri_list_root2[1][pAmpang2[j + 1]]));
							}
							break;

						case 7:
							if (pAmpang2[j] > pAmpang1[j + 2] && pAmpang2[j] >= pAmpang3[j - 2])
							{
								pEdge[c] = 255;

								const float tmp = (float)(pAmpang3[j - 2] - pAmpang1[j + 2]) /
									(pAmpang1[j + 2] + pAmpang3[j - 2] - (pAmpang2[j] << 1));

								v->push_back(
									Point2f(c - tmp * tri_list_root2[0][pAmpang2[j + 1]],
										r - tmp * tri_list_root2[1][pAmpang2[j + 1]]));
							}
							break;

						default:
							break;
						}
					}
				}
			}

			param->mtx.lock();
			param->filter_count++;
			param->mtx.unlock();

		}, &amp_ang, &edge, &vvtmp[i], i, &edge_param);

		f.detach();
	}

	while (edge_param.filter_count < parts)
	{
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}

	for (int i = 0; i < parts; i++)
	{
		vPts.insert(vPts.end(), vvtmp[i].begin(), vvtmp[i].end());
	}

	for (int i = 0;i < vPts.size();i++)
	{
		x_c = x_c + vPts[i].x;
		y_c = y_c + vPts[i].y;

	}
	x_c = x_c / vPts.size();
	y_c = y_c / vPts.size();

}