#include<opencv2/opencv.hpp>
#include<iostream>
#include <thread>
//#include "Serial.hpp"
#define USESERPORT
#define DEVICE "/dev/ttyUSB0"
#define FIRSPEED 31.0//子弹速度33m/s最好
#define DISTANCE 7//距离
#define h0 1.4//车与大R相对高度差
using namespace cv;
using namespace std;
const int color = 0;//0红 1蓝
const int type = 0;//0小符前两位+大符后两位 1大符（不用改，勿动）
const int shizhen = 1;//0顺时针 1逆时针
double rot_speed = 10.0;//风车转速
double PI = 3.1415926535;
double fcradius; //扇叶半径（定义的全局变量）
Point pROI;//扇叶上大装甲重心
Point fcp;//风车中心点符
Point2f pre_beat_center;//大符预测点
double Y, P, xuanwu, Y_big, P_big;
double seita;
double dx;//dx单位mm/pi、像素与实际坐标的转换、距离一定的情况下是一个定值
double ox;//图像坐标系\单位m、ox、oy
double oy;
double ox_big, oy_big;//大符时图像坐标系
double buff[4] = { 0,0,0,0 };
int fd = -1;
//void send()
//{
// while (1)
// {
// sendXYZ(fd, buff);
// cout << buff[0] << "***" << buff[1] << "***" << buff[2] << "***" << buff[3] << endl;
// usleep(__useconds_t(1e6 / 50)); /** 1e6/100 每秒100次*/
// }
//}
void armor_dete(const char *cap_id)
{
//#ifdef USESERPORT
// fd = openPort(DEVICE);
// if (fd != -1)
// {
// configurePort(fd);
// printf("open port /dev/ttyUSB0 done.\n");
// }
// else
// printf("open_port fail: Unable to open port. \n");
//#endif
cout << "图形处理线程即将开始" << endl;
//摄像头读取视频
VideoCapture capture;
capture = VideoCapture(CV_CAP_DSHOW);
//capture.open(1);
capture.open("E:\\vsproject\\Project2\\Project2\\a2.mp4");
capture.set(CV_CAP_PROP_FRAME_WIDTH, 640);
capture.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
Mat frame;
if (!capture.isOpened())
{
cout << "Cannot open the cam" << endl;
//return -1;
}
while (true)
{
capture >> frame;
//circle(frame, Point(320, 240), 15, Scalar(0, 0, 255), 1, 8);//画出(320,240)点*************************************
if (!frame.data)
{
break;
}
Mat imgHSV;
medianBlur(frame, frame, 7);
cvtColor(frame, imgHSV, COLOR_BGR2HSV);//rgb转为HSV
Mat imgdst, imgdst1, imgdst2, imgdst3, imgdst4, imgdst5;
if (color == 0)
{
//红色装甲版
inRange(imgHSV, Scalar(0, 96, 46), Scalar(6, 255, 255), imgdst1);
inRange(imgHSV, Scalar(174, 96, 46), Scalar(180, 255, 255), imgdst2);
imgdst = imgdst1 | imgdst2;
//imshow("hsv红色", imgdst);//******************
}
else if (color == 1)
{
//蓝色装甲版
inRange(imgHSV, Scalar(100, 43, 46), Scalar(124, 255, 255), imgdst1);//蓝色100.124 青色78.99 可能会掺杂灰色饱和度s
inRange(imgHSV, Scalar(78, 43, 46), Scalar(99, 255, 255), imgdst2);
inRange(imgHSV, Scalar(35, 43, 46), Scalar(77, 255, 255), imgdst3);
imgdst = imgdst1 | imgdst2 | imgdst3;
//imshow("hsv蓝色", imgdst);//******************
}
Mat element1 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element = getStructuringElement(MORPH_RECT, Size(2, 2));
//进行滤波操作
dilate(imgdst, imgdst, element);
erode(imgdst, imgdst, element);
//******开始找ROI******
vector<vector<Point>> contours;
vector<Vec4i>hierarchy;
Mat threshold_output;
threshold(imgdst, threshold_output, 50, 255, THRESH_BINARY);
findContours(threshold_output, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE, Point(0, 0));
vector<vector<Point> > contours_poly(contours.size());//******
vector<Rect> boundRect(contours.size());//******
Mat drawing = Mat::zeros(imgdst.size(), CV_8UC3);
bool one = true;
bool two = true;
bool three = true;
int zdy = 0;
vector<Moments> mu(contours.size());
for (int i = 0; i < contours.size(); i++)
{
//计算轮廓的面积
double Area = contourArea(contours[i]);
//找击打点(pROI.x,pROI.y)
if (Area > 1000 && Area < 16560)
{
if (one)
{
zdy++;
one = false;
}
drawContours(drawing, contours, i, Scalar(0, 0, 255), 2);
//包围矩形
approxPolyDP(Mat(contours[i]), contours_poly[i], 3, true);//******
boundRect[i] = boundingRect(Mat(contours[i]));//******
rectangle(drawing, boundRect[i].tl(), boundRect[i].br(), Scalar(0, 255, 0), 2, 8, 0);//******
//******最小包围圆******
Point2f center;//变量圆心坐标
float radius = 0;//初始化扇叶最小包围圆半径
minEnclosingCircle(contours[i], center, radius);//寻找最小包围圆
circle(drawing, center, cvRound(radius), Scalar(0, 255, 0), 1, 8);//绘制最小包围圆，cvRound对半径进行四舍五入
circle(drawing, center, 1, Scalar(0, 255, 0), 3, 8);//圆心
double mianji = 3.14*radius*radius;//最小包围圆面积
double bizhi = mianji / Area;//模拟6.0左右 官方4.2
//最小包围矩形
RotatedRect minRect = minAreaRect(Mat(contours[i]));
Point2f vertex[4];//用于存放最小矩形的四个顶点
minRect.points(vertex);//返回矩形的四个顶点给vertex
//绘制最小面积包围矩形
vector<Point>min_rectangle;
for (int i = 0; i < 4; i++)
{
line(drawing, vertex[i], vertex[(i + 1) % 4], Scalar(255, 0, 0), 1, 8);//非常巧妙的表达式
min_rectangle.push_back(vertex[i]);//将最小矩形的四个顶点以Point的形式放置在vector容器中
}
double recArea = contourArea(min_rectangle);//最小包围矩形面积
double bizhi2 = recArea / Area;//最小包围矩形面积/轮廓面积
//找轮廓重心
mu[i] = moments(contours[i], false);
double a = mu[i].m10 / mu[i].m00;
double b = mu[i].m01 / mu[i].m00;
Point p = Point(a, b);
circle(drawing, p, 1, Scalar(0, 0, 255), 3, 8);//重心
if (bizhi >= 3.5 && bizhi <= 7 && bizhi2 >= 2 && bizhi2 <= 3.3)//扇叶最小包围圆与扇叶轮廓比值
{
if (two)
{
zdy++;
two = false;
}
//切割ROI
Mat ROI = threshold_output(Rect(boundRect[i].tl(), boundRect[i].br()));
//定义轮廓和层次结构
vector<vector<Point>> contoursROI;
vector<Vec4i> hierarchyROI;
//查找轮廓ROI
findContours(ROI, contoursROI, hierarchyROI, CV_RETR_TREE, CHAIN_APPROX_SIMPLE);
Mat drawingROI = Mat::zeros(ROI.size(), CV_8UC3);
for (int j = 0; j < contoursROI.size(); j++)
{
//计算轮廓的面积ROI
double AreaROI = contourArea(contoursROI[j]);
if (Area / AreaROI < 6 && Area / AreaROI >1.5)//模拟2.3 官方2.85轮廓面积与ROI轮廓面积比值，找到ROI全部轮廓中的矩形的那个
{
if (three)
{
zdy++;
three = false;
}
drawContours(drawingROI, contoursROI, j, Scalar(0, 0, 255), 2);
//找轮廓重心ROI
vector<Moments> muROI(contoursROI.size());
muROI[j] = moments(contoursROI[j], false);
double aROI = muROI[j].m10 / muROI[j].m00;
double bROI = muROI[j].m01 / muROI[j].m00;
pROI = Point(aROI + boundRect[i].tl().x, bROI + boundRect[i].tl().y);
circle(frame, pROI, 1, Scalar(0, 255, 0), 3, 8);//重心roi
putText(frame, "(" + to_string(pROI.x) + "," + to_string(pROI.y) + ")", Point(pROI.x + 11, pROI.y + 11), 2, 0.8, Scalar(0, 255, 0));
fcradius = radius;//扇叶半径是否存入全局变量
//测距改
dx = 600 / (2 * radius);
double yu = DISTANCE / dx;
cout << "7m虚拟像素距离yu为" << yu << endl;
xuanwu = 1230 * dx;
cout << "当前实际距离" << xuanwu << endl;
ox = (pROI.x - 320)*dx;//图像坐标系\单位mm、ox、oy
oy = (240 - pROI.y)*dx;
}
}
}
}
//找风车中心点(fcx,fcy)
if (Area > ((fcradius / 11 * 2)*(fcradius / 11 * 2)) && Area < ((fcradius / 5.5 * 2)*(fcradius / 5.5 * 2)))//约束找出大R（圈1.扇叶半径/大R半径大约为9[guanfang]5.5[moni]）******10-3
{
drawContours(drawing, contours, i, Scalar(0, 0, 255), 2);
//包围矩形---2
approxPolyDP(Mat(contours[i]), contours_poly[i], 3, true);//******大R
boundRect[i] = boundingRect(Mat(contours[i]));//******大R
//最小包围圆---2
Point2f center;//大R圆心坐标
float radius = 0;//初始大R最小包围圆半径
minEnclosingCircle(contours[i], center, radius);//寻找大R最小包围圆
circle(drawing, center, radius, Scalar(0, 255, 0), 1, 8);
double quan2 = (PI * radius * radius) / Area;//官方视频1.39~1.46```模拟神符1.5
//进一步约束（有杂点日后加强）
if (quan2 < 2)
{
//找轮廓重心
vector<Moments> mu(contours.size());
mu[i] = moments(contours[i], false);
double fcx = mu[i].m10 / mu[i].m00;
double fcy = mu[i].m01 / mu[i].m00;
fcp = Point(fcx, fcy);//风车中心点
circle(drawing, fcp, 1, Scalar(0, 255, 0), 3, 8);//风车中心点
circle(frame, fcp, 1, Scalar(0, 255, 0), 3, 8);//风车中心点
//跟随预测******参数shizhen：0顺时针1逆时针
seita = atan2((pROI.y - fcp.y), (pROI.x - fcp.x));//相位角弧度制
double fly_time = DISTANCE / FIRSPEED;//子弹飞的时间
double pre_seita;
if (shizhen == 0)
{
pre_seita = seita + (fly_time * (rot_speed / 60)) * PI * 2 + 0.2;//预测位置的相位角
}
else if (shizhen == 1)
{
pre_seita = seita - (fly_time * (rot_speed / 60)) * PI * 2 + 0.0;//预测位置的相位角
}
double rou = sqrt(pow(fcp.x - pROI.x, 2) + pow(fcp.y - pROI.y, 2));//极径
pre_beat_center = Point2f((rou - 0)*cos(pre_seita) + fcp.x, (rou - 0)*sin(pre_seita) + fcp.y);
line(frame, pre_beat_center, fcp, Scalar(148, 0, 211));
circle(frame, pre_beat_center, 1, Scalar(0, 0, 255), 3, 8);//pre_beat_center
//返角度******以光心为图像坐标系、相机坐标系圆点
ox_big = (pre_beat_center.x - 320)*dx;//图像坐标系\单位m、ox、oy
oy_big = (240 - pre_beat_center.y)*dx;
//cout << "相机坐标系(" << ox << "," << oy << "," << xuanwu << ")mm" << endl;//关心为原点、单位mm
}
}
}
//******弹道补偿******
double h1 = (pROI.y - fcp.y) * dx / 1000.0;//大R相对于打击的高度差m
double h = h0 - h1;//车与打击点相对高度差m
double beita = atan(h / DISTANCE);//P轴枪口与水平夹角
double t = DISTANCE / (FIRSPEED*cos(beita));//子弹飞行时间s
double deerta_H = 9.8*t*t / 2.0;//子弹下落高度me
double deerta_pROIy = deerta_H * 1000 / dx;//子弹下落高度pi
circle(frame, Point(pre_beat_center.x, pre_beat_center.y - deerta_pROIy), 1, Scalar(255, 0, 0), 3, 8);//枪口补偿后打击点
//弹道补偿后结算角度(小符)
Y = atan(ox / xuanwu);
P = atan((oy - deerta_H * 1000) / xuanwu);//枪口补偿后
//弹道补偿后结算角度(大符)
Y_big = atan(ox_big / xuanwu);
//P_big = atan(oy_big / xuanwu);
P_big = atan((oy_big - deerta_H * 1000) / xuanwu);//枪口补偿后
//串口通信
if (zdy == 3)
{
if (type == 0)//小符角度+大符角度
{
buff[0] = (Y / PI * 180 - 0) * 1000;//*1000发出小符Y******
buff[1] = P / PI * 180 * 1000;//小符P******
// buff[2]=xuanwu;
// buff[3]=seita / PI * 180;
buff[2] = (Y_big / PI * 180 - 0) * 1000;//大符Y******预测角加减调rou
buff[3] = P_big / PI * 180 * 1000;//大符P******
//cout<<"xiaofu"<<buff[0]<<endl;
}
else if (type == 1)//大符角度（舍弃）
{
buff[0] = (Y_big / PI * 180 - 0) * 1000;//*1000发出
buff[1] = P_big / PI * 180 * 1000;
buff[2] = xuanwu;
buff[3] = seita / PI * 180;
//cout<<"dafu"<<buff[0]<<endl;
}
}
else
{
buff[0] = 0;
buff[1] = 0;
buff[2] = 0;
buff[3] = 0;
}
//cout << "角度Y=" << buff[0] / 1000 << " P=" << buff[1] / 1000 << " 大符Y=" << buff[2] / 1000 << " 大符P=" << buff[3] / 1000 << endl;
cout << buff[0] << "***" << buff[1] << "***" << buff[2] << "***" << buff[3] << endl;
//imshow("轮廓", drawing);//******************
imshow("原图", frame);//原图
waitKey(1);
}
}
int main()
{
//armor_dete();
fd = 0;
std::thread armor_dec_th0(armor_dete, "/dev/video1");
armor_dec_th0.join();
return 0;
}YU
猜你喜欢
转载自www.cnblogs.com/xingkongcanghai/p/11973112.html
今日推荐
周排行