#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <time.h>
#include <stdio.h>
#include <string.h>
#include<stdint.h>
#include <unistd.h>
#include <fcntl.h> /* File control definitions */
#include <errno.h>
#include <termios.h> /* POSIX terminal control definitions */
#include "ros/ros.h"
using namespace cv;
using namespace std;
//using namespace boost::asio;
//#define drawArmor(...)
//cvMat 版本
/*
int main()
{
//读取视频文件
VideoCapture capture("/home/mzm/robomasters/robomasters/stepRedSide.MOV");
if(!capture.isOpened())
return 1;
double rate=capture.get(CV_CAP_PROP_FPS);
Mat frame,tmp,dst,hsv,result;
int delay=1000/rate;
//创建调参模块
namedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"
//实验出的最佳参数:H(0,15),S(130,255),V(90,255)
int iLowH = 0;
int iHighH = 15;
int iLowS = 130;
int iHighS = 255;
int iLowV = 90;
int iHighV = 255;
//Create trackbars in "Control" window
cvCreateTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
cvCreateTrackbar("HighH", "Control", &iHighH, 179);
cvCreateTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
cvCreateTrackbar("HighS", "Control", &iHighS, 255);
cvCreateTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
cvCreateTrackbar("HighV", "Control", &iHighV, 255);
while(1)
{
capture>>frame;
//压缩图片
tmp = frame;
dst = tmp;
pyrDown( tmp, dst, Size( tmp.cols/2, tmp.rows/2 ) );
//转换成HSV格式
//GaussianBlur(dst,dst,Size(3,3),0,0);
cvtColor(dst, hsv, CV_BGR2HSV );
//vector<Mat> hsvSplit;
//split(hsv, hsvSplit);
//equalizeHist(hsvSplit[2],hsvSplit[2]);
//merge(hsvSplit,hsv);
inRange(hsv, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV),result); //Threshold the image
//去除噪点,连接连通域
//Mat element = getStructuringElement(MORPH_RECT, Size(1, 1));
//morphologyEx(result, result, MORPH_OPEN, element);
//morphologyEx(result, result, MORPH_CLOSE, element);
//膨胀
//dilate(result,result,Mat(3,3,CV_8U),Point(-1,-1),1);
//显示压缩后原图
imshow( "yasuo", dst);
//显示HSV提取后图像
imshow("hsv",result);
waitKey(delay);
}
capture.release();
}*/
void findRed(Mat &src, vector<Rect> &reds);
void findArmor(Mat &src, vector<Rect> &reds);
void drawArmor(vector<Point>&l1, vector<Point>&l2, Mat &src);
bool adjacentLights(vector<Point>&l1,vector<Point>&l2);
void drawBoundingRect(vector<Point>&l, Scalar color,Mat &src);
double dis_x = 0;
double dis_y = 0;
typedef struct{
uint16_t dev_yaw;
uint16_t dev_pitch;
uint16_t target_vl;
uint16_t target_dis;
uint8_t DLC;
}xdata_to_send;
void vInsert( uint8_t a[], uint8_t i, uint8_t n, uint8_t number){
for (int j=n;j>i;j--){
a[j]=a[j-1];
}
a[i]=number;
if (i==n)
a[i]=number;
}
void vCheck( uint8_t a[] ){
for(uint8_t i = 1; i <= 8; i++)
{switch ( a[i] ){
case 0x7d : vInsert( a, i, 14, 0xff);
a[i+1] = 0X00; break;
case 0x7e : vInsert( a, i, 14, 0xff);
a[i+1] = 0X01; break;
case 0xff : vInsert( a, i, 14, 0xff);
a[i+1] = 0X02; break;
}}
}
int vSendUart(int fd,xdata_to_send data)
{
uint8_t tempdata[14] = {0,0,0,0,0,0,0,0,0,0,0,0};
tempdata[0] = 0x7d;
tempdata[1] = data.dev_yaw >> 8;
tempdata[2] = data.dev_yaw & 0x00ff;
tempdata[3] = data.dev_pitch >> 8;
tempdata[4] = data.dev_pitch & 0x00ff;
tempdata[5] = data.target_vl >> 8;
tempdata[6] = data.target_vl & 0x00ff;
tempdata[7] = data.target_dis >> 8;
tempdata[8] = data.target_dis & 0x00ff;
tempdata[9] = data.DLC;
tempdata[10] = 0x7e;
vCheck( tempdata );
uint8_t i ;
//for(i = 0; i <14;i++)
//{
// printf("%x\n", tempdata[i]);
//}
return write(fd, tempdata, 14);
}
int set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
{
struct termios newtio,oldtio;
if ( tcgetattr( fd,&oldtio) != 0)
{
perror("SetupSerial 1");
return -1;
}
bzero( &newtio, sizeof( newtio ) );
newtio.c_cflag |= CLOCAL | CREAD;
newtio.c_cflag &= ~CSIZE;
newtio.c_cflag &= ~CRTSCTS;//不使用流控制
switch( nBits )
{
case 7:
newtio.c_cflag |= CS7;
break;
case 8:
newtio.c_cflag |= CS8;
break;
}
switch( nEvent )
{
case 'O': //奇校验
newtio.c_cflag |= PARENB;
newtio.c_cflag |= PARODD;
//newtio.c_iflag |= (INPCK | ISTRIP);
break;
case 'E': //偶校验
//newtio.c_iflag |= (INPCK | ISTRIP);
newtio.c_cflag |= PARENB;
newtio.c_cflag &= ~PARODD;
break;
case 'N': //无校验
newtio.c_cflag &= ~PARENB;
break;
}
switch( nSpeed )
{
case 2400:
cfsetispeed(&newtio, B2400);
cfsetospeed(&newtio, B2400);
break;
case 4800:
cfsetispeed(&newtio, B4800);
cfsetospeed(&newtio, B4800);
break;
case 9600:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
case 115200:
cfsetispeed(&newtio, B115200);
cfsetospeed(&newtio, B115200);
break;
default:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
}
if( nStop == 1 )
{
newtio.c_cflag &= ~CSTOPB;
}
else if ( nStop == 2 )
{
newtio.c_cflag |= CSTOPB;
}
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 0;
tcflush(fd,TCIFLUSH);
if((tcsetattr(fd,TCSANOW,&newtio))!=0)
{
perror("com set error");
return -1;
}
printf("set done!\n");
return 0;
}
int main(int argc,char *argv[])
{
//int fdstd= open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);
//dup2(fdstd,1);
cout<<"1"<<endl;
//读取视频文件
cv::VideoCapture cap(0);
if(!cap.isOpened())
return -1;
//int rate=cap.get(CV_CAP_PROP_FRAME_COUNT);
//cap.set(CV_CAP_PROP_POS_FRAMES,230);
//cout<<rate<<endl;
//double delay=1000/rate;
cout<<"camera opened\n"<<endl;
int fd= open("/dev/ttyTHS0", O_RDWR | O_NOCTTY | O_NDELAY);
if(fd <0)
{
perror("open_port error");
return -1;
}
int i;
if((i=set_opt(fd,115200,8,'N',1))<0)
{
perror("set_opt error");
return -1;
}
xdata_to_send ab;
char buf[1000];
while(cap.isOpened())
{
clock_t start, finish;
start = clock();
Mat tmp;
Mat dst;
Mat red;
cap>>tmp;
cv::resize(tmp,tmp,Size(640,480));
//cv::imshow("src",tmp);
dst = tmp;
GaussianBlur(dst,dst,Size(3,3),0,0);
//
vector<Rect>reds;
dis_x=0;
dis_y=0;
findRed(tmp,reds);
findArmor(tmp,reds);
//write serialport
// ab.dev_pitch=0;
// ab.dev_yaw=0;
ab.DLC=8;
ab.dev_pitch=57.8*atan2(dis_y,933.185)*100+5000;
ab.dev_yaw=57.8*atan2(dis_x,932.843)*100+9000;
int n=vSendUart(fd,ab);
if (n < 0)
printf("write() of 9 bytes failed!\n");
n=read(fd, buf, 30);
if(n >0)
printf("recieved:%s\r\n",buf);
else
printf("recieve bytes failed!\n");
printf("pitch:%f yaw:%f\r\n",57.8*atan2(dis_y,933.185),57.8*atan2(dis_x,932.843));
if(dis_x >= -10 && dis_x <= 10){
//write(sp,buffer("R0XXXXX\r\n",10));
cout<<"R0"<<endl;
}else if(dis_x < -180){
//write(sp,buffer("L3XXXXX\r\n",10));
cout<<"L3"<<endl;
}else if(dis_x < -90){
//write(sp,buffer("L2XXXXX\r\n",10));
cout<<"L2"<<endl;
}else if(dis_x < 0){
//write(sp,buffer("L1XXXXX\r\n",10));
cout<<"L1"<<endl;
}else if(dis_x >180){
//write(sp,buffer("R3XXXXX\r\n",10));
cout<<"R3"<<endl;
}else if(dis_x > 90){
//write(sp,buffer("R2XXXXX\r\n",10));
cout<<"R2"<<endl;
}else if(dis_x > 0){
//write(sp,buffer("R1XXXXX\r\n",10));
cout<<"R1"<<endl;
}
//ROS_INFO("loop");
Point pl(0,240);
Point pr(640,240);
Point pu(320,0);
Point pd(320,480);
line(tmp,pl,pr,Scalar(0,255,0),1);
line(tmp,pu,pd,Scalar(0,255,0),1);
imshow("FINAL",tmp);
finish = clock();
double duration = (double)(finish - start) / CLOCKS_PER_SEC;
printf( "%f seconds\n", duration );
cout<<"dis_x = "<<dis_x<<"; dis_y = "<<dis_y<<endl;
waitKey(1);
}
cap.release();
return 0;
}
void findRed(Mat &src, vector<Rect> &reds)
{
Mat dst ;
Mat hsv,result;
// GaussianBlur(src,dst,Size(3,3),0,0);
// imshow("blur",dst);
cv::cvtColor(src, hsv, CV_BGR2HSV );
//static int iLowH = 0;
//static int iHighH = 15;
//static int iLowS = 80;
//static int iHighS = 255;
//static int iLowV = 160;
//static int iHighV = 255;
static int iLowH = 0;
static int iHighH = 5;
static int iLowS = 63;
static int iHighS = 255;
static int iLowV = 102;
static int iHighV = 255;
//Create trackbars in "Control" window
//cv::createTrackbar("LowH", "src", &iLowH, 179); //Hue (0 - 179)
//cv::createTrackbar("HighH", "src", &iHighH, 179);
//cv::createTrackbar("LowS", "src", &iLowS, 255); //Saturation (0 - 255)
//cv::createTrackbar("HighS", "src", &iHighS, 255);
//cv::createTrackbar("LowV", "src", &iLowV, 255); //Value (0 - 255)
//cv::createTrackbar("HighV", "src", &iHighV, 255);
/*
namedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"
cvCreateTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
cvCreateTrackbar("HighH", "Control", &iHighH, 179);
cvCreateTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
cvCreateTrackbar("HighS", "Control", &iHighS, 255);
cvCreateTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
cvCreateTrackbar("HighV", "Control", &iHighV, 255);
*/
cv::inRange(hsv, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV),result); //Threshold the image
// cv::imshow("red bin ",result);
Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
// cv::dilate(result,result,element,Point(-1,-1),5);
// element = getStructuringElement(MORPH_RECT, Size(3, 1));
cv::dilate(result,result,element,Point(-1,-1),2);
// element = getStructuringElement(MORPH_RECT, Size(2, 2));
// cv::erode(result,result,element,Point(-1,-1),2);
// find red region
vector<vector<Point > >redRegion;
cv::findContours(result,redRegion,CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE);
Mat temp;
src.copyTo(temp);
for(int i=0;i<redRegion.size();i++)
{
Rect r=cv::boundingRect(redRegion.at(i));
Rect r1=Rect(r.x-4,r.y-13,r.width+8,r.height+26);
reds.push_back(r1);
cv::rectangle(temp,r1,Scalar(0,0,255),2,4);
}
//cv::imshow("red",temp);
}
void findArmor(Mat &src, vector<Rect> &reds)
{
if(src.channels()!=3)
return;
Mat gray;
cv::cvtColor(src,gray,COLOR_RGB2GRAY);
Mat bin;
//cv::namedWindow("bin",1);
static int binThresh=170;
//cv::createTrackbar("binthresh","bin",&binThresh,255);
cv::threshold(gray,bin,binThresh,255,THRESH_BINARY);
Mat element=cv::getStructuringElement(MORPH_RECT,Size(2,2));
cv::dilate(bin,bin,element,Point(-1,-1),1);
//cv::imshow("bin",bin);
std::vector<std::vector<cv::Point> > contours;
Mat temp;
src.copyTo(temp);
cv::findContours(bin,contours,CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE);
if(contours.size()>0)
{
vector<vector<Point > > lights;
for(int i=0;i<contours.size();i++ )
{
int a=cv::contourArea(contours.at(i));
if(a>1500)
continue;
// in red region?
int ctn=0;
for(int j=0;j<reds.size();j++)
{
Rect redRect=reds.at(j);
RotatedRect conRect=cv::minAreaRect(contours.at(i));
if(redRect.contains(conRect.center))
{
ctn++;
break;
}
}
if(ctn==0)
continue;
Rect br=cv::boundingRect(contours.at(i));
if(br.width>br.height)
continue;
if(br.width*br.height<35.0)
continue;
// double max=br.width>br.height?br.width:br.height;
// double min=br.width<br.height?br.width:br.height;
Point2f pt1[4];
RotatedRect r1=cv::minAreaRect(contours.at(i));
r1.points(pt1);
double a1=sqrt(pow(pt1[0].x-pt1[3].x,2)+pow(pt1[0].y-pt1[3].y,2));
double b1=sqrt(pow(pt1[0].x-pt1[1].x,2)+pow(pt1[0].y-pt1[1].y,2));
double max=a1>b1?a1:b1;
double min=a1<b1?a1:b1;
if((max+0.000001)/(min+0.000001)<1.7)
continue;
lights.push_back(contours[i]);
}
drawContours(src,lights,-1,Scalar(255,255,255),3);
for(int i=0;i<lights.size();i++)
{
drawBoundingRect(lights.at(i),Scalar(255,0,0),src);
}
vector<vector<Point > >armlights;
for(int i=0;i<lights.size();i++)
{
for(int j=0;j<lights.size();j++)
{
if(i==j)
continue;
if(adjacentLights(lights.at(i),lights.at(j)))
{
armlights.push_back(lights.at(i));
// drawArmor(lights.at(i),lights.at(j),src,Scalar(0,255,0));
drawBoundingRect(lights.at(i),Scalar(0,0,255),src);
break;
}
}
}
if(armlights.size()==2)
{
drawArmor(armlights.at(0),armlights.at(1),src/*,Scalar(255,255,0)*/);
}
if(armlights.size()==3)
{
vector<vector<Point> > sortLights;
int id[3];
for(int k=0;k<3;k++)
{
if(minAreaRect(armlights.at(k%3)).center.x<=minAreaRect(armlights.at((k+1)%3)).center.x&&
minAreaRect(armlights.at(k%3)).center.x<=minAreaRect(armlights.at((k+2)%3)).center.x)
id[0]=k;
else if(minAreaRect(armlights.at(k%3)).center.x>=minAreaRect(armlights.at((k+1)%3)).center.x&&
minAreaRect(armlights.at(k%3)).center.x>=minAreaRect(armlights.at((k+2)%3)).center.x)
id[2]=k;
else
id[1]=k;
}
for(int k=0;k<3;k++)
sortLights.push_back(armlights.at(id[k]));
RotatedRect r0=cv::minAreaRect(sortLights[0]);
RotatedRect r1=cv::minAreaRect(sortLights[1]);
RotatedRect r2=cv::minAreaRect(sortLights[2]);
double ag0=r0.angle<-45?r0.angle+90:r0.angle;
double ag1=r1.angle<-45?r1.angle+90:r1.angle;
double ag2=r2.angle<-45?r2.angle+90:r2.angle;
double t01=fabs(ag0-ag1);
double t12=fabs(ag1-ag2);
if(ag0*ag1<0)
drawArmor(sortLights.at(1),sortLights.at(2),src/*,Scalar(255,255,0)*/);
else if(ag1*ag2<0)
drawArmor(sortLights.at(1),sortLights.at(0),src/*,Scalar(255,255,0)*/);
else
{
if(t01<t12)
drawArmor(sortLights.at(1),sortLights.at(0),src/*,Scalar(255,255,0)*/);
else
drawArmor(sortLights.at(1),sortLights.at(2),src/*,Scalar(255,255,0)*/);
}
// if(adjacentLights(armlights[0],armlights[1])&&
// adjacentLights(armlights[0],armlights[2])&&adjacentLights(armlights[1],armlights[2]))
// {
// RotatedRect r0=cv::minAreaRect(armlights[0]);
// RotatedRect r1=cv::minAreaRect(armlights[1]);
// RotatedRect r2=cv::minAreaRect(armlights[2]);
// double ag0=r0.angle<-45?r0.angle+90:r0.angle;
// double ag1=r1.angle<-45?r1.angle+90:r1.angle;
// double ag2=r2.angle<-45?r2.angle+90:r2.angle;
// double t01=fabs(ag0-ag1);
// double t02=fabs(ag0-ag2);
// double t12=qFabs(ag1-ag2);
// if(t01<t02&&t01<t12)
// {
// drawArmor(armlights.at(0),armlights.at(1),src,Scalar(255,255,0));
// }
// if(t02<t01&&t02<t12)
// {
// drawArmor(armlights.at(0),armlights.at(2),src,Scalar(255,255,0));
// }
// if(t12<t01&&t12<t02)
// {
// drawArmor(armlights.at(1),armlights.at(2),src,Scalar(255,255,0));
// }
// }
// else
// {
// if(!adjacentLights(armlights[0],armlights[1])&&!adjacentLights(armlights[0],armlights[2]))
// drawArmor(armlights.at(1),armlights.at(2),src,Scalar(255,255,0));
// if(!adjacentLights(armlights[1],armlights[0])&&!adjacentLights(armlights[1],armlights[2]))
// drawArmor(armlights.at(0),armlights.at(2),src,Scalar(255,255,0));
// if(!adjacentLights(armlights[2],armlights[1])&&!adjacentLights(armlights[2],armlights[0]))
// drawArmor(armlights.at(1),armlights.at(0),src,Scalar(255,255,0));
// }
}
if(armlights.size()==4)
{
//sort
vector<int> ids;
for(int i=0;i<armlights.size();i++)
{
// RotatedRect r=cv::minAreaRect(armlights.at(i));
// qDebug()<<"rect center x:"<<r.center.x;
double min=10000.0;
int id1=-1;
for(int j=0;j<armlights.size();j++)
{
bool flag=false;
for(int k=0;k<ids.size();k++)
{
if(ids.at(k)==j)
{
flag=true;
break;
}
}
if(flag)
continue;
RotatedRect r=cv::minAreaRect(armlights.at(j));
if(r.center.x<min)
{
min=r.center.x;
id1=j;
}
}
ids.push_back(id1);
}
RotatedRect r1=cv::minAreaRect(armlights.at(ids.at(1)));
RotatedRect r2=cv::minAreaRect(armlights.at(ids.at(2)));
double ag1=r1.angle<-45?r1.angle+90:r1.angle;
double ag2=r2.angle<-45?r2.angle+90:r2.angle;
if(ag1*ag2>=0)
{
drawArmor(armlights.at(ids.at(1)),armlights.at(ids.at(2)),src/*,Scalar(255,255,0)*/);
}
else
{
drawArmor(armlights.at(ids.at(0)),armlights.at(ids.at(1)),src/*,Scalar(255,255,0)*/);
drawArmor(armlights.at(ids.at(2)),armlights.at(ids.at(3)),src/*,Scalar(255,255,0)*/);
}
}
}
//cv::imshow("contour",src);
}
//*********************************************************************************//
void drawArmor(vector<Point>&l1, vector<Point>&l2, Mat &src)
{
Rect br1=cv::boundingRect(l1);
Rect br2=cv::boundingRect(l2);
double dis=0;
Point p3(320,240);
cv::circle(src,p3,5,cv::Scalar(0,255,0));
//double
if(br1.x>br2.x)
{
Point p1(br1.x+br1.width+10,br1.y+br1.height+10);
Point p2(br2.x-10,br2.y-10);
Point apc((p1.x+p2.x)/2,(p1.y+p2.y)/2);
line(src,apc,p3,Scalar(0,0,255),2);
// cv::rectangle(src,p2,p1,Scalar(0,0,255),2);
cv::rectangle(src,p2,p1,Scalar(255,255,0),2);
//dis=sqrt(((p1.x+p2.x)/2-p3.x)*((p1.x+p2.x)/2-p3.x)+((p1.y+p2.y)/2-p3.y)*((p1.y+p2.y)/2-p3.y));
dis_x = ((p1.x-320)+(p2.x-320))/2;
dis_y = (-1)*((p1.y-240)+(p2.y-240))/2;
//cout<<"dis="<<dis<<endl;
//cv::circle(src,p2,2,cv::Scalar(0,255,0));
//cv::circle(src,p1,2,cv::Scalar(0,0,255));
}
else
{
Point p1(br1.x-10,br1.y-10);
Point p2(br2.x+br2.width+10,br2.y+br2.height+10);
Point apc((p1.x+p2.x)/2,(p1.y+p2.y)/2);
line(src,apc,p3,Scalar(0,0,255),2);
// cv::rectangle(src,p2,p1,Scalar(0,0,255),2);
cv::rectangle(src,p2,p1,Scalar(255,255,0),2);
//dis=sqrt(((p1.x+p2.x)/2-p3.x)*((p1.x+p2.x)/2-p3.x)+((p1.y+p2.y)/2-p3.y)*((p1.y+p2.y)/2-p3.y));
dis_x = ((p1.x-320)+(p2.x-320))/2;
dis_y = (-1)*((p1.y-240)+(p2.y-240))/2;
//cout<<"dis="<<dis<<endl;
//cv::circle(src,p2,2,cv::Scalar(0,255,0));
//cv::circle(src,p1,2,cv::Scalar(0,0,255));
}
}
void drawBoundingRect(vector<Point>&l, Scalar color,Mat &src)
{
RotatedRect r=cv::minAreaRect(l);
Point2f pt[4];
r.points(pt);
cv::line(src,pt[0],pt[1],color,2);
cv::line(src,pt[1],pt[2],color,2);
cv::line(src,pt[2],pt[3],color,2);
cv::line(src,pt[3],pt[0],color,2);
}
bool adjacentLights(vector<Point>&l1,vector<Point>&l2)
{
Point2f pt1[4];
RotatedRect r1=cv::minAreaRect(l1);
r1.points(pt1);
RotatedRect r2=cv::minAreaRect(l2);
Point2f pt2[4];
r2.points(pt2);
double ag1=r1.angle;
double ag2=r2.angle;
if(ag1<-45)
ag1+=90;
if(ag2<-45)
ag2+=90;
double t=fabs(ag1-ag2);
if(t>9.1||ag1*ag2<0)
{
//qDebug()<<"different slope";
return false;
}
double x12=r1.center.x-r2.center.x;
double y12=r1.center.y-r2.center.y;
double anglel;
if(x12>=0)
anglel=atan2(y12,x12)*180/3.141592;
else
anglel=atan2(-y12,-x12)*180/3.141592;
double angle1=r1.angle;
double angle2=r2.angle;
if(angle1<-45)
angle1+=90;
if(angle2<-45)
angle2+=90;
// qDebug()<<angle1<<" "<<angle2;
double angle12=(angle1+angle2)/2;
// qDebug("angle1 2 after,%d,%d",ag1,ag2);
// qDebug("error %d",t);
// qDebug("(%lf,%lf),(%lf,%lf)",r1.center.x,r1.center.y,r2.center.x,r2.center.y);
// qDebug("angle line :%lf,angle rect%lf",anglel,angle12);
if(fabs(angle1-anglel)>20.0||fabs(angle2-anglel)>20.0)
{
//qDebug()<<"not prependicular with central line";
return false;
}
//draw rectangle
Rect br1=cv::boundingRect(l1);
double max1=br1.width>br1.height?br1.width:br1.height;
double x1=br1.width/2.0+br1.x;
double y1=br1.height/2.0+br1.y;
Rect br2=cv::boundingRect(l2);
double max2=br2.width>br2.height?br2.width:br2.height;
double x2=br2.width/2.0+br2.x;
double y2=br2.height/2.0+br2.y;
double max=max1>max2?max1:max2;
double dist=sqrt(pow(x1-x2,2)+pow(y1-y2,2));
if(dist/max>6.1||dist/max<0.5)
{
//qDebug()<<"too far or too close";
return false;
}
return true;
}
Robotmasters
最新推荐文章于 2022-08-11 13:42:23 发布