Forstner点特征提取算子c++

承接上一篇的Moravec算子，继续完成教材上第二个点特征提取算子Forstner，参考教材仍然是、张祖勋《数字摄影测量学》，算法原理不再赘述。

#include <iostream>
#include<opencv2/opencv.hpp>
using namespace std;
using namespace cv;
float Tq = 0.5;//阈值Tq
vector<Point2f>featurePoints;
int k = 2;//兴趣值窗口(5*5)
int k2 = 5;//极值点窗口
int main()
{
	Mat src = imread("C:\\Users\\Administrator\\Desktop\\images\\标定板.png", IMREAD_GRAYSCALE);
	//1.计算各像素的Robert's梯度
	Mat matq = Mat::zeros(src.rows, src.cols, CV_32FC1);//存储各像素点的q值
	Mat matw = Mat::zeros(src.rows, src.cols, CV_32FC1);//存储各像素点的w值
	double sum_w = 0;
	for (int r = k; r < src.rows - k; r++)
	{
		for (int c = k; c < src.cols - k; c++)
		{
			double gu2 = 0, gv2 = 0, guv = 0;
			for (int i = r - k; i <= r + k - 1; i++)
			{
				for (int j = c - k; j <= c + k - 1; j++)
				{
					//2.计算5*5窗口的灰度协方差矩阵
					double gu = src.at<uchar>(i + 1, j + 1) - src.at<uchar>(i, j);
					double gv = src.at<uchar>(i, j + 1) - src.at<uchar>(i + 1, j);
					gu2 += gu*gu;
					gv2 += gv*gv;
					guv += gu*gv;
				}
			}
			double detN = gu2 * gv2 - guv * guv;//行列式
			double traceN = gu2 + gv2;//迹
			if (traceN == 0)
				continue;
			//cout << r<<" "<<c << endl;
			//3.计算兴趣值q，w
			double q = 4.0 * detN / (traceN * traceN);
			double w = detN / traceN;
			sum_w += w;
			matq.at<float>(r, c) = q;
			matw.at<float>(r, c) = w;
		}
	}
	cout << "计算各像素的Robert's梯度完成" << endl;
	cout << "兴趣值q，w计算完毕" << endl;
	double w_ave = sum_w / ((src.rows - 2*(double)k) * (src.cols - 2*(double)k));//w的平均值
	double f = 1.5;
	double Tw = f * w_ave;//阈值Tw
	cout << "Tw:" << Tw << endl;
	//4.确定候选点
	for (int r = k; r < src.rows-k; r++)
	{
		for (int c = k; c < src.cols-k; c++)
		{
			if (matq.at<float>(r, c) > Tq && matw.at<float>(r, c) > Tw)
				continue;
			else
				matw.at<float>(r, c) = 0;
		}
	}
	//5.选取极值点
	for (int r = k2; r < src.rows - k2; r += k2)
	{
		for (int c = k2; c < src.cols - k2; c += k2)
		{
			float max = 0;
			int Flag = 0;
			Point2f point;
			for (int j = -k2; j <= k2; j++)
			{
				for (int i = -k2; i <= k2; i++)
				{
					float value = matw.at<float>(r + j, c + i);
					//局部非最大值抑制
					if (value > max)
					{
						max = value;
						point.x = c + i;
						point.y = r + j;
						Flag = 1;
					}
				}
			}
			if (Flag == 1)
			{
				featurePoints.push_back(point);
			}
		}
	}
	cout << "特征点个数：" << featurePoints.size() << endl;
	Mat img = imread("C:\\Users\\Administrator\\Desktop\\images\\标定板.png", IMREAD_COLOR);
	//6.绘制特征点
	for (int i = 0; i < featurePoints.size(); i++)
	{
		int centerx = featurePoints[i].x;
		int centery = featurePoints[i].y;
		circle(img, Point(centerx, centery), 4, Scalar(255, 0, 0), 1);
	}
	imwrite("D:\\Software\\VS2019\\source\\repos\\摄影测量与三维重建\\2.Forstner点特征提取算子\\result\\Forstner点特征提取算子.jpg", img);
	cv::waitKey(0);
	return 0;
}

                                                             原图

                                                                           运行结果