461 lines
12 KiB
C++
461 lines
12 KiB
C++
#include<iostream>
|
||
#include<opencv2\opencv.hpp>
|
||
#include<unordered_map>
|
||
#include<math.h>
|
||
#include <corecrt_math_defines.h>
|
||
#include "lineDetection_steger.h"
|
||
|
||
#if 1
|
||
using namespace std;
|
||
using namespace cv;
|
||
|
||
struct EV_VAL_VEC {
|
||
double nx, ny;//nx、ny为最大特征值对应的特征向量
|
||
};
|
||
|
||
void SetFilterMat(Mat& Dx, Mat& Dy, Mat& Dxx, Mat& Dyy, Mat& Dxy, int size_m)//radius * 2= size
|
||
{
|
||
if (size_m % 2 == 0 || size_m == 1) cout << "size is not illegal !!" << endl;
|
||
Mat m1 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dx
|
||
Mat m2 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dy
|
||
Mat m3 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dxx
|
||
Mat m4 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dyy
|
||
Mat m5 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dxy
|
||
|
||
for (int i = 0; i < size_m / 2; i++)
|
||
{
|
||
m1.row(i) = 1;
|
||
m2.col(i) = 1;
|
||
m3.row(i) = -1;
|
||
m4.col(i) = -1;
|
||
}
|
||
for (int i = size_m / 2 + 1; i < size_m; i++)
|
||
{
|
||
m1.row(i) = -1;
|
||
m2.col(i) = -1;
|
||
m3.row(i) = -1;
|
||
m4.col(i) = -1;
|
||
}
|
||
m3.row(size_m / 2) = (size_m / 2) * 2;
|
||
m4.col(size_m / 2) = (size_m / 2) * 2;
|
||
m5.row(size_m / 2) = 1;
|
||
m5.col(size_m / 2) = 1;
|
||
m5.at<double>(size_m / 2, size_m / 2) = -(size_m / 2) * 4;
|
||
if (size_m == 5) m5 = (Mat_<double>(5, 5) << 0, 0, 1, 0, 0, 0, 1, 2, 1, 0, 1, 2, -16, 2, 1, 0, 1, 2, 1, 0, 0, 0, 1, 0, 0);
|
||
Dx = m2;
|
||
Dy = m1;
|
||
Dxx = m4;
|
||
Dyy = m3;
|
||
Dxy = m5;
|
||
cout << Dx << endl;
|
||
cout << Dy << endl;
|
||
cout << Dxx << endl;
|
||
cout << Dyy << endl;
|
||
cout << Dxy << endl;
|
||
return;
|
||
}
|
||
|
||
#if 0
|
||
bool isMax(int i, int j, Mat& img, int dx, int dy)//在法线方向上是否为极值
|
||
{
|
||
double val = img.at<double>(j, i);
|
||
double max_v = max(img.at<double>(j + dy, i + dx), img.at<double>(j - dy, i - dx));
|
||
if (val >= max_v) return true;
|
||
else return false;
|
||
}
|
||
|
||
void StegerLine(Mat& img0, vector<Point2d>& sub_pixel, int size_m)
|
||
{
|
||
Mat img;
|
||
cvtColor(img0, img0, cv::COLOR_BGR2GRAY);
|
||
img = img0.clone();
|
||
|
||
//高斯滤波
|
||
img.convertTo(img, CV_64FC1);
|
||
cv::GaussianBlur(img, img, Size(3, 3), 0.9, 0.9);
|
||
cv::imwrite("gauss_blur_src.bmp", img);
|
||
|
||
//ho_Image = Mat2Hobject(img);
|
||
//Threshold(ho_Image, &ho_Region, 80, 255);
|
||
//GetRegionPoints(ho_Region, &hv_Rows, &hv_Columns);
|
||
|
||
|
||
//一阶偏导数
|
||
Mat m1, m2;
|
||
//二阶偏导数
|
||
Mat m3, m4, m5;
|
||
SetFilterMat(m1, m2, m3, m4, m5, size_m);
|
||
|
||
cout << m5 << endl;
|
||
|
||
Mat dx, dy;
|
||
filter2D(img, dx, CV_64FC1, m1);
|
||
filter2D(img, dy, CV_64FC1, m2);
|
||
|
||
Mat dxx, dyy, dxy;
|
||
filter2D(img, dxx, CV_64FC1, m3);
|
||
filter2D(img, dyy, CV_64FC1, m4);
|
||
filter2D(img, dxy, CV_64FC1, m5);
|
||
|
||
//hessian矩阵
|
||
int imgcol = img.cols;
|
||
int imgrow = img.rows;
|
||
vector<double> Pt;
|
||
|
||
for (int i = 0; i < imgcol; i++)
|
||
{
|
||
for (int j = 0; j < imgrow; j++)
|
||
{
|
||
double pixel_val = img.at<double>(j, i);
|
||
if (img.at<double>(j, i) > 50) //需要确定ROI！！！
|
||
{
|
||
Mat hessian(2, 2, CV_64FC1);
|
||
hessian.at<double>(0, 0) = dxx.at<double>(j, i);
|
||
hessian.at<double>(0, 1) = dxy.at<double>(j, i);
|
||
hessian.at<double>(1, 0) = dxy.at<double>(j, i);
|
||
hessian.at<double>(1, 1) = dyy.at<double>(j, i);
|
||
|
||
Mat eValue;
|
||
Mat eVectors;
|
||
eigen(hessian, eValue, eVectors);
|
||
|
||
double nx, ny;
|
||
double EV_max = 0;//特征值
|
||
double EV_min = 0;
|
||
if (fabs(eValue.at<double>(0, 0)) >= fabs(eValue.at<double>(1, 0))) //求特征值最大时对应的特征向量
|
||
{
|
||
nx = eVectors.at<double>(0, 0);//大的特征值对应的特征向量
|
||
ny = eVectors.at<double>(0, 1);
|
||
EV_max = max(eValue.at<double>(0, 0), eValue.at<double>(1, 0));
|
||
EV_min = min(eValue.at<double>(0, 0), eValue.at<double>(1, 0));
|
||
}
|
||
else
|
||
{
|
||
nx = eVectors.at<double>(1, 0);//大的特征值对应的特征向量
|
||
ny = eVectors.at<double>(1, 1);
|
||
EV_max = max(eValue.at<double>(0, 0), eValue.at<double>(1, 0));
|
||
EV_min = min(eValue.at<double>(0, 0), eValue.at<double>(1, 0));
|
||
}
|
||
|
||
//t 公式 泰勒展开到二阶导数
|
||
double a = nx * nx * dxx.at<double>(j, i) + 2 * nx * ny * dxy.at<double>(j, i) + ny * ny * dyy.at<double>(j, i);
|
||
double b = nx * dx.at<double>(j, i) + ny * dy.at<double>(j, i);
|
||
double t = -b / a;
|
||
|
||
int dx;
|
||
int dy;
|
||
|
||
if (abs(nx) >= 2 * abs(ny)) dx = 1, dy = 0;//垂直
|
||
else if (abs(ny) >= 2 * abs(nx)) dx = 0, dy = 1;//水平
|
||
else if (nx > 0) dx = 1, dy = 1;
|
||
else if (ny > 0) dx = -1, dy = -1;
|
||
|
||
if (isMax(i, j, img, dx, dy))
|
||
//if(EV_min<-120)
|
||
{
|
||
double temp1 = t * nx;
|
||
double temp2 = t * ny;
|
||
if (fabs(t * nx) <= 0.5 && fabs(t * ny) <= 0.5)//(x + t * Nx, y + t * Ny)为亚像素坐标
|
||
{
|
||
Pt.push_back(i + t * nx);
|
||
Pt.push_back(j + t * ny);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int k = 0; k < Pt.size() / 2; k++)
|
||
{
|
||
Point2d rpt;
|
||
rpt.x = Pt[2 * k + 0];
|
||
rpt.y = Pt[2 * k + 1];
|
||
sub_pixel.push_back(rpt);
|
||
}
|
||
|
||
}
|
||
|
||
//E:/workplace/Line_Dtection/Project1/1/1-0.bmp
|
||
|
||
int main()//E:/workplace/get_line_width/两头亮二原图/两头亮二/1300us.bmp
|
||
{
|
||
Mat src = imread("E:/workplace/Line_Dtection/Project1/1/1-9.bmp");
|
||
vector<Point2d>sub_pixel;
|
||
StegerLine(src, sub_pixel, 5);
|
||
ofstream outfile_r, outfile_c;
|
||
outfile_r.open("subpixel_row.txt");
|
||
outfile_c.open("subpixel_col.txt");
|
||
outfile_r << sub_pixel.size() << endl;
|
||
outfile_c << sub_pixel.size() << endl;
|
||
for (int i = 0; i < sub_pixel.size(); i++)
|
||
{
|
||
outfile_r << setprecision(15) << "2 " << sub_pixel[i].y << endl;
|
||
outfile_c << setprecision(15) << "2 " << sub_pixel[i].x << endl;
|
||
}
|
||
outfile_c.close();
|
||
outfile_r.close();
|
||
return 0;
|
||
}
|
||
#endif
|
||
#endif
|
||
|
||
typedef struct
|
||
{
|
||
double dx;
|
||
double dy;
|
||
double dxx;
|
||
double dyy;
|
||
double dxy;
|
||
}sPtDerivate;
|
||
|
||
//直接采样法
|
||
double gaussKernel2D(double x, double y, double sigma)
|
||
{
|
||
double sigma2 = sigma * sigma;
|
||
double norm = 1.0 / (2 * M_PI * sigma2);
|
||
return (norm * exp(-(x * x + y * y) / (2 * sigma2)));
|
||
}
|
||
|
||
// 2D高斯一阶导数掩模生成
|
||
void generate_1st_2Dmask_directSample(double sigma, int radius, cv::Mat& mask_x, cv::Mat& mask_y)
|
||
{
|
||
int size = 2 * radius + 1;
|
||
mask_x = cv::Mat::zeros(size, size, CV_64FC1);
|
||
mask_y = cv::Mat::zeros(size, size, CV_64FC1);
|
||
double norm = -1.0 / (sigma * sigma);
|
||
for (int row = 0; row < size; row++)
|
||
{
|
||
double y = (double)(row - radius);
|
||
for (int col = 0; col < size; col++)
|
||
{
|
||
double x = (double)(col - radius);
|
||
double dx = x * norm * gaussKernel2D(x, y, sigma);
|
||
double dy = y * norm * gaussKernel2D(x, y, sigma);
|
||
mask_x.at<double>(row, col) = dx;
|
||
mask_y.at<double>(row, col) = dy;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 2D高斯二阶导数掩模生成
|
||
void generate_2nd_2Dmask_directSample(double sigma, int radius, cv::Mat& mask_xx, cv::Mat& mask_yy, cv::Mat& mask_xy)
|
||
{
|
||
int size = 2 * radius + 1;
|
||
mask_xx = cv::Mat::zeros(size, size, CV_64FC1);
|
||
mask_yy = cv::Mat::zeros(size, size, CV_64FC1);
|
||
mask_xy = cv::Mat::zeros(size, size, CV_64FC1);
|
||
double sigma2 = sigma * sigma;
|
||
double norm = 1.0 / (sigma2 * sigma2);
|
||
double offset = 1.0 / sigma2;
|
||
for (int row = 0; row < size; row++)
|
||
{
|
||
double y = (double)(row - radius);
|
||
for (int col = 0; col < size; col++)
|
||
{
|
||
double x = (double)(col - radius);
|
||
double dxx = (x * x * norm - offset) * gaussKernel2D(x, y, sigma);
|
||
double dyy = (y * y * norm - offset) * gaussKernel2D(x, y, sigma);
|
||
double dxy = x * y * norm * gaussKernel2D(x, y, sigma);
|
||
mask_xx.at<double>(row, col) = dxx;
|
||
mask_yy.at<double>(row, col) = dyy;
|
||
mask_xy.at<double>(row, col) = dxy;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 单点卷积函数
|
||
sPtDerivate pointConvolve(
|
||
cv::Point ptPos, //点位置
|
||
int radius, //卷积窗半径
|
||
cv::Mat& img, //double型单通道图像
|
||
cv::Mat& mask_x, //一阶掩膜x
|
||
cv::Mat& mask_y, //一阶掩膜y
|
||
cv::Mat& mask_xx, //二阶掩膜xx
|
||
cv::Mat& mask_yy, //二阶掩膜yy
|
||
cv::Mat& mask_xy //二阶掩膜xy
|
||
)
|
||
{
|
||
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 i = 0; i < size; i++)
|
||
{
|
||
int row = i - radius;
|
||
int y = ptPos.y + row;
|
||
for (int j = 0; j < size; j++)
|
||
{
|
||
int col = j - radius;
|
||
int x = ptPos.x + col;
|
||
double value = img.at<double>(y, x);
|
||
result.dx += mask_x.at<double>(i, j) * value;
|
||
result.dy += mask_y.at<double>(i, j) * value;
|
||
result.dxx += mask_xx.at<double>(i, j) * value;
|
||
result.dyy += mask_yy.at<double>(i, j) * value;
|
||
result.dxy += mask_xy.at<double>(i, j) * value;
|
||
}
|
||
}
|
||
return result;
|
||
}
|
||
|
||
bool isMax(int i, int j, cv::Mat& img, int dx, int dy)//在法线方向上是否为极值
|
||
{
|
||
double val = img.at<double>(j, i);
|
||
double max_v = std::max(img.at<double>(j + dy, i + dx), img.at<double>(j - dy, i - dx));
|
||
if (val >= max_v) return true;
|
||
else return false;
|
||
}
|
||
|
||
//计算Hessian得到亚像素
|
||
cv::Point2f computeHessianSubpix(
|
||
cv::Point ptPos, //点位置
|
||
cv::Mat& img, //double型单通道图像
|
||
sPtDerivate _ptDerivate
|
||
)
|
||
{
|
||
double subTh = 0.7;
|
||
cv::Mat hessian(2, 2, CV_64FC1);
|
||
hessian.at<double>(0, 0) = _ptDerivate.dxx;
|
||
hessian.at<double>(0, 1) = _ptDerivate.dxy;
|
||
hessian.at<double>(1, 0) = _ptDerivate.dxy;
|
||
hessian.at<double>(1, 1) = _ptDerivate.dyy;
|
||
|
||
cv::Mat eigenValue, eigenVector;
|
||
cv::eigen(hessian, eigenValue, eigenVector);
|
||
|
||
double nx, ny;
|
||
double vnx, vny;
|
||
double EV_max = 0;//特征值
|
||
double EV_min = 0;
|
||
if (fabs(eigenValue.at<double>(0, 0)) >= fabs(eigenValue.at<double>(1, 0))) //求特征值最大时对应的特征向量
|
||
{
|
||
nx = eigenVector.at<double>(0, 0);//大的特征值对应的特征向量
|
||
ny = eigenVector.at<double>(0, 1);
|
||
vnx = eigenVector.at<double>(1, 0);//小的特征值对应的特征向量
|
||
vny = eigenVector.at<double>(1, 1);
|
||
EV_max = std::max(eigenValue.at<double>(0, 0), eigenValue.at<double>(1, 0));
|
||
EV_min = std::min(eigenValue.at<double>(0, 0), eigenValue.at<double>(1, 0));
|
||
}
|
||
else
|
||
{
|
||
nx = eigenVector.at<double>(1, 0);//大的特征值对应的特征向量
|
||
ny = eigenVector.at<double>(1, 1);
|
||
vnx = eigenVector.at<double>(0, 0);//小的特征值对应的特征向量
|
||
vny = eigenVector.at<double>(0, 1);
|
||
EV_max = std::max(eigenValue.at<double>(0, 0), eigenValue.at<double>(1, 0));
|
||
EV_min = std::min(eigenValue.at<double>(0, 0), eigenValue.at<double>(1, 0));
|
||
}
|
||
|
||
cv::Point2f subPix = { -1.0f, -1.0f }; //初始化成无效亚像素值
|
||
//t 公式 泰勒展开到二阶导数
|
||
double a = nx * nx * _ptDerivate.dxx + 2 * nx * ny * _ptDerivate.dxy + ny * ny * _ptDerivate.dyy;
|
||
double b = nx * _ptDerivate.dx + ny * _ptDerivate.dy;
|
||
if (a != 0.0)
|
||
{
|
||
double t = b / a; // -b / a;
|
||
int dx;
|
||
int dy;
|
||
|
||
if (abs(nx) >= 2 * abs(ny)) dx = 1, dy = 0;//垂直
|
||
else if (abs(ny) >= 2 * abs(nx)) dx = 0, dy = 1;//水平
|
||
else if (nx > 0) dx = 1, dy = 1;
|
||
else if (ny > 0) dx = -1, dy = -1;
|
||
|
||
if (isMax(ptPos.x, ptPos.y, img, dx, dy))
|
||
//if(EV_min<-120)
|
||
{
|
||
double temp1 = t * nx;
|
||
double temp2 = t * ny;
|
||
if (fabs(t * nx) <= subTh && fabs(t * ny) <= subTh)//(x + t * Nx, y + t * Ny)为亚像素坐标
|
||
{
|
||
subPix.x = (float)(ptPos.x + t * nx);
|
||
subPix.y = (float)(ptPos.y + t * ny);
|
||
}
|
||
}
|
||
}
|
||
return subPix;
|
||
}
|
||
|
||
// 从点的整数坐标计算亚像素
|
||
void computePointSubpix(
|
||
cv::Mat& inputImg, //uchar型单通道输入图像（灰度图像）
|
||
std::vector<cv::Point>&pos, //峰值像素的整数坐标。
|
||
int gaussWin, //使用steger算法时指定的窗口宽度。此宽度需要与激光线的宽度基本吻合
|
||
std::vector<cv::Point2f>& posSubpix //计算出的亚像素坐标
|
||
)
|
||
{
|
||
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);
|
||
#else
|
||
generate_1st_2Dmask_directSample(sigma, gaussWin, mask_x, mask_y);
|
||
generate_2nd_2Dmask_directSample(sigma, gaussWin, mask_xx, mask_yy, mask_xy);
|
||
#endif
|
||
|
||
#if 0
|
||
std::cout << "mask::" << std::endl;
|
||
std::cout << mask_x << std::endl;
|
||
std::cout << mask_y << std::endl;
|
||
std::cout << mask_xx << std::endl;
|
||
std::cout << mask_yy << std::endl;
|
||
std::cout << mask_xy << std::endl;
|
||
#endif
|
||
//高斯滤波
|
||
cv::Mat img;
|
||
inputImg.convertTo(img, CV_64FC1);
|
||
cv::GaussianBlur(img, img, cv::Size(3, 3), 0.9, 0.9);
|
||
//cv::imwrite("gauss_blur_src.bmp", img);
|
||
|
||
for (int i = 0, i_max = (int)pos.size(); i < i_max; i++)
|
||
{
|
||
cv::Point ptPos = pos[i];
|
||
int doSubPix = 0;
|
||
while (doSubPix >= 0)
|
||
{
|
||
sPtDerivate a_ptDerivate = pointConvolve(
|
||
ptPos, //点位置
|
||
gaussWin, //卷积窗半径
|
||
img, //double型单通道图像
|
||
mask_x, //一阶掩膜x
|
||
mask_y, //一阶掩膜y
|
||
mask_xx, //二阶掩膜xx
|
||
mask_yy, //二阶掩膜yy
|
||
mask_xy //二阶掩膜xy
|
||
);
|
||
cv::Point2f subpix = computeHessianSubpix(
|
||
ptPos, //点位置
|
||
img, //double型单通道图像
|
||
a_ptDerivate
|
||
);
|
||
if ((subpix.x >= 0) && (subpix.y >= 0))
|
||
{
|
||
posSubpix.push_back(subpix);
|
||
doSubPix = -1;
|
||
}
|
||
else
|
||
{
|
||
if (0 == doSubPix)
|
||
{
|
||
ptPos.x = pos[i].x - 1;
|
||
doSubPix = 1;
|
||
}
|
||
else if (1 == doSubPix)
|
||
{
|
||
ptPos.x = pos[i].x + 1;
|
||
doSubPix = 2;
|
||
}
|
||
else
|
||
{
|
||
doSubPix = -1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
} |