opencv控制云台激光点沿符文运动（符文构造）

机器人校内赛的视觉题目，虽然代码有点屎山，但是能跑，性能还行，放上来存个档

写完就忘，有问题可以问但是不保证会，有错误的地方自己纠正吧我也不记得了

题目如下：

任务概述：在⽐赛场地的⼀边，设置有三块⼤⼩为A4的⽩板，上有由宽度为 的简单

⿊⾊线条构造⽽成的规则图形，板中央有⼀个半径为 的圆。⼩⻋需先将⼿动将激光点移动⾄

⽩板中央圆内，并⾃动地将激光点沿着⿊⾊线条移动。激光点在移出中⼼圆接触到⿊⾊轨迹线后，若

运动轨迹偏离⿊⾊轨迹程度过⼤，则本次符⽂构造任务结束，不得分。

代码：

from inspect import FrameInfo
import cv2
import numpy as np
import math
import matplotlib.pyplot as plt
import time
import os, struct, array
import serial
from fcntl import ioctl



def drawpoints(img, contours):
    p1num=contours[0]
    p2num=contours[1]

    # ##抽样看情况用吧
    # max_points = 400  # 最大点集数量
    # # 对 p1num 进行抽样
    # num_points_p1 = len(p1num)
    # indices_p1 = np.random.choice(num_points_p1, size=min(num_points_p1, max_points), replace=False)
    # p1num= p1num[indices_p1]

    # # 对 p2num 进行抽样
    # num_points_p2 = len(p2num)
    # indices_p2 = np.random.choice(num_points_p2, size=min(num_points_p2, max_points), replace=False)
    # p2num= p2num[indices_p2]
    # fewer_points=None
    # more_points=None
    # 确定较少点的点集和较多点的点集
    print(len(p1num), len(p2num))
    if len(p1num) < len(p2num):
        fewer_points = p1num
        more_points = p2num
    else:
        fewer_points = p2num
        more_points = p1num
    pair_points=[]
    for point in more_points:
        min_distance = float('inf')  # 初始化最小距离为正无穷大
        closest_point = None  # 初始化最近点为None
        for p in fewer_points:
            distance = np.linalg.norm(point - p)
            if distance < min_distance:
                min_distance = distance
                closest_point = p
        pair = (point, closest_point)
        pair_points.append(pair)
        #filtered_points.append(closest_point)
    print(len(pair_points))
    # 画中点
    midpoints = []  # 存储每对点集连接线的中点
    for pair in pair_points:
        point1, point2 = pair  # 提取点集中的两个点
        midpoint = (point1 + point2) / 2  # 计算连接线的中点
        midpoints.append(midpoint)  # 将中点添加到 midpoints 列表中
    return midpoints

def findcontours(img):
    # img = cv2.imread(path)  # 读取原图

    # 将图像转换为灰度图像
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # 对灰度图像进行二值化处理
    _, binary = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU)

    # 应用Canny边缘检测算法
    edges = cv2.Canny(binary, 200, 400)

    # 查找轮廓
    contours, _ = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

    # # 过滤掉过小的轮廓
    threshold = 50  # 设置轮廓的最小面积阈值
    filtered_contours = [cnt for cnt in contours if cv2.contourArea(cnt) > threshold]
     # 遍历所有轮廓，删除周长相差小于10的轮廓之一
    i = 0
    while i < len(filtered_contours):
        cnt1 = filtered_contours[i]
        perimeter1 = cv2.arcLength(cnt1, True)
        j = i + 1
        while j < len(filtered_contours):
            cnt2 = filtered_contours[j]
            perimeter2 = cv2.arcLength(cnt2, True)
            if abs(perimeter1 - perimeter2) < 10:
                # 删除其中一个轮廓
                del filtered_contours[j]
                print("Deleted contour {}".format(j))
            else:
                j += 1
        i += 1

    # 输出检测到的轮廓数量
    print("Detected contours: {}".format(len(filtered_contours)))

    # 对轮廓进行排序，以确保最外层的轮廓排在前面
    filtered_contours.sort(key=cv2.contourArea, reverse=True)

    # 选择最外层的两个轮廓
    selected_contours = filtered_contours[:2]
    #drawscatter(selected_contours[0],(0,0))
    #drawscatter(selected_contours[1],(0,)
    midpoints=drawpoints(img, selected_contours)
    # 在原图上绘制选定的轮廓
    # cv2.drawContours(img, selected_contours, -1, (0, 255, 0), 2)


    # # 显示处理后的图像
    # cv2.imshow("Filtered Contours", img)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()
    return midpoints

def findred(img):
    hsv_image = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    lower_red = np.array([0, 100, 70])
    upper_red = np.array([20, 255, 255])
    mask = cv2.inRange(hsv_image, lower_red, upper_red)
    kernel = np.ones((1, 1), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
    laser_center = None
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    
    if len(contours) > 0:
        laser_contour = max(contours, key=cv2.contourArea)
        (x, y, w, h) = cv2.boundingRect(laser_contour)
        laser_center = (x + w // 2, y + h // 2)
        
        #cv2.circle(img, laser_center, 5, (0, 0, 255), -1)  # 在图像上标注红色点
        #cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)
        #cv2.putText(img, 'Laser', (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2)
    
    #cv2.imshow('Image', img)
   # cv2.waitKey(0)
    #cv2.destroyAllWindows()
    
    return laser_center
# 计算两点之间的极角
def calculate_distance(point1, point2):
    x1, y1 = point1
    x2, y2 = point2
    return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)

def remove_close_points(points, min_distance):
    i = 0
    while i < len(points) - 1:
        current_point = points[i]
        next_point = points[i + 1]
        distance = calculate_distance(current_point, next_point)
        if distance < min_distance:
            del points[i + 1]
        else:
            i += 1
    return points

def drawscatter(points,red):
    plt.clf()
    # 提取x和y坐标
    x_coords = [point[0] for point in points]
    y_coords = [point[1] for point in points]

    # 绘制散点图
    plt.scatter(x_coords, y_coords)
    if red!=None:
        plt.scatter(red[0],red[1],color="red")
    # 添加标题和轴标签
    plt.title("Scatter Plot of Points")
    plt.xlabel("X-axis")
    plt.ylabel("Y-axis")

    # 显示图形
    plt.show()
def linear_interpolation(points):
    interpolated_points = [points[0]]  # 初始化插值后的点集，包含原始的第一个点

    for i in range(len(points) - 1):
        p1 = points[i]  # 当前点
        p2 = points[i + 1]  # 下一个点

        distance = ((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2) ** 0.5  # 计算两点之间的距离

        if distance > 50:
            num_interpolations = int(distance / 50)  # 计算需要插入的点的数量

            dx = (p2[0] - p1[0]) / (num_interpolations + 1)  # 计算每个插入点在 x 轴上的增量
            dy = (p2[1] - p1[1]) / (num_interpolations + 1)  # 计算每个插入点在 y 轴上的增量

            for j in range(1, num_interpolations + 1):
                new_point = (int(p1[0] + j * dx), int(p1[1] + j * dy))  # 取整得到插入点的坐标
                interpolated_points.append(new_point)  # 将插入点添加到插值后的点集中

        interpolated_points.append(p2)  # 将下一个点添加到插值后的点集中

    p1 = points[0]  # 当前点
    p2 = points[len(points)-1]  # 下一个点
    distance = ((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2) ** 0.5  # 计算两点之间的距离

    if distance > 50:
        num_interpolations = int(distance / 50)  # 计算需要插入的点的数量

        dx = (p2[0] - p1[0]) / (num_interpolations + 1)  # 计算每个插入点在 x 轴上的增量
        dy = (p2[1] - p1[1]) / (num_interpolations + 1)  # 计算每个插入点在 y 轴上的增量

        for j in range(1, num_interpolations + 1):
            new_point = (int(p1[0] + j * dx), int(p1[1] + j * dy))  # 取整得到插入点的坐标
            interpolated_points.append(new_point)  # 将插入点添加到插值后的点集中

    return interpolated_points
def sortpoints(points,red):
    points_with_angles = []
    for point in points:
        x, y = point
        angle = math.atan2(y - red[1], x - red[0])
        points_with_angles.append((angle, point))
    # 按角度对点进行排序
    sorted_points = sorted(points_with_angles, key=lambda x: x[0])

    # # 提取排序后的点集
    clockwise_points = [point for _, point in sorted_points]
    # print(clockwise_points)
    return clockwise_points
def move(kx,ky,x,y):
    #实际上这段代码是要给单片机写入运动控制信息
    sx=0
    sy=0
    if kx*x<0:
        sx=1
    if ky*y<0:
        sy=1
    strx=str(abs(round(kx*x)))
    stry=str(abs(round(ky*y)))
    lenx=5-len(strx)
    leny=5-len(stry)
    data="099"
    if sx==1:
        data+='-'
    for i in range(lenx):
        data+="0"
    data+=strx
    data+="098"
    if sy==1:
        data+='-'
    for i in range(leny):
        data+="0"
    data+=stry
    ser.write(data.encode())
    print(data)

    
def follow(points):
    # global pid_controller
    global kx,ky
    cap = cv2.VideoCapture(0)
    index=0
    for point in points:
        index+=1
        delx=0
        dely=0
        backcnt=0
        
        while True:
            time.sleep(0.1)
            
            ret, frame = cap.read()
            #if ret==True:
                #cv2.imshow('Camera', frame)
            red=findred(frame)
            
            print('the moving red')
            print(red)
            if red==None:
                if index==1 or backcnt>=10:
                    return False
                else:
                    backcnt+=1
                    #input("red==none,move back")
                    move(kx,ky,-delx,dely)
            else:
                # 创建一个新的图形
                #plt.figure()
                #plt.plot(point[0], point[1], 'bo', label='Point')
                #plt.plot(red[0], red[1], 'ro', label='Red')
                #x_coords = [p[0] for p in clockwise_points]
                #y_coords = [p[1] for p in clockwise_points]
    # 绘制散点图
                #plt.scatter(x_coords, y_coords)
# 设置图形的标题和坐标轴标签
                #plt.title('Point and Red')
                #plt.xlabel('X')
                #plt.ylabel('Y')
# 显示图例
                #plt.legend()
# 显示图形
                #plt.show()
                print("movingred"+str(red))
                print(point)
                backcnt=0
                delx=point[0]-red[0]
                dely=point[1]-red[1]
                print('delx:'+str(delx))
                print('dely:'+str(dely))
                if abs(delx)<=6:
                    delx=0
                if abs(dely)<=6:
                    dely=0
                if abs(delx)>15 or abs(dely)>15:
                    #input("moving toward red")
                    move(kx,ky,delx,-dely)
                else:
                    print("follow is ok")
                    break
    cap.release()
    cv2.destroyAllWindows()
    return True

def init(red,first_point,kx,ky):
    kx=1
    ky=1
    print('firstpoint'+str(first_point))
    print('red'+str(red))
    if red==None:
        print("未检测到初始化红色点")
        return False
    delx=first_point[0]-red[0]
    dely=first_point[1]-red[1]
    print(delx,dely)
    
    nonecnt=0
    cap = cv2.VideoCapture(0)
    while True :
        # last=nowdata()
        time.sleep(0.1)
        
        ret, frame = cap.read()
        cv2.imshow('Camera', frame)
        rednow=findred(frame)
        
        if rednow==None:
            if nonecnt>20:
                print("cant find moving red in init")
                return False
            nonecnt+=1
            print("init moving red=none")
            move(kx,ky,delx,-dely)
            continue
        else:
            nonecnt=0
            print('rednow'+str(rednow))
            print('xdel'+str(abs(rednow[0]-first_point[0])))
            if abs(rednow[0]-first_point[0])>6:
                print("too far")
                delx=first_point[0]-rednow[0]
                dely=first_point[1]-rednow[1]
                move(kx,ky,delx,-dely)
            else :
                print("init the first point success!")
                return True
            #sumx+=kx*delx
            #sumy+=ky*dely
    #kx=sumx/delx
    #ky=sumy/dely
    cap.release()
    cv2.destroyAllWindows()
    return True

def prepare(img):
    global midpoints
    global mp
    global rounded_mp
    global clockwise_points
    global middle
    midpoints=findcontours(img)
    if findred(img)==None:
        height, width, _ = img.shape
        center_x = width // 2
        center_y = height // 2
        middle=(center_x,center_y)
    else:
        middle=findred(img)
    mp=[]
    for p in midpoints:
        mp.append(tuple(p[0]))
    # print(mp)
    rounded_mp = []
    for point in mp:
        rounded_point = (int(round(point[0])), int(round(point[1])))
        rounded_mp.append(rounded_point)
    min_distance = 10
    rounded_mp=remove_close_points(rounded_mp, min_distance)
    # drawscatter(rounded_mp,middle)
    # print(rounded_mp)
    print('middle'+str(middle))
    clockwise_points = sortpoints(rounded_mp, middle)
    clockwise_points = linear_interpolation(clockwise_points)
    
    print("点集长度"+str(len(clockwise_points)))
    print(clockwise_points)
    
ser=serial.Serial('/dev/ttyACM0',9600)#如果是1就写1
midpoints=None
mp=[]
rounded_mp = []
clockwise_points=[]
middle=None
kx=1
ky=1
cap = cv2.VideoCapture(0)
init_img=None
 # 读取摄像头画面
ret, frame = cap.read()
cnt=0
while True:
        # 读取摄像头画面
    ret, frame = cap.read()
        # 显示画面
    cv2.imshow('Camera', frame)
        # 检测按键，按下 's' 键保存照片，按下 'q' 键退出循环
    key = cv2.waitKey(1)
    if key == ord('s'):
            # 保存照片
        cnt+=1
        cv2.imwrite(str(cnt)+'photo.jpg', frame)
        print("照片已保存")
    elif key == ord('q'):
        break
ret, frame = cap.read()
#cv2.imshow('Camera', frame)
#在这里完成初始化
init_img=frame
flag=1
prepare(init_img)
drawscatter(clockwise_points,middle)
cap.release()
cv2.destroyAllWindows()
csh=init(findred(init_img),clockwise_points[0],kx,ky)
if csh==False:
    print("init fail")
else:
    print('clock')
    print(clockwise_points)
    print('initover')
    p=findred(frame)
    print("init red"+str(p))
    print('zhunbeikaishi')
    go=follow(clockwise_points)
    if go==True:
        print("successful!")
    else:
        print("fail!")