import cv2
import numpy as np
'''
1、预处理---原图形处理，得到源坐标点即特征点（后续数据工具处理得到，这里只是方便识别调配）
2、读取摄像头后，根据颜色区间不同，提取roi区域，图像滤波平滑处理(这里使用中值滤波)
3、根据面积最大区域，拟合多边形，并返回特征点approx**，用作后续match
4、match，点点距离

待优化：
1、颜色提取:白平衡？or 深度学习？（橙色识别问题很大）
2、匹配算法
3、有时异常报错，怀疑也是颜色的锅。。。。。。
'''

#原图形处理
frame1 = cv2.imread('D:/python/opencv/picture/test1.jpg')
r,c,h = frame1.shape

frame1 = cv2.resize(frame1,(int(c/2),int(r/2)))


img11 = cv2.cvtColor(frame1,cv2.COLOR_BGR2HSV)

#紫色三角

lopul1 = np.array([125,20,30])
uppul1 = np.array([155,255,255])
maskpul1 = cv2.inRange(img11,lopul1,uppul1)

respul1 = cv2.bitwise_and(frame1,frame1,mask = maskpul1)
kernel = np.ones((5,5),np.uint8) 
maskpul1 = cv2.medianBlur(maskpul1, 5)#中值
maskpul1 = cv2.morphologyEx(maskpul1,cv2.MORPH_OPEN,kernel)    ##效果最好
    

contpul1,hierpul1 = cv2.findContours(maskpul1,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)  #轮廓检索

cntpul1 = contpul1[0]
max_pul1 = cv2.contourArea(cntpul1)

for contp1 in contpul1:
    if cv2.contourArea(contp1) > max_pul1:
        cntpul1 = contp1
        max_pul1 = cv2.contourArea(contp1)   #最大面积确定roi

    
perimeterpul1 = cv2.arcLength(cntpul1,True)
epsilonpul1 = 0.05*cv2.arcLength(cntpul1,True)  #拟合逼近 常数值越大，拟合度越高，信息丢失越明显
approxpul1 = cv2.approxPolyDP(cntpul1,epsilonpul1,True)   #逼近点

hullpul1 = cv2.convexHull(cntpul1) 


cv2.imshow('respul1',respul1)
cv2.imshow('maskpul1',maskpul1)


#红色三角
lored11 = np.array([156,20,46])
upred11 = np.array([180,255,255])

maskred1 = cv2.inRange(img11,lored11,upred11)

resred1 = cv2.bitwise_and(frame1,frame1,mask = maskred1)

contred1,hierred1 = cv2.findContours(maskred1,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

cntred1 = contred1[0]
max_red1 = cv2.contourArea(cntred1)

for contr1 in contred1:
    if cv2.contourArea(contr1) > max_red1:
        max_red1 = cv2.contourArea(contr1)
        cntred1 = contr1

perimeterred1 = cv2.arcLength (cntred1,True)
epsilonred1 = 0.03*cv2.arcLength(cntred1,True)
approxred1 = cv2.approxPolyDP(cntred1,epsilonred1,True)

hullred1 = cv2.convexHull(cntred1)


    

cv2.imshow('resred1',resred1)
cv2.imshow('maskred1',maskred1)


#黄色三角
loyell1 = np.array([20,43,46])
upyell1 = np.array([30,255,255])
maskyell1 = cv2.inRange(img11,loyell1,upyell1)

resyell1 = cv2.bitwise_and(frame1,frame1,mask = maskyell1)

contyell1,hieryell1 = cv2.findContours(maskyell1,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )

cntyell1 = contyell1[0]
max_yell1 = cv2.contourArea(cntyell1)

for conty1 in contyell1:
    if cv2.contourArea(conty1) > max_yell1:
        max_yell1 = cv2.contourArea(conty1)
        cntyell1 = conty1

perimeteryell1 = cv2.arcLength (cntyell1,True)
epsilonyell1 = 0.05*cv2.arcLength(cntyell1,True)
approxyell1 = cv2.approxPolyDP(cntyell1,epsilonyell1,True)

hullyell1 = cv2.convexHull (cntyell1)


    
    

cv2.imshow('maskyell1',maskyell1)
cv2.imshow('resyell1',resyell1)


#蓝色三角
loblue1 = np.array([80,60,46])
upblue1 = np.array([124,255,255])
maskblue1 = cv2.inRange(img11,loblue1,upblue1)

resblue1 = cv2.bitwise_and(frame1,frame1,mask = maskblue1)

contblue1,hierblue1 = cv2.findContours(maskblue1,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )

cntblue1 = contblue1[0]
max_blue1 = cv2.contourArea(cntblue1)

for contb1 in contblue1:
    if cv2.contourArea(contb1) > max_blue1:
        max_blue1 = cv2.contourArea(contb1)
        cntblue1 = contb1

perimeterblue1 = cv2.arcLength (cntblue1,True)
epsilonblue1 = 0.05*cv2.arcLength(cntblue1,True)
approxblue1 = cv2.approxPolyDP(cntblue1,epsilonblue1,True)

hullblue1 = cv2.convexHull (cntblue1)

    

cv2.imshow('maskblue1',maskblue1)
cv2.imshow('resblue1',resblue1)



    


#绿色正方形
logre1 = np.array([31,43,46])
upgre1 = np.array([78,255,255])
maskgre1 = cv2.inRange(img11,logre1,upgre1)

resgre1 = cv2.bitwise_and(frame1,frame1,mask = maskgre1)

contgre1,hiergre1 = cv2.findContours(maskgre1,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

cntgre1 = contgre1[0]
max_gre1 = cv2.contourArea(cntgre1)

for contg1 in contgre1:
    if cv2.contourArea(contg1) > max_gre1:
        max_gre1 = cv2.contourArea(contg1)
        cntgre1 = contg1

perimetergre1 = cv2.arcLength (cntgre1,True)
epsilongre1 = 0.05*cv2.arcLength(cntgre1,True)
approxgre1 = cv2.approxPolyDP(cntgre1,epsilongre1,True)

hullgre1 = cv2.convexHull (cntgre1)



cv2.imshow('maskgre1',maskgre1)
cv2.imshow('resgre1',resgre1)

#橙色平行四边形
loora1 = np.array([11,40,46])
upora1 = np.array([20,255,255])
maskora1 = cv2.inRange(img11,loora1,upora1)

resora1 = cv2.bitwise_and(frame1,frame1,mask = maskora1)

contora1,hierora1 = cv2.findContours(maskora1,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )

cntora1 = contora1[0]
max_ora1 = cv2.contourArea(cntora1)

for conto1 in contora1:
    if cv2.contourArea(conto1) > max_ora1:
        max_ora1 = cv2.contourArea(conto1)
        cntora1 = conto1

perimeterora1 = cv2.arcLength (cntora1,True)
epsilonora1 = 0.05*cv2.arcLength(cntora1,True)
approxora1 = cv2.approxPolyDP(cntora1,epsilonora1,True)

hullora1 = cv2.convexHull (cntora1)



cv2.imshow('maskora1',maskora1)
cv2.imshow('resora1',resora1)



#cv2.drawContours(frame1, cntpul1, -1, (93, 25, 103), 3)
cv2.drawContours(frame1, approxpul1, -1, (0, 0, 0), 3)
cv2.polylines (frame1,[approxpul1],True,(255,0,0),3)

#cv2.drawContours(frame1, cntred1, -1, (0,0,255), 3)
cv2.drawContours(frame1, approxred1, -1, (0,0,0), 3)
cv2.polylines (frame1,[approxred1],True,(255,0,0),3)


#cv2.drawContours(frame1,cntora1,-1,(0,172,225),3)
cv2.drawContours(frame1,approxora1,-1,(0,0,0),3)
cv2.polylines (frame1,[approxora1],True,(255,0,0),3)


#cv2.drawContours(frame1,cntgre1,-1,(0,255,0),3)
cv2.drawContours(frame1,approxgre1,-1,(0,0,255),3)
cv2.polylines (frame1,[approxgre1],True,(255,0,0),3)


#cv2.drawContours(frame1,cntblue1,-1,(255,0,0),3)
cv2.drawContours(frame1,approxblue1,-1,(0,0,0),3)
cv2.polylines (frame1,[approxblue1],True,(255,0,0),3)



#cv2.drawContours(frame1,cntyell1,-1,(0,249,225),3)
cv2.drawContours(frame1,approxyell1,-1,(0,0,0),3)
cv2.polylines (frame1,[approxyell1],True,(255,0,0),3)



cv2.imshow('photo',frame1)

cntt = [approxblue1,approxgre1,approxred1,approxpul1,approxyell1,approxora1]
print(cntt)
print(approxblue1.size)


#cntt = np.vstack((approxblue1,approxgre1,approxred1,approxpul1,approxyell1,approxora1))
#print(cntt)




cap = cv2.VideoCapture(0)


while (1):
    ret,frame = cap.read()
    
    frame = cv2.flip(frame,1)
    (a,b,d) = frame.shape
    
    
    #frame = cv2.resize(frame,(2*b,2*a))
    #frame = frame[int(c/2),int(r/2)]
    img1 = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
    #img1 = cv2.GaussianBlur(img1,(3,3),3)
    #kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
   # img1 = cv2.erode (img1,kernel)
    #img1 = cv2.dilate (img1,kernel)

    

    #紫色三角
    lopul = np.array([125,10,46])
    uppul = np.array([155,255,255])
    maskpul = cv2.inRange(img1,lopul,uppul)
    

    respul = cv2.bitwise_and(frame,frame,mask = maskpul)
    kernel = np.ones((5,5),np.uint8)
    pul_median = cv2.medianBlur(maskpul, 5)#中值
    pul_median = cv2.morphologyEx(pul_median,cv2.MORPH_OPEN,kernel)

    contpul,hierpul = cv2.findContours(pul_median,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )


    #原位置轮廓
    cv2.polylines(frame,[approxpul1],True,(93,25,103),1)
    

    if contpul != []:   
        cntpul = contpul[0]

        max_pul = cv2.contourArea(cntpul)

        for contp in contpul:
            if cv2.contourArea(contp) > max_pul:
                cntpul = contp
                max_pul = cv2.contourArea(contp)

        
        perimeterpul = cv2.arcLength(cntpul,True)
        epsilonpul = 0.05*cv2.arcLength(cntpul,True)
        approxpul = cv2.approxPolyDP(cntpul,epsilonpul,True)


        hullpul = cv2.convexHull(cntpul)
        #cv2.drawContours(frame,hullpul,-1,(0,255,0),8)
        
        if approxpul.size <7:
            cv2.drawContours(frame,approxpul,-1,(0,0,0),8)
            #cv2.drawContours(frame, cntpul, -1, (93, 25, 103), 3)
            
            ds0 = np.sqrt(np.sum(np.square(approxpul[0] - approxpul1[0])))
            ds1 = np.sqrt(np.sum(np.square(approxpul[1] - approxpul1[1])))
            ds2 = np.sqrt(np.sum(np.square(approxpul[2] - approxpul1[2])))
            if ds0 < 20 and ds1 < 20 and ds2 < 20:
                
                cv2.polylines (frame,[approxpul],True,(255,0,0),3) 
                
        

              
        cv2.imshow('respul',respul)
        cv2.imshow('maskpul',maskpul)

    else :
        approxpul = np.array([[[0,0]]],dtype=np.int32)
        cntpul = []
    
    
    #红色三角
    lored1 = np.array([0,150,46])
    upred1 = np.array([8,255,255])
    #lored1 = np.array([156,43,46])
    #upred1 = np.array([180,255,255])
    
    maskred = cv2.inRange(img1,lored1,upred1)

    resred = cv2.bitwise_and(frame,frame,mask = maskred)
    kernel = np.ones((5,5),np.uint8)
    red_median = cv2.medianBlur(maskred, 5)#中值
    red_median = cv2.morphologyEx(red_median,cv2.MORPH_OPEN,kernel)

    contred,hierred = cv2.findContours(red_median,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )


    #原图位置与轮廓
    cv2.polylines(frame,[approxred1],True,(0,0,255),1)
    

    if contred != []:
    

        cntred = contred[0]
        max_red = cv2.contourArea(cntred)

        for contr in contred:
            if cv2.contourArea(contr) > max_red:
                max_red = cv2.contourArea(contr)
                cntred = contr

        perimeterred = cv2.arcLength (cntred,True)
        epsilonred = 0.05*cv2.arcLength(cntred,True)
        approxred = cv2.approxPolyDP(cntred,epsilonred,True)

        hullred = cv2.convexHull(cntred)
        cntred11 = cntred1
        approxred11 = approxred1

        if approxred.size < 7:
            cv2.drawContours(frame,approxred,-1,(0,0,0),8)
            #cv2.drawContours(frame, cntpul, -1, (93, 25, 103), 3)
            
            ds0 = np.sqrt(np.sum(np.square(approxred[0] - approxred1[0])))
            ds1 = np.sqrt(np.sum(np.square(approxred[1] - approxred1[1])))
            ds2 = np.sqrt(np.sum(np.square(approxred[2] - approxred1[2])))
            if ds0 < 20 and ds1 < 20 and ds2 < 20:
                
                cv2.polylines (frame,[approxred],True,(255,0,0),3) 
                
        cv2.imshow('resred',resred)
        cv2.imshow('maskred',maskred)
        

    else  :
        approxred = np.array([[[0,0]]],dtype=np.int32)
        cntred = []


    #黄色三角
    loyell = np.array([32,60,46])
    upyell = np.array([37,255,255])
    #loyell = np.array([20,43,46])
    #upyell = np.array([30,255,255])
    
    maskyell = cv2.inRange(img1,loyell,upyell)

    resyell = cv2.bitwise_and(frame,frame,mask = maskyell)
    kernel = np.ones((5,5),np.uint8)
    yell_median = cv2.medianBlur(maskyell, 5)#中值
    yell_median = cv2.morphologyEx(yell_median,cv2.MORPH_OPEN,kernel)

    contyell,hieryell = cv2.findContours(yell_median,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )

    #画原图位置点轮廓
    cv2.polylines(frame,[approxyell1],True,(0,249,255),1)
    if contyell != []:
    
        cntyell = contyell[0]
        max_yell = cv2.contourArea(cntyell)

        for conty in contyell:
            if cv2.contourArea(conty) > max_yell:
                max_yell = cv2.contourArea(conty)
                cntyell = conty

        perimeteryell = cv2.arcLength (cntyell,True)
        epsilonyell = 0.05*cv2.arcLength(cntyell,True)
        approxyell = cv2.approxPolyDP(cntyell,epsilonyell,True)

        #hullyell = cv2.convexHull (cntyell)
        cntyell11 = cntyell1
        approxyell11 = approxyell1

        
              
        if approxyell.size < 9:
            #cv2.drawContours(frame,cntyell,-1,(0,249,225),3)
            cv2.drawContours(frame,approxyell,-1,(0,0,0),8)
            #cv2.polylines (frame,[approxyell],True,(255,0,0),3)

            ds0 = np.sqrt(np.sum(np.square(approxyell[0] - approxyell1[0])))
            ds1 = np.sqrt(np.sum(np.square(approxyell[1] - approxyell1[1])))
            ds2 = np.sqrt(np.sum(np.square(approxyell[2] - approxyell1[2])))
            if ds0 < 20 and ds1 < 20 and ds2 < 20:
                
                cv2.polylines (frame,[approxyell],True,(255,0,0),3) 
            
           
        
             

        cv2.imshow('maskyell',maskyell)
        cv2.imshow('resyell',resyell)
        cv2.imshow('yell_median',yell_median)



    else  :
        approxyell = np.array([[[0,0]]],dtype=np.int32)
        cntyell = []
        
    #蓝色三角
    loblue = np.array([99,60,46])
    upblue = np.array([109,255,255])
    #loblue = np.array([80,60,46])
    #upblue = np.array([124,255,255])
    
    maskblue = cv2.inRange(img1,loblue,upblue)

    resblue = cv2.bitwise_and(frame,frame,mask = maskblue)
    kernel = np.ones((5,5),np.uint8)
    blue_median = cv2.medianBlur(maskblue, 5)#中值
    blue_median = cv2.morphologyEx(blue_median,cv2.MORPH_OPEN,kernel)

    contblue,hierblue = cv2.findContours(blue_median,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )



    #画蓝色原图位置与轮廓
    cv2.polylines(frame,[approxblue1],True,(255,0,0),1)
    if contblue != []:

    
        cntblue = contblue[0]
        max_blue = cv2.contourArea(cntblue)

        for contb in contblue:
            if cv2.contourArea(contb) > max_blue:
                max_blue = cv2.contourArea(contb)
                cntblue = contb

        perimeterblue = cv2.arcLength (cntblue,True)
        epsilonblue = 0.05*cv2.arcLength(cntblue,True)
        approxblue = cv2.approxPolyDP(cntblue,epsilonblue,True)

        #hullblue = cv2.convexHull (cntblue)
        cntblue11 = cntblue1
        approxblue11 = approxblue1

        
              
        if approxblue.size < 8:
            #cv2.drawContours(frame,cntblue,-1,(0,249,225),3)
            cv2.drawContours(frame,approxblue,-1,(0,0,0),8)
            #cv2.polylines (frame,[approxblue],True,(255,0,0),3)

            ds0 = np.sqrt(np.sum(np.square(approxblue[0] - approxblue1[0])))
            ds1 = np.sqrt(np.sum(np.square(approxblue[1] - approxblue1[1])))
            ds2 = np.sqrt(np.sum(np.square(approxblue[2] - approxblue1[2])))
            if ds0 < 20 and ds1 < 20 and ds2 < 20:
                
                cv2.polylines (frame,[approxblue],True,(255,0,0),3) 

        cv2.imshow('maskblue',maskblue)
        cv2.imshow('resblue',resblue)
        cv2.imshow('blue_median',blue_median)
   

    else  :
        approxblue = np.array([[[0,0]]],dtype=np.int32)
        cntblue = []


    
    #绿色正方形
    #logre = np.array([40,60,46])
    #upgre = np.array([60,255,255])
    logre = np.array([31,43,46])
    upgre = np.array([78,255,255])
    
    maskgre = cv2.inRange(img1,logre,upgre)

    resgre = cv2.bitwise_and(frame,frame,mask = maskgre)
    kernel = np.ones((5,5),np.uint8)
    gre_median = cv2.medianBlur(maskgre, 5)#中值
    gre_median = cv2.morphologyEx(gre_median,cv2.MORPH_OPEN,kernel)

    contgre,hiergre = cv2.findContours(gre_median,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )

    #画原图位置点轮廓
    cv2.polylines(frame,[approxgre1],True,(0,249,255),1)
    if contgre != []:
    
        cntgre = contgre[0]
        max_gre = cv2.contourArea(cntgre)

        for contg in contgre:
            if cv2.contourArea(contg) > max_gre:
                max_gre = cv2.contourArea(contg)
                cntgre = contg

        perimetergre = cv2.arcLength (cntgre,True)
        epsilongre = 0.1*cv2.arcLength(cntgre,True)
        approxgre = cv2.approxPolyDP(cntgre,epsilongre,True)

        cntgre11 = cntgre1
        approxgre11 = approxgre1

        
              
        if approxgre.size < 10:
            #cv2.drawContours(frame,cntgre,-1,(0,249,225),3)
            cv2.drawContours(frame,approxgre,-1,(0,0,0),8)
            #cv2.polylines (frame,[approxgre],True,(255,0,0),3)

            ds0 = np.sqrt(np.sum(np.square(approxgre[0] - approxgre1[0])))
            ds1 = np.sqrt(np.sum(np.square(approxgre[1] - approxgre1[1])))
            ds2 = np.sqrt(np.sum(np.square(approxgre[2] - approxgre1[2])))
            ds3 = np.sqrt(np.sum(np.square(approxgre[3] - approxgre1[3])))
            if ds0 < 20 and ds1 < 20 and ds2 < 20 and ds3 < 20:
                
                cv2.polylines (frame,[approxgre],True,(255,0,0),3) 
        
        cv2.imshow('maskgre',maskgre)
        cv2.imshow('resgre',resgre)
        cv2.imshow('gre_median',gre_median)



    else  :
        approxgre = np.array([[[0,0]]],dtype=np.int32)
        cntgre = []


    #橙色平行四边形
    #loora = np.array([15,100,46])
    #upora = np.array([25,255,255])
    loora = np.array([11,50,40])
    upora = np.array([17,255,255])
    
    maskora = cv2.inRange(img1,loora,upora)

    resora = cv2.bitwise_and(frame,frame,mask = maskora)
    kernel = np.ones((5,5),np.uint8)
    ora_median = cv2.medianBlur(maskora, 5)#中值
    ora_median = cv2.morphologyEx(ora_median,cv2.MORPH_OPEN,kernel)

    contora,hierora = cv2.findContours(ora_median,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE )

    #画原图位置点轮廓
    cv2.polylines(frame,[approxora1],True,(0,249,255),1)
    if contora != []:
    
        cntora = contora[0]
        max_ora = cv2.contourArea(cntora)

        for conto in contora:
            if cv2.contourArea(conto) > max_ora:
                max_ora = cv2.contourArea(conto)
                cntora = conto

        perimeterora = cv2.arcLength (cntora,True)
        epsilonora = 0.03*cv2.arcLength(cntora,True)
        approxora = cv2.approxPolyDP(cntora,epsilonora,True)

        
        cntora11 = cntora1
        approxora11 = approxora1

        
              
        if approxora.size < 10:
            #cv2.drawContours(frame,cntora,-1,(0,249,225),3)
            cv2.drawContours(frame,approxora,-1,(0,0,0),8)
            #cv2.polylines (frame,[approxora],True,(255,0,0),3)

            ds0 = np.sqrt(np.sum(np.square(approxora[0] - approxora1[0])))
            ds1 = np.sqrt(np.sum(np.square(approxora[1] - approxora1[1])))
            ds2 = np.sqrt(np.sum(np.square(approxora[2] - approxora1[2])))
            ds3 = np.sqrt(np.sum(np.square(approxora[3] - approxora1[3])))
            if ds0 < 20 and ds1 < 20 and ds2 < 20 and ds3 < 20:
                
                cv2.polylines (frame,[approxora],True,(255,0,0),3) 
        
        cv2.imshow('maskora',maskora)
        cv2.imshow('resora',resora)
        cv2.imshow('ora_median',ora_median)



    else  :
        approxora = np.array([[[0,0]]],dtype=np.int32)
        cntora = []

    #不知颜色三角形....
    


      
        
    cv2.imshow('frame',frame)

    k = cv2.waitKey(1) & 0xFF
    if k == ord('q'):
        break
  


cap.release()
cv2.destroyAllWindows()