//-------------自动打击函数-----------------------------------------
//-------------自动打击 函数 全局变量----------------------------------
//-------------自动打击 函数 全局变量----------------------------------
//------------------------自动打击 全局变量----------------------
#define color 1//1red 0blue
#define pi 3.14
#define match_num 5
#define height_threshold 0
#define distance_single_value 10
#define distance_multiple_value 10
#define drift_angle 18
#define a_r_final 3.3
#define a_r_b_max 5.5
#define a_r_b_min 4.1
#define a_r_s_max 3.5
#define a_r_s_min 1.3
#define o_r_max 2
#define o_r_min 0.5
#define x_multiple 2.2
#define y_multiple 3.5
#define r_r_max 2
#define r_r_min 0.5
#define similar_degree 0.85
#define area_value_min 50//40 2.3m 50 2.0m 70 1.7m
#define color_threshold 160
#define blueratio 0.2
#define blueratio2 0.2
#define redratio 0.3
#define redratio2 0.25
#define anglediff1 6
#define anglediff2 7
#define limit 1
#define horizontal_angle_thres 5
Mat model, element1, element2;
Point2f center, store_tem; //center实时中心点，store_tem临时存放中心点。
Rect external_rect, rect1, rect2, rect3, rect4, rect5, rrr, rect6, rect7, rect8, rect9, box;
float distancee, area;
int flag = 0;
int detection_flag=0;
float Max, a_r, o_r, r_r, angle_first, angle_second;
float horizontal_angle;
vector<Point> points1, points2, points3, points4, points5;
Mat Matrix=(Mat_<double>(3,3)<<501.9406,0.0,330.2514,0.0,502.2422,234.5579,0.0,0.0,1.0);
Mat coefficients=(Mat_<double>(5,1)<<0.0266,-0.0613,0.0,0.0,-0.0015);
// int stateNum = 2;
// int measureNum = 1;
// KalmanFilter KF(stateNum, measureNum, 0);
// //Mat processNoise(stateNum, 1, CV_32F);
// Mat measurement = Mat::zeros(measureNum, 1, CV_32F);
// //KF.transitionMatrix = (Mat_<float>(stateNum, stateNum) << 1, 0, 1, 0,//A 状态转移矩阵
// // 0, 1, 0, 1,
// // 0, 0, 1, 0,
// // 0, 0, 0, 1);
// KF.transitionMatrix = (Mat_<float>(stateNum, stateNum) << 1, 1, 0, 1);
// //这里没有设置控制矩阵B，默认为零
// setIdentity(KF.measurementMatrix);//H=[1,0,0,0;0,1,0,0] 测量矩阵
// setIdentity(KF.processNoiseCov, Scalar::all(1e-5));//Q高斯白噪声，单位阵
// setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));//R高斯白噪声，单位阵
// setIdentity(KF.errorCovPost, Scalar::all(1));//P后验误差估计协方差矩阵，初始化为单位阵
// randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
//两点距离
float juli(Point aa, Point bb)
{
double sd;
sd = sqrt((aa.x - bb.x)*(aa.x - bb.x) + (aa.y - bb.y)*(aa.y - bb.y));
return sd;
}
//判断是否为红色
double colorRatioR(Mat pic1, Mat pic2)
{
double sum1 = 0, sum2 = 0;
double ratio;
/*-------遍历两张图片中间一列的红色分量-------*/
for (int t = 0; t < pic1.rows; t++)//逐行遍历
{
sum1 = sum1 + pic1.at<Vec3b>(t, pic1.cols/2)[2] / 255; //每一行中点的像素红色分量除以255的比值
}
for (int t = 0; t < pic2.rows; t++)//逐行遍历
{
sum2 = sum2 + pic2.at<Vec3b>(t, pic2.cols/2)[2] / 255; //每一行中点的像素红色分量除以255的比值
}
/*-------遍历两张图片中间一列的红色分量-------*/
ratio = (sum1 / (pic1.rows) + sum2 / (pic2.rows) ) / 2;//ratio:取值范围0~1
return ratio;
}
//判断是否为蓝色
double colorRatioB(Mat pic1, Mat pic2)
{
double sum1 = 0, sum2 = 0;
double ratio;
/*-------遍历两张图片中间一列的蓝色分量-------*/
for (int t = 0; t < pic1.rows; t++)
{
sum1 = sum1 + pic1.at<Vec3b>(t, pic1.cols/2)[0] / 255;//每一行中点的像素蓝色分量除以255的比值
}
for (int t = 0; t < pic2.rows; t++)
{
sum2 = sum2 + pic2.at<Vec3b>(t, pic2.cols/2)[0] / 255;//每一行中点的像素蓝色分量除以255的比值
}
/*-------遍历两张图片中间一列的蓝色分量-------*/
ratio = (sum1 / (pic1.rows) + sum2 / (pic2.rows)) / 2;//ratio:取值范围0~1
return ratio;
}
double colorRatioHSV(Mat pic1, Mat pic2) //瘦高矩形内核
{
Mat element = getStructuringElement(MORPH_RECT, Size(1,7));
erode(pic1,pic1,element);
erode(pic2,pic2,element);
cvtColor(pic1,pic1,CV_BGR2HSV);
cvtColor(pic2,pic2,CV_BGR2HSV);
double sum1 = 0, sum2 = 0;
double ratio;
for (int t = 0; t < pic1.rows; t++)
{
for (int tt = 0; tt < pic1.cols; tt++)
{
sum1 = sum1 + pic1.at<Vec3b>(t, tt)[0]; //蓝色
}
}
for (int t = 0; t < pic2.rows; t++)
{
for (int tt = 0; tt < pic2.cols; tt++)
{
sum2 = sum2 + pic2.at<Vec3b>(t, tt)[0];
}
}
ratio = (sum1 / (pic1.rows*pic1.cols) + sum2 / (pic2.rows*pic2.cols) ) / 2;
return ratio;
}
double colorRatioHSV_S(Mat pic1, Mat pic2) //正方形内核
{
Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
erode(pic1, pic1, element);
erode(pic2, pic2, element);
cvtColor(pic1, pic1, CV_BGR2HSV);
cvtColor(pic2, pic2, CV_BGR2HSV);
double sum1 = 0, sum2 = 0;
double ratio;
for (int t = 0; t < pic1.rows; t++)
{
for (int tt = 0; tt < pic1.cols; tt++)
{
sum1 = sum1 + pic1.at<Vec3b>(t, tt)[1]; //绿色
}
}
for (int t = 0; t < pic2.rows; t++)
{
for (int tt = 0; tt < pic2.cols; tt++)
{
sum2 = sum2 + pic2.at<Vec3b>(t, tt)[1];
}
}
ratio = (sum1 / (pic1.rows*pic1.cols) + sum2 / (pic2.rows*pic2.cols)) / 2;
return ratio;
}
Mat pretreat(Mat picture)
{
Mat out;
cvtColor(picture, out, CV_BGR2GRAY);
Mat element1, element2;
threshold(out, out, color_threshold, 255, CV_THRESH_BINARY);
element1 = getStructuringElement(MORPH_RECT, Size(1, 5));
erode(out, out, element1);
element2 = getStructuringElement(MORPH_RECT, Size(3, 7));
dilate(out, out, element2);
//imshow("out",out);
return out;
}
//水平角
double hor_angle(Point p1, Point p2)
{
int a, b;
double rad, angle;
a = abs(p1.x - p2.x);
b = abs(p1.y - p2.y);
if (a != 0) //a==0除法会炸
{
rad = atan(b / a); //坡角，单位为rad
angle = rad * 180 / pi; //坡度
}
else
angle = 90;
return angle;
};
int auto_shot(Mat frame)
{
Mat temp_frame = frame.clone();
Mat src=pretreat(frame);
vector<vector<Point> > contours;
findContours(src, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
vector<RotatedRect> ellipsee(contours.size());
int array1[1000];
int count1 = 0;
for (int i0 = 0; i0 < contours.size(); i0++)
{
if (contours[i0].size() < 5)
continue;
ellipsee[i0] = fitEllipse(contours[i0]);
external_rect = ellipsee[i0].boundingRect();
area = external_rect.area();
if (area>30 && external_rect.height > height_threshold && (double)external_rect.height / (double)external_rect.width>1.8 && (ellipsee[i0].angle<drift_angle || ellipsee[i0].angle>(180 - drift_angle)))
{
points1.push_back(Point(ellipsee[i0].center));
array1[count1] = i0;
count1++;
if(count1>999)
{break;}
}
}
int count2 = 0;
int array2[2000];
for (int i1 = 0; i1 < count1; i1++)
{
for (int j1 = i1 + 1; j1 < count1; j1++)
{
horizontal_angle = hor_angle(points1[i1], points1[j1]);
if (horizontal_angle < horizontal_angle_thres)
{
points2.push_back(points1[i1]);
points2.push_back(points1[j1]);
array2[2 * count2] = array1[i1];
array2[2 * count2 + 1] = array1[j1];
count2++;
if(count2>999)
{break;}
}
}
if(count2>999)
{break;}
}
int array3[1000];
int count3 = 0;
for (int i2 = 0; i2 < count2; i2++)
{
int label = 0;
rect1 = boundingRect(contours[array2[i2 * 2]]);
rect2 = boundingRect(contours[array2[i2 * 2 + 1]]);
rect3 = rect1 | rect2; //求两个矩形的并集
for (int ii = 0; ii < points1.size(); ii++)
label = label + rect3.contains(points1[ii]);
if (label == 2)
{
points3.push_back(points2[i2 * 2]);
points3.push_back(points2[i2 * 2 + 1]);
array3[count3 * 2] = array2[i2 * 2];
array3[count3 * 2 + 1] = array2[i2 * 2 + 1];
count3++;
if(count3>499)
{break;}
}
else
label = 0;
}
int array4[1000];
int count4 = 0;
for (int i3 = 0; i3 < count3; i3++)
{
rect4 = boundingRect(contours[array3[i3 * 2]]);
rect5 = boundingRect(contours[array3[i3 * 2 + 1]]);
rrr = rect4 | rect5;
Max = max((double)rect4.height, (double)rect5.height);
a_r = rrr.width / Max; //即使正对，装甲板相对地面必定是倾斜的，灯条也就会倾斜一点，所以包围的矩形rrr比两者最大值会大一点
o_r = (double)rect4.height / (double)rect5.height; //两个灯条宽度比，上一步进行限制后可以看成是两个灯条大小比
r_r = ((double)rect4.height / (double)rect4.width) / ((double)rect5.height / (double)rect5.width); //两个灯条长宽比例
angle_first = ellipsee[array3[i3 * 2]].angle; //偏转角度
angle_second = ellipsee[array3[i3 * 2 + 1]].angle; //偏转角度
//rectangle(frame, rrr, 255, 1, 1);
double s = colorRatioHSV_S(frame(rect4), frame(rect5)); //用3*3腐蚀+hsv绿色找出蓝色分量
//double hsv = colorRatioHSV(frame(rect4), frame(rect5)); //用1*7腐蚀+hsv蓝色分量。。。然后不成功？
double red = colorRatioR(temp_frame(rect4), temp_frame(rect5));
double blue = colorRatioB(temp_frame(rect4), temp_frame(rect5));
if (color == 1 && s>43)
{
if (a_r < a_r_s_max && a_r > a_r_s_min)
{
if (red > blue && red > redratio && blue < limit && (abs(angle_first - angle_second) < anglediff1 || abs(angle_first - angle_second) > (180 - anglediff1))
&& rrr.height > height_threshold && o_r < o_r_max && o_r > o_r_min && r_r < r_r_max && r_r > r_r_min && (rect4.area()>area_value_min || rect5.area()>area_value_min))
{
//cout << a_r <<" 1"<<endl;
//cout<<red<<" "<<blue<<" "<<white<<endl;
points4.push_back(points3[i3 * 2]);
points4.push_back(points3[i3 * 2 + 1]);
array4[2 * count4] = array3[i3 * 2];
array4[2 * count4 + 1] = array3[i3 * 2 + 1];
count4++;
}
}
if (a_r > a_r_b_min && a_r < a_r_b_max)
{
if (red > blue && red > redratio2 && blue < limit && (abs(angle_first - angle_second) < anglediff2 || abs(angle_first - angle_second) > (180 - anglediff2))
&& rrr.height > height_threshold && o_r < o_r_max && o_r > o_r_min && r_r < r_r_max && r_r > r_r_min)
{
//cout << a_r << " 2" << endl;
//cout<<red<<" "<<blue<<" "<<white<<endl;
points4.push_back(points3[i3 * 2]);
points4.push_back(points3[i3 * 2 + 1]);
array4[2 * count4] = array3[i3 * 2];
array4[2 * count4 + 1] = array3[i3 * 2 + 1];
count4++;
}
}
}
if (color == 0)
{
//cout << a_r << endl;
if (a_r < a_r_s_max && a_r > a_r_s_min)
{
if (((red < blue && blue > blueratio) || (red == blue)) && (abs(angle_first - angle_second) < anglediff1 || abs(angle_first - angle_second) > (180 - anglediff1))
&& rrr.height > height_threshold && o_r < o_r_max && o_r > o_r_min && r_r < r_r_max && r_r > r_r_min && (rect4.area()>area_value_min || rect5.area()>area_value_min))
{
//cout<<red<<" "<<blue<<" "<<white<<endl;
points4.push_back(points3[i3 * 2]);
points4.push_back(points3[i3 * 2 + 1]);
array4[2 * count4] = array3[i3 * 2];
array4[2 * count4 + 1] = array3[i3 * 2 + 1];
count4++;
}
}
if (a_r > a_r_s_max && a_r < a_r_b_max)
{
if (red < blue && blue > blueratio2 && (abs(angle_first - angle_second) < anglediff2 || abs(angle_first - angle_second) > (180 - anglediff2))
&& rrr.height > height_threshold && o_r < o_r_max && o_r > o_r_min && r_r < r_r_max && r_r > r_r_min)
{
//cout<<red<<" "<<blue<<" "<<white<<endl;
points4.push_back(points3[i3 * 2]);
points4.push_back(points3[i3 * 2 + 1]);
array4[2 * count4] = array3[i3 * 2];
array4[2 * count4 + 1] = array3[i3 * 2 + 1];
count4++;
}
}
}
}
int sizer = 3;//1big 0small
int mark = 1;
int markk = 1;
Point point_temp1, point_temp2, temp;
Mat trans = (Mat_<float>(3, 1));
Mat r;
Mat_<float> zuo;
vector<cv::Point3f> point3d_da;
vector<cv::Point2f> point2d_da;
Point3f daji_3d;
point3d_da.push_back(Point3f(-6.5, 3, 0));
point3d_da.push_back(Point3f(6.5, 3, 0));
point3d_da.push_back(Point3f(6.5, -3, 0));
point3d_da.push_back(Point3f(-6.5, -3, 0)); //
Point zs, ys, yx, zx;
if (count4 > 1) //识别装甲板数量大于1块
{
int aa, bb; //aa，bb保存本次修改的索引
int cc = 100, dd = 100; //cc,dd备份装甲板灯条在总轮廓中的索引。赋值100是为了下面cc*dd》9999的条件
if (flag == 1) //是否能发弹标志
{
for (int i4 = 0; i4 < points4.size() / 2; i4++)
{
point_temp1.x = points4[2 * i4].x;
point_temp2.x = points4[2 * i4 + 1].x;
point_temp1.y = points4[2 * i4].y;
point_temp2.y = points4[2 * i4 + 1].y;
temp.x = (point_temp1.x + point_temp2.x) / 2; //本次装甲板中心x
temp.y = (point_temp1.y + point_temp2.y) / 2; //本次装甲板中心y
distancee = juli(store_tem, temp); //计算storetem和本次装甲板中心的距离
if (distancee < distance_multiple_value) //如果两个装甲板的距离不超过distance_multiple_value，说明移动幅度不大，继续更新
{ //把本次的值保存到aa，bb，cc，dd
aa = i4 * 2;
bb = i4 * 2 + 1;
cc = array4[i4 * 2];
dd = array4[i4 * 2 + 1];
}
}
if (cc*dd<9999) //大于10000则是初始化的情况
{
rect6 = boundingRect(contours[cc]);
rect7 = boundingRect(contours[dd]);
rect8 = rect6 | rect7;
//将装甲板四个顶点保存到point2d-da里
zs = rect8.tl();
yx = rect8.br();
ys = zs + Point(rect8.width,0);
zx = zs + Point(0,rect8.height);
point2d_da.push_back(zs);
point2d_da.push_back(ys);
point2d_da.push_back(yx);
point2d_da.push_back(zx);
//姿态解算，利用平面已知四点坐标确定摄像头相对世界坐标系的平移和旋转
//coefficients相机外参畸变系数
//Matrix相机内参
//point3d和point2d：2d-3d匹配点
solvePnP(point3d_da, point2d_da, Matrix, coefficients, r, trans);
trans.convertTo(zuo, CV_32F);
daji_3d.z = zuo.at<float>(0, 2);
//rect9：装甲板除去灯条的矩形，应该是想做数字识别
rect9.x = rect8.x - ((x_multiple - 1) / 2)*rect8.width;
rect9.y = rect8.y - ((y_multiple - 1) / 2)*rect8.height;
rect9.width = rect8.width * x_multiple;
rect9.height = rect8.height * y_multiple;
if (rect8.height>height_threshold && rect8.x > 0 && rect8.x + rect8.width < 640 && rect8.y > 0 && rect8.y + rect8.height < 480)
model = src(rect8);
center.x = (points4[aa].x + points4[bb].x) / 2;
center.y = (points4[aa].y + points4[bb].y) / 2;
//顺便算出中心坐标
flag = 1;
mark = 1;
//判断大小装甲
if(rect8.width/rect8.height>a_r_final)
{sizer = 1;}
if(rect8.width/rect8.height<a_r_final)
{sizer = 0;}
}
if (cc*dd>9999 && rect9.area() != 0 && rect8.area() != 0 && rect9.x > 0 && rect9.x + rect9.width<640 && rect9.y>0 && rect9.y + rect9.height < 480)
{
Mat similarity;
matchTemplate(src(rect9), model, similarity, match_num); //模板识别数字
double mag_r;
Point point;
minMaxLoc(similarity, 0, &mag_r, 0, &point); //找出极大值为mag-r的匹配的矩形的左上点，保存为point
if (model.cols > height_threshold && mag_r > similar_degree)
{
box.x = point.x + rect9.x;
box.y = point.y + rect9.y;
box.width = rect8.width;
box.height = rect8.height;
center.x = box.x + box.width / 2;
center.y = box.y + box.height / 2;
//rectangle(frame, box, Scalar(0, 0, 255), 2);
//进一步更新装甲板中心坐标
model = src(box);
rect9.x = box.x - ((x_multiple - 1) / 2)*box.width;
rect9.y = box.y - ((y_multiple - 1) / 2)*box.height;
rect9.width = box.width * x_multiple;
rect9.height = box.height * y_multiple;
flag = 1;
mark = 1;
zs = box.tl();
yx = box.br();
ys = zs + Point(box.width, 0);
zx = zs + Point(0, box.height);
point2d_da.push_back(zs);
point2d_da.push_back(ys);
point2d_da.push_back(yx);
point2d_da.push_back(zx);
solvePnP(point3d_da, point2d_da, Matrix, coefficients, r, trans);
trans.convertTo(zuo, CV_32F);
daji_3d.z = zuo.at<float>(0, 2);
if(box.width/box.height>a_r_final)
{sizer = 1;}
if(box.width/box.height<a_r_final)
{sizer = 0;}
}
if (mag_r < similar_degree)
{
flag = 0;
mark = 0;
}
}
}
if (flag == 0 && mark == 1)
{
rect6 = boundingRect(contours[array4[0]]);
rect7 = boundingRect(contours[array4[1]]);
rect8 = rect6 | rect7;
zs = rect8.tl();
yx = rect8.br();
ys = zs + Point(rect8.width, 0);
zx = zs + Point(0, rect8.height);
point2d_da.push_back(zs);
point2d_da.push_back(ys);
point2d_da.push_back(yx);
point2d_da.push_back(zx);
solvePnP(point3d_da, point2d_da, Matrix, coefficients, r, trans);
trans.convertTo(zuo, CV_32F);
daji_3d.z = zuo.at<float>(0, 2);
rect9.x = rect8.x - ((x_multiple - 1) / 2)*rect8.width;
rect9.y = rect8.y - ((y_multiple - 1) / 2)*rect8.height;
rect9.width = rect8.width * x_multiple;
rect9.height = rect8.height * y_multiple;
if (rect8.height > height_threshold && rect8.x > 0 && rect8.x + rect8.width < 640 && rect8.y > 0 && rect8.y + rect8.height < 480)
model = src(rect8);
center.x = (points4[0].x + points4[1].x) / 2;
center.y = (points4[0].y + points4[1].y) / 2;
if(rect8.width/rect8.height>a_r_final)
{sizer = 1;}
if(rect8.width/rect8.height<a_r_final)
{sizer = 0;}
flag = 1;
}
}
if (count4 == 1)
{
if (flag == 1)
{
center.x = (points4[0].x + points4[1].x) / 2;
center.y = (points4[0].y + points4[1].y) / 2;
distancee = juli(center, store_tem);
if (distancee < distance_single_value)
{
rect6 = boundingRect(contours[array4[0]]);
rect7 = boundingRect(contours[array4[1]]);
rect8 = rect6 | rect7;
zs = rect8.tl();
yx = rect8.br();
ys = zs + Point(rect8.width, 0);
zx = zs + Point(0, rect8.height);
point2d_da.push_back(zs);
point2d_da.push_back(ys);
point2d_da.push_back(yx);
point2d_da.push_back(zx);
solvePnP(point3d_da, point2d_da, Matrix, coefficients, r, trans);
trans.convertTo(zuo, CV_32F);
daji_3d.z = zuo.at<float>(0, 2);
rect9.x = rect8.x - ((x_multiple - 1) / 2)*rect8.width;
rect9.y = rect8.y - ((y_multiple - 1) / 2)*rect8.height;
rect9.width = rect8.width * x_multiple;
rect9.height = rect8.height * y_multiple;
if (rect8.height > height_threshold && rect8.x > 0 && rect8.x + rect8.width < 640 && rect8.y > 0 && rect8.y + rect8.height < 480)
model = src(rect8);
if(rect8.width/rect8.height>a_r_final)
{sizer = 1;}
if(rect8.width/rect8.height<a_r_final)
{sizer = 0;}
flag = 1;
markk = 1;
}
if (distancee > distance_single_value)
{
if (rect9.area() != 0 && rect8.area() != 0 && rect9.x > 0 && rect9.x + rect9.width<640 && rect9.y>0 && rect9.y + rect9.height < 480)
{
Mat similarity;
matchTemplate(src(rect9), model, similarity, match_num);
double mag_r;
Point point;
minMaxLoc(similarity, 0, &mag_r, 0, &point);
if (model.cols > height_threshold && mag_r > similar_degree)
{
box.x = point.x + rect9.x;
box.y = point.y + rect9.y;
box.width = rect8.width;
box.height = rect8.height;
center.x = box.x + box.width / 2;
center.y = box.y + box.height / 2;
rectangle(frame, box, Scalar(0, 0, 255), 2);
model = src(box);
rect9.x = box.x - ((x_multiple - 1) / 2)*box.width;
rect9.y = box.y - ((y_multiple - 1) / 2)*box.height;
rect9.width = box.width * x_multiple;
rect9.height = box.height * y_multiple;
flag = 1;
markk = 1;
zs = box.tl();
yx = box.br();
ys = zs + Point(box.width, 0);
zx = zs + Point(0, box.height);
point2d_da.push_back(zs);
point2d_da.push_back(ys);
point2d_da.push_back(yx);
point2d_da.push_back(zx);
solvePnP(point3d_da, point2d_da, Matrix, coefficients, r, trans);
trans.convertTo(zuo, CV_32F);
daji_3d.z = zuo.at<float>(0, 2);
if(box.width/box.height>a_r_final)
{sizer = 1;}
if(box.width/box.height<a_r_final)
{sizer = 0;}
}
if (mag_r < similar_degree)
{
flag = 0;
markk = 0;
}
}
}
}
if (flag == 0 && markk == 1)
{
//自动补充
rect6 = boundingRect(contours[array4[0]]);
rect7 = boundingRect(contours[array4[1]]);
rect8 = rect6 | rect7;
zs = rect8.tl();
yx = rect8.br();
ys = zs + Point(rect8.width, 0);
zx = zs + Point(0, rect8.height);
point2d_da.push_back(zs);
point2d_da.push_back(ys);
point2d_da.push_back(yx);
point2d_da.push_back(zx);
solvePnP(point3d_da, point2d_da, Matrix, coefficients, r, trans);
trans.convertTo(zuo, CV_32F);
daji_3d.z = zuo.at<float>(0, 2);
rect9.x = rect8.x - ((x_multiple - 1) / 2)*rect8.width;
rect9.y = rect8.y - ((y_multiple - 1) / 2)*rect8.height;
rect9.width = rect8.width * x_multiple;
rect9.height = rect8.height * y_multiple;
if (rect8.height > height_threshold && rect8.x > 0 && rect8.x + rect8.width < 640 && rect8.y > 0 && rect8.y + rect8.height < 480)
model = src(rect8);
center.x = (points4[0].x + points4[1].x) / 2;
center.y = (points4[0].y + points4[1].y) / 2;
if(rect8.width/rect8.height>a_r_final)
{sizer = 1;}
if(rect8.width/rect8.height<a_r_final)
{sizer = 0;}
flag = 1;
}
}
if (count4 < 1)
{
if (rect9.area() != 0 && rect8.area() != 0 && rect9.x > 0 && rect9.x + rect9.width<640 && rect9.y>0 && rect9.y + rect9.height < 480)
{
Mat similarity;
matchTemplate(src(rect9), model, similarity, match_num);
double mag_r;
Point point;
minMaxLoc(similarity, 0, &mag_r, 0, &point);
if (model.cols > height_threshold && mag_r > similar_degree)
{
box.x = point.x + rect9.x;
box.y = point.y + rect9.y;
box.width = rect8.width;
box.height = rect8.height;
center.x = box.x + box.width / 2;
center.y = box.y + box.height / 2;
rectangle(frame, box, Scalar(0, 0, 255), 2);
model = src(box);
rect9.x = box.x - ((x_multiple - 1) / 2)*box.width;
rect9.y = box.y - ((y_multiple - 1) / 2)*box.height;
rect9.width = box.width * x_multiple;
rect9.height = box.height * y_multiple;
flag = 1;
zs = box.tl();
yx = box.br();
ys = zs + Point(box.width, 0);
zx = zs + Point(0, box.height);
point2d_da.push_back(zs);
point2d_da.push_back(ys);
point2d_da.push_back(yx);
point2d_da.push_back(zx);
solvePnP(point3d_da, point2d_da, Matrix, coefficients, r, trans);
trans.convertTo(zuo, CV_32F);
daji_3d.z = zuo.at<float>(0, 2);
if(box.width/box.height>a_r_final)
{
sizer = 1;
}
if(box.width/box.height<a_r_final)
{sizer = 0;}
}
if (mag_r < similar_degree)
flag = 0;
}
}
//一开始设置size=3
if (center.x*center.y != 0 && sizer < 2)
{
flag = 1;
circle(frame, center, 2, Scalar(255, 255, 255), 2, 2);
if(sizer == 1)
{daji_3d.z = daji_3d.z * 2;}
if(sizer == 0)
{daji_3d.z = daji_3d.z * 1.2;}
enemy_msg.enemy_dist = daji_3d.z;
enemy_msg.enemy_yaw = center.x-320; //320为摄像头中心x坐标
enemy_msg.enemy_pitch = center.y-240; //同上
cout << center.x << " " << center.y << endl;
ROS_INFO("write[%d]",center.x); //发送数据
detection_flag=1;
//enemy_msg.mode=sizer;
}
else
{
flag = 0;
center.x=1000;
center.y=1000;
daji_3d.z=1000;
sizer=2;
enemy_msg.enemy_dist = daji_3d.z;
enemy_msg.enemy_yaw = center.x-320;
enemy_msg.enemy_pitch = center.y-240;
detection_flag=0;
//enemy_msg.mode=sizer;
}
if_send=true;
imshow("1", frame);
waitKey(1);
store_tem.x = center.x;
store_tem.y = center.y;
center.x = 0;
center.y = 0;
mark = 1;
markk = 1;
points1.clear();
points2.clear();
points3.clear();
points4.clear();
points5.clear();
return 0;
}
//------------------------------------------------------------