添加了亚像素1D计算方法

camCalib/camCalib.cpp

@@ -8,7 +8,7 @@
 #include "sourceCode/MonoLaserCalibrate.h"
 #include "sourceCode/FitMapParam.h"
 
-#define _DO_CAMERA_CALIB    1
+#define _DO_CAMERA_CALIB    0
 
 void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
 {
@@ -338,7 +338,7 @@ int main()
 
 #else
         char calibKDName[256];
-        sprintf_s(calibKDName, "%scalib_param_K_D.txt", cbImagePath);
+        sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
         cv::Mat K, D;
         sg_readCalibKD(calibKDName, K, D);
 #endif
@@ -475,7 +475,7 @@ int main()
 #if 1
             sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
             cv::imwrite(filename, laserImg);
-
+/*
             cv::Mat calibImg;
             remap(laserImg,
                 calibImg,
@@ -486,6 +486,7 @@ int main()
                 cv::Scalar(0, 0, 0));
             sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index);
             cv::imwrite(filename, calibImg);
+*/
 
 #endif
             std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
@@ -527,8 +528,8 @@ int main()
         std::cout << "pe: " << pe << std::endl;
 
         //output K and D
-        char calibKDName[256];
+        char calibKDPName[256];
-        sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
+        sprintf_s(calibKDPName, "%scalib_param_K_D.txt", calibDataPath[grp]);
         sg_outputCalibKD(calibKDName, K, D, pe);
     }
 

camCalib/lineDetection_steger.cpp

@@ -5,6 +5,7 @@
 #include <corecrt_math_defines.h>
 #include "lineDetection_steger.h"
 
+#define USING_1D_MASK	1
 #if 1
 using namespace std;
 using namespace cv;
@@ -265,6 +266,45 @@ void generate_2nd_2Dmask_directSample(double sigma, int radius, cv::Mat& mask_xx
 	return;
 }
 
+double gaussKernel_1D(double x, double sigma)
+{
+	double sigma2 = sigma * sigma;
+	double norm = 1.0 / (sqrt(2 * M_PI) * sigma);
+	return (norm * exp(-(x * x ) / (2 * sigma2)));
+}
+
+// 2D高斯一阶导数掩模生成
+void generate_1st_1Dmask_directSample(double sigma, int radius, cv::Mat& mask_x)
+{
+	int size = 2 * radius + 1;
+	mask_x = cv::Mat::zeros(1, size, CV_64FC1);
+	double norm = -1.0 / (sigma * sigma);
+	for (int col = 0; col < size; col++)
+	{
+		double x = (double)(col - radius);
+		double dx = x * norm * gaussKernel_1D(x, sigma);
+		mask_x.at<double>(0, col) = dx;
+	}
+	return;
+}
+
+// 2D高斯二阶导数掩模生成
+void generate_2nd_1Dmask_directSample(double sigma, int radius, cv::Mat& mask_xx)
+{
+	int size = 2 * radius + 1;
+	mask_xx = cv::Mat::zeros(1, size, CV_64FC1);
+	double sigma2 = sigma * sigma;
+	double norm = 1.0 / (sigma2 * sigma2);
+	double offset = 1.0 / sigma2;
+	for (int col = 0; col < size; col++)
+	{
+		double x = (double)(col - radius);
+		double dxx = (x * x * norm - offset) * gaussKernel_1D(x, sigma);
+		mask_xx.at<double>(0, col) = dxx;
+	}
+	return;
+}
+
 // 单点卷积函数
 sPtDerivate pointConvolve(
 	cv::Point ptPos, //点位置  
@@ -302,6 +342,35 @@ sPtDerivate pointConvolve(
 	return result;
 }
 
+// 单点卷积函数
+sPtDerivate pointConvolve_1D(
+	cv::Point ptPos, //点位置  
+	int radius,  //卷积窗半径
+	cv::Mat& img,  //double型单通道图像
+	cv::Mat& mask_x, //一阶掩膜x
+	cv::Mat& mask_xx //二阶掩膜xx
+)
+{
+	sPtDerivate result = { 0.0, 0.0, 0.0, 0.0, 0.0 };
+	cv::Size imgSize = img.size(); //
+	if ((ptPos.x < radius) || (ptPos.x > (imgSize.width - radius - 1)) || (ptPos.y < radius) || (ptPos.y > (imgSize.height - radius - 1)))
+		return result;
+
+	int size = 2 * radius + 1;
+	for (int j = 0; j < size; j++)
+	{
+		int col = j - radius;
+		int x = ptPos.x + col;
+		double value = img.at<double>(ptPos.y, x);
+		result.dx += mask_x.at<double>(0, j) * value;
+		result.dy += 0;
+		result.dxx += mask_xx.at<double>(0, j) * value;
+		result.dyy += 0;
+		result.dxy += 0;
+	}
+	return result;
+}
+
 bool isMax(int i, int j, cv::Mat& img, int dx, int dy)//在法线方向上是否为极值
 {
 	double val = img.at<double>(j, i);
@@ -380,6 +449,30 @@ cv::Point2f computeHessianSubpix(
 	return subPix;
 }
 
+//计算1D得到亚像素
+cv::Point2f compute1DSubpix(
+	cv::Point ptPos, //点位置
+	cv::Mat& img,  //double型单通道图像
+	sPtDerivate _ptDerivate
+)
+{
+	double subTh = 0.7;
+	cv::Point2f subPix = { -1.0f, -1.0f };  //初始化成无效亚像素值
+	//t 公式 泰勒展开到二阶导数
+	double a = _ptDerivate.dxx;
+	double b = _ptDerivate.dx;
+	if (a != 0.0)
+	{
+		double t = b / a; // -b / a;
+		if (fabs(t) <= subTh )//(x + t * Nx, y + t * Ny)为亚像素坐标
+		{
+			subPix.x = (float)(ptPos.x + t);
+			subPix.y = (float)(ptPos.y);
+		}
+	}
+	return subPix;
+}
+
 // 从点的整数坐标计算亚像素
 void computePointSubpix(
 	cv::Mat& inputImg,  //uchar型单通道输入图像（灰度图像）
@@ -391,8 +484,10 @@ void computePointSubpix(
 	cv::Mat mask_x, mask_y, mask_xx, mask_yy, mask_xy;
 	double sigma = (double)gaussWin / sqrt(3);
 	//double sigma = 1.0;
-#if 0
+#if USING_1D_MASK
-	SetFilterMat(mask_x, mask_y, mask_xx, mask_yy, mask_xy, gaussWin*2+1);
+	//SetFilterMat(mask_x, mask_y, mask_xx, mask_yy, mask_xy, gaussWin*2+1);
+	generate_1st_1Dmask_directSample(sigma, gaussWin, mask_x);
+	generate_2nd_1Dmask_directSample(sigma, gaussWin, mask_xx);
 #else
 	generate_1st_2Dmask_directSample(sigma, gaussWin, mask_x, mask_y);
 	generate_2nd_2Dmask_directSample(sigma, gaussWin, mask_xx, mask_yy, mask_xy);
@@ -409,7 +504,7 @@ void computePointSubpix(
 	//高斯滤波
 	cv::Mat img;
 	inputImg.convertTo(img, CV_64FC1);
-	cv::GaussianBlur(img, img, cv::Size(3, 3), 0.9, 0.9);
+	cv::GaussianBlur(img, img, cv::Size(5, 5), 1.2, 1.2);
 	//cv::imwrite("gauss_blur_src.bmp", img);
 
 	for (int i = 0, i_max = (int)pos.size(); i < i_max; i++)
@@ -418,6 +513,20 @@ void computePointSubpix(
 		int doSubPix = 0;
 		while (doSubPix >= 0)
 		{
+#if USING_1D_MASK
+			sPtDerivate a_ptDerivate = pointConvolve_1D(
+				ptPos, //点位置  
+				gaussWin,  //卷积窗半径
+				img,  //double型单通道图像
+				mask_x, //一阶掩膜x
+				mask_xx //二阶掩膜xx
+			);
+			cv::Point2f subpix = compute1DSubpix(
+				ptPos, //点位置
+				img,  //double型单通道图像
+				a_ptDerivate
+			);
+#else
 			sPtDerivate a_ptDerivate = pointConvolve(
 				ptPos, //点位置  
 				gaussWin,  //卷积窗半径
@@ -433,6 +542,7 @@ void computePointSubpix(
 				img,  //double型单通道图像
 				a_ptDerivate
 			);
+#endif
 			if ((subpix.x >= 0) && (subpix.y >= 0))
 			{
 				subpix.x += 0.5; //图像中的像素位置表示的是像素是左下角，而此处像素坐标是指向像素中间位置，所以有0.5像素差

camCalib/sourceCode/MonoLaserCalibrate.cpp

@@ -497,7 +497,7 @@ std::vector<cv::Point2f> detectLaserLine(
     //cv::imwrite("gauss_blur_src.bmp", img);
     // 提取最大值点
     int scaleWin = 5;
-    double minPkValue = 100;
+    double minPkValue = 20;
     std::vector< cv::Point> pkPoints;
     for (int i = 0; i < img.rows; i++)
     {