diff --git a/camCalib/camCalib.cpp b/camCalib/camCalib.cpp index 510e1a3..d2af778 100644 --- a/camCalib/camCalib.cpp +++ b/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 pts2d = detectLaserLine(laserImg); @@ -527,8 +528,8 @@ int main() std::cout << "pe: " << pe << std::endl; //output K and D - char calibKDName[256]; - sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]); + char calibKDPName[256]; + sprintf_s(calibKDPName, "%scalib_param_K_D.txt", calibDataPath[grp]); sg_outputCalibKD(calibKDName, K, D, pe); } diff --git a/camCalib/lineDetection_steger.cpp b/camCalib/lineDetection_steger.cpp index 26cae93..ddba793 100644 --- a/camCalib/lineDetection_steger.cpp +++ b/camCalib/lineDetection_steger.cpp @@ -5,6 +5,7 @@ #include #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(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(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(ptPos.y, x); + result.dx += mask_x.at(0, j) * value; + result.dy += 0; + result.dxx += mask_xx.at(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(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 - SetFilterMat(mask_x, mask_y, mask_xx, mask_yy, mask_xy, gaussWin*2+1); +#if USING_1D_MASK + //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像素差 diff --git a/camCalib/sourceCode/MonoLaserCalibrate.cpp b/camCalib/sourceCode/MonoLaserCalibrate.cpp index 3da2d32..8e513b2 100644 --- a/camCalib/sourceCode/MonoLaserCalibrate.cpp +++ b/camCalib/sourceCode/MonoLaserCalibrate.cpp @@ -497,7 +497,7 @@ std::vector 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++) {