def _final_frame(self,
im,
fill,
lcurv,
rcurv,
offset,
font=cv.FONT_HERSHEY_DUPLEX,
scale_font=1,
color_font=(255, 0, 0),
show=False):
fill = prsp.warp(fill, inverse=True)
out = im.copy()
out = cv.addWeighted(out, 0.7, fill, 0.5, 0)
xtxt = 50
lcurv_text = 'Left curvature={0:.01f}m'.format(lcurv)
rcurv_text = 'Right curvature={0:.01f}m'.format(rcurv)
offset_text = 'Offset={0:.02f}m'.format(offset)
out = cv.putText(out, lcurv_text, (xtxt, 30), font, scale_font,
color_font)
out = cv.putText(out, rcurv_text, (xtxt, 60), font, scale_font,
color_font)
out = cv.putText(out, offset_text, (xtxt, 90), font, scale_font,
color_font)
if show == True:
show_images(im, out, 'origin', 'lanes', 'Lanes detected')
return out
def test(filename, n=0, show=False):
import importlib
importlib.reload(mask)
print(persp._perspective)
im_ = plt.imread(filename)
im = clb.undistort(im_)
if n == -1:
return persp.warp(im, show=show)
elif n == 0:
im2 = persp.warp(im, show=show)
color_mask.hls_channel(im2, channel='h', thresh=(100, 255), show=show)
color_mask.hls_channel(im2, channel='l', thresh=(200, 255), show=show)
color_mask.hls_channel(im2, channel='s', thresh=(100, 255), show=show)
elif n == 1:
im_eq = equalize_hist(im, show=show)
im2 = persp.warp(im_eq, show=show)
color_mask.hls_channel(im2, channel='h', thresh=(0, 100), show=show)
color_mask.hls_channel(im2, channel='l', thresh=(200, 255), show=show)
color_mask.hls_channel(im2, channel='s', thresh=(100, 255), show=show)
elif n == 2:
im2 = persp.warp(im, show=show)
color_mask.rgb_channel(im2, channel='r', thresh=(220, 255), show=show)
color_mask.rgb_channel(im2, channel='g', thresh=(210, 255), show=show)
color_mask.rgb_channel(im2, channel='b', thresh=(0, 100), show=show)
elif n == 3:
im2 = persp.warp(im, show=show)
color_mask.hls_channel(im2, channel='h', thresh=(0, 90), show=show)
color_mask.hls_channel(im2, channel='l', thresh=(200, 255), show=show)
color_mask.hls_channel(im2, channel='s', thresh=(110, 255), show=show)
elif n == 4:
im2 = persp.warp(im, show=show)
return custom_mask.special_mask(im2, show=show)
def process(self, frame, show=False):
im = clb.undistort(frame)
warped = prsp.warp(im)
masked = custom_mask.apply(warped, show=show)
lp, rp = self._find_lane_points(masked,
num_strips=10,
radius=70,
show=show)
height = masked.shape[0]
width = masked.shape[1]
llane = Lane(lp, end=height - 1, num=height)
rlane = Lane(rp, end=height - 1, num=height)
offset = Lane.offset(llane,
rlane,
y=masked.shape[0] - 1,
frame_width=width)
lcurv = llane.curvature(height // 2)
rcurv = rlane.curvature(height // 2)
fill = Lane.fill_lanes(warped, llane, rlane, show=show)
return self._final_frame(im, fill, lcurv, rcurv, offset, show=show)
class WhiteBoard: running = False mouseclicked = False calibrating = False KEEPLIGHTTIME = 3 #will not consider if we lose the light for x frames CLICKMAXTIME = 5 #longest that we will consider as a click for touchpad mode CLICKMINTIME = 3 # will not move the pointer for the first x frames after click to help a singleclic with shaky hand MAXTIMEBETWEENCLICKDRAG = 5 #... def __init__(self): self.mouse = MouseControl() self.XRES,self.YRES = self.mouse.get_screen_resolution() self.XRES, self.YRES = float(self.XRES), float(self.YRES) self.ABCD = [(0,0),(self.XRES-5,0),(0,self.YRES-5),(self.XRES-5,self.YRES-5)] self.perspective = Perspective() self.perspective.setdst((0.0,0.0),(self.XRES,0.0),(0.0,self.YRES),(self.XRES,self.YRES)) self.cursor = Cursor(self) self.Options = Options() def printOut(self,strList,newLine = True): for all in strList: print all, #if newLine: print "" print "" def GetBattery(self): try: bat = self.wiimote.WiimoteState.Battery except AttributeError: return None if bat: return bat*100/200 else: return None def Connect(self): self.wiimote = Wiimote() self.OnConnecting() if sys.platform != 'win32': self.printOut(["Press 1 and 2 on wiimote"]) #try: self.wiimote.Connect() #except: # self.OnNoWiimoteFound() # raise SystemExit self.OnConnected() self.wiimote.SetLEDs(True,False,False,False) self.wiimote.SetReportType(self.wiimote.InputReport.IRAccel,True) def IsConnected(self): try: cn = self.wiimote.running except: return False return cn def IsRunning(self): return self.running def OnRun(self): #to be overloaded pass def OnConnected(self): #to be overloaded self.printOut(["Battery: ", self.GetBattery(), "%"]) def OnConnecting(self): #to be overloaded pass def OnDisconnect(self): #to be overloaded pass def OnStop(self): #to be overloaded pass def OnCalibrated(self): #to be overloaded pass def OnCalibrating(self): pass def OnNoWiimoteFound(self): print "No Wiimote or no Bluetooth found..." pass def OnOptionsChanged(self): pass def Run(self): self.running = True keep_light = self.KEEPLIGHTTIME x,y = (0,0) first_x, first_y = (0,0) light_state = False timer = 0 self.OnRun() while self.running: if self.wiimote.WiimoteState.ButtonState.A: self.Stop() continue time.sleep(0.030) if self.wiimote.WiimoteState.IRState.Found1: if not light_state: #if the light appears light_state = True first_x,first_y = map(int,self.perspective.warp(self.wiimote.WiimoteState.IRState.RawX1, self.wiimote.WiimoteState.IRState.RawY1)) x,y = first_x, first_y timer = 0 #reset timer self.cursor.update(light_state, True, (first_x,first_y), (x,y), timer) else: #if the light is still lit x,y = map(int,self.perspective.warp(self.wiimote.WiimoteState.IRState.RawX1, self.wiimote.WiimoteState.IRState.RawY1)) timer += 1 keep_light = self.KEEPLIGHTTIME #keep it high while light is seen. self.cursor.update(light_state, False, (first_x,first_y), (x,y), timer) else: if keep_light and timer > self.KEEPLIGHTTIME: keep_light -= 1 #keep_light will not prevent cut-off if the light goes out quick else: if light_state: light_state = False self.cursor.update(light_state, True, (first_x,first_y), (x,y), timer) timer = self.KEEPLIGHTTIME - keep_light #this is the true delay since the light has gone off, not 0 else: timer += 1 self.cursor.update(light_state, False, (first_x,first_y), (x,y), timer) def Stop(self): self.running = False self.OnStop() def Disconnect(self): if self.IsRunning: self.Stop() self.wiimote.Disconnect() self.OnDisconnect() if self.calibrating: raise SystemExit del self.wiimote def Calibrate(self): self.Stop() self.calibrating = True self.OnCalibrating() self.printOut(["Point to the top left corner of the screen... "],False) self.ABCD[0] = self.WaitForLight() self.printOut(["OK"]) self.WaitNoLight() self.printOut(["Point to the top right corner of the screen... "],False) self.ABCD[1] = self.WaitForLight() self.printOut([ "OK"]) self.WaitNoLight() self.printOut([ "Point to the bottom left corner of the screen... "],False) self.ABCD[2] = self.WaitForLight() self.printOut([ "OK"]) self.WaitNoLight() self.printOut([ "Point to the bottom right corner of the screen... "],False) self.ABCD[3] = self.WaitForLight() self.printOut([ "OK"]) self.WaitNoLight() x0,y0 = self.ABCD[0] x1,y1 = self.ABCD[1] x2,y2 = self.ABCD[2] x3,y3 = self.ABCD[3] self.perspective.setsrc((x0,y0),(x1,y1),(x2,y2),(x3,y3)) self.calibrating = False self.printOut([ "Calibration complete!"]) self.OnCalibrated() def WaitForLight(self): dot = False while not dot: checkdot = self.wiimote.WiimoteState.IRState.RawX1, self.wiimote.WiimoteState.IRState.RawY1 if self.wiimote.WiimoteState.IRState.Found1: dot = checkdot return dot time.sleep(0.030) def WaitNoLight(self): time.sleep(0.5) while 1: time.sleep(0.030) if self.wiimote.WiimoteState.IRState.Found1 == False: return
class WhiteBoard:
running = False
mouseclicked = False
calibrating = False
KEEPLIGHTTIME = 3 #will not consider if we lose the light for x frames
CLICKMAXTIME = 5 #longest that we will consider as a click for touchpad mode
CLICKMINTIME = 3 # will not move the pointer for the first x frames after click to help a singleclic with shaky hand
MAXTIMEBETWEENCLICKDRAG = 5 #...
def __init__(self):
self.mouse = MouseControl()
self.XRES, self.YRES = self.mouse.get_screen_resolution()
self.XRES, self.YRES = float(self.XRES), float(self.YRES)
self.ABCD = [(0, 0), (self.XRES - 5, 0), (0, self.YRES - 5),
(self.XRES - 5, self.YRES - 5)]
self.perspective = Perspective()
self.perspective.setdst((0.0, 0.0), (self.XRES, 0.0), (0.0, self.YRES),
(self.XRES, self.YRES))
self.cursor = Cursor(self)
self.Options = Options()
def printOut(self, strList, newLine=True):
for all in strList:
print all,
#if newLine: print ""
print ""
def GetBattery(self):
try:
bat = self.wiimote.WiimoteState.Battery
except AttributeError:
return None
if bat: return bat * 100 / 200
else: return None
def Connect(self):
self.wiimote = Wiimote()
self.OnConnecting()
if sys.platform != 'win32': self.printOut(["Press 1 and 2 on wiimote"])
#try:
self.wiimote.Connect()
#except:
# self.OnNoWiimoteFound()
# raise SystemExit
self.OnConnected()
self.wiimote.SetLEDs(True, False, False, False)
self.wiimote.SetReportType(self.wiimote.InputReport.IRAccel, True)
def IsConnected(self):
try:
cn = self.wiimote.running
except:
return False
return cn
def IsRunning(self):
return self.running
def OnRun(self): #to be overloaded
pass
def OnConnected(self): #to be overloaded
self.printOut(["Battery: ", self.GetBattery(), "%"])
def OnConnecting(self): #to be overloaded
pass
def OnDisconnect(self): #to be overloaded
pass
def OnStop(self): #to be overloaded
pass
def OnCalibrated(self): #to be overloaded
pass
def OnCalibrating(self):
pass
def OnNoWiimoteFound(self):
print "No Wiimote or no Bluetooth found..."
pass
def OnOptionsChanged(self):
pass
def Run(self):
self.running = True
keep_light = self.KEEPLIGHTTIME
x, y = (0, 0)
first_x, first_y = (0, 0)
light_state = False
timer = 0
self.OnRun()
while self.running:
if self.wiimote.WiimoteState.ButtonState.A:
self.Stop()
continue
time.sleep(0.030)
if self.wiimote.WiimoteState.IRState.Found1:
if not light_state: #if the light appears
light_state = True
first_x, first_y = map(
int,
self.perspective.warp(
self.wiimote.WiimoteState.IRState.RawX1,
self.wiimote.WiimoteState.IRState.RawY1))
x, y = first_x, first_y
timer = 0 #reset timer
self.cursor.update(light_state, True, (first_x, first_y),
(x, y), timer)
else: #if the light is still lit
x, y = map(
int,
self.perspective.warp(
self.wiimote.WiimoteState.IRState.RawX1,
self.wiimote.WiimoteState.IRState.RawY1))
timer += 1
keep_light = self.KEEPLIGHTTIME #keep it high while light is seen.
self.cursor.update(light_state, False, (first_x, first_y),
(x, y), timer)
else:
if keep_light and timer > self.KEEPLIGHTTIME:
keep_light -= 1 #keep_light will not prevent cut-off if the light goes out quick
else:
if light_state:
light_state = False
self.cursor.update(light_state, True,
(first_x, first_y), (x, y), timer)
timer = self.KEEPLIGHTTIME - keep_light #this is the true delay since the light has gone off, not 0
else:
timer += 1
self.cursor.update(light_state, False,
(first_x, first_y), (x, y), timer)
def Stop(self):
self.running = False
self.OnStop()
def Disconnect(self):
if self.IsRunning: self.Stop()
self.wiimote.Disconnect()
self.OnDisconnect()
if self.calibrating: raise SystemExit
del self.wiimote
def Calibrate(self):
self.Stop()
self.calibrating = True
self.OnCalibrating()
self.printOut(["Point to the top left corner of the screen... "],
False)
self.ABCD[0] = self.WaitForLight()
self.printOut(["OK"])
self.WaitNoLight()
self.printOut(["Point to the top right corner of the screen... "],
False)
self.ABCD[1] = self.WaitForLight()
self.printOut(["OK"])
self.WaitNoLight()
self.printOut(["Point to the bottom left corner of the screen... "],
False)
self.ABCD[2] = self.WaitForLight()
self.printOut(["OK"])
self.WaitNoLight()
self.printOut(["Point to the bottom right corner of the screen... "],
False)
self.ABCD[3] = self.WaitForLight()
self.printOut(["OK"])
self.WaitNoLight()
x0, y0 = self.ABCD[0]
x1, y1 = self.ABCD[1]
x2, y2 = self.ABCD[2]
x3, y3 = self.ABCD[3]
self.perspective.setsrc((x0, y0), (x1, y1), (x2, y2), (x3, y3))
self.calibrating = False
self.printOut(["Calibration complete!"])
self.OnCalibrated()
def WaitForLight(self):
dot = False
while not dot:
checkdot = self.wiimote.WiimoteState.IRState.RawX1, self.wiimote.WiimoteState.IRState.RawY1
if self.wiimote.WiimoteState.IRState.Found1:
dot = checkdot
return dot
time.sleep(0.030)
def WaitNoLight(self):
time.sleep(0.5)
while 1:
time.sleep(0.030)
if self.wiimote.WiimoteState.IRState.Found1 == False:
return
plt.ylim(720, 0)
plt.axis("off")
# Save visualization
if save_image:
plt.savefig('./output_images/vis_area.png', bbox_inches='tight')
plt.show()
if __name__ == '__main__':
polyfit = Polyfit()
perspective = Perspective()
image_file = 'test_images/test2.jpg'
image = mpimg.imread(image_file)
undistorted = undistort(image)
# Create binary outputs
abs_thresh, mag_thresh, dir_thresh, hls_thresh, hsv_thresh, combined_output = combined_threshold(
undistorted)
plt.imshow(combined_output, cmap='gray')
warped = perspective.warp(combined_output)
plt.imshow(warped, cmap='gray')
# Find lanes
left_fit, right_fit, vars = polyfit.poly_fit_skip(warped)
left_curve_rad, right_curve_rad = polyfit.curvature()
position = polyfit.vehicle_position(warped)
print('curvature: {:.2f}m'.format((left_curve_rad + right_curve_rad) / 2))
print('vehicle position: {:.2f}m from center'.format(position))
# Visualize lane finding
polyfit.visualize_window(warped, vars, save_image=True)
polyfit.visualize_area(warped, vars, save_image=True)
class LaneLines:
def __init__(self, camera_matrix, distortion_coefficients, buffer_size = 50, img_size = (1280,720)):
self.camera_matrix = camera_matrix
self.distortion_coefficients = distortion_coefficients
src = np.float32(
[[(img_size[0] / 2) - 75, img_size[1] / 2 + 100],
[((img_size[0] / 6) - 10), img_size[1]],
[(img_size[0] * 5 / 6) + 60, img_size[1]],
[(img_size[0] / 2 + 75), img_size[1] / 2 + 100]])
dst = np.float32(
[[(img_size[0] / 4), 0],
[(img_size[0] / 4), img_size[1]],
[(img_size[0] * 3 / 4), img_size[1]],
[(img_size[0] * 3 / 4), 0]])
self.perspective = Perspective(src,dst)
self.search_engine = HistogramSearch(
window_width = 200,
window_height = 80,
min_pixels_to_recenter=50,
visualisation = None)
self.left = Lane(buffer_size)
self.right = Lane(buffer_size)
def draw(self, img):
preprocessed_img = self.preprocess(img)
left, right = self.find_lanes(preprocessed_img)
result = img
if left.best_fit is not None and right.best_fit is not None:
area = self.lane_area(preprocessed_img, left, right)
area = cv2.warpPerspective(area, self.perspective.matrix, (img.shape[1], img.shape[0]), flags=cv2.WARP_INVERSE_MAP)
result = cv2.addWeighted(img, 1, area, 0.3, 0)
ls = LaneStatistics(preprocessed_img, left)
rs = LaneStatistics(preprocessed_img, right)
radius = "radius of curvature {0:.0f} m" \
.format((ls.radius_of_curvature_in_m + rs.radius_of_curvature_in_m)/2)
offset = "offset {0:.2f} m" \
.format(np.abs(ls.distance_to_vehicle_center_in_m - rs.distance_to_vehicle_center_in_m))
cv2.putText(result, radius, (50, 65), cv2.FONT_HERSHEY_PLAIN, 3,(255,255,255),2);
cv2.putText(result, offset, (50, 135), cv2.FONT_HERSHEY_PLAIN, 3,(255,255,255),2);
return result
def find_lanes(self, preprocessed_img):
l_detection, r_detection = self.search_engine.find_lane_polynomials(
preprocessed_img,
self.left.best_fit,
self.right.best_fit)
if l_detection is None or r_detection is None or not l_detection.trustful() or not r_detection.trustful():
l_detection, r_detection = self.search_engine.find_lane_polynomials(preprocessed_img)
self.left.update(l_detection)
self.right.update(r_detection)
return (self.left, self.right)
def lane_area(self, warped, left_lane, right_lane):
warp_zero = np.zeros_like(warped).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
arguments_y = np.linspace(0, warped.shape[0]-1, warped.shape[0] )
left_values_x = left_lane.best_fit[0]*arguments_y**2 + left_lane.best_fit[1]*arguments_y + left_lane.best_fit[2]
right_values_x = right_lane.best_fit[0]*arguments_y**2 + right_lane.best_fit[1]*arguments_y + right_lane.best_fit[2]
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_values_x, arguments_y]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_values_x, arguments_y])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
return color_warp
def preprocess(self, img):
img = self.undistort(img)
img = self.blur(img)
img = self.threshold(img)
img = self.warp(img)
return img
def warp(self, img):
return self.perspective.warp(img)
def blur(self, img, kernel_size = 5):
return cv2.GaussianBlur(img, (kernel_size, kernel_size), 0)
def undistort(self, img):
return cv2.undistort(img, self.camera_matrix, self.distortion_coefficients, None, self.camera_matrix)
def threshold(self, img):
img_hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS).astype(np.float)
img_lab = cv2.cvtColor(img, cv2.COLOR_BGR2LAB).astype(np.float)
img_luv = cv2.cvtColor(img, cv2.COLOR_BGR2LUV).astype(np.float)
threshold = Threshold()
w_binary = threshold.white(img_luv, min_max= (180, 255))
l_x = threshold.gradient_magnitude_x(img_luv[:,:,0], sobel_kernel=5, min_max = (15,255))
l_d = threshold.gradient_direction_xy(img_luv[:,:,0], sobel_kernel=3, min_max = (0.7, 1.3))
white = np.zeros_like(w_binary)
white[(img[:,:,0] >= 200) & (img[:,:,1] >= 200) & (img[:,:,2] >= 200)] = 1
r_x = threshold.gradient_magnitude_x(white, sobel_kernel=5, min_max = (10,255))
l_binary = threshold.lightness(img_hls, min_max = (210, 255))
g_m = threshold.gradient_magnitude_x(l_binary, sobel_kernel=31, min_max = (100, 255))
white_lane = np.zeros_like(w_binary)
white_lane [(r_x == 1) | ((l_d == 1) & (l_x == 1) & (w_binary == 1)) | (g_m == 1)] = 1
y_binary = threshold.blue_yellow(img_lab, min_max= (145, 255))
y_m = threshold.gradient_magnitude_xy(img_lab[:,:,2], sobel_kernel=3, min_max = (50, 255))
s_binary = threshold.saturation(img_hls, min_max = (10,255))
s_x = threshold.gradient_magnitude_x(img_hls[:,:,2], sobel_kernel=31, min_max = (100,255))
s_d = threshold.gradient_direction_xy(img_hls[:,:,2], sobel_kernel=31, min_max = (0.6, 1.2))
yellow_line = np.zeros_like(y_binary)
yellow_line[(y_binary == 1) | (y_m == 1) | ((s_d == 1) & (s_x == 1) & (s_binary == 1))] = 1
result = np.zeros_like(w_binary)
result[(yellow_line == 1) | (white_lane == 1)] = 1
return result
