首页 > 编程语言 >生长算法和巡中线算法python实现代码示例(自用)

生长算法和巡中线算法python实现代码示例(自用)

时间:2022-11-06 18:59:39浏览次数:48  
标签:return 示例 python self 算法 ._ line frame dot

生长算法和巡中线算法python实现代码示例(自用)

import cv2
import time
import numpy as np
from math import pi, isnan


# PID算法类
class PID():
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1 / (2 * pi * 20)

    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')

    def get_pid(self, error, scaler):
        tnow = int(round(time.time() * 1000))
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                derivative = (error - self._last_error) / delta_time
            derivative = self._last_derivative + \
                         ((delta_time / (self._RC + delta_time)) *
                          (derivative - self._last_derivative))
            self._last_error = error
            self._last_derivative = derivative
            output += self._kd * derivative
        output *= scaler
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax:
                self._integrator = -self._imax
            elif self._integrator > self._imax:
                self._integrator = self._imax
            output += self._integrator
        return output

    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')


class RobotControl():
    def __init__(self):
        # PID调参的地方
        self.rho_pid = PID(p=0.4, i=0)
        self.theta_pid = PID(p=0.001, i=0)
        # 图像大小处理
        self.h = 0  # 480
        self.w = 0  # 640
        self.middle_line_x = 0
        # 存中线坐标
        self.middle_line_list = []
        # 生长算法初始点
        self.first_dot = (0, 0)

    def skip_image(self, cap, num):
        for i in range(num):
            _, frame = cap.read()

    def img_dispose(self, cap):
        _, frame = cap.read()
        frame = cv2.GaussianBlur(frame, (3, 3), 1)
        # 灰度
        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        # 二值化
        frame = cv2.adaptiveThreshold(frame, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 25, 5)
        # retval, frame = cv2.threshold(frame, 127, 255, cv2.THRESH_BINARY_INV)
        # kernel = np.ones((3, 3), np.uint8)
        # img = cv2.morphologyEx(img, cv2.MORPH_OPEN, kernel)
        # 边界提取
        # img1 = cv2.Canny(img, 80, 150, (5, 5))
        # img = cv2.Canny(img, 80, 150, (3, 3))
        # img = np.concatenate((frame, img), axis=1)
        # img = np.hstack((img1, img2))
        return frame

    def find_middle_line(self, frame):
        # print(len(frame[0]))
        point_size = 1
        point_color = (127, 127, 127)  # BGR
        thickness = 4  # 可以为 0 、4、8
        left_line = []
        right_line = []
        countall = 0
        count = 0
        # middle_dot = []
        for i in range(self.h):
            # 找左边
            for j in range(int(self.middle_line_x), 0, -1):
                if frame[i][j - 1] == 0:
                    left_line.append((i, j - 1))
                    break
            else:
                left_line.append((i, -1))
            # 找右边
            for j in range(int(self.middle_line_x), self.w):
                if frame[i][j] == 0:
                    right_line.append((i, j))
                    break
            else:
                right_line.append((i, -1))
        for i in range(self.h):
            if left_line[i][1] != -1 and right_line[i][1] != -1:
                # middle_dot.append((i, (left_line[i][1]+right_line[i][1])//2))
                count += 1
                a = (left_line[i][1] + right_line[i][1]) // 2
                countall += a
                cv2.circle(frame, (a, i), point_size, point_color, thickness)
        if count != 0:
            self.middle_line_x = countall // count  # 更新赛道中线的位置
        return frame

    # 生长算法
    def grow_line(self, img, mode, begin_dot=(-1, -1)):
        """
        :param img: 传入的图片
        :param left_dot: 左边开始找的初始点
        :param right_dot: 右边开始找的初始点
        :param mode: 0 1 o代表找左边, 1代表找右边
        :return: 返回找的线的元组列表
        """
        dot_dic = {}

        def road_true(dot, mode, num, fx, fy):
            """
            :param dot: 开始的点
            :param mode: 当前情况下查点的模式
            :param num:  希望查的点的个数
            :param fx: 查横的方向
            :param fy: 查竖的方向
            :return: 返回是否合法
            """
            if fx == 0 and fy == 0:
                return -1
            elif fx == 1 and fy == 1:
                if mode == 0:  # 向左
                    if dot[0] - num >= 0 and dot[1] - num >= 0:
                        return 1
                    else:
                        return 0
                else:  # 向右
                    if dot[0] + num < self.w - 1 and dot[1] - num >= 0:
                        return 1
                    else:
                        return 0
            elif fx == 1:
                if mode == 0:
                    if dot[0] - num >= 0:
                        return 1
                    else:
                        return 0
                else:
                    if dot[0] + num < self.w - 1:
                        return 1
                    else:
                        return 0
            elif fy == 1:
                if mode == 0:
                    if dot[1] - num >= 0:
                        return 1
                    else:
                        return 0
                else:
                    if dot[1] - num >= 0:
                        return 1
                    else:
                        return 0

        # 先假设路是白色255, 边界是黑色0
        road = 255
        middle_road = 127
        borad = 0

        if begin_dot[0] != -1:
            current_dot = begin_dot  # current_dot[1]为y, current_dot[0]为x
            if mode == 0:  # 向左找
                while 1:
                    if road_true(current_dot, mode, 1, fx=1, fy=0) and road_true(current_dot, mode, 1, fx=0, fy=1):
                        if (img[current_dot[1]][current_dot[0]] == road or img[current_dot[1]][
                            current_dot[0]] == middle_road) and img[current_dot[1]][current_dot[0] - 1] == borad:
                            dot_dic[current_dot[1]] = current_dot[0]
                            if current_dot[1] <= 0:
                                break
                            else:
                                current_dot = (current_dot[0], current_dot[1] - 1)
                        elif img[current_dot[1]][current_dot[0]] == borad:
                            current_dot = (current_dot[0] + 1, current_dot[1])
                        elif img[current_dot[1]][current_dot[0]] == road or img[current_dot[1]][
                            current_dot[0]] == middle_road:
                            current_dot = (current_dot[0] - 1, current_dot[1])
                    elif road_true(current_dot, mode, 1, fx=0, fy=1):
                        current_dot = (begin_dot[0], current_dot[1] - 1)
                    else:
                        break

            else:  # 向右找
                while 1:
                    if road_true(current_dot, mode, 1, fx=1, fy=0) and road_true(current_dot, mode, 1, fx=0, fy=1):
                        if (img[current_dot[1]][current_dot[0]] == road or img[current_dot[1]][
                            current_dot[0]] == middle_road) and img[current_dot[1]][current_dot[0] + 1] == borad:
                            dot_dic[current_dot[1]] = current_dot[0]
                            if current_dot[1] <= 0:
                                break
                            else:
                                current_dot = (current_dot[0], current_dot[1] - 1)
                        elif img[current_dot[1]][current_dot[0]] == borad:
                            current_dot = (current_dot[0] - 1, current_dot[1])
                        elif img[current_dot[1]][current_dot[0]] == road or img[current_dot[1]][
                            current_dot[0]] == middle_road:
                            current_dot = (current_dot[0] + 1, current_dot[1])
                    elif road_true(current_dot, mode, 1, fx=0, fy=1):
                        current_dot = (begin_dot[0], current_dot[1] - 1)
                    else:
                        break

        return img, dot_dic

    def run(self):
        cap = cv2.VideoCapture(0)
        cap.set(cv2.CAP_PROP_FRAME_WIDTH, 160)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 120)
        self.skip_image(cap, 30)
        _, frame = cap.read()
        self.w, self.h = frame.shape[1], frame.shape[0]
        # 输出当前的图片大小
        print(self.w, self.h)
        self.middle_line_x = self.w // 2
        left_first_dot = (self.middle_line_x, self.h - 1)
        right_first_dot = (self.middle_line_x, self.h - 1)
        self.first_dot = (self.middle_line_x, self.h - 1)
        while 1:
            try:
                flag = 1
                # 返回处理过的黑白图像
                frame = self.img_dispose(cap)
                # 找中线并更新数据
                frame, dot_dic1 = self.grow_line(frame, 0, left_first_dot)
                frame, dot_dic2 = self.grow_line(frame, 1, right_first_dot)
                # print(dot_dic1, dot_dic2)
                for i in range(self.h + 1):
                    if dot_dic1.get(i, -1) != -1 and dot_dic2.get(i, -1) != -1:
                        frame[i][(dot_dic1.get(i, -1) + dot_dic2.get(i, -1)) // 2] = 127
                # 更新起始点
                for i in range(self.h - 1, self.h - 12, -5):
                    if dot_dic1.get(i, -1) != -1 and dot_dic2.get(i, -1) != -1:
                        left_first_dot = (dot_dic1.get(i, -1) + 5, self.h - 1)
                        right_first_dot = (dot_dic2.get(i, -1) - 5, self.h - 1)
                        break
                else:
                    left_first_dot = self.first_dot
                    right_first_dot = self.first_dot
                cv2.imshow("1", frame)
                command = cv2.waitKey(1) & 0xFF
                if command == ord('q'):
                    break
            except Exception as e:
                print(e)
                cv2.imshow("1", frame)


if __name__ == '__main__':
    robot = RobotControl()
    robot.run()

标签:return,示例,python,self,算法,._,line,frame,dot
From: https://www.cnblogs.com/hnu-hua/p/16863350.html

相关文章

  • 实验二:逻辑回归算法实验
    【实验目的】1.理解逻辑回归算法原理,掌握逻辑回归算法框架;2.理解逻辑回归的sigmoid函数;3.理解逻辑回归的损失函数;4.针对特定应用场景及数据,能应用逻辑回归算法解决实际分......
  • 拓端数据tecdat:Python | ARIMA时间序列模型预测航空公司的乘客数量
     时间序列 被定义为一系列按时间顺序索引的数据点。时间顺序可以是每天,每月或每年。以下是一个时间序列示例,该示例说明了从1949年到1960年每月航空公司的乘客数量。 时间......
  • 拓端数据tecdat|Python用ARIMA和SARIMA模型预测销量时间序列数据
     介绍ARIMA模型是时间序列预测中一种常用的统计方法。指数平滑和ARIMA模型是时间序列预测中应用最为广泛的两种方法,它们是解决这一问题的补充方法。指数平滑模型是基于对数......
  • Python批量生成合同文档
    假设某日我开了一家空调公司,暂且就叫他天强空调安装设备公司吧,假装自己有公司,接了一单大生意,就是给甘肃省的各个高校安装空调(其实这边的气候基本用不到空调,就是假想一下),那么......
  • python学习第五周总结
    面向对象前戏之人狗大战#编写代码简单的实现人打狗狗咬人的小游戏(剧情需要)"""推导步骤1:代码定义出人和狗"""person1={'name':'jason','age':18,......
  • python 单元测试
    importunittestclassMyTestCase(unittest.TestCase):deftest_something(self):self.assertEqual(0,False)if__name__=='__main__':unitte......
  • K-近邻算法
    1.K-近邻算法的概述K最近邻(k-NearestNeighbor,KNN)分类算法,是一个理论上比较成熟的方法,也是最简单的机器学习算法之一。该方法的思路是:在特征空间中,如果一个样本附近的k......
  • python 协程
    什么是协程、异步举个例子:假设有1个洗衣房,里面有10台洗衣机,有一个洗衣工在负责这10台洗衣机。那么洗衣房就相当于1个进程,洗衣工就相当1个线程。如果有10个洗衣工,就相当于1......
  • 排序——数据结构与算法学习
    排序冒泡排序法(交换)基本原理:依次比较相邻元素的值,使值较大的元素逐渐前移或者后移,因为每一轮排序后值最大的元素一定是后移了一位。//手写冒泡排序法publicstaticvo......
  • 实验二:逻辑回归算法实验
    实验二:逻辑回归算法实验 【实验目的】理解逻辑回归算法原理,掌握逻辑回归算法框架;理解逻辑回归的sigmoid函数;理解逻辑回归的损失函数;针对特定应用场景及数据,能应用逻......