滚球控制系统视觉部分代码-Python

2017年电赛控制方向，视觉部分，Python写的。
处理设备：Jetson Nano
摄像头：KS1A552，100万像素，镜头2.4mm焦距。

import cv2          #cv2读取的格式是BGR
import numpy as np
import time
import serial

serial_port = serial.Serial(
    port="/dev/ttyTHS1",   #这里注意一下这个端口号，使用前要赋予串口权限：sudo chmod 777 /dev/ttyTHS1
    baudrate=115200,
    bytesize=serial.EIGHTBITS,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
)
# Wait a second to let the port initialize
time.sleep(1)

# 角点检测
def harris(image):
    blockSize = 5
    apertureSize = 3
    k = 0.06
    gray = np.float32(image)
    T = (0.05, 0.01, 0.005)
    for t in T:
        dst = cv2.cornerHarris(gray, blockSize, apertureSize, k)
        a = dst>t*dst.max()
        T = (0.05, 0.01, 0.005)
        for t in T:
            dst = cv2.cornerHarris(gray, blockSize, apertureSize, k)
            a = dst > t * dst.max()
            image[a] = (0, 0, 255)
    dst = cv2.cornerHarris(gray, blockSize, apertureSize, k)
    dst_norm = np.empty(dst.shape, dtype=np.float32)
    cv2.normalize(dst, dst_norm, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX)
    xy = []
    for i in range(dst_norm.shape[0]):
        for j in range(dst_norm.shape[1]):
            if int(dst_norm[i, j]) > 135:  # 120是角点检测阈值
                cv2.circle(image, (j, i), 2, (0, 255, 0), 2)
                xy.append([i, j])
    return image,xy

# 透视变换
def PerspectImage(xy1=[], xy2=[], xy3=[], xy4=[]):
    srcArr = np.float32([xy1[0], xy2[0], xy3[0], xy4[0]])
    dstArr = np.float32([[0, 0], [650, 0], [0, 650], [650, 650]])
    Matrix = cv2.getPerspectiveTransform(srcArr, dstArr)
    # dst = cv2.warpPerspective(image, Matrix,(650,650))  # 转换图片
    return Matrix


# 中值滤波
def midfilter(image):
    img_2 = cv2.medianBlur(image,5)
    return img_2

# 找圆
def findcircles(image,y0,x0,y1,x1):
    image = image[y0+10:y1-10, x0+20:x1-10]
    ret, image_2 = cv2.threshold(image, 220, 255, cv2.THRESH_BINARY)
    """
    # 高斯模糊,降低噪声
    blurred = cv2.GaussianBlur(gray, (3, 3), 0)
    # 图像梯度
    xgrad = cv2.Sobel(blurred, cv2.CV_16SC1, 1, 0)
    ygrad = cv2.Sobel(blurred, cv2.CV_16SC1, 0, 1)
    # 计算边缘
    #     # 50和150参数必须符合1：3或者1：2
    edge_output = cv.Canny(xgrad, ygrad, 50, 150)
    """
    contours, hierarchy = cv2.findContours(image_2, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    global sparecircle
    for i in range(len(contours)):
        cnt = contours[i]
        (x, y), radius = cv2.minEnclosingCircle(cnt)   # 找到最小外接圆
        if (x,y) is None:
            (x, y) = sparecircle
        if radius<12 and radius>7:
            (x, y, radius) = np.int0((x+x0+20, y+y0+10, radius))  # 得到的圆心像素点坐标
            sparecircle = (x, y)
            cv2.drawContours(image, cnt, -1, (0, 0, 255), 2)
            break
    return image, image_2, (x, y), radius


# 矩阵乘法得到圆心在木板上坐标
def matrix(a=[], b=[]):
    point = []
    point.append((a[0][0] * b[0] + a[0][1] * b[1] + a[0][2] * b[2])/(a[2][0] * b[0] + a[2][1] * b[1] + a[2][2] * b[2]))
    point.append((a[1][0] * b[0] + a[1][1] * b[1] + a[1][2] * b[2])/(a[2][0] * b[0] + a[2][1] * b[1] + a[2][2] * b[2]))
    point.append(a[2][0] * b[0] + a[2][1] * b[1] + a[2][2] * b[2])
    return point


xy11 = 0
xy22 = 0
xy33 = 0
xy44 = 0
video = cv2.VideoCapture(1)
video.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
video.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
sparecircle = (294, 116)
while True:
    ret, frame = video.read()
    if ret:
        gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)  # 转成灰度图
        # 角点检测
        cropped1 = gray[0:50, 40:100]
        xy1 = []
        done1, xy1 = harris(cropped1)
        xy1 = [[xy1[0][0] + 40, xy1[0][1]]] + xy1
        if xy1 == None:
            xy1 = xy11
        if xy1!=None:
            xy11 = xy1
        cropped2 = gray[5:60, 470:550]
        xy2 = []
        done2, xy2 = harris(cropped2)
        xy2 = [[xy2[0][0] + 470, xy2[0][1] + 5]] + xy2
        if xy2 == None:
            xy2 = xy22
        if xy2 != None:
            xy22 = xy2
        cropped3 = gray[410:480, 50:110]
        xy3 = []
        done3, xy3 = harris(cropped3)
        xy3 = [[xy3[0][0] + 50, xy3[0][1] + 410]] + xy3
        if xy3 == None:
            xy3 = xy33
        if xy3 != None:
            xy33 = xy3
        cropped4 = gray[410:480, 430:550]
        xy4 = []
        done4, xy4 = harris(cropped4)
        xy4 = [[xy4[0][0] + 430, xy4[0][1] + 410]] + xy4
        if xy4 == None:
            xy4 = xy44
        if xy4!=None:
            xy44 = xy4

        # 获得转换矩阵 Z=[]
        Z = PerspectImage(xy1, xy2, xy3, xy4)
        cen = ()
        img1, img2, cen, rad = findcircles(gray,xy1[0][1], xy1[0][0], xy4[0][1], xy4[0][0])
        print("原圆心坐标:", (cen[0], cen[1]))
        cent = [cen[0], cen[1], 1]
        point = matrix(Z, cent)
        x = cen[0]
        y = cen[1]
        print("圆心坐标:", point)
        # serial_port.write((x+"$"+y+'\r\n').encode())
        # print("serial sent ok")
        cv2.imshow('test',gray)
        # cv2.imshow('test3', img_z)
        cv2.imshow('test2', img2)
        if cv2.waitKey(10) & 0xff == 27:
            break
    else:
        print("摄像头打开失败，请检查")

"""
img = cv2.imread("./4.png")
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)  # 转成灰度图
cen = ()
img1, img2, cen, rad = findcircles(gray)
print(cen, rad)
cent = [cen[0],cen[1],1]
print(cent, rad)
img4 = np.hstack((gray, img1, img2))  # 第一张是灰度图，第二张是画了圈的灰度图，第三张是二值化
cv2.imshow('result', img4)
cv2.waitKey(0)
cv2.destroyAllWindows()
"""
cv2.destroyAllWindows()