/******************************************************************
程序说明放置挡板，为了保证挡板不超过误差，
更新时间：8月19日18点
更新时间：8月20日16点
*******************************************************************/
#include "Dangban.h"
extern VideoCapture cap_up;
extern VideoCapture cap_down;
extern Mat imgOriginal_up, imgOriginal_down;//用于缓存图像
extern int G_threshold_down;
extern int G_threshold_up;
//RGB转HSV
void RGB2HSV(double red, double green, double blue, double& hue, double& saturation, double& intensity)
{
double r, g, b;
double h, s, i;
double sum;
double minRGB, maxRGB;
double theta;
r = red / 255.0;
g = green / 255.0;
b = blue / 255.0;
minRGB = ((r<g) ? (r) : (g));
minRGB = (minRGB<b) ? (minRGB) : (b);
maxRGB = ((r>g) ? (r) : (g));
maxRGB = (maxRGB>b) ? (maxRGB) : (b);
sum = r + g + b;
i = sum / 3.0;
if (i<0.001 || maxRGB - minRGB<0.001)
{
h = 0.0;
s = 0.0;
}
else
{
s = 1.0 - 3.0*minRGB / sum;
theta = sqrt((r - g)*(r - g) + (r - b)*(g - b));
theta = acos((r - g + r - b)*0.5 / theta);
if (b <= g)
h = theta;
else
h = 2 * 3.1415926 - theta;
if (s <= 0.01)
h = 0;
}
hue = (int)(h * 180 / 3.1415926);
saturation = (int)(s * 100);
intensity = (int)(i * 100);
}
//
///***** 求两点间距离*****/
//float getDistance(CvPoint pointO, CvPoint pointA)
//{
// float distance;
// distance = powf((pointO.x - pointA.x), 2) + powf((pointO.y - pointA.y), 2);
// distance = sqrtf(distance);
// return distance;
//}
//原点到直线的距离
//输入一堆直线，返回直线到坐标原点的距离
vector <float> get_0_distance(vector<Vec4i> lines)
{
vector <float> diatance;
for (unsigned int i = 0; i < lines.size(); i++)
{
double k = (double)(lines[i][1] - lines[i][3]) / (double)(lines[i][0] - lines[i][2]);//斜率
double b = (double)lines[i][1] - (double)(lines[i][0])* k; //截距
double diatance_temp = fabs(b / sqrt(1 + k*k));
diatance.push_back(diatance_temp);
}
return diatance;
}
/***** 点到直线的距离:P到AB的距离*****/
//P为线外一点，AB为线段两个端点,有正负，点在左边为负，右边为正
float getDist_P2L_zhengfu(CvPoint pointP, CvPoint pointA, CvPoint pointB)
{
//求直线方程
float A = 0, B = 0, C = 0;
A = pointA.y - pointB.y;
B = pointB.x - pointA.x;
C = pointA.x*pointB.y - pointA.y*pointB.x;
//代入点到直线距离公式
float distance = 0;
if (pointP.x>(-(B*pointP.y + C) / A))
{//点在线右边
distance = ((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
}
else
{//点在线左边
distance = -((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
}
return distance;
}
/***** 点到直线的距离:P到AB的距离*****/
//P为线外一点，剩下三个参数为直线一般式参数，0为横挡板，1为竖挡板
float getDist_P2L_zhengfu_2(CvPoint pointP, float Param_A1, float Param_A2, float Param_A3,int mode)
{
//求直线方程
float A = 0, B = 0, C = 0;
A = Param_A1;
B = Param_A2;
C = Param_A3;
//代入点到直线距离公式
float distance = 0;
if (mode == 0)//横挡板判断上下
{
if (pointP.y>(-(A*pointP.x + C) / B))
{//点在线上面
distance = ((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
}
else
{//点在线下面
distance = -((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
}
}
else //竖挡板判断左右
{
if (pointP.x>(-(B*pointP.y + C) / A))
{//点在线右边
distance = ((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
}
else
{//点在线左边
distance = -((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
}
}
return distance;
}
//输入一堆直线，返回每条直线与水平直线的角度，为弧度
vector <float> get_lines_arctan(vector<Vec4i> lines)
{
float k = 0; //直线斜率
vector <float> lines_arctan;//直线斜率的反正切值
for (unsigned int i = 0; i<lines.size(); i++)
{
k = (double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
lines_arctan.push_back(atan(k));
}
return lines_arctan;
}
/***** 点到一堆直线的距离:P到lines的距离*****/
vector <float> getDist_P2L_yibanshi(Point2f pointP, vector<Vec4i> lines)
{
//求直线方程
Point2f pointA, pointB;
vector <float> distance;
for (unsigned int i = 0; i < lines.size(); i++)
{
pointA.x = (float)lines[i][0];
pointA.y = (float)lines[i][1];
pointB.x = (float)lines[i][2];
pointB.x = (float)lines[i][3];
float A = 0, B = 0, C = 0;
A = pointA.y - pointB.y;
B = pointB.x - pointA.x;
C = pointA.x*pointB.y - pointA.y*pointB.x;
//代入点到直线距离公式
float distance_temp;
distance_temp = ((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
distance.push_back(distance_temp);
}
return distance;
}
//输入一堆直线，返回每条直线的斜率和截距
//Vec2f为2个点的float，参照存储直线的数据结构
vector <Point2d> get_lines_fangcheng(vector<Vec4i> lines)
{
double k = 0; //直线斜率
double b = 0; //直线截距
vector <Point2d> lines_fangcheng;//
for (unsigned int i = 0; i<lines.size(); i++)
{
k = (double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率// -3.1415926/2-----+3.1415926/2
b = (double)lines[i][1] - k * (double)lines[i][0]; //求出直线的截距
lines_fangcheng.push_back(Point2d(k, b));
}
return lines_fangcheng;
}
//填充
void fillHole(const Mat srcBw, Mat &dstBw)
{
Size m_Size = srcBw.size();
Mat Temp = Mat::zeros(m_Size.height + 2, m_Size.width + 2, srcBw.type());//延展图像
srcBw.copyTo(Temp(Range(1, m_Size.height + 1), Range(1, m_Size.width + 1)));
cv::floodFill(Temp, Point(0, 0), Scalar(255, 255, 255));//填充区域
Mat cutImg;//裁剪延展的图像
Temp(Range(1, m_Size.height + 1), Range(1, m_Size.width + 1)).copyTo(cutImg);
dstBw = srcBw | (~cutImg);
}
//输入一堆直线，返回每条直线的一般式方程
//Vec2f为2个点的float，参照存储直线的数据结构
vector <float> get_lines_yibanshi_fangcheng(vector<Vec4i> lines)
{
//1.分别求角点0、1与角点2、3所对应的两根直线
//vector <vector<float>> fangcheng_PARAM;
vector<float> fangcheng_temp;
for (unsigned int i = 0; i < lines.size(); i++)
{
float A0 = 0, B0 = 0, C0 = 0;
A0 = (float)(lines[i][1] - lines[i][3]);
if ((lines[i][1] - lines[i][3]) == 0)
{
A0 = 0.0001;
}
B0 = (float)(lines[i][2] - lines[i][0]);
if ((lines[i][2] - lines[i][0]) == 0)
{
B0 = 0.0001;
}
C0 = (float)(lines[i][0] * lines[i][3] - lines[i][1] * lines[i][2]);
if (C0 == 0)
{
C0 = 0.0001;
}
/* if ((lines[i][2] - lines[i][0]) == 0)
{
B0 =0.001;
if (A0>0)
{
C0 =-fabs( (float)(lines[i][0] * lines[i][3] - lines[i][1] * lines[i][2]));
}
else
{
C0 = fabs((float)(lines[i][0] * lines[i][3] - lines[i][1] * lines[i][2]));
}
}
else
{
B0 = (float)(lines[i][2] - lines[i][0]);
C0 = (float)(lines[i][0] * lines[i][3] - lines[i][1] * lines[i][2]);
}
*/
//A0 = (float)(Connor[0].y - Connor[1].y);
//B0 = (float)(Connor[1].x - Connor[0].x);
//C0 = (float)(Connor[0].x*Connor[1].y - Connor[0].y*Connor[1].x);
fangcheng_temp.push_back(A0);
fangcheng_temp.push_back(B0);
fangcheng_temp.push_back(C0);
// fangcheng_PARAM.push_back(fangcheng_temp);
}
return fangcheng_temp;
}
/*******************************************************************************************
*函数功能 ： 输入两条直线（每条直线以斜率和截距确定），返回两直线夹角，0为弧度，1为角度（不能计算垂直直线）
*输入参数 ： line_1_k为一条直线斜率，line_2_k为另一条直线斜率，aaa为0则为弧度，反之则为角度
*返 回 值 ： float型弧度或者角度,有正负，为直线2相对于直线1的角度
*编写时间 ： 2018.8.5
*作 者 ： 毛哥
********************************************************************************************/
float get_lines_arctan(float line_1_k, float line_2_k, int aaa)
{
if (aaa == 0)
{
float tan_k = 0; //直线夹角正切值
float lines_arctan;//直线斜率的反正切值
tan_k = (line_2_k - line_1_k) / (1 + line_2_k*line_1_k); //求直线夹角的公式
lines_arctan = atan(tan_k);
return lines_arctan;
}
else
{
float tan_k = 0; //直线夹角正切值
float lines_arctan;//直线斜率的反正切值
tan_k = (line_2_k - line_1_k) / (1 + line_2_k*line_1_k); //求直线夹角的公式
lines_arctan = atan(tan_k)* 180.0 / 3.1415926;
return lines_arctan;
}
}
/*******************************************************************************************
*函数功能 ： 输入两条直线(输入一般式三个参数)，返回两直线夹角，0为弧度
*输入参数 ： Param_A1、Param_B1、Param_C1 \Param_2、Param_B2、Param_C2输入一般式三个参数
*返 回 值 ： double 型角度,有正负，为直线2相对于直线1的角度
*编写时间 ： 2018.8.6
*作 者 ： 毛哥
********************************************************************************************/
double get_lines_yibanshi_arctan(double Param_A1, double Param_B1, double Param_C1, double Param_A2, double Param_B2, double Param_C2)
{
double tan_k = 0; //直线夹角正切值
double lines_arctan;//直线斜率的反正切值
double line_1_k = 0;
double line_2_k = 0;
// tan_k = (line_2_k - line_1_k) / (1 + line_2_k*line_1_k); //求直线夹角的公式
if (fabs(Param_B1) > 0.00001 && fabs(Param_B2) > 0.00001)
{
line_1_k = -Param_A1 / Param_B1;
line_2_k = -Param_A2 / Param_B2;
//若两直线垂直，即k1k2 = －1，此时夹角为90°；
if (line_1_k*line_2_k != -1)
{
tan_k = (line_2_k - line_1_k) / (1 + line_2_k*line_1_k); //求直线夹角的公式
//tan_k = (Param_A1*Param_A2 + Param_B1*Param_B2) / (sqrt(Param_A1*Param_A1 + Param_B1*Param_B1)*sqrt(Param_A2*Param_A2 + Param_B2*Param_B2));
lines_arctan = atan(tan_k)* 180.0 / 3.1415926;;
}
}
else if (Param_B1 < 0.00001&& Param_B2>0.00001)
{
tan_k = -Param_A2 / Param_B2;
lines_arctan = atan(tan_k);
lines_arctan = (3.1415926 / 2 - lines_arctan)* 180.0 / 3.1415926;
}
else if (Param_B1 > 0.00001&&Param_B2 < 0.00001)
{
tan_k = -Param_A1 / Param_B1;
lines_arctan = atan(tan_k);
lines_arctan = (lines_arctan - 3.1415926 / 2)* 180.0 / 3.1415926;
}
else
{
lines_arctan = 0;
}
return lines_arctan;
}
//输入一直线的两个端点，返回该直线的一般式方程
vector <float> get_lines_yibanshi_fangcheng_2(Point2f Point_A, Point2f Point_B)
{
//1.分别求角点0、1与角点2、3所对应的两根直线
//vector <vector<float>> fangcheng_PARAM;
vector<float> fangcheng_temp;
float A0 = 0, B0 = 0, C0 = 0;
//A0 = (float)(lines[i][1] - lines[i][3]);
//B0 = (float)(lines[i][2] - lines[i][0]);
//C0 = (float)(lines[i][0] * lines[i][3] - lines[i][1] * lines[i][2]);
A0 = (float)(Point_A.y - Point_B.y);
B0 = (float)(Point_B.x - Point_A.x);
C0 = (float)(Point_A.x*Point_B.y - Point_A.y*Point_B.x);
fangcheng_temp.push_back(A0);
fangcheng_temp.push_back(B0);
fangcheng_temp.push_back(C0);
//fangcheng_PARAM.push_back(fangcheng_temp);
return fangcheng_temp;
}
/***** 7、输入A，B，C,画出线在dstImage*****/
void draw_line_1(float A, float B, float C, Mat dstImage)
{
//Ax+By+C=0
float y0, y1, x0, x1;
if (B == 0)
{
B = 0.001;
}
if (A == 0)
{
A = 0.001;
}
float k = 0, b = 0;
k = -A / B;
b = -C / B;
y0 = k * 0.0 + b;
y1 = k * 800.0 + b;
x0 = (0.0 - b) / k;
x1 = (600.0 - b) / k;
vector<Point> draw_point;
if (y0 >= 0 && y0 <= 600)
{
Point p;
p.x = 0;
p.y = y0;
draw_point.push_back(p);
}
if (y1 >= 0 && y1 <= 600)
{
Point p;
p.x = 800;
p.y = y1;
draw_point.push_back(p);
}
if (x0 >= 0 && x0 <= 800)
{
Point p;
p.x = x0;
p.y = 0;
draw_point.push_back(p);
}
if (x1 >= 0 && x1 <= 800)
{
Point p;
p.x = x1;
p.y = 600;
draw_point.push_back(p);
}
if (draw_point.size() >= 2)
{
line(dstImage, draw_point[0], draw_point[1], Scalar(0, 0, 255), 2, LINE_AA);
}
}
/*******************************************************************************************
*函数功能 ： 找每个轮廓的中心坐标
*输入参数 ：轮廓或者凸包
*返 回 值 ： 点向量
*编写时间 ： 2018.8.9
*作 者 ： 毛哥
********************************************************************************************/
vector< Point2f> find_lunkuo_zhongxin(vector<vector<Point>> dangban_RectContours)
{
vector<Point2f> zhongxin_zuobiao;
/// 计算矩
vector<Moments> mu(dangban_RectContours.size());
for (int i = 0; i < dangban_RectContours.size(); i++)
{
mu[i] = moments(dangban_RectContours[i], false);
}
/// 计算中心矩:
vector<Point2f> mc(dangban_RectContours.size());
for (int i = 0; i < dangban_RectContours.size(); i++)
{
mc[i] = Point2f(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
zhongxin_zuobiao.push_back(mc[i]);
}
return zhongxin_zuobiao;
}
/*函数功能：求两条直线交点*/
/*输入：两条Vec4i类型直线*/
/*返回：Point2f类型的点*/
Point2f getCrossPoint(Vec4i LineA, Vec4i LineB)
{
double ka, kb;
Point2f crossPoint;
if ((LineA[2] - LineA[0]) != 0)
{
ka = (double)(LineA[3] - LineA[1]) / (double)(LineA[2] - LineA[0]); //求出LineA斜率
kb = (double)(LineB[3] - LineB[1]) / (double)(LineB[2] - LineB[0]); //求出LineB斜率
crossPoint.x = (ka*LineA[0] - LineA[1] - kb*LineB[0] + LineB[1]) / (ka - kb);
crossPoint.y = (ka*kb*(LineA[0] - LineB[0]) + ka*LineB[1] - kb*LineA[1]) / (ka - kb);
}
return crossPoint;
}
//两点之间距离
float getDistance_1(CvPoint pointO, CvPoint pointA)
{
float distance;
distance = powf((pointO.x - pointA.x), 2) + powf((pointO.y - pointA.y), 2);
distance = sqrtf(distance);
return distance;
}
/*******************************************************************************************
*函数功能 ： 用于顶部摄像头拟合出横着或者竖着的直线（大体）
*输入参数 ： 图片，0为横着，1为竖着
*返 回 值 ： 单条直线的ABC值，
*编写时间 ： 2018.8.12
*作 者 ： 毛哥
********************************************************************************************/
vector <float> nihe_zhixian_dantiao(int flag)
{
vector <float> fangcheng_PARAM_1;
vector <float> fangcheng_PARAM_final;
double Param_A1[4] = { 0 };
double Param_B1[4] = { 0 };
double Param_C1[4] = { 0 };
int lvbo = 0;
Mat frame_2;
while (1)
{
frame_2 = imgOriginal_up;
Mat srcImg1, midImage, dstImage;//临时变量和目标图的定义
int width = frame_2.cols; //宽
int height = frame_2.rows;
Mat image_copy_C3;
if (flag == 0)//横着
{
Rect rect(0, frame_2.rows*0.3, frame_2.cols, frame_2.rows*(0.95 - 0.3)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
Mat image_cut = Mat(frame_2, rect);
image_copy_C3 = image_cut.clone(); //clone函数创建新的图片
}
else //竖着
{
Rect rect(frame_2.cols*0.075, frame_2.rows*0.2, frame_2.cols*(0.875 - 0.075), frame_2.rows*(0.83 - 0.2)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
Mat image_cut = Mat(frame_2, rect);
image_copy_C3 = image_cut.clone(); //clone函数创建新的图片
}
//从img中按照rect进行切割，此时修改image_cut时image中对应部分也会修改，因此需要copy
midImage = Mat::zeros(image_copy_C3.size(), CV_8UC3);
//再次通过找轮廓，然后拟合出直线，通过斜率滤除剩下四条数线（或横线），然后通过长度滤除挡板的边缘直线，剩下白色过道的直线方程，一切问题得到解决。
cvtColor(image_copy_C3, srcImg1, CV_BGR2GRAY);//灰度化
//【3】srcImage取大于阈值119的那部分
srcImg1 = srcImg1 > G_threshold_up;
//namedWindow("二值化的图", 0);
//imshow("二值化的图", srcImg1);
// midImage=findMaxContouns(srcImg1);
/******* 检测直线优化 开始 ****************************************************************/
/*****************************用找轮廓代替边缘检测************************************************/
vector<vector<Point>>contours_1, RectContours; //轮廓
findContours(srcImg1, contours_1, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//找轮廓
if (contours_1.size() == 0)
{
cout << "警告：没有在裁剪后的图片里找到轮廓（第二次找轮廓）" << endl;
continue;
}
if (contours_1.size() > 0)//如果没有找到轮廓退出
{
vector<float> length(contours_1.size());
for (int i = 0; i < contours_1.size(); i++)
{//历遍所有的轮廓
length[i] = arcLength(contours_1[i], true);
}
if (contours_1.size() == 0)
{
cout << "警告：没有（第一次找轮廓）" << endl;
continue;
}
for (int i = 0; i < contours_1.size(); i++)
{
Scalar color = (255, 255, 255);//白色线画轮廓
drawContours(midImage, contours_1, i, color, 1, 8);//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
}
}
cvtColor(midImage, midImage, CV_BGR2GRAY);//灰度化
midImage = midImage>20;
vector<Vec4i> lines, lines_final;//定义一个矢量结构lines用于存放得到的线段矢量集合
HoughLinesP(midImage, lines, 1, CV_PI / 180, 95, 40, 60);
if (lines.size() == 0)
{
cout << "没有检测到直线" << endl;
continue;
}
/******* 检测直线优化 结束 ****************************************************************/
cvtColor(midImage, dstImage, COLOR_GRAY2BGR);//转化边缘检测后的图为灰度图
//画出挡板中轴线
//cout << "\n共检测到原始 直线" << lines.size() << "条" << endl;
//【4】依次计算出直线长度
float zhixian_changdu;
float to_0_distance;
for (size_t i = 0; i < lines.size(); i++)
{
Vec4i l = lines[i];
to_0_distance = getDist_P2L_zhengfu(CvPoint(0, 0), Point(l[0], l[1]), Point(l[2], l[3]));
zhixian_changdu = getDistance_1(Point(l[0], l[1]), Point(l[2], l[3]));
// cout << "\n直线长度为" << zhixian_changdu << endl;
// cout << "到原点的距离" << to_0_distance << endl;
//画出原始直线
line(image_copy_C3, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(0, 255, 0), 2, LINE_AA);
float line_k;
int a;
a = lines[i][2] - lines[i][0];
if (flag == 0)//横着
{
if (fabs((double)(lines[i][2] - lines[i][0])) > 0.0001)
{
line_k = (double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
// cout << "\n直线的斜率为\n" << line_k << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
if (zhixian_changdu > 120 && fabs(line_k)<0.6&&fabs(to_0_distance)>20)
{
lines_final.push_back(lines[i]);
}
}
}
else //竖着
{
if (fabs((double)(lines[i][2] - lines[i][0])) != 0)
{
line_k = (double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
// cout << "\n直线的斜率为\n" << line_k << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
if (zhixian_changdu >80 && fabs(to_0_distance)>30/* && (lines[i][1]<100 || lines[i][3]<100) */ && fabs(line_k)>1.7)
{
lines_final.push_back(lines[i]);
}
else if (fabs(line_k) > 30 && zhixian_changdu >60 && to_0_distance>110)
{
lines_final.push_back(lines[i]);
}
}
else
{
if (zhixian_changdu>60 && fabs(to_0_distance)>110 /*&& (lines[i][1] < 100 || lines[i][3] < 100)*/)
{
lines_final.push_back(lines[i]);
}
}
}
}
//namedWindow("直线未滤除前", 0);
//imshow("直线未滤除前", image_copy_C3);
//waitKey(1);
float A_Proportion, B_Proportion, C_Proportion;
//最终只能检测出两条直线
int zhixian_geshu = lines_final.size();
cout << "共有" << zhixian_geshu << "条直线进来了" << endl;
if (lines_final.size() >= 2)
{
//先求最终直线的方程的三个参数 A_top, B_bottom
fangcheng_PARAM_1 = get_lines_yibanshi_fangcheng(lines_final);
//将相同直线过滤为一条
if (lines_final.size() >= 3)
{
for (int j = 0; j <zhixian_geshu - 1; j++)
{
for (int k = j + 1; k <zhixian_geshu; k++)
{
A_Proportion = fangcheng_PARAM_1[j + 0] / fangcheng_PARAM_1[k + 0];
B_Proportion = fangcheng_PARAM_1[j + 1] / fangcheng_PARAM_1[k + 1];
C_Proportion = fangcheng_PARAM_1[j + 2] / fangcheng_PARAM_1[k + 2];
if ((A_Proportion > 0.75&&A_Proportion<1.3) && (B_Proportion>0.7 && B_Proportion<1.3) && (C_Proportion>0.7 && C_Proportion < 1.4))
{
cout << "第 " << j << "条直线" << "和" << k << "条直线相同" << endl;
fangcheng_PARAM_1[j + 0] = (fangcheng_PARAM_1[j + 0] + fangcheng_PARAM_1[k + 0]) / 2;
fangcheng_PARAM_1[j + 1] = (fangcheng_PARAM_1[j + 1] + fangcheng_PARAM_1[k + 0]) / 2;
fangcheng_PARAM_1[j + 2] = (fangcheng_PARAM_1[j + 2] + fangcheng_PARAM_1[k + 0]) / 2;
}
}
}
}
//【4】依次在图中绘制出每条线段
for (size_t i = 0; i < lines_final.size(); i++)
{
Vec4i l = lines_final[i];
line(dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(0, 255, 0), 1, LINE_AA);
}
// 滤波
Param_A1[3] = Param_A1[2];
Param_A1[2] = Param_A1[1];
Param_A1[1] = Param_A1[0];
Param_B1[3] = Param_B1[2];
Param_B1[2] = Param_B1[1];
Param_B1[1] = Param_B1[0];
Param_C1[3] = Param_C1[2];
Param_C1[2] = Param_C1[1];
Param_C1[1] = Param_C1[0];
Param_A1[0] = ((fabs(fangcheng_PARAM_1[0]) + fabs(fangcheng_PARAM_1[3])) / 2.0) * (fangcheng_PARAM_1[0] / fabs(fangcheng_PARAM_1[0]));
Param_B1[0] = ((fabs(fangcheng_PARAM_1[1]) + fabs(fangcheng_PARAM_1[4])) / 2.0) *(fangcheng_PARAM_1[1] / fabs(fangcheng_PARAM_1[1]));
Param_C1[0] = ((fabs(fangcheng_PARAM_1[2]) + fabs(fangcheng_PARAM_1[5])) / 2.0) *(fangcheng_PARAM_1[2] / fabs(fangcheng_PARAM_1[2]));
Param_A1[0] = 0.6* Param_A1[0] + 0.2* Param_A1[1] + 0.1* Param_A1[2] + 0.1* Param_A1[3];
Param_B1[0] = 0.6* Param_B1[0] + 0.2* Param_B1[1] + 0.1* Param_B1[2] + 0.1* Param_B1[3];
Param_C1[0] = 0.6* Param_C1[0] + 0.2* Param_C1[1] + 0.1* Param_C1[2] + 0.1* Param_C1[3];
float a1_bizhi;
if (Param_A1[0] != 0)
{
a1_bizhi = Param_A1[1] / Param_A1[0];
}
else
{
a1_bizhi = 1;
}
float a2_bizhi = Param_B1[1] / Param_B1[0];
float a3_bizhi = Param_C1[1] / Param_C1[0];
if ((a1_bizhi>0.9 || a1_bizhi<1.1) && (a2_bizhi>0.9 || a2_bizhi<1.1) && (a3_bizhi>0.9 || a3_bizhi<1.1))
{
lvbo++;
}
else /*if (lvbo>0)*/
{
lvbo=0;
}
if (lvbo>10)
{
fangcheng_PARAM_final.push_back(Param_A1[0]);
fangcheng_PARAM_final.push_back(Param_B1[0]);
fangcheng_PARAM_final.push_back(Param_C1[0]);
cout << "已拟合出直线方程" << endl;
draw_line_1(Param_A1[0], Param_B1[0], Param_C1[0], dstImage);
namedWindow("【检测直线效果图】", 0);//参数为零，则可以自由拖动
imshow("【检测直线效果图】", dstImage);
waitKey(1);
break;
}
/***** 7、输入A，B，C,画出线在dstImage*****/
}
else
{
//如果没有正确找到直线，就返回-1，函数外边再判断
// cout << "没有正确的找到最终的两条直线" << endl;
continue;
}
}
return fangcheng_PARAM_final;
}
/*******************************************************************************************
*函数功能 ： 用于顶部摄像头找横竖两条直线交点
*输入参数 ： 图片
*返 回 值 ： 点向量,如果直线没有正确检测到，返回0，0点，方便后面判断
*编写时间 ： 2018.8.12
*作 者 ： 毛哥
********************************************************************************************/
Point2f nihe_jiaodian()
{
Point2f final_jiaodian;
vector <float> fangcheng_PARAM_1, fangcheng_PARAM_2;
double Param_A1, Param_B1, Param_C1;//过道直线
double Param_A2, Param_B2, Param_C2;//挡板中轴线
fangcheng_PARAM_1 = nihe_zhixian_dantiao(0);
Param_A1 = fangcheng_PARAM_1[0];
Param_B1 = fangcheng_PARAM_1[1];
Param_C1 = fangcheng_PARAM_1[2];
fangcheng_PARAM_2 = nihe_zhixian_dantiao(1);
Param_A2 = fangcheng_PARAM_2[0];
Param_B2 = fangcheng_PARAM_2[1];
Param_C2 = fangcheng_PARAM_2[2];
final_jiaodian.y = (Param_A1*Param_C2 + Param_A2*Param_C1) / (Param_A2*Param_B1 - Param_A1*Param_B2);
final_jiaodian.x = (Param_C1 - Param_B1*final_jiaodian.y) / Param_A1;
//画出来
//circle(image, final_jiaodian, 2, Scalar(0, 0, 255), -1, 8);
return final_jiaodian;
}
/*******************************************************************************************
*函数功能 ： 用于顶部摄像头放置横着的挡板,此时顶部摄像头已经找到
*输入参数 ：第一个参数是 0为横着，1为竖着，第一个参数是 1为第一块挡板，2为第二块挡板，
*返 回 值 ： 单条直线的ABC值，
*编写时间 ： 2018.8.12
*作 者 ： 毛哥
********************************************************************************************/
void Put_dangban(int hengshu, int yier)
{
double Param_A1, Param_B1, Param_C1;//过道直线
double Param_A2, Param_B2, Param_C2;//挡板中轴线
//vector <float> fangcheng_PARAM_1 = nihe_zhixian_dantiao(hengshu);
vector <float> fangcheng_PARAM_1 = getVHlineParam(hengshu);
Param_A1 = fangcheng_PARAM_1[0];
Param_B1 = fangcheng_PARAM_1[1];
Param_C1 = fangcheng_PARAM_1[2];
if (yier == 1)
{
Send_to_K60(10, 0, 0);//将第一块挡板放在悬空位置（离地2cm）
Sleep(1200);
}
else
{
Send_to_K60(11, 0, 0);//将第二块挡板放在悬空位置（离地2cm）
Sleep(2000);
}
//实时找红色挡板
//printf("\n\n\t\t\t 实时找红色挡板开始\n");
int jiaodu_jishu = 0;
Mat frame_2;
while (1)
{
Mat srcImage0;
frame_2 = imgOriginal_up;
int width = frame_2.cols;
int height = frame_2.rows;
Mat image_cut; //从img中按照rect进行切割，此时修改image_cut时image中对应部分也会修改，因此需要copy
Mat image_copy_C3; //clone函数创建新的图片 彩色
if (hengshu == 0)//横着
{
Rect rect(0, frame_2.rows*0.3, frame_2.cols, frame_2.rows*(0.95 - 0.3)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
Mat image_cut = Mat(frame_2, rect);
image_copy_C3 = image_cut.clone(); //clone函数创建新的图片
}
else //竖着
{
Rect rect(frame_2.cols*0.075, frame_2.rows*0.2, frame_2.cols*(0.875 - 0.075), frame_2.rows*(0.83 - 0.2)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
Mat image_cut = Mat(frame_2, rect);
image_copy_C3 = image_cut.clone(); //clone函数创建新的图片
}
int x, y;
double B = 0.0, G = 0.0, R = 0.0, H = 0.0, S = 0.0, V = 0.0;
int width1 = image_copy_C3.cols * 3;
int height1 = image_copy_C3.rows;
for (x = 0; x < height1; x++)
{
uchar* data = image_copy_C3.ptr<uchar>(x);//获取第i行的首地址
for (y = 0; y < width1; y += 3)
{
B = data[y];
G = data[y + 1];
R = data[y + 2];
RGB2HSV(R, G, B, H, S, V);
//红色范围，范围参考的网上。可以自己调
// if ((H >= 286 && H <= 360 || H >= 0 && H <= 20) && (S >= 0 && S <= 360) && (V>0 && V < 360))
if ((H >= 270 && H <= 360 || H >= 0 && H <= 20) && (S >= 0 && S <= 360) && (V>0 && V < 360))
data[y] = data[y + 1] = data[y + 2] = 255;
else data[y] = data[y + 1] = data[y + 2] = 0;
}
}
Mat vec_rgb;
cvtColor(image_copy_C3, vec_rgb, CV_BGR2GRAY);//灰度化
//【3】srcImage取大于阈值119的那部分
vec_rgb = vec_rgb > 120;
//namedWindow("hsv空间图像", 0);
//imshow("hsv空间图像", vec_rgb);
//waitKey(1);
//Mat element = getStructuringElement(MORPH_ELLIPSE, Size(2 * 1 + 1, 2 * 1 + 1), Point(1, 1));
Mat element1 = getStructuringElement(MORPH_ELLIPSE, Size(2 * 3 + 1, 2 * 3 + 1), Point(3, 3));
// Mat element1 = getStructuringElement(MORPH_RECT, Size(45, 45));
dilate(vec_rgb, vec_rgb, element1);//膨胀
/*namedWindow("膨胀", 0);
imshow("膨胀", vec_rgb);
waitKey(1);*/
erode(vec_rgb, vec_rgb, element1);//腐蚀
/*namedWindow("腐蚀", 0);
imshow("腐蚀", vec_rgb);
waitKey(1);*/
vector<vector<Point>>contours, max_contours; //轮廓
vector<Vec4i> hierarchy;//分层
Mat drawing_text = Mat::zeros(vec_rgb.size(), CV_8UC3);
findContours(vec_rgb, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//找轮廓
if (contours.size() == 0)
{
cout << "警告：没有红色挡板（第一次找轮廓）" << endl;
continue;
}
// cout << "第一次找轮廓结束，共找到轮廓：" << contours.size() << " 条" << endl;
vector<vector<Point>> hull(contours.size());//用于存放凸包
vector<float> length(contours.size());
vector<float> Area_contours(contours.size()), Area_hull(contours.size()), Rectangularity(contours.size()), circularity(contours.size());
vector< Point2f> lunkuo_zhongxin_first;
lunkuo_zhongxin_first = find_lunkuo_zhongxin(contours);
int Xmin = (int)(vec_rgb.cols * 0);
int Xmax = (int)(vec_rgb.cols);
int Ymin = (int)(vec_rgb.rows * 0);
int Ymax = (int)(vec_rgb.rows);
for (int i = 0; i < contours.size(); i++)
{//历遍所有的轮廓
length[i] = arcLength(contours[i], true);
//cout << "轮廓长度为" << length[i] << endl;
if (length[i] >5 && lunkuo_zhongxin_first[i].x>Xmin&&lunkuo_zhongxin_first[i].x<Xmax&&lunkuo_zhongxin_first[i].y>Ymin&&lunkuo_zhongxin_first[i].y<Ymax)
{//通过长度匹配滤除小轮廓
convexHull(Mat(contours[i]), hull[i], false);//把凸包找出来
max_contours.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组
}
}
if (max_contours.size() == 0)
{
cout << "警告：y有红色物体，但是可能没有红色挡板，或者红色挡板被遮挡严重" << endl;
continue;
}
//将线连接起来
vector<Point2f> zhongxin_zuobiao;
zhongxin_zuobiao = find_lunkuo_zhongxin(max_contours);
if (zhongxin_zuobiao.size() >= 2)
{
for (int i = 0; i < zhongxin_zuobiao.size() - 1; i++)
{
for (int j = i + 1; j < zhongxin_zuobiao.size(); j++)
{
float dis_juxin = getDistance_1(zhongxin_zuobiao[i], zhongxin_zuobiao[j]);
if (dis_juxin <65)
{
line(vec_rgb, zhongxin_zuobiao[i], zhongxin_zuobiao[j], Scalar(255), 4, LINE_AA);
}
}
}
}
//namedWindow("连接直线后", 0);
//imshow("连接直线后", vec_rgb);
//waitKey(1);
Mat vec_rgb1;
dilate(vec_rgb, vec_rgb1, element1);//膨胀
namedWindow("膨胀1", 0);
imshow("膨胀1", vec_rgb1);
waitKey(1);
erode(vec_rgb1, vec_rgb1, element1);//腐蚀
//namedWindow("腐蚀1", 0);
//imshow("腐蚀1", vec_rgb1);
//waitKey(1);
//第二次找轮廓
vector<vector<Point>>contours_1, RectContours; //轮廓
Mat drawing_1 = Mat(vec_rgb.size(), CV_8UC3, Scalar(0, 0, 0));
Mat dstImage = Mat(vec_rgb.size(), CV_8UC3, Scalar(0, 0, 0)); //clone函数创建新的图片
findContours(vec_rgb, contours_1, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//找轮廓
if (contours_1.size() == 0)
{
cout << "警告：没有在裁剪后的图片里找到轮廓（第二次找轮廓）" << endl;
continue;
}
//cout << "第二次找轮廓结束，共找到轮廓：" << contours_1.size() << " 条" << endl;
//cout << "第二次找轮廓结束" << endl;
vector< Point2f> lunkuo_zhongxin1, lunkuo_zhongxin_temp_first;
lunkuo_zhongxin_temp_first = find_lunkuo_zhongxin(contours_1);
if (contours_1.size() > 0)//如果没有找到轮廓退出
{
vector<vector<Point>> hull(contours_1.size());//用于存放凸包
vector<float> length(contours_1.size());
for (int i = 0; i < contours_1.size(); i++)
{//历遍所有的轮廓
length[i] = arcLength(contours_1[i], true);
if (length[i] >170 && lunkuo_zhongxin_temp_first[i].x>120 && lunkuo_zhongxin_temp_first[i].x<190 && lunkuo_zhongxin_temp_first[i].y>20 && lunkuo_zhongxin_temp_first[i].y<130)
{//通过长度匹配滤除小轮廓
convexHull(Mat(contours_1[i]), hull[i], false);//把凸包找出来
RectContours.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组
}
}
// cout << "已找到裁剪后的新凸包" << endl;
if (RectContours.size() == 0)
{
cout << "警告：没有红色挡板（第一次找轮廓）" << endl;
continue;
}
lunkuo_zhongxin1 = find_lunkuo_zhongxin(RectContours);
for (int i = 0; i < RectContours.size(); i++)
{
Scalar color = (255, 255, 255);//白色线画轮廓
drawContours(drawing_1, RectContours, i, color, 1, 8, hierarchy, 0, Point());//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
drawContours(dstImage, RectContours, i, color, 1, 8, hierarchy, 0, Point());//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
//画圆形
}
cvtColor(drawing_1, drawing_1, CV_BGR2GRAY);//灰度化
//【3】srcImage取大于阈值119的那部分
drawing_1 = drawing_1 > 20;
}
/******* 检测直线优化 开始 ****************************************************************/
Mat vec_rgb2;
vector<Vec4i> lines1, lines_final;//定义一个矢量结构lines用于存放得到的线段矢量集合
// Canny(drawing_1, vec_rgb2, cannyThreshold, cannyThreshold * factor);
HoughLinesP(drawing_1, lines1, 1, CV_PI / 180, 35, 30, 30);
if (lines1.size() == 0)
{
cout << "红色挡板直线数量为零" << endl;
continue;
}
//【4】依次计算出直线长度
float zhixian_changdu;
for (size_t i = 0; i < lines1.size(); i++)
{
Vec4i l = lines1[i];
zhixian_changdu = getDistance_1(Point(l[0], l[1]), Point(l[2], l[3]));
//cout << "\n红色挡板直线长度为" << zhixian_changdu << endl;
//画出原始直线
line(dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(0, 255, 0), 1, LINE_AA);
float line_k = 0;
int a;
a = lines1[i][2] - lines1[i][0];
if (hengshu == 0)//横着
{
if (fabs((double)(lines1[i][2] - lines1[i][0])) > 0.001)
{
line_k = (double)(lines1[i][3] - lines1[i][1]) / (double)(lines1[i][2] - lines1[i][0]); //求出直线的斜率
//cout << "\n红色挡板直线的斜率为\n" << line_k << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
if (zhixian_changdu > 40 /*&& fabs(line_k)<1*/)
{
lines_final.push_back(lines1[i]);
}
}
}
else //竖着
{
if (fabs((double)(lines1[i][2] - lines1[i][0])) > 0.0001)
{
line_k = (double)(lines1[i][3] - lines1[i][1]) / (double)(lines1[i][2] - lines1[i][0]); //求出直线的斜率
//cout << "\n红色挡板直线的斜率为\n" << line_k << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
if (zhixian_changdu > 40 && fabs(line_k)>0.7)
{
lines_final.push_back(lines1[i]);
}
}
else
{
if (zhixian_changdu>40)
{
lines_final.push_back(lines1[i]);
}
}
}
}
//namedWindow("挡板直线", 0);
//imshow("挡板直线", drawing_1);
//waitKey(2);
vector <float> fangcheng_PARAM_1;
//最终只能检测出两条直线
int zhixian_geshu = lines_final.size();
// cout << "共有" << zhixian_geshu << "条直线进来了" << endl;
float line_k[4] = { 0 };
float line_b[4] = { 0 };
if (lines_final.size() == 0)
{
cout << "红色挡板最终直线数量为零" << endl;
continue;
}
if (lines_final.size() >= 1)
{
int a, b = 0;
for (size_t i = 0; i < lines_final.size(); i++)
{
float line_k_temp;
a = lines_final[i][2] - lines_final[i][0];
// cout << "参数为： " << a << endl;
if (fabs((double)(lines_final[i][2] - lines_final[i][0])) > 0.001)
{
b++;
line_k_temp = (double)(lines_final[i][3] - lines_final[i][1]) / (double)(lines_final[i][2] - lines_final[i][0]); //求出直线的斜率
// cout << "\n最终直线的斜率为\n" << line_k_temp << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
line_k[0] = line_k_temp + line_k[0];
}
else
{
line_k[0] = 1000;
//曹，直线是个直的
// line_k = 8000;
}
}
line_k[3] = line_k[2];
line_k[2] = line_k[1];
line_k[1] = line_k[0];
//line_b[3] = line_b[2];
//line_b[2] = line_b[1];
//line_b[1] = line_b[0];
line_k[0] = line_k[0] / b;
if (fabs(line_k[1] / line_k[0]) >10)//去抖动
{
line_k[0] = line_k[1];
}
line_k[0] = 0.6* line_k[0] + 0.2* line_k[1] + 0.1* line_k[2] + 0.1* line_k[3];
//line_b[0] = lunkuo_zhongxin1[0].y - lunkuo_zhongxin1[0].x*line_k[0];
//line_b[0] = 0.35* line_b[0] + 0.3* line_b[1] + 0.25* line_b[2] + 0.1* line_b[3];
//
if (fabs(line_k[0])<50)
{
Param_A2 = -line_k[0];
Param_B2 = 1;
Param_C2 = line_k[0] * lunkuo_zhongxin1[0].x - lunkuo_zhongxin1[0].y;
}
else
{
Param_A2 = -line_k[0];
Param_B2 = 0.0001;
Param_C2 = line_k[0] * lunkuo_zhongxin1[0].x - lunkuo_zhongxin1[0].y;
}
//Pt2.x = lunkuo_zhongxin1[0].x + 50;
//Pt2.y = 50 * line_k[0] + lunkuo_zhongxin1[0].y;
// line(drawing_1, lunkuo_zhongxin1[0], Pt2, Scalar(0, 0, 255), 4, LINE_AA);
draw_line_1(Param_A2, Param_B2, Param_C2, dstImage);
draw_line_1(Param_A1, Param_B1, Param_C1, dstImage);
}
float theta, shuiping_diatance;
//fangcheng_PARAM_2 = get_lines_yibanshi_fangcheng_2((Point2f)lunkuo_zhongxin1[0], (Point2f)Pt2);
//Param_A2 = fangcheng_PARAM_2[0];
//Param_B2 = fangcheng_PARAM_2[1];
//Param_C2 = fangcheng_PARAM_2[2];
/***** 7、输入A，B，C,画出线在dstImage*****/
//最终求夹角（旋转误差）和平移误差
//两直线夹角，挡板相对于白色过道的夹角，因此白色过道直线斜率为第一个参数
theta = get_lines_yibanshi_arctan(Param_A1, Param_B1, Param_C1, Param_A2, Param_B2, Param_C2);
//点到直线距离
//P为线外一点，AB为线段两个端点
shuiping_diatance = getDist_P2L_zhengfu_2(lunkuo_zhongxin1[0], Param_A1, Param_B1, Param_C1,1);
cout << "\n最终结果\n挡板中轴线和白色过道中轴线夹角为： " << theta << " 度" << endl;
cout << "挡板中点与白色过道中轴线误差为： " << shuiping_diatance << endl;
//namedWindow("直线未滤除前", 0);
//imshow("直线未滤除前", image_copy_C3);
//waitKey(1);
namedWindow("【挡板夹角和位移示意图】", 0);//参数为零，则可以自由拖动
imshow("【挡板夹角和位移示意图】", dstImage);
waitKey(1);
float error1 = 0, error2 = 0;
error1 = theta*1.3;
error2 = shuiping_diatance*1.4;//目标行数，291
error1 = error1 > 127 ? 127 : error1;
error1 = error1 < -127 ? -127 : error1;
error2 = error2 > 127 ? 127 : error2;
error2 = error2 < -127 ? -127 : error2;
if (fabs(error1) < 1 && fabs(error2) <1.5)
{
Send_to_K60(12 , 0 , 0);
jiaodu_jishu++;
if (jiaodu_jishu>6)
{
Send_to_K60(13, 0, 0);//将挡板放下并归右边位置
Sleep(1000);
cout << "\n放置挡板成功" << endl;
break;
}
}
else
{
Send_to_K60(12, (int)error1, (int)error2);
if (jiaodu_jishu>0)
jiaodu_jishu--;
}
}
}
/*******************************************************************************************
*函数功能 ： 用于顶部摄像头放置横着的挡板,此时顶部摄像头已经找到
*输入参数 ：第一个参数是 0为横着，1为竖着，第一个参数是 1为第一块挡板，2为第二块挡板，
*返 回 值 ： 单条直线的ABC值，
*编写时间 ： 2018.8.12
*作 者 ： 毛哥
********************************************************************************************/
void Put_dangban_heng(int hengshu, int yier)
{
double Param_A1, Param_B1, Param_C1;//过道直线
double Param_A2, Param_B2, Param_C2;//挡板中轴线
//vector <float> fangcheng_PARAM_1 = nihe_zhixian_dantiao(hengshu);
vector <float> fangcheng_PARAM_1 = getVHlineParam(hengshu);
Param_A1 = fangcheng_PARAM_1[0];
Param_B1 = fangcheng_PARAM_1[1];
Param_C1 = fangcheng_PARAM_1[2];
Send_to_K60(14, 0, 0);//将横挡板第一块挡板放在悬空位置（离地2cm）
Sleep(2000);
//实时找红色挡板
//printf("\n\n\t\t\t 实时找红色挡板开始\n");
int jiaodu_jishu = 0;
Mat frame_2;
while (1)
{
Mat srcImage0;
frame_2 = imgOriginal_up;
int width = frame_2.cols;
int height = frame_2.rows;
Mat image_copy_C3; //clone函数创建新的图片 彩色
if (hengshu == 0)//横着
{
Rect rect(0, frame_2.rows*0.3, frame_2.cols, frame_2.rows*(0.95 - 0.3)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
Mat image_cut = Mat(frame_2, rect);
image_copy_C3 = image_cut.clone(); //clone函数创建新的图片
}
else //竖着
{
Rect rect(frame_2.cols*0.075, frame_2.rows*0.2, frame_2.cols*(0.875 - 0.075), frame_2.rows*(0.83 - 0.2)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
Mat image_cut = Mat(frame_2, rect);
image_copy_C3 = image_cut.clone(); //clone函数创建新的图片
}
int x=0, y=0;
double B = 0.0, G = 0.0, R = 0.0, H = 0.0, S = 0.0, V = 0.0;
int width1 = image_copy_C3.cols * 3;
int height1 = image_copy_C3.rows;
for (x = 0; x < height1; x++)
{
uchar* data = image_copy_C3.ptr<uchar>(x);//获取第i行的首地址
for (y = 0; y < width1; y += 3)
{
B = data[y];
G = data[y + 1];
R = data[y + 2];
RGB2HSV(R, G, B, H, S, V);
//红色范围，范围参考的网上。可以自己调
if ((H >= 286 && H <= 360 || H >= 0 && H <= 20) && (S >= 0 && S <= 360) && (V>0 && V < 360))
data[y] = data[y + 1] = data[y + 2] = 255;
else data[y] = data[y + 1] = data[y + 2] = 0;
}
}
Mat vec_rgb;
cvtColor(image_copy_C3, vec_rgb, CV_BGR2GRAY);//灰度化
//【3】srcImage取大于阈值119的那部分
vec_rgb = vec_rgb > 120;
Mat element1 = getStructuringElement(MORPH_ELLIPSE, Size(2 * 3 + 1, 2 * 3 + 1), Point(3, 3));
// Mat element1 = getStructuringElement(MORPH_RECT, Size(45, 45));
dilate(vec_rgb, vec_rgb, element1);//膨胀
/*namedWindow("膨胀", 0);
imshow("膨胀", vec_rgb);
waitKey(1);*/
erode(vec_rgb, vec_rgb, element1);//腐蚀
/*namedWindow("腐蚀", 0);
imshow("腐蚀", vec_rgb);
waitKey(1);*/
vector<vector<Point>>contours, max_contours; //轮廓
vector<Vec4i> hierarchy;//分层
Mat drawing_text = Mat::zeros(vec_rgb.size(), CV_8UC3);
findContours(vec_rgb, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//找轮廓
if (contours.size() == 0)
{
cout << "警告：没有红色挡板（第一次找轮廓）" << endl;
continue;
}
// cout << "第一次找轮廓结束，共找到轮廓：" << contours.size() << " 条" << endl;
vector<vector<Point>> hull(contours.size());//用于存放凸包
vector<float> length(contours.size());
vector<float> Area_contours(contours.size()), Area_hull(contours.size()), Rectangularity(contours.size()), circularity(contours.size());
vector< Point2f> lunkuo_zhongxin_first;
lunkuo_zhongxin_first = find_lunkuo_zhongxin(contours);
int Xmin = (int)(vec_rgb.cols * 0);
int Xmax = (int)(vec_rgb.cols);
int Ymin = (int)(vec_rgb.rows * 0);
int Ymax = (int)(vec_rgb.rows);
for (int i = 0; i < contours.size(); i++)
{//历遍所有的轮廓
length[i] = arcLength(contours[i], true);
//cout << "轮廓长度为" << length[i] << endl;
if (length[i] >5 && lunkuo_zhongxin_first[i].x>Xmin&&lunkuo_zhongxin_first[i].x<Xmax&&lunkuo_zhongxin_first[i].y>Ymin&&lunkuo_zhongxin_first[i].y<Ymax)
{//通过长度匹配滤除小轮廓
convexHull(Mat(contours[i]), hull[i], false);//把凸包找出来
max_contours.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组
}
}
if (max_contours.size() == 0)
{
cout << "警告：y有红色物体，但是可能没有红色挡板，或者红色挡板被遮挡严重" << endl;
continue;
}
//将线连接起来
vector<Point2f> zhongxin_zuobiao;
zhongxin_zuobiao = find_lunkuo_zhongxin(max_contours);
if (zhongxin_zuobiao.size() >= 2)
{
for (int i = 0; i < zhongxin_zuobiao.size() - 1; i++)
{
for (int j = i + 1; j < zhongxin_zuobiao.size(); j++)
{
float dis_juxin = getDistance_1(zhongxin_zuobiao[i], zhongxin_zuobiao[j]);
if (dis_juxin <65)
{
line(vec_rgb, zhongxin_zuobiao[i], zhongxin_zuobiao[j], Scalar(255), 4, LINE_AA);
}
}
}
}
//namedWindow("连接直线后", 0);
//imshow("连接直线后", vec_rgb);
//waitKey(1);
Mat vec_rgb1;
dilate(vec_rgb, vec_rgb1, element1);//膨胀
namedWindow("膨胀1", 0);
imshow("膨胀1", vec_rgb1);
waitKey(1);
erode(vec_rgb1, vec_rgb1, element1);//腐蚀
//namedWindow("腐蚀1", 0);
//imshow("腐蚀1", vec_rgb1);
//waitKey(1);
//第二次找轮廓
vector<vector<Point>>contours_1, RectContours; //轮廓
Mat drawing_1 = Mat(vec_rgb.size(), CV_8UC3, Scalar(0, 0, 0));
Mat dstImage = Mat(vec_rgb.size(), CV_8UC3, Scalar(0, 0, 0)); //clone函数创建新的图片
findContours(vec_rgb, contours_1, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//找轮廓
if (contours_1.size() == 0)
{
cout << "警告：没有在裁剪后的图片里找到轮廓（第二次找轮廓）" << endl;
continue;
}
//cout << "第二次找轮廓结束，共找到轮廓：" << contours_1.size() << " 条" << endl;
//cout << "第二次找轮廓结束" << endl;
vector< Point2f> lunkuo_zhongxin1, lunkuo_zhongxin_temp_first;
lunkuo_zhongxin_temp_first = find_lunkuo_zhongxin(contours_1);
if (contours_1.size() > 0)//如果没有找到轮廓退出
{
vector<vector<Point>> hull(contours_1.size());//用于存放凸包
vector<float> length(contours_1.size());
for (int i = 0; i < contours_1.size(); i++)
{//历遍所有的轮廓
length[i] = arcLength(contours_1[i], true);
if (length[i] >170 && lunkuo_zhongxin_temp_first[i].x>0 && lunkuo_zhongxin_temp_first[i].x<140)
{//通过长度匹配滤除小轮廓
convexHull(Mat(contours_1[i]), hull[i], false);//把凸包找出来
RectContours.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组
}
}
// cout << "已找到裁剪后的新凸包" << endl;
if (RectContours.size() == 0)
{
cout << "警告：没有红色挡板（第一次找轮廓）" << endl;
continue;
}
lunkuo_zhongxin1 = find_lunkuo_zhongxin(RectContours);
if (lunkuo_zhongxin1.size() != 1)
{
cout << "警告：有两个红色挡板" << endl;
continue;
}
for (int i = 0; i < RectContours.size(); i++)
{
Scalar color = (255, 255, 255);//白色线画轮廓
drawContours(drawing_1, RectContours, i, color, 1, 8, hierarchy, 0, Point());//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
drawContours(dstImage, RectContours, i, color, 1, 8, hierarchy, 0, Point());//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
//画圆形
}
cvtColor(drawing_1, drawing_1, CV_BGR2GRAY);//灰度化
//【3】srcImage取大于阈值119的那部分
drawing_1 = drawing_1 > 20;
}
/******* 检测直线优化 开始 ****************************************************************/
Mat vec_rgb2;
vector<Vec4i> lines1, lines_final;//定义一个矢量结构lines用于存放得到的线段矢量集合
// Canny(drawing_1, vec_rgb2, cannyThreshold, cannyThreshold * factor);
HoughLinesP(drawing_1, lines1, 1, CV_PI / 180, 35, 30, 30);
if (lines1.size() == 0)
{
cout << "直线数量为零" << endl;
continue;
}
//【4】依次计算出直线长度
float zhixian_changdu;
for (size_t i = 0; i < lines1.size(); i++)
{
Vec4i l = lines1[i];
zhixian_changdu = getDistance_1(Point(l[0], l[1]), Point(l[2], l[3]));
//cout << "\n直线长度为" << zhixian_changdu << endl;
//画出原始直线
line(dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(0, 255, 0), 1, LINE_AA);
float line_k = 0;
int a;
a = lines1[i][2] - lines1[i][0];
if (hengshu == 0)//横着
{
if (fabs((double)(lines1[i][2] - lines1[i][0])) > 0.001)
{
line_k = (double)(lines1[i][3] - lines1[i][1]) / (double)(lines1[i][2] - lines1[i][0]); //求出直线的斜率
//cout << "\n直线的斜率为\n" << line_k << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
if (zhixian_changdu > 40 /*&& fabs(line_k)<1*/)
{
lines_final.push_back(lines1[i]);
}
}
}
else //竖着
{
if (fabs((double)(lines1[i][2] - lines1[i][0])) > 0.001)
{
line_k = (double)(lines1[i][3] - lines1[i][1]) / (double)(lines1[i][2] - lines1[i][0]); //求出直线的斜率
cout << "\n直线的斜率为\n" << line_k << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
if (zhixian_changdu > 40 && fabs(line_k)>0.7)
{
lines_final.push_back(lines1[i]);
}
}
else
{
if (zhixian_changdu>40)
{
lines_final.push_back(lines1[i]);
}
}
}
}
vector <float> fangcheng_PARAM_1;
//最终只能检测出两条直线
int zhixian_geshu = lines_final.size();
float line_k[4] = { 0 };
float line_b[4] = { 0 };
if (lines_final.size() == 0)
{
cout << "最终直线数量为零" << endl;
continue;
}
if (lines_final.size() >= 1)
{
int a, b = 0;
for (size_t i = 0; i < lines_final.size(); i++)
{
float line_k_temp;
a = lines_final[i][2] - lines_final[i][0];
if (fabs((double)(lines_final[i][2] - lines_final[i][0])) > 0.001)
{
b++;
line_k_temp = (double)(lines_final[i][3] - lines_final[i][1]) / (double)(lines_final[i][2] - lines_final[i][0]); //求出直线的斜率
// cout << "\n最终直线的斜率为\n" << line_k_temp << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
line_k[0] = line_k_temp + line_k[0];
}
else
{
line_k[0] = 1000;
//曹，直线是个直的
// line_k = 8000;
}
}
line_k[3] = line_k[2];
line_k[2] = line_k[1];
line_k[1] = line_k[0];
line_k[0] = line_k[0] / (float)b;
if (fabs(line_k[1] / line_k[0]) >8)//去抖动
{
line_k[0] = line_k[1];
}
line_k[0] = 0.6* line_k[0] + 0.2* line_k[1] + 0.1* line_k[2] + 0.1* line_k[3];
Param_A2 = -line_k[0];
Param_B2 = 1;
Param_C2 = line_k[0] * lunkuo_zhongxin1[0].x - lunkuo_zhongxin1[0].y;
draw_line_1(Param_A2, Param_B2, Param_C2, dstImage);
draw_line_1(Param_A1, Param_B1, Param_C1, dstImage);
}
float theta=0.0, shuiping_diatance=0.0;
/***** 7、输入A，B，C,画出线在dstImage******/
//最终求夹角（旋转误差）和平移误差
//两直线夹角，挡板相对于白色过道的夹角，因此白色过道直线斜率为第一个参数
theta = get_lines_yibanshi_arctan(Param_A1, Param_B1, Param_C1, Param_A2, Param_B2, Param_C2);
//点到直线距离
//P为线外一点，AB为线段两个端点
shuiping_diatance = getDist_P2L_zhengfu_2(lunkuo_zhongxin1[0], Param_A1, Param_B1, Param_C1,0);
cout << "\n最终结果\n挡板中轴线和白色过道中轴线夹角为： " << theta << " 度" << endl;
cout << "挡板中点与白色过道中轴线误差为： " << -shuiping_diatance << endl;
namedWindow("【挡板夹角和位移示意图】", 0);//参数为零，则可以自由拖动
imshow("【挡板夹角和位移示意图】", dstImage);
waitKey(1);
float error1, error2;
error1 = theta;
error2 =-shuiping_diatance*1.5;//目标行数，291
error1 = error1 > 127 ? 127 : error1;
error1 = error1 < -127 ? -127 : error1;
error2 = error2 > 127 ? 127 : error2;
error2 = error2 < -127 ? -127 : error2;
if (fabs(error1) < 1.1 && fabs(error2) <1.5)
{
jiaodu_jishu++;
if (jiaodu_jishu>5)
{
Send_to_K60(13, 0, 0);//将挡板放下并归右边位置
Sleep(2000);
cout << "\n放置第一块挡板成功" << endl;
break;
}
}
else if (jiaodu_jishu>0)
{
jiaodu_jishu--;
}
Send_to_K60(12, (int)error1, (int)error2);
}
Sleep(1000);
Send_to_K60(15, 0, 0);//将将横挡板第二块挡板放在悬空位置（离地2cm）
Sleep(2000);
Sleep(1000);
jiaodu_jishu = 0;
while (1)
{
Mat srcImage0;
Mat frame_2 = imgOriginal_up;
int width = frame_2.cols;
int height = frame_2.rows;
Mat image_cut; //从img中按照rect进行切割，此时修改image_cut时image中对应部分也会修改，因此需要copy
Mat image_copy_C3; //clone函数创建新的图片 彩色
if (hengshu == 0)//横着
{
Rect rect(0, frame_2.rows*0.3, frame_2.cols, frame_2.rows*(0.95 - 0.3)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
Mat image_cut = Mat(frame_2, rect);
image_copy_C3 = image_cut.clone(); //clone函数创建新的图片
}
else //竖着
{
Rect rect(frame_2.cols*0.075, frame_2.rows*0.2, frame_2.cols*(0.875 - 0.075), frame_2.rows*(0.83 - 0.2)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
Mat image_cut = Mat(frame_2, rect);
image_copy_C3 = image_cut.clone(); //clone函数创建新的图片
}
int x, y;
double B = 0.0, G = 0.0, R = 0.0, H = 0.0, S = 0.0, V = 0.0;
int width1 = image_copy_C3.cols * 3;
int height1 = image_copy_C3.rows;
for (x = 0; x < height1; x++)
{
uchar* data = image_copy_C3.ptr<uchar>(x);//获取第i行的首地址
for (y = 0; y < width1; y += 3)
{
B = data[y];
G = data[y + 1];
R = data[y + 2];
RGB2HSV(R, G, B, H, S, V);
//红色范围，范围参考的网上。可以自己调
if ((H >= 286 && H <= 360 || H >= 0 && H <= 20) && (S >= 0 && S <= 360) && (V>0 && V < 360))
data[y] = data[y + 1] = data[y + 2] = 255;
else data[y] = data[y + 1] = data[y + 2] = 0;
}
}
Mat vec_rgb;
cvtColor(image_copy_C3, vec_rgb, CV_BGR2GRAY);//灰度化
//【3】srcImage取大于阈值119的那部分
vec_rgb = vec_rgb > 120;
//Mat element = getStructuringElement(MORPH_ELLIPSE, Size(2 * 1 + 1, 2 * 1 + 1), Point(1, 1));
Mat element1 = getStructuringElement(MORPH_ELLIPSE, Size(2 * 3 + 1, 2 * 3 + 1), Point(3, 3));
// Mat element1 = getStructuringElement(MORPH_RECT, Size(45, 45));
dilate(vec_rgb, vec_rgb, element1);//膨胀
/*namedWindow("膨胀", 0);
imshow("膨胀", vec_rgb);
waitKey(1);*/
erode(vec_rgb, vec_rgb, element1);//腐蚀
/*namedWindow("腐蚀", 0);
imshow("腐蚀", vec_rgb);
waitKey(1);*/
vector<vector<Point>>contours, max_contours; //轮廓
vector<Vec4i> hierarchy;//分层
Mat drawing_text = Mat::zeros(vec_rgb.size(), CV_8UC3);
findContours(vec_rgb, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//找轮廓
if (contours.size() == 0)
{
cout << "警告：没有红色挡板（第一次找轮廓）" << endl;
continue;
}
vector<vector<Point>> hull(contours.size());//用于存放凸包
vector<float> length(contours.size());
vector<float> Area_contours(contours.size()), Area_hull(contours.size()), Rectangularity(contours.size()), circularity(contours.size());
// vector< Point2f> lunkuo_zhongxin_first;
// lunkuo_zhongxin_first = find_lunkuo_zhongxin(contours);
//int Xmin = (int)(vec_rgb.cols * 0);
//int Xmax = (int)(vec_rgb.cols);
//int Ymin = (int)(vec_rgb.rows * 0);
//int Ymax = (int)(vec_rgb.rows);
for (int i = 0; i < contours.size(); i++)
{//历遍所有的轮廓
length[i] = arcLength(contours[i], true);
//cout << "轮廓长度为" << length[i] << endl;
if (length[i] >5 /*&& lunkuo_zhongxin_first[i].x>Xmin&&lunkuo_zhongxin_first[i].x<Xmax&&lunkuo_zhongxin_first[i].y>Ymin&&lunkuo_zhongxin_first[i].y<Ymax*/)
{//通过长度匹配滤除小轮廓
convexHull(Mat(contours[i]), hull[i], false);//把凸包找出来
max_contours.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组
}
}
if (max_contours.size() == 0)
{
cout << "警告：y有红色物体，但是可能没有红色挡板，或者红色挡板被遮挡严重" << endl;
continue;
}
//将线连接起来
vector<Point2f> zhongxin_zuobiao;
zhongxin_zuobiao = find_lunkuo_zhongxin(max_contours);
if (zhongxin_zuobiao.size() >= 2)
{
for (int i = 0; i < zhongxin_zuobiao.size() - 1; i++)
{
for (int j = i + 1; j < zhongxin_zuobiao.size(); j++)
{
float dis_juxin = getDistance_1(zhongxin_zuobiao[i], zhongxin_zuobiao[j]);
if (dis_juxin < 65)
{
line(vec_rgb, zhongxin_zuobiao[i], zhongxin_zuobiao[j], Scalar(255), 4, LINE_AA);
}
}
}
}
//namedWindow("连接直线后", 0);
//imshow("连接直线后", vec_rgb);
//waitKey(1);
Mat vec_rgb1;
dilate(vec_rgb, vec_rgb1, element1);//膨胀
namedWindow("膨胀1", 0);
imshow("膨胀1", vec_rgb1);
waitKey(1);
erode(vec_rgb1, vec_rgb1, element1);//腐蚀
//namedWindow("腐蚀1", 0);
//imshow("腐蚀1", vec_rgb1);
//waitKey(1);
//第二次找轮廓
vector<vector<Point>>contours_1, RectContours; //轮廓
Mat drawing_1 = Mat(vec_rgb.size(), CV_8UC3, Scalar(0, 0, 0));
Mat dstImage = Mat(vec_rgb.size(), CV_8UC3, Scalar(0, 0, 0)); //clone函数创建新的图片
findContours(vec_rgb, contours_1, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//找轮廓
if (contours_1.size() == 0)
{
cout << "警告：没有在裁剪后的图片里找到轮廓（第二次找轮廓）" << endl;
continue;
}
vector< Point2f> lunkuo_zhongxin_temp, lunkuo_zhongxin1;
lunkuo_zhongxin_temp = find_lunkuo_zhongxin(contours_1);
if (contours_1.size() > 0)//如果没有找到轮廓退出
{
vector<vector<Point>> hull(contours_1.size());//用于存放凸包
vector<float> length(contours_1.size());
for (int i = 0; i < contours_1.size(); i++)
{//历遍所有的轮廓
length[i] = arcLength(contours_1[i], true);
if (length[i] >170 && lunkuo_zhongxin_temp[i].x>190 && lunkuo_zhongxin_temp[i].x<305)
{//通过长度匹配滤除小轮廓
convexHull(Mat(contours_1[i]), hull[i], false);//把凸包找出来
RectContours.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组
}
}
// cout << "已找到裁剪后的新凸包" << endl;
if (RectContours.size() == 0)
{
cout << "警告：没有红色挡板（第一次找轮廓）" << endl;
continue;
}
lunkuo_zhongxin1 = find_lunkuo_zhongxin(RectContours);
if (lunkuo_zhongxin1.size() != 1)
{
cout << "警告：有两个红色挡板" << endl;
continue;
}
for (int i = 0; i < RectContours.size(); i++)
{
Scalar color = (255, 255, 255);//白色线画轮廓
drawContours(drawing_1, RectContours, i, color, 1, 8, hierarchy, 0, Point());//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
drawContours(dstImage, RectContours, i, color, 1, 8, hierarchy, 0, Point());//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
//画圆形
}
cvtColor(drawing_1, drawing_1, CV_BGR2GRAY);//灰度化
//【3】srcImage取大于阈值119的那部分
drawing_1 = drawing_1 > 20;
}
/******* 检测直线优化 开始 ****************************************************************/
Mat vec_rgb2;
vector<Vec4i> lines1, lines_final;//定义一个矢量结构lines用于存放得到的线段矢量集合
// Canny(drawing_1, vec_rgb2, cannyThreshold, cannyThreshold * factor);
HoughLinesP(drawing_1, lines1, 1, CV_PI / 180, 35, 30, 30);
if (lines1.size() == 0)
{
cout << "直线数量为零" << endl;
continue;
}
// cout << "canny边缘检测阈值为:" << cannyThreshold << endl;
/******* 检测直线优化 结束 ****************************************************************/
// cout << "\n共检测到原始 直线" << lines1.size() << "条" << endl;
//【4】依次计算出直线长度
float zhixian_changdu;
for (size_t i = 0; i < lines1.size(); i++)
{
Vec4i l = lines1[i];
zhixian_changdu = getDistance_1(Point(l[0], l[1]), Point(l[2], l[3]));
//cout << "\n直线长度为" << zhixian_changdu << endl;
//画出原始直线
line(dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(0, 255, 0), 1, LINE_AA);
float line_k = 0;
int a;
a = lines1[i][2] - lines1[i][0];
// cout << "参数为： " << a << endl;
if (hengshu == 0)//横着
{
if (fabs((double)(lines1[i][2] - lines1[i][0])) > 0.001)
{
line_k = (double)(lines1[i][3] - lines1[i][1]) / (double)(lines1[i][2] - lines1[i][0]); //求出直线的斜率
//cout << "\n直线的斜率为\n" << line_k << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
if (zhixian_changdu > 40 /*&& fabs(line_k)<1*/)
{
lines_final.push_back(lines1[i]);
}
}
}
else //竖着
{
if (fabs((double)(lines1[i][2] - lines1[i][0])) > 0.001)
{
line_k = (double)(lines1[i][3] - lines1[i][1]) / (double)(lines1[i][2] - lines1[i][0]); //求出直线的斜率
//cout << "\n直线的斜率为\n" << line_k << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
if (zhixian_changdu > 40 && fabs(line_k)>0.7)
{
lines_final.push_back(lines1[i]);
}
}
else
{
if (zhixian_changdu>40)
{
lines_final.push_back(lines1[i]);
}
}
}
}
//namedWindow("挡板直线", 0);
//imshow("挡板直线", drawing_1);
//waitKey(2);
vector <float> fangcheng_PARAM_1;
//最终只能检测出两条直线
int zhixian_geshu = lines_final.size();
// cout << "共有" << zhixian_geshu << "条直线进来了" << endl;
float line_k[4] = { 0 };
float line_b[4] = { 0 };
if (lines_final.size() == 0)
{
cout << "最终直线数量为零" << endl;
continue;
}
if (lines_final.size() >= 2)
{
int a, b = 0;
for (size_t i = 0; i < lines_final.size(); i++)
{
float line_k_temp;
//a = lines_final[i][2] - lines_final[i][0];
// cout << "参数为： " << a << endl;
if (fabs((double)(lines_final[i][2] - lines_final[i][0])) > 0.001)
{
b++;
line_k_temp = (double)(lines_final[i][3] - lines_final[i][1]) / (double)(lines_final[i][2] - lines_final[i][0]); //求出直线的斜率
// cout << "\n最终直线的斜率为\n" << line_k_temp << endl;
//斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
//通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
line_k[0] = line_k_temp + line_k[0];
}
else
{
line_k[0] = 70;
//曹，直线是个直的
// line_k = 8000;
}
}
line_k[3] = line_k[2];
line_k[2] = line_k[1];
line_k[1] = line_k[0];
line_k[0] = line_k[0] / b;
line_k[0] = 0.6* line_k[0] + 0.2* line_k[1] + 0.1* line_k[2] + 0.1* line_k[3];
Param_A2 = -line_k[0];
Param_B2 = 1;
Param_C2 = line_k[0] * lunkuo_zhongxin1[0].x - lunkuo_zhongxin1[0].y;
draw_line_1(Param_A2, Param_B2, Param_C2, dstImage);
draw_line_1(Param_A1, Param_B1, Param_C1, dstImage);
}
float theta, shuiping_diatance;
/***** 7、输入A，B，C,画出线在dstImage*****/
//最终求夹角（旋转误差）和平移误差
//两直线夹角，挡板相对于白色过道的夹角，因此白色过道直线斜率为第一个参数
theta = get_lines_yibanshi_arctan(Param_A1, Param_B1, Param_C1, Param_A2, Param_B2, Param_C2);
//点到直线距离
//P为线外一点，AB为线段两个端点
shuiping_diatance = getDist_P2L_zhengfu_2(lunkuo_zhongxin1[0], Param_A1, Param_B1, Param_C1,0);
cout << "\n最终结果\n挡板中轴线和白色过道中轴线夹角为： " << theta << " 度" << endl;
cout << "挡板中点与白色过道中轴线误差为： " << shuiping_diatance << endl;
//namedWindow("直线未滤除前", 0);
//imshow("直线未滤除前", image_copy_C3);
//waitKey(1);
namedWindow("【挡板夹角和位移示意图】", 0);//参数为零，则可以自由拖动
imshow("【挡板夹角和位移示意图】", dstImage);
//waitKey(1);
int error1 = 0, error2 = 0;
error1 = theta;
error2 = shuiping_diatance;//目标行数，291
// error2 = 0;//目标行数，291
error1 = error1 > 127 ? 127 : error1;
error1 = error1 < -127 ? -127 : error1;
error2 = error2 > 127 ? 127 : error2;
error2 = error2 < -127 ? -127 : error2;
Send_to_K60(12, error1, error2);
if (fabs((double)error1) < 1 && fabs((double)error2) <1.5)
{
jiaodu_jishu++;
if (jiaodu_jishu>5)
{
Send_to_K60(13, 0, 0);//将挡板放下并归右边位置
cout << "\n放置挡板成功" << endl;
Sleep(1000);
break;
}
}
else if (jiaodu_jishu>0)
{
jiaodu_jishu--;
}
}
}
/*******************************************************************************************
*函数功能 ： 用于顶部摄像头放置横着或者竖着的挡板前的定位
*输入参数 ： 第一个参数为目标数字第二个参数是 0为横着，1为竖着，第三个参数是 1为第一块挡板，2为第二块挡板，
*返 回 值 ： 单条直线的ABC值，如果没有正确找到直线，就返回-1，函数外边再判断
*编写时间 ： 2018.8.13
*作 者 ： 毛哥（参考叶少及姚总）
********************************************************************************************/
void Dangban_dingwei(int num_obj, int mode, int cishu)
{
//if (num_obj<10)
//{
// Dangban_DingWei_Single_Num(num_obj, mode, cishu);
//}
//else
//{
// Dangban_DingWei_Double_Num(num_obj, mode, cishu);
//}
}
/*******************************************************************************************
*函数功能 ： 输入二值化图像，提取横竖线并拟合求出交点
*输入参数 ： 二值化图像
*返 回 值 ： 交点坐标
*编写时间 ： 2018.8.15
*作 者 ： Biger姚
********************************************************************************************/
Point findCrossPoint(Mat img)
{
Point CrossPoint;
vector<vector<Point>> contours;
vector<Vec4i>hierarchy;
Mat midImage, dstImage;
dstImage = findMaxContouns(img);
cvtColor(dstImage, midImage, COLOR_BGR2GRAY);
midImage = midImage > 2;
vector<Vec2f> lines;//定义一个矢量结构lines用于存放得到的线段矢量集合
HoughLines(midImage, lines, 1, CV_PI / 180, 50);
if (lines.size() > 0)
{//横竖线分类 求出两条横竖线后求交点
vector<Vec2f> lines_h;//定义一个矢量结构lines用于存放得到的线段矢量集合
vector<Vec2f> lines_v;//定义一个矢量结构lines用于存放得到的线段矢量集合
float rho_min_h = 10000, rho_max_h = -10000, rho_min_v = 10000, rho_max_v = -10000;
float theta_sum_v = 0, theta_sum_h = 0;
float theta;
float rho_h, theta_h, rho_v, theta_v;
Point pt1, pt2, pt3, pt4;
double a, b;
double x0, y0;
//#pragma omp parallel for
for (int i = 0; i < lines.size(); i++)
{
theta = lines[i][1];
if (theta >= 2.355 || theta <= 0.785)lines_v.push_back(lines[i]);//shu
else if (theta >= 0.785&&theta <= 2.355)lines_h.push_back(lines[i]);//heng
}
/*heng*/
for (size_t i = 0; i < lines_h.size(); i++)
{
rho_h = lines_h[i][0], theta_h = lines_h[i][1];
theta_sum_h += theta_h;
/*a = cos(theta_h), b = sin(theta_h);
x0 = a*rho_h, y0 = b*rho_h;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
line(src, pt1, pt2, Scalar(0, 0, 0), 2, LINE_AA);*/
if (rho_h >= rho_max_h)rho_max_h = rho_h;
if (rho_h <= rho_min_h)rho_min_h = rho_h;
}
rho_h = (rho_max_h + rho_min_h) / 2;
theta_h = theta_sum_h / lines_h.size();
a = cos(theta_h), b = sin(theta_h);
x0 = a*rho_h, y0 = b*rho_h;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
//line(src, pt1, pt2, Scalar(0, 0, 255), 2, LINE_AA);
//shu
for (size_t i = 0; i < lines_v.size(); i++)
{
rho_v = lines_v[i][0], theta_v = lines_v[i][1];
if (theta_v<0.6)
{
theta_sum_v += theta_v;
theta_sum_v += CV_PI;
}
else theta_sum_v += theta_v;
/*a = cos(theta_v), b = sin(theta_v);
x0 = a*rho_v, y0 = b*rho_v;
pt3.x = cvRound(x0 + 1000 * (-b));
pt3.y = cvRound(y0 + 1000 * (a));
pt4.x = cvRound(x0 - 1000 * (-b));
pt4.y = cvRound(y0 - 1000 * (a));
line(src, pt3, pt4, Scalar(0, 0, 0), 2, LINE_AA);*/
if (rho_v < 0)rho_v = -rho_v;
if (rho_v >= rho_max_v)rho_max_v = rho_v;
if (rho_v <= rho_min_v)rho_min_v = rho_v;
}
rho_v = (rho_max_v + rho_min_v) / 2;
theta_v = theta_sum_v / lines_v.size();
if (theta_v > CV_PI)theta_v -= CV_PI;
else rho_v = -rho_v;
a = cos(theta_v), b = sin(theta_v);
x0 = a*rho_v, y0 = b*rho_v;
pt3.x = cvRound(x0 + 1000 * (-b));
pt3.y = cvRound(y0 + 1000 * (a));
pt4.x = cvRound(x0 - 1000 * (-b));
pt4.y = cvRound(y0 - 1000 * (a));
//line(src, pt3, pt4, Scalar(0, 0, 255), 2, LINE_AA);
/*求直线交点*/
if (lines_v.size()>0 && lines_h.size() && lines_v.size())
{
float a1, b1, a2, b2;
if (pt1.x != pt2.x)a1 = (float)(pt1.y - pt2.y) / (float)(pt1.x - pt2.x);
else a1 = 1000;
b1 = (float)pt1.y - a1*(float)pt1.x;
if (pt3.x != pt4.x)a2 = (float)(pt3.y - pt4.y) / (float)(pt3.x - pt4.x);
else a2 = 1000;
b2 = (float)pt3.y - a2*(float)pt3.x;
CrossPoint.x = (b1 - b2) / (a2 - a1);
CrossPoint.y = a1 * (float)CrossPoint.x + b1;
}
}
return CrossPoint;
}
/*******************************************************************************************
*函数功能 ： 用于顶部摄像头放置横着或者竖着的挡板前的定位
*输入参数 ： 第一个参数 0为横着，1为竖着
*返 回 值 ： 无
*编写时间 ： 2018.8.15
*作 者 ： Biger姚
********************************************************************************************/
void CrossLocate(int mode)
{
Mat srcImage = Mat::zeros(240, 320, CV_8UC1);
Rect rect(80, 50, 160, 190);//前两个参数：原图中开始裁剪的坐标 后两个参数:矩形的列和行
Mat src, img;
Point center, target;
if (mode == 0)
{
target.x = 88; //88
target.y = 104; //104
}
else
{
target.x = 84;
target.y = 149;
}
int error_x, error_y;
error_x = 0;
error_y = 0;
while (true)
{
Forward_flag = 3;
img = imgOriginal_up;
img = img(rect);
img.copyTo(src);//复制原图用于显示
Mat imgHSV;
cvtColor(img, imgHSV, COLOR_BGR2HSV);
int iLowH_1 = 0;//红色的H：0-20,160-180
int iHighH_1 = 20;
int iLowH_2 = 140;//红色的H：0-20,160-180
int iHighH_2 = 180;
int iLowS = 0;
int iHighS = 255;
int iLowV = 0;
int iHighV = 255;
Mat imgThresholded1(img.size(), CV_8UC1, cv::Scalar(0));
Mat imgThresholded2(img.size(), CV_8UC1, cv::Scalar(0));
inRange(imgHSV, Scalar(iLowH_1, iLowS, iLowV), Scalar(iHighH_1, iHighS, iHighV), imgThresholded1); //Threshold the image
inRange(imgHSV, Scalar(iLowH_2, iLowS, iLowV), Scalar(iHighH_2, iHighS, iHighV), imgThresholded2); //Threshold the image
Mat imgThresholded_add;
addWeighted(imgThresholded1, 1, imgThresholded2, 1, 0, imgThresholded_add);
cvtColor(img, srcImage, CV_BGR2GRAY);
addWeighted(imgThresholded_add, 1, srcImage, 1, 0, srcImage);
srcImage = srcImage > 120;//改为全局变量
//输入二值化原图 找十字交点
center = findCrossPoint(srcImage);
//画出中心
circle(src, center, 10, Scalar(0, 0, 255), FILLED);
/*namedWindow("【结果】", 0);
imshow("【结果】", src);
waitKey(1);*/
error_x = -(target.x - center.x) / 2.8;//让右边数字移到右边
error_y = (target.y - center.y) / 2.8;
if (error_x > 10 || error_x<-10)
{
error_y = 0;
}
error_x = error_x > 120 ? 120 : error_x;
error_x = error_x < -120 ? -120 : error_x;
error_y = error_y > 120 ? 120 : error_y;
error_y = error_y < -120 ? -120 : error_y;
static int break_cnt = 0;
if (error_x>-3 && error_x<3 && error_y>-2 && error_y<2)
{
break_cnt++;
Send_to_K60(16, 0, 0);
Sleep(10);
if (break_cnt>8)
{
break_cnt = 0;
break;
}
}
else
{
Send_to_K60(Forward_flag, error_x, error_y);
if (break_cnt > 0)
{
break_cnt--;
}
}
}
Send_to_K60(16, 0, 0);
}
/*******************************************************************************************
*函数功能 ： 放置挡板前目标数字为个位数的定位，有横竖和第一块第二块之分
*输入参数 ： 第一个参数为目标数字，第二个参数是 0为横着，1为竖着，第三个参数是 1为第一块挡板，2为第二块挡板，
*返 回 值 ： 空
*编写时间 ： 2018.8.13
*作 者 ： 毛哥（参考叶少及姚总）
********************************************************************************************/
void Dangban_DingWei_Single_Num(int num_obj, int mode, int cishu)
{
int flag_finish = 0;
Forward_flag = 3;
int Finish_cnt = 0;
int error1 = 0, error2 = 0, text = 0;
//目标数字及目标数字隔壁的
int num_obj_units = num_obj;//个位
int num_obj_1 = num_obj + 1;//必须是两位数
int num_obj_units_1 = num_obj_1;//个位
//int pic_name = 0;
//char str1[2000];
Mat srcImage0;
for (int i = 0; i < 6; i++)
{//取几帧图像缓冲
cap_up >> srcImage0;
}
Mat srcImage = Mat::zeros(600, 800, CV_8UC3);
Point Put_obj;
if (mode == PUT_CHESS)
{
Put_obj.x = 386;
Put_obj.y = 347;
}
else if (mode == PUT_BLOCK_H)
{
Put_obj.x = 386;
Put_obj.y = 140;
}
else if (mode == PUT_BLOCK_V)
{
Put_obj.x = 175;
Put_obj.y = 369;
}
while (1)
{
Point center_obj;
//clock_t start = clock();
Forward_flag = 3;
//cap_up >> srcImage0;
srcImage0 = imgOriginal_up;
if (!srcImage0.empty())
{
//imshow("原图0", srcImage0);
Mat drawing(srcImage.size(), CV_8UC3, cv::Scalar(0));
resize(srcImage0, srcImage, srcImage.size());
cvtColor(srcImage, srcImage, CV_BGR2GRAY);
srcImage = srcImage > G_threshold_up;//二值化
vector<Vec2f>lines = findHoughLines(srcImage);
vector<float> V_line = find_V_Lines(lines);
float angle = 0;
draw_line(V_line[0], V_line[1], V_line[2], drawing);
if (lines.size()>0)
{
angle = cvFastArctan(V_line[1], V_line[0]);
}
if (angle>90)
{
angle = angle - 180;
}
angle = angle * 2;
srcImage = Spin_Img(srcImage, angle);//车身倾斜旋转图像以识别数字
vector<Min_Rect> Doubt_num_decade, Doubt_num_units;
vector<Min_Rect> Doubt_num_decade_1, Doubt_num_units_1;
vector<Min_Rect>Doubt_numRect = Find_Doubt_NumRect(srcImage);//各种滤波找怀疑是数字框的矩形框
if (Doubt_numRect.size()>0)
{
for (int i = 0; i < Doubt_numRect.size(); i++)
{
int num = number_identify(Doubt_numRect[i].Pic, SVM_params_UP);
if (num == num_obj_units)
{//识别数字滤波(个位)
Doubt_num_units.push_back(Doubt_numRect[i]);
}
if (num == num_obj_units_1)
{//识别隔壁数字(个位)
Doubt_num_units_1.push_back(Doubt_numRect[i]);
}
}
}
int flag_1 = 0;
if (Doubt_num_units.size()>0)
{//在怀疑是数字的方框里找最可能是需要的,得到目标坐标
float dis_min = 100000, dis = 0;
for (size_t i = 0; i < Doubt_num_units.size(); i++)
{
Point temp_center;
temp_center = Doubt_num_units[i].center;
dis = getDistance(Point(386, 401), temp_center);
if (dis<dis_min&&dis<400)
{
dis_min = dis;
center_obj = temp_center;
flag_1 = 1;
计算是否符合机械臂可抓取的范围
//float holder_Dis = getDistance(center_obj, holder_center);
//int theta = cvFastArctan((float)(holder_center.x - center_obj.x), (float)(holder_center.y - center_obj.y));
//if (holder_Dis>holder_radius - dia&&holder_radius + dia)
//{//可以抓取
// Send_to_K60(0, 0, 0);
// Sleep(50);
// Send_to_K60(7, -theta, 0);
// flag_finish++;
//}
//else
//{//计算运动目标
// Put_obj.x = holder_center.x - (float)(holder_radius)*sin(theta*0.0174);
// Put_obj.y = holder_center.y - (float)(holder_radius)*cos(theta*0.0174);
//}
circle(drawing, center_obj, 5, Scalar(0, 255, 0), FILLED, LINE_AA);
}
}
if (flag_finish>10)
{//如果判断可以放，break出while1
break;
}
error1 = (Put_obj.x - center_obj.x);//让右边数字移到右边
error2 = (Put_obj.y - center_obj.y);
error1 = error1 > 120 ? 120 : error1;
error1 = error1 < -120 ? -120 : error1;
error2 = error2 > 120 ? 120 : error2;
error2 = error2 < -120 ? -120 : error2;
}
if (flag_1 == 0 && Doubt_num_units_1.size()>0)
{//在怀疑是数字的方框里找最可能是需要的,得到目标坐标(隔壁数字)
float dis_min = 100000, dis = 0;
for (size_t i = 0; i < Doubt_num_units_1.size(); i++)
{
Point temp_center;
temp_center = Doubt_num_units_1[i].center;
dis = getDistance(Point(386, 401), temp_center);
if (dis<dis_min&&dis<400)
{
dis_min = dis;
center_obj = temp_center;
circle(drawing, center_obj, 5, Scalar(0, 0, 255), FILLED, LINE_AA);
}
}
error1 = -(800 - center_obj.x);//让右边数字移到右边
error2 = (401 - center_obj.y);
error1 = error1 > 120 ? 120 : error1;
error1 = error1 < -120 ? -120 : error1;
error2 = error2 > 120 ? 120 : error2;
error2 = error2 < -120 ? -120 : error2;
}
cout << "放置挡板定位error1为：" << error1 << endl;
cout << "放置挡板定位error2为：" << error2 << endl;
cout << "Forward_flag为：" << (int)Forward_flag << endl;
if (error1>30 || error1<-30)
{
error2 = 0;
}
if (center_obj.x == 0 && center_obj.y == 0)
{
error1 = -10;
error2 = -10;
}
Send_to_K60(Forward_flag, error1, error2);
//imshow("drawing1", drawing);
//imshow("原图", srcImage);
waitKey(1);
}
}
cout << "放置挡板定位成功" << endl;
}
//放挡板，必须是两位数
//第一个参数是数字，第二个参数是竖着还是横着0为横着，1为竖着
//*flag 4 爪子直立起来的，且有磁性，此时手动往机械臂上放置挡板
// * flag - 4 爪子到了位置，开始放置，发送一次即可
/*******************************************************************************************
*函数功能 ： 放置挡板前目标数字为双数的定位，有横竖和第一块第二块之分
*输入参数 ： 第一个参数为目标数字，第二个参数是 0为横着，1为竖着，第三个参数是 1为第一块挡板，2为第二块挡板，
*返 回 值 ： 单条直线的ABC值，如果没有正确找到直线，就返回-1，函数外边再判断
*编写时间 ： 2018.8.13
*作 者 ： 毛哥（参考叶少及姚总）
********************************************************************************************/
void Dangban_DingWei_Double_Num(int num_obj, int mode, int cishu)
{
int flag_finish = 0;//跳出计数，稳定性
Forward_flag = 3;
int Finish_cnt = 0;
int error1 = 0, error2 = 0, text = 0;
//目标数字及目标数字隔壁的
int num_obj_decade = num_obj / 10;//十位
int num_obj_units = num_obj % 10;//个位
int num_obj_1 = num_obj + 1;//必须是两位数
int num_obj_decade_1 = num_obj_1 / 10;//十位
int num_obj_units_1 = num_obj_1 % 10;//个位
Mat srcImage0;
Mat srcImage = Mat::zeros(600, 800, CV_8UC3);
Point Put_obj;
if (mode == PUT_CHESS)
{
Put_obj.x = 386;
Put_obj.y = 347;
}
else if (mode == PUT_BLOCK_H)//0为水平，已经调好（8月14日）
{
Put_obj.x = 198;
Put_obj.y = 158;
}
else if (mode == PUT_BLOCK_V)//1为竖直，已经调好（8月13日）
{
Put_obj.x = 200;
Put_obj.y = 270;
}
while (1)
{
Point center_obj;
//clock_t start = clock();
Forward_flag = 3;
srcImage0 = imgOriginal_up;
if (!srcImage0.empty())
{
//imshow("原图0", srcImage0);
Mat drawing(srcImage.size(), CV_8UC3, cv::Scalar(0));
resize(srcImage0, srcImage, srcImage.size());
cvtColor(srcImage, srcImage, CV_BGR2GRAY);
srcImage = srcImage > G_threshold_up;//二值化
vector<Vec2f>lines = findHoughLines(srcImage);
vector<float> V_line = find_V_Lines(lines);
float angle = 0;
draw_line(V_line[0], V_line[1], V_line[2], drawing);
if (lines.size() > 0)
{
angle = cvFastArctan(V_line[1], V_line[0]);
}
if (angle > 90)
{
angle = angle - 180;
}
angle = angle * 2;
//计算出角度后旋转图片
srcImage = Spin_Img(srcImage, angle);
vector<Min_Rect> Doubt_num_decade, Doubt_num_units;
vector<Min_Rect> Doubt_num_decade_1, Doubt_num_units_1;
//bug1，如果当前数字位置有
vector<Min_Rect>Doubt_numRect = Find_Doubt_NumRect(srcImage);//各种滤波找怀疑是数字框的矩形框
if (Doubt_numRect.size()>0)
{
for (int i = 0; i < Doubt_numRect.size(); i++)
{
int num = number_identify(Doubt_numRect[i].Pic, SVM_params_UP);
if (num == num_obj_decade)
{//识别数字滤波(十位)
Doubt_num_decade.push_back(Doubt_numRect[i]);
}
if (num == num_obj_units)
{//识别数字滤波(个位)
Doubt_num_units.push_back(Doubt_numRect[i]);
}
if (num == num_obj_decade_1)
{//识别隔壁数字(十位)
Doubt_num_decade_1.push_back(Doubt_numRect[i]);
}
if (num == num_obj_units_1)
{//识别隔壁数字(个位)
Doubt_num_units_1.push_back(Doubt_numRect[i]);
}
}
}
int flag_1 = 0;
if (Doubt_num_decade.size()>0 && Doubt_num_units.size()>0)
{//在怀疑是数字的方框里找最可能是需要的,得到目标坐标
vector<Min_Rect>Doubt_num_obj(2);
float dis_min = 100000, dis = 0;
for (int i = 0; i < Doubt_num_decade.size(); i++)
{
for (int j = 0; j < Doubt_num_units.size(); j++)
{
if (Doubt_num_decade[i].center.x<Doubt_num_units[j].center.x)
{//dis为两中心点距离，center与目标点的距离加权
Point temp_center;
temp_center.x = (Doubt_num_decade[i].center.x + Doubt_num_units[j].center.x) / 2;
temp_center.y = (Doubt_num_decade[i].center.y + Doubt_num_units[j].center.y) / 2;
dis = getDistance(Doubt_num_decade[i].center, Doubt_num_units[j].center)
+ getDistance(Point(386, 347), temp_center);
if (dis<dis_min&&dis<400)
{//如果可以找到目标数字中心
dis_min = dis;
Doubt_num_obj[0] = Doubt_num_decade[i];
Doubt_num_obj[1] = Doubt_num_units[j];
center_obj = temp_center; //数字中心
flag_1 = 1;
circle(drawing, center_obj, 5, Scalar(255, 0, 0), FILLED, LINE_AA);
//计算是否符合机械臂可抓取的范围
//float holder_Dis = getDistance(center_obj, holder_center);
//int theta = cvFastArctan((float)(holder_center.x - center_obj.x), (float)(holder_center.y - center_obj.y));
//if (holder_Dis>holder_radius - dia&&holder_radius + dia)
//{//可以抓取
// Send_to_K60(0, 0, 0);
// Sleep(50);
// Send_to_K60(7, -theta, 0);
// flag_finish++;
//}
//else
//{//计算运动目标
// Put_obj.x = holder_center.x - (float)(holder_radius)*sin(theta*0.0174);
// Put_obj.y = holder_center.y - (float)(holder_radius)*cos(theta*0.0174);
//}
}
}
}
}
//error1 = (int)((365.0 - (float)center_obj.x) / 1.6);//1.6这个系数刚好，电压8.2V左右
//error2 = (int)((136.0 - (float)center_obj.y) / 1.0);//1.0这个系数刚好
error1 = -(float)(Put_obj.x - center_obj.x) / 8.0;//让右边数字移到右边
error2 = (float)(Put_obj.y - center_obj.y) / 8.0;
error1 = error1 > 120 ? 120 : error1;
error1 = error1 < -120 ? -120 : error1;
error2 = error2 > 120 ? 120 : error2;
error2 = error2 < -120 ? -120 : error2;
}
if (flag_1 == 0 && Doubt_num_decade_1.size()>0 && Doubt_num_units_1.size()>0)
{//在怀疑是数字的方框里找最可能是需要的,得到目标坐标(隔壁数字)
vector<Min_Rect>Doubt_num_obj(2);
float dis_min = 100000, dis = 0;
for (int i = 0; i < Doubt_num_decade_1.size(); i++)
{
for (int j = 0; j < Doubt_num_units_1.size(); j++)
{
if (Doubt_num_decade_1[i].center.x<Doubt_num_units_1[j].center.x)
{//dis为两中心点距离，center与目标点的距离加权
Point temp_center;
temp_center.x = (Doubt_num_decade_1[i].center.x + Doubt_num_units_1[j].center.x) / 2;
temp_center.y = (Doubt_num_decade_1[i].center.y + Doubt_num_units_1[j].center.y) / 2;
dis = getDistance(Doubt_num_decade_1[i].center, Doubt_num_units_1[j].center)
+ getDistance(Point(386, 347), temp_center);
if (dis<dis_min&&dis<400)
{
dis_min = dis;
Doubt_num_obj[0] = Doubt_num_decade_1[i];
Doubt_num_obj[1] = Doubt_num_units_1[j];
center_obj = temp_center;
circle(drawing, center_obj, 5, Scalar(255, 0, 0), FILLED, LINE_AA);
}
}
}
}
error1 = -(800 - center_obj.x);//让右边数字移到右边
error2 = (Put_obj.y - center_obj.y);
error1 = error1 > 120 ? 120 : error1;
error1 = error1 < -120 ? -120 : error1;
error2 = error2 > 120 ? 120 : error2;
error2 = error2 < -120 ? -120 : error2;
//目标 放棋子：center:x=386,y=347
//目标 放挡板：center:x=386,y=149
}
//cout << "放置挡板定位error1为：" << error1 << endl;
//cout << "放置挡板定位error2为：" << error2 << endl;
//cout << "Forward_flag为：" << (int)Forward_flag << endl;
if (error1>30 || error1<-30)
{
error2 = 0;
}
if (center_obj.x == 0 && center_obj.y == 0)
{
error1 = -25;
error2 = -25;
}
if (error1<7 && error2<7 && error1>-7 && error2>-7)
{
Forward_flag = 16;
flag_finish++;
// cout << "Finish_cnt为：" << Finish_cnt << endl;
if (flag_finish>5)
{
break;
}
}
else
{
flag_finish = 0;
}
Send_to_K60(Forward_flag, error1, error2);
//imshow("drawing1", drawing);
//imshow("原图", srcImage);
waitKey(1);
}
}
//放下
cout << "放置挡板定位成功" << endl;
}
/*找最大的轮廓*/
Mat findMaxContouns(Mat img)
{
vector<vector<Point>> contours;
vector<Vec4i>hierarchy;
int row_i, col_i;
float row_max = 0.0, col_min = 3.3;
findContours(img, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
Mat dstImage = Mat::zeros(img.size(), CV_8UC3);
long int max_l = 0;
int max_i = 0;
if (contours.size() > 0)
{
vector<float> length(contours.size());
//找最大的轮廓
#pragma omp parallel for
for (int i = 0; i < contours.size(); i++)
{
length[i] = arcLength(contours[i], true);
}
for (int i = 0; i < contours.size(); i++)
{
if (length[i] > max_l)
{
max_l = length[i];
max_i = i;
}
}
drawContours(dstImage, contours, max_i, (0, 0, 255), 1, 8, hierarchy);
//imshow("轮廓图", dstImage);
}
//center = findContunsCenter(contours, max_i);
return dstImage;
}
/*******************************************************************************************
*函数功能 ： 获取横线或竖线一般式方程参数
*输入参数 ： mode,0横1竖
*返 回 值 ： PARAM 一般式ABC三个参数
*编写时间 ： 2018.8.15
*作 者 ： 姚
********************************************************************************************/
vector <float> getVHlineParam(int mode)
{
int cnt = 0;
//vector <float> PARAM;
float A = 0, B = 0, C = 0;
float filter_A[4] = { 0 }, filter_B[4] = { 0 }, filter_C[4] = { 0 };
vector<vector<Point>> contours;
vector<Vec4i>hierarchy;
Mat midImage, dstImage, img, src, img0;
int break_cnt = 0;
Point pt1, pt2, pt3, pt4;
while (true)
{
img = imgOriginal_up;
if (mode == 0)//横着
{
Rect rect(0, img.rows*0.3, img.cols, img.rows*(0.95 - 0.3)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
img = img(rect);
}
else //竖着
{
Rect rect(img.cols*0.075, img.rows*0.2, img.cols*(0.875 - 0.075), img.rows*(0.83 - 0.2)); //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height
img = img(rect);
}
img.copyTo(src);
cvtColor(img, img0, COLOR_BGR2GRAY);
img0 = img0 > 110;
dstImage = findMaxContouns(img0);
cvtColor(dstImage, midImage, COLOR_BGR2GRAY);
midImage = midImage > 2;
vector<Vec2f> lines;//定义一个矢量结构lines用于存放得到的线段矢量集合
HoughLines(midImage, lines, 1, CV_PI / 180, 60);
if (lines.size() > 0)
{//横竖线分类 求出两条横竖线后求交点
vector<Vec2f> lines_h;//定义一个矢量结构lines用于存放得到的线段矢量集合
vector<Vec2f> lines_v;//定义一个矢量结构lines用于存放得到的线段矢量集合
float rho_min_h = 10000, rho_max_h = -10000, rho_min_v = 10000, rho_max_v = -10000;
float theta_sum_v = 0, theta_sum_h = 0;
float theta;
float rho_h, theta_h, rho_v, theta_v;
double a, b;
double x0, y0;
for (int i = 0; i < lines.size(); i++)
{
theta = lines[i][1];
if (theta >= 2.355 || theta <= 0.785)lines_v.push_back(lines[i]);//shu
else if (theta >= 0.785&&theta <= 2.355)lines_h.push_back(lines[i]);//heng
}
/*heng*/
if (mode == 0)
{
for (size_t i = 0; i < lines_h.size(); i++)
{
rho_h = lines_h[i][0], theta_h = lines_h[i][1];
theta_sum_h += theta_h;
/*a = cos(theta_h), b = sin(theta_h);
x0 = a*rho_h, y0 = b*rho_h;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
line(src, pt1, pt2, Scalar(0, 0, 0), 1, LINE_AA);*/
if (rho_h >= rho_max_h)rho_max_h = rho_h;
if (rho_h <= rho_min_h)rho_min_h = rho_h;
}
rho_h = (rho_max_h + rho_min_h) / 2;
theta_h = theta_sum_h / lines_h.size();
a = cos(theta_h), b = sin(theta_h);
x0 = a*rho_h, y0 = b*rho_h;
pt1.x = cvRound(x0 + 1000 * (-b));
pt1.y = cvRound(y0 + 1000 * (a));
pt2.x = cvRound(x0 - 1000 * (-b));
pt2.y = cvRound(y0 - 1000 * (a));
line(src, pt1, pt2, Scalar(0, 0, 255), 2, LINE_AA);
filter_A[3] = filter_A[2];
filter_B[3] = filter_B[2];
filter_C[3] = filter_C[2];
filter_A[2] = filter_A[1];
filter_B[2] = filter_B[1];
filter_C[2] = filter_C[1];
filter_A[1] = filter_A[0];
filter_B[1] = filter_B[0];
filter_C[1] = filter_C[0];
filter_A[0] = pt2.y - pt1.y;
filter_B[0] = pt1.x - pt2.x;
filter_C[0] = pt1.y*(pt2.y - pt1.y) - pt1.x*(pt2.y - pt1.y);
if (fabs(filter_A[0] - filter_A[1])<2)
{
cnt++;
A = 0.4*filter_A[0] + 0.3*filter_A[1] + 0.2*filter_A[2] + 0.1*filter_A[3];
B = 0.4*filter_B[0] + 0.3*filter_B[1] + 0.2*filter_B[2] + 0.1*filter_B[3];
C = 0.4*filter_C[0] + 0.3*filter_C[1] + 0.2*filter_C[2] + 0.1*filter_C[3];
if (cnt>8)
{
break;
}
}
else
{
if (cnt>0)cnt--;
}
}
else
{
//shu
for (size_t i = 0; i < lines_v.size(); i++)
{
rho_v = lines_v[i][0], theta_v = lines_v[i][1];
if (theta_v<0.6)
{
theta_sum_v += theta_v;
theta_sum_v += CV_PI;
}
else theta_sum_v += theta_v;
/*a = cos(theta_v), b = sin(theta_v);
x0 = a*rho_v, y0 = b*rho_v;
pt3.x = cvRound(x0 + 1000 * (-b));
pt3.y = cvRound(y0 + 1000 * (a));
pt4.x = cvRound(x0 - 1000 * (-b));
pt4.y = cvRound(y0 - 1000 * (a));
line(src, pt3, pt4, Scalar(0, 0, 0), 1, LINE_AA);*/
if (rho_v < 0)rho_v = -rho_v;
if (rho_v >= rho_max_v)rho_max_v = rho_v;
if (rho_v <= rho_min_v)rho_min_v = rho_v;
}
rho_v = (rho_max_v + rho_min_v) / 2;
theta_v = theta_sum_v / lines_v.size();
if (theta_v > CV_PI)theta_v -= CV_PI;
else rho_v = -rho_v;
a = cos(theta_v), b = sin(theta_v);
x0 = a*rho_v, y0 = b*rho_v;
pt3.x = cvRound(x0 + 1000 * (-b));
pt3.y = cvRound(y0 + 1000 * (a));
pt4.x = cvRound(x0 - 1000 * (-b));
pt4.y = cvRound(y0 - 1000 * (a));
line(src, pt3, pt4, Scalar(0, 0, 255), 2, LINE_AA);
filter_A[3] = filter_A[2];
filter_B[3] = filter_B[2];
filter_C[3] = filter_C[2];
filter_A[2] = filter_A[1];
filter_B[2] = filter_B[1];
filter_C[2] = filter_C[1];
filter_A[1] = filter_A[0];
filter_B[1] = filter_B[0];
filter_C[1] = filter_C[0];
filter_A[0] = pt4.y - pt3.y;
filter_B[0] = pt3.x - pt4.x;
filter_C[0] = pt3.y*(pt4.y - pt3.y) - pt3.x*(pt4.y - pt3.y);
if (fabs(filter_A[0] - filter_A[1])<2)
{
cnt++;
A = 0.4*filter_A[0] + 0.3*filter_A[1] + 0.2*filter_A[2] + 0.1*filter_A[3];
B = 0.4*filter_B[0] + 0.3*filter_B[1] + 0.2*filter_B[2] + 0.1*filter_B[3];
C = 0.4*filter_C[0] + 0.3*filter_C[1] + 0.2*filter_C[2] + 0.1*filter_C[3];
if (cnt>8)
{
break;
}
}
else
{
if (cnt>0)cnt--;
}
}
namedWindow("图像", 0);
imshow("图像", src);
waitKey(1);
}
}
vector<float> fangcheng_PARAM;
if (mode == 0)
{
fangcheng_PARAM = get_lines_yibanshi_fangcheng_2(pt1, pt2);
}
else
{
fangcheng_PARAM = get_lines_yibanshi_fangcheng_2(pt3, pt4);
}
return fangcheng_PARAM;
//PARAM.push_back(A);
//PARAM.push_back(B);
//PARAM.push_back(C);
//return PARAM;
}
扫一扫
07-12
5392
5392
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
