标签：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