팀 프로젝트/자율주행 개발(baqu4)
[Python] OpenCV로 자율주행 만들기(Threshold)
MIRIP
2022. 9. 24. 00:00
반응형
이전 포스팅에서는 Canny threshold를 사용하였다. 하지만 이번에는 일반 Threshold를 사용하여 콘을 인식해 보았다. Canny threshold는 물체의 윤곽선을 따는 함수인데, 일반 Threshold는 물체의 윤곽선이 아니라 면으로 보여준다. 따라서 윤곽선만 있는 것보다 더 인식을 잘할 수 있을 것 같아서 Threshold를 사용하였다.
이전 포스팅을 보고 싶다면 아래 링크를 참조하기 바란다.
[Python] Canny threshold를 이용하여_02
저번에 Canny threshold에서 추출한 물체의 좌표를 기반으로 선을 그어서 화면에 표현해 보았다. 목표 : 콘 인식 및 주행 개발환경 : IDLE 언어 : python 방식 : 단순 알고리즘(Canny threshold) 직전 포스팅에
codezaram.tistory.com
Threshold란 밝기가 일정 수준을 넘어가면 1, 아니면 0으로 이진으로 나타내는 함수이다. 이 함수를 사용하기 위해서는 먼저 이미지를 흑백 처리를 해주어야 한다. 회색으로 바꾸기 전에 먼저 프레임을 HSV색상 영역으로 바꾸어서 그중 명도에 해당하는 부분을 흑백으로 처리를 해주었다. 그랬을 때, RGB 색상에서 바로 흑백으로 변환하는 것보다 정확도가 높았다.
import matplotlib.pyplot as plt
import numpy as np
import cv2
import math
import time
path = "../source/2022-07-04_16-14-30.mp4"
obj_b = cv2.imread('../source/corn_data/lavacorn_b.png', cv2.IMREAD_GRAYSCALE)#wad
obj_s = cv2.imread('../source/corn_data/lavacorn_s.png', cv2.IMREAD_GRAYSCALE)#wad
cap=cv2.VideoCapture(path)
obj_contours_b,_=cv2.findContours(obj_b, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)#wad
obj_contours_s,_=cv2.findContours(obj_s, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)#wad
obj_pts_b=obj_contours_b[0]#wad
obj_pts_s=obj_contours_s[0]#wad
fps = cap.get(cv2.CAP_PROP_FPS)
WIDTH = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
HEIGHT = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
codec = cv2.VideoWriter_fourcc(*'DIVX')
out=cv2.VideoWriter('../output/output_8.mp4', codec, 30.0, (int(WIDTH),int(HEIGHT)))
kernel_size=5
low_threshold=127
high_threshold=255
rho=2
theta=np.pi/180
threshold=90
min_line_len=10
max_line_gap=50
a=0.8
b=1.
theta_w=0.
p_cross_x=-1
p_cross_y=-1
p_left_calculated_bias=-1
p_right_calculated_bias=-1
p_l_target=-1
p_r_target=-1
def grayscale(img):
hsl=cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
imgH,imgS,imgL=cv2.split(hsl)
return imgL
def gaussian_blur(img, kernel_size):
return cv2.GaussianBlur(img, (kernel_size, kernel_size), 0)
def threshold(img, low_threshold, high_threshold):
"""Applies the Canny transform"""
return cv2.threshold(img, low_threshold, high_threshold, cv2.THRESH_BINARY)
def auto_canny(img, sigma):
"""Applies the Canny transform"""
v=np.median(img)
lower=int(max(0,(1.0-sigma)*v))
upper=int(min(255, (1.0+sigma)*v))
return cv2.Canny(img, lower, upper)
def region_of_interest(img, vertices):
mask=np.zeros_like(img)
if len(img.shape)>2:
channel_count = img.shape[2]
ignore_mask_color=(255,)*channel_count
else:
ignore_mask_color=255
cv2.fillPoly(mask, vertices, ignore_mask_color)
masked_image=cv2.bitwise_and(img,mask)
return masked_image
def draw_line(img, line, color=[255,0,0], thickness=5):
if len(line)<1:
return
else:
for i in range(1, len(line)):
cv2.line(img, (line[i-1][0],line[i-1][1]), (line[i][0],line[i][1]), color, thickness)
def draw_lines(img, l_mid, r_mid):
line_img=np.zeros((img.shape[0], img.shape[1], 3), dtype=np.uint8)
draw_line(line_img, r_mid)
draw_line(line_img, l_mid)
return line_img
def weighted_img(img, initial_img):
return cv2.addWeighted(initial_img, a, img, b, theta_w)
def depart_points(img, points, r_points, l_points):
if len(r_points)<2 or len(l_points)<2:
return[],[]
r_list=[]
l_list=[]
r_start=max(r_points, key=lambda s:s[1])
l_start=max(l_points, key=lambda s:s[1])
r_list.append(r_start)
l_list.append(l_start)
points.remove(r_start)
points.remove(l_start)
r_sec=min(points, key=lambda d:point_distance(r_start,d))
points.remove(r_sec)
l_sec=min(points, key=lambda d:point_distance(l_start,d))
points.remove(l_sec)
r_list.append(r_sec)
l_list.append(l_sec)
r_m=gradient(r_start, r_sec)
l_m=gradient(l_start ,l_sec)
r_n=y_intercept(r_m, r_start)
l_n=y_intercept(l_m, l_start)
for p in points:
#if (r_m*r_sec[0]+r_sec[1]+r_n!=0) or (l_m*l_sec[0]+l_sec[1]+r_n!=0):
# continue
if point2linear_distance(r_m,r_n,p)<point2linear_distance(l_m,l_n,p):
r_list.append(p)
else:
l_list.append(p)
return r_list, l_list
def point_distance(points1, points2):
return (points1[0]-points2[0])**2+(points1[1]-points2[1])**2
def gradient(p1, p2):
return (p2[1]-p1[1])/(p1[0]-p2[0])
def y_intercept(m, p):
return (-1)*p[1]-m*p[0]
def point2linear_distance(m,n,p):
return abs(m*p[0]+p[1]+n)/(m**2+1)**(1/2)
def linear_reg(img, left, right):
left_x=[]
left_y=[]
right_x=[]
right_y=[]
global p_cross_x
global p_cross_y
global p_left_calculated_bias
global p_right_calculated_bias
global p_l_target
global p_r_target
for i in range(len(left)):
left_x.append(left[i][0])
left_y.append(left[i][1])
for i in range(len(right)):
right_x.append(right[i][0])
right_y.append(right[i][1])
mean_lx=np.mean(left_x)
mean_ly=np.mean(left_y)
left_calculated_weight=least_square(left_x, left_y, mean_lx, mean_ly)
left_calculated_bias=mean_ly-left_calculated_weight*mean_lx
target_l=left_calculated_weight*WIDTH+left_calculated_bias
#print(f"y = {left_calculated_weight} * X + {left_calculated_bias}")
mean_rx=np.mean(right_x)
mean_ry=np.mean(right_y)
right_calculated_weight=least_square(right_x, right_y, mean_rx, mean_ry)
right_calculated_bias=mean_ry-right_calculated_weight*mean_rx
target_r=right_calculated_weight*WIDTH+right_calculated_bias
print(f"y = {right_calculated_weight} * X + {right_calculated_bias}")
im0 = cv2.line(img,(int(WIDTH/2),HEIGHT),(int(WIDTH/2),int(0)),(255,0,0),3)
cross_x = (right_calculated_bias - left_calculated_bias) / (left_calculated_weight - right_calculated_weight)
cross_y = left_calculated_weight*((right_calculated_bias - left_calculated_bias)/(left_calculated_weight - right_calculated_weight)) + left_calculated_bias
print(f"y = {left_calculated_weight} * X + {left_calculated_bias}")
print(type(left_calculated_bias))
if p_cross_x==-1 and p_cross_y==-1:
p_cross_x=cross_x
p_cross_y=cross_y
p_left_calculated_bias=left_calculated_bias
p_right_calculated_bias=right_calculated_bias
p_l_target=target_l
p_r_target=target_r
if np.isnan(left_calculated_bias)==True or np.isnan(right_calculated_bias)==True or np.isnan(left_calculated_weight)==True or np.isnan(right_calculated_weight)==True:
cross_x=p_cross_x
cross_y=p_cross_y
left_calculated_bias=p_left_calculated_bias
right_calculated_bias=p_right_calculated_bias
target_l=p_l_target
target_r=p_r_target
if np.isnan(left_calculated_weight) == True:
print('++++++++우회전++++++++')
if np.isnan(right_calculated_bias) == True:
print("+++++++좌회전+++++++++")
if np.isnan(cross_x)!=True and np.isnan(cross_y)!=True:
img = cv2.line(img,(0,int(left_calculated_bias)),(int(WIDTH),int(target_l)),(0,0,0),10)
img = cv2.line(img,(int(0),int(right_calculated_bias)),(WIDTH,int(target_r)),(0,0,0),10)
cv2.circle(img, (int(cross_x), int(cross_y)), 10, (0, 0, 255), -1, cv2.LINE_AA)
if 80 < steering_theta(left_calculated_weight, right_calculated_weight) < 100:
print('소실점 조향 서보모터 각도: ', steering_vanishing_point(cross_x))
else:
print("기울기 조향 서보모터 각도: ", steering_theta(left_calculated_weight, right_calculated_weight))
p_cross_x=cross_x
p_cross_y=cross_y
p_left_calculated_bias=left_calculated_bias
p_right_calculated_bias=right_calculated_bias
p_l_target=target_l
p_r_target=target_r
#print('Done.')
return img
def least_square(val_x, val_y, mean_x, mean_y):
return ((val_x - mean_x) * (val_y - mean_y)).sum() / ((val_x - mean_x)**2).sum()
def steering_theta(w1, w2):
if np.abs(w1) > np.abs(w2): # 우회전
if w1 * w2 < 0: #정방향 or 약간 틀어진 방향
w1 = -w1
angle = np.arctan(np.abs(math.tan(w1) - math.tan(w2)) / (1 + math.tan(w1)*math.tan(w2)))
theta = matching(angle, 0, np.pi/2, 90, 180)
elif w1 * w2 > 0: #극한으로 틀어진 방향
if w1 > w2:
theta = 90
else:
theta = 90
else:
theta = 0
elif np.abs(w1) < np.abs(w2) : # 좌회전
if w1 * w2 < 0: #정방향 or 약간 틀어진 방향
w1 = -w1
angle = np.arctan(np.abs(math.tan(w1) - math.tan(w2)) / (1 + math.tan(w1)*math.tan(w2)))
theta = matching(angle, 0, np.pi/2, 90, 0)
elif w1 * w2 > 0: #극한으로 틀어진 방향
if w1 > w2:
theta = 90
else:
theta = 90
else:
theta = 0
else:
theta = 90
return theta
def matching(x,input_min,input_max,output_min,output_max):
return (x-input_min)*(output_max-output_min)/(input_max-input_min)+output_min #map()함수 정의.
def steering_vanishing_point(x):
standard_x = int(WIDTH/2)
diff = standard_x - x
if diff > 0: #좌회전
theta = matching(diff, 0, WIDTH/2, 90, 45)
elif diff < 0:
theta = matching(diff, 0, -WIDTH/2, 90, 135)
return theta
while cap.isOpened():
ret, img = cap.read()
if not ret:
print("프레임을 수신할 수 없습니다. 종료 중 ...")
break
t=time.time()
img_s = cv2.resize(img, dsize=(int(WIDTH/2), int(HEIGHT/2)))
gray=grayscale(img_s)
blur_gray=gaussian_blur(gray, kernel_size)
ret2, edges=threshold(blur_gray, low_threshold, high_threshold)
mask=np.zeros_like(img)
if len(img.shape)>2:
channel_count=img.shape[2]
ignore_mask_color=(255,) * channel_count
else:
ignore_mask_color=255
imshape=img.shape
vertices=np.array([[(30, 305),
(30, 240),
(180, 170),
(450, 170),
(610, 240),
(610, 305)]], dtype=np.int32)
cv2.fillPoly(mask, vertices, ignore_mask_color)
masked_image=region_of_interest(edges, vertices)
bigimg=cv2.resize(masked_image, dsize=(int(WIDTH), int(HEIGHT)))
## 작업공간 ##
#wad : 동영상 크기 에러 수정
contours,_=cv2.findContours(bigimg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
r_mid=[]
l_mid=[]
m_mid=[]
for pts in contours:
if cv2.contourArea(pts) <100:
continue
rc=cv2.boundingRect(pts)
dist_b=cv2.matchShapes(obj_pts_b, pts, cv2.CONTOURS_MATCH_I3, 0)
dist_s=cv2.matchShapes(obj_pts_s, pts, cv2.CONTOURS_MATCH_I3, 0)
if dist_b <0.3 or dist_s<0.1:
cv2.rectangle(bigimg, rc, (255, 0,0),1)
mid = [int((rc[0]*2+rc[2])/2), int((rc[1]*2+rc[3])/2)]
if mid[0]>530: #by vertices[2~3]/2 value
r_mid.append(mid)
m_mid.append(mid)
else:
l_mid.append(mid)
m_mid.append(mid)
r_mid, l_mid = depart_points(img, m_mid, r_mid, l_mid)
#line_img = draw_lines(bigimg, l_mid, r_mid)
#lines_edges=weighted_img(line_img, img)
for r in r_mid:
cv2.circle(img, r, 10, (0, 0, 255), -1, cv2.LINE_AA)
for l in l_mid:
cv2.circle(img, l, 10, (0, 255, 0), -1, cv2.LINE_AA)
linear_img=linear_reg(img, l_mid, r_mid)
dt=time.time()-t
print("delay : ", dt)
cv2.imshow("ex", linear_img)#lines_edges
out.write(linear_img)
if cv2.waitKey(10) == ord('q'):
break
# 작업 완료 후 해제
cap.release()
out.release()
cv2.destroyAllWindows()다음은 위의 알고리즘을 담은 파이썬 모듈이다. 이전까지는 콘 인식에만 집중했다면, 현재는 조향 알고리즘까지 같이 고민하고 있다. 또한 이번에는 엉뚱한 데로 튀는 값이나, 오른쪽, 왼쪽 차선의 콘을 정확하게 구분하기 위해 2개의 linear regression(선형 회귀)를 사용하였다.
threshold를 보았을 때, canny threshold를 사용했을 때보다 더 선명하게 콘이 나타나는 점을 알 수 있었다. 그리고 왼쪽 차선의 콘에는 초록색 점을, 오른쪽 차선의 콘에는 빨간색 점이 찍히도록 만들었다. 콘을 인식한 후에 인공지능이 우회전을 해야 할지, 좌회전을 해야 할지 결정하는 조향 알고리즘을 적용시켜 보았다. 조향 알고리즘은 오른쪽 차선, 왼쪽 차선을 구분하여 교차 지점을 구한 후, 이 교차 지점의 각도와 화면의 가운데를 넘어갔을 때를 고려하여 우회전과 좌회전을 출력하도록 하였다. 또한 한 프레임을 계산할 때까지의 지연 시간을 계산해 보았는데, 0.03초로 30 fps정도 되었다. 위의 알고리즘을 더욱 발전시켜서 배경 노이즈는 줄이고, 정확도는 더 높이도록 해보겠다. 다음에는 더 발전된 모습으로 찾아오겠다.
728x90
반응형