collab/igvc2018

git clone https://git.julia.blue/collab/igvc2018

Vivek Malneediuntested odometryd809afd

camera
5.3 KiB146 linesraw
1###This is a test for lane detection using a still image and openCV
2## Created by: Daniel Diamont
3## Created: 2/15/2018
4## Last: 2/16/2018
5
6## TODO:
7# 1) Test this on different images
8# 2) Select a capture rate that does not produce too much overhead (2-5 Hz)
9# 3) Remove line drawing code
10# 4) Merge mask operations to reduce time complexity
11
12##import openCV packages, numpy, and timeit for testing
13import cv2
14import numpy as np
15import timeit
16import matplotlib.pyplot as plt
17
18
19def compute_perspective_transform(binary_image):
20    # Define 4 source and 4 destination points = np.float32([[,],[,],[,],[,]])
21    shape = binary_image.shape[::-1]  # (width,height)
22    w = shape[0]
23    h = shape[1]
24    transform_src = np.float32([[250, 100], [0, 150], [w, h], [600, 100]])
25    transform_dst = np.float32([[0, 0], [0, h], [w, h], [w, 0]])
26    M = cv2.getPerspectiveTransform(transform_src, transform_dst)
27    return M
28
29
30def apply_perspective_transform(binary_image, M, plot=False):
31    warped_image = cv2.warpPerspective(
32        binary_image,
33        M, (binary_image.shape[1], binary_image.shape[0]),
34        flags=cv2.INTER_NEAREST)  # keep same size as input image
35    cv2.imshow('warp', warped_image)
36    if (plot):
37        # Ploting both images Binary and Warped
38        f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20, 10))
39        ax1.set_title('Binary/Undistorted and Tresholded')
40        ax1.imshow(binary_image, cmap='gray')
41        ax2.set_title('Binary/Undistorted and Warped Image')
42        ax2.imshow(warped_image, cmap='gray')
43        plt.show()
44
45    return warped_image
46
47
48#Set margins for isolation of the color white
49sensitivity = 20  #variable sensitivity value (can be tuned further down the line)
50whiteLower = (0, 0, 255 - sensitivity)
51whiteUpper = (255, sensitivity, 255)
52
53#Read image
54start = timeit.default_timer()  #clock algorithm start
55img = cv2.imread('igvc_lanes.jpg')
56img = cv2.resize(img, (640, 360))  #resize image to decrease overhead
57#cv2.imshow('original',img)							#debugging
58
59# Create the basic black image
60white_rec = np.zeros(img.shape, dtype="uint8")
61# Draw a white, filled rectangle on the mask image
62cv2.rectangle(white_rec, (0, 100), (720, 500), (255, 255, 255), -1)
63
64#cv2.imshow('white rectangle',white_rec)			#debugging
65
66imgROI = cv2.bitwise_and(
67    img, white_rec)  #get rid of the top part to decrease overhead (O(1))
68#cv2.imshow('masked img',imgROI)					#debugging
69
70imgHSV = cv2.cvtColor(
71    imgROI, cv2.COLOR_BGR2HSV
72)  #change color scheme from BGR to HSV for easier color isolation (time complexity O(N^3)))
73
74# Create a image that filters out every color besides white
75mask = cv2.inRange(imgHSV, whiteLower,
76                   whiteUpper)  #time complexity estimate O(N^3)
77
78#helper variables for masking the part of the image immediately in front of the robot
79rows = imgHSV.shape[0]
80cols = imgHSV.shape[1]
81offset = 50  #offset for drawing the polygon
82mask_immediate_front = np.array(
83    [[[cols / 5, rows / 2], [cols * 4 / 5, rows / 2], [cols - offset, rows],
84      [offset, rows]]],
85    dtype=np.int32)
86
87cv2.fillPoly(mask, mask_immediate_front, 0)  #create the polygon on the mask
88#cv2.imshow('mask',mask)							#debugging
89
90res = cv2.bitwise_and(
91    imgROI, imgROI, mask=mask
92)  #apply the mask to the pre-processed image (time complexity O(1))
93cv2.imshow('res', res)  #debugging code
94
95warped_image = apply_perspective_transform(res,
96                                           compute_perspective_transform(res))
97
98#cv2.imshow('warp', warped_image)
99
100#Select hysteresis thresholds for Canny edge detection algorithm
101low_threshold = 100
102high_threshold = 200
103
104#run canny edge detection algorithm
105edges = cv2.Canny(
106    res, low_threshold, high_threshold
107)  #notes: Convolutions using Fast Fourier Transform are O(n*log(n)).
108#		For an m by n image, complexity is O(mn(log(mn))).
109#		This is the bottleneck in the pipeline
110#cv2.imshow('edges',edges)							#debugging code
111
112#Select parameters for probabilistic Hough lines transform
113rho = 1  # distance resolution in pixels of the Hough grid
114theta = np.pi / 180  # angular resolution in radians of the Hough grid
115threshold = 15  # minimum number of votes (intersections in Hough grid cell)
116min_line_length = 20  # minimum number of pixels making up a line
117max_line_gap = 50  # maximum gap in pixels between connectable line segments
118line_image = np.copy(res) * 0  # creating a blank to draw lines on
119
120# Run Hough on edge detected image
121# Output "lines" is an array containing endpoints of detected line segments **** NOTE: "Navigation" will use these data points ****
122lines = cv2.HoughLinesP(edges, rho, theta, threshold, np.array([]),
123                        min_line_length, max_line_gap)
124
125stop = timeit.default_timer()  #clock algorithm end
126
127#for demo purposes...
128#iterate through to create the Hough lines on top of the data set returned by the Hough transform
129if (lines is not None):
130    for line in lines:
131        for x1, y1, x2, y2 in line:
132            cv2.line(line_image, (x1, y1), (x2, y2), (255, 50, 50), 5)
133
134#add the lines on top of the original image for display
135lines_edges = cv2.addWeighted(img, 0.8, line_image, 1, 0)
136
137#cv2.imshow('hough image', res)							# debugging code
138#cv2.imshow(
139#    'Lane Detection',
140#    lines_edges)  # display original image with lanes detected and overlayed
141
142time = str((stop - start) * 1000)
143print(time + ' ms')
144
145cv2.waitKey(0)
146cv2.destroyAllWindows()