collab/igvc2018

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

Vivek Malneediuntested odometryd809afd

camera
7.0 KiB199 linesraw
1#!/usr/bin/env python
2#
3
4import roslib
5import rospy
6import cv2
7import timeit
8import numpy as np
9from sensor_msgs.msg import Image
10from cv_bridge import CvBridge, CvBridgeError
11
12
13def process_img_Daniel(img):
14    #Set margins for isolation of the color white
15    sensitivity = 20  #variable sensitivity value (can be tuned further down the line)
16    whiteLower = (0, 0, 255 - sensitivity)
17    whiteUpper = (255, sensitivity, 255)
18
19    #Read image
20    start = timeit.default_timer()  #clock algorithm start
21    img = cv2.resize(img, (640, 360))  #resize image to decrease overhead
22    #cv2.imshow('original',img)                         #debugging
23
24    # Create the basic black image
25    white_rec = np.zeros(img.shape, dtype="uint8")
26    # Draw a white, filled rectangle on the mask image
27    cv2.rectangle(white_rec, (0, 100), (720, 500), (255, 255, 255), -1)
28
29    #cv2.imshow('white rectangle',white_rec)            #debugging
30
31    imgROI = cv2.bitwise_and(
32        img, white_rec)  #get rid of the top part to decrease overhead (O(1))
33    #cv2.imshow('masked img',imgROI)                    #debugging
34
35    imgHSV = cv2.cvtColor(
36        imgROI, cv2.COLOR_BGR2HSV
37    )  #change color scheme from BGR to HSV for easier color isolation (time complexity O(N^3)))
38
39    # Create a image that filters out every color besides white
40    mask = cv2.inRange(imgHSV, whiteLower,
41                       whiteUpper)  #time complexity estimate O(N^3)
42
43    #helper variables for masking the part of the image immediately in front of the robot
44    rows = imgHSV.shape[0]
45    cols = imgHSV.shape[1]
46    offset = 50  #offset for drawing the polygon
47    mask_immediate_front = np.array(
48        [[[cols / 5, rows / 2], [cols * 4 / 5, rows / 2],
49          [cols - offset, rows], [offset, rows]]],
50        dtype=np.int32)
51
52    cv2.fillPoly(mask, mask_immediate_front,
53                 0)  #create the polygon on the mask
54    #cv2.imshow('mask',mask)                            #debugging
55
56    res = cv2.bitwise_and(
57        imgROI, imgROI, mask=mask
58    )  #apply the mask to the pre-processed image (time complexity O(1))
59    #cv2.imshow('res',res)                              #debugging code
60
61    #Select hysteresis thresholds for Canny edge detection algorithm
62    low_threshold = 100
63    high_threshold = 200
64
65    #run canny edge detection algorithm
66    edges = cv2.Canny(
67        res, low_threshold, high_threshold
68    )  #notes: Convolutions using Fast Fourier Transform are O(n*log(n)).
69    #       For an m by n image, complexity is O(mn(log(mn))).
70    #       This is the bottleneck in the pipeline
71    #cv2.imshow('edges',edges)                          #debugging code
72
73    #Select parameters for probabilistic Hough lines transform
74    rho = 1  # distance resolution in pixels of the Hough grid
75    theta = np.pi / 180  # angular resolution in radians of the Hough grid
76    threshold = 15  # minimum number of votes (intersections in Hough grid cell)
77    min_line_length = 20  # minimum number of pixels making up a line
78    max_line_gap = 50  # maximum gap in pixels between connectable line segments
79    line_image = np.copy(res) * 0  # creating a blank to draw lines on
80
81    # Run Hough on edge detected image
82    # Output "lines" is an array containing endpoints of detected line segments **** NOTE: "Navigation" will use these data points ****
83    lines = cv2.HoughLinesP(edges, rho, theta, threshold, np.array([]),
84                            min_line_length, max_line_gap)
85
86    stop = timeit.default_timer()  #clock algorithm end
87
88    #for demo purposes...
89    #iterate through to create the Hough lines on top of the data set returned by the Hough transform
90    if (lines is not None):
91        for line in lines:
92            for x1, y1, x2, y2 in line:
93                cv2.line(line_image, (x1, y1), (x2, y2), (255, 50, 50), 5)
94
95    #add the lines on top of the original image for display
96    lines_edges = cv2.addWeighted(img, 0.8, line_image, 1, 0)
97
98    #cv2.imshow('hough image', res)                         # debugging code
99
100    time = str((stop - start) * 1000)
101    print(time + ' ms')
102
103    return lines_edges
104
105
106## Processes image and returns image with drawn lines
107#  @type image: Mat
108#  @param image input image to process
109#
110#  @return image with detected edges
111def process_img_old(image):
112    # Keep a safe copy of the original image
113    original_image = image
114
115    # convert the image to Grayscale
116    processed_img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
117
118    # Edge detection: cv2.Canny(image to run detection, lower bound of intensity to ignore, upper bound of intensity to consider an edge)
119    edgeIntensityMin = 200
120    edgeIntensityMax = 300
121    processed_img = cv2.Canny(
122        processed_img,
123        threshold1=edgeIntensityMin,
124        threshold2=edgeIntensityMax)
125
126    # Gaussian Blur: cv2.GaussianBlur(image to run blur on, kernel size, sigma)
127    processed_img = cv2.GaussianBlur(processed_img, (5, 5), 0)
128
129    # Define a blank mask
130    masking = np.zeros_like(processed_img)
131
132    # Helper variables
133    rows = image.shape[0]
134    cols = image.shape[1]
135
136    # Define points of the region of interest, in this case we defined a trapezoid for the lower half of the screen
137    # It needs to be 32 bit for masking to work correctly
138    roi = np.array(
139        [[[cols / 5, rows / 2], [cols * 4 / 5, rows / 2], [cols, rows],
140          [0, rows]]],
141        dtype=np.int32)
142
143    # Actually draw the polygon now
144    cv2.fillPoly(masking, roi, 255)
145    # cv2.imshow("mask", masking) # debugging code
146
147    # Apply mask to the original image we mask here so we don't get weird edges created by masking the original image
148    processed_img = cv2.bitwise_and(processed_img, processed_img, mask=masking)
149
150    # Hough Line Transforms
151    lines = cv2.HoughLinesP(processed_img, 1, np.pi / 180, 180, 20, 15)
152
153    # Draw the lines on the image
154    draw_lines(original_image, lines)
155
156    return original_image
157
158
159## Helper function to draw lines on an image
160# @param image the image to draw on
161# @param lines output from Hough Line Transform to draw onto image
162def draw_lines(image, lines):
163    # Prevents some errors if there were no lines detected
164    if lines is None:
165        return
166
167    # Draw detected lines on the image (passed by reference)
168    for line in lines:
169        coords = line[0]
170        cv2.line(image, (coords[0], coords[1]), (coords[2], coords[3]),
171                 [0, 0, 255], 3)
172
173
174## ROS callback for subscriber image topic
175# @param msg the image advertised from another node
176#
177def image_callback(msg):
178    img = msg
179    cv_image = bridge.imgmsg_to_cv2(img, desired_encoding="passthrough")
180    cv2.imshow("Image window", process_img_Daniel(cv_image))
181    # If 'q' is detected, quit
182    if (chr(cv2.waitKey(3) & 255) == 'q'):
183        rospy.signal_shutdown("Quit detected")
184
185
186bridge = CvBridge()
187
188# Initialize a node in ROS
189rospy.init_node('image_listener')
190
191# Create a subscriber to read the published camera image
192rospy.Subscriber("/camera/image_raw", Image, image_callback)
193
194# Loop until we die
195try:
196    rospy.spin()
197except KeyboardInterrupt:
198    print("Shutting down")
199cv2.destroyAllWindows()