Robotmasters

#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;
}