I have been trying to make line detection using my Robobo (Robot), and I got stuck. This is the code I am using (it is also the same code hat is published on there tutorial (https://github.com/mintforpeople/robobo-programming/wiki/Lane-detection-library):
import cv2
import numpy as np
from robobo_video.robobo_video import RoboboVideo
from robobopy.Robobo import Robobo
def draw_line_pro(coeffs1, coeffs2, M, frame):
# Get height and width to the frame
try:
height, width, _ = frame.shape
# Mask for draw the lines
mask = np.zeros_like(frame)
# Create an Array with numbers to 0 at height -1
plot_y = np.linspace(0, height - 1, height)
# Arrays evaluating the coefficients
left_x = coeffs1['a'] * plot_y 2 \
+ coeffs1['b'] * plot_y \
+ coeffs1['c']
right_x = coeffs2['a'] * plot_y 2 + \
coeffs2['b'] * plot_y + \
coeffs2['c']
# Draw the lanes (blue for left lane and red for right lane)
cv2.polylines(mask, [np.int32(np.stack((left_x, plot_y), axis=1))], False, (255, 0, 0), 20)
cv2.polylines(mask, [np.int32(np.stack((right_x, plot_y), axis=1))], False, (0, 0, 255), 20)
# Warp the perspective
mask = cv2.warpPerspective(mask, np.float32(M), (width, height))
# Add the lane to the original frame
img = cv2.addWeighted(frame, 1., mask, 0.5, 0)
except ValueError:
pass
return img
IP = '...'
rob = Robobo(IP)
rob.connect()
rob.moveTiltTo(120, 50)
video = RoboboVideo(IP)
video.connect()
rob.setLaneColorInversion(False)
rob.startLaneDetection()
while True:
frame = video.getImage()
# obtein the coefficients to the left and right lane and the reverse transformation matrix
obj = rob.readLanePro()
frame = draw_line_pro(obj.coeffs1, obj.coeffs2, obj.minv, frame)
# show the frame draw
cv2.imshow('smartphone camera', frame)
# if you press the key 'q', the script ends
if cv2.waitKey(1) & 0xFF == ord('q'):
video.disconnect()
cv2.destroyAllWindows()
break
rob.stopLaneDetection()
When I run it the lines just keep flying around and I dont know whats wrong but I think there is something wrong with this lines of code:
left_x = coeffs1['a'] * plot_y 2 \
+ coeffs1['b'] * plot_y \
+ coeffs1['c']
right_x = coeffs2['a'] * plot_y 2 + \
coeffs2['b'] * plot_y + \
coeffs2['c']
If someone can tell me whats wrong and help me I would appreciate it.