collab/igvc2018

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

Ricky ChenTesting lower camera position6ac3dce

simulation
12.4 KiB407 linesraw
1#!/usr/bin/env python
2
3import roslib
4import rospy
5import cv2
6import numpy as np
7from sensor_msgs.msg import Image
8from cv_bridge import CvBridge, CvBridgeError
9import time
10
11# termination criteria
12criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
13
14# checkerboard dimensions (defined by corners)
15# Must be asymmetrical
16cbrow = 7
17cbcol = 6
18
19# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
20objp = np.zeros((cbrow*cbcol,3), np.float32)
21objp[:,:2] = np.mgrid[0:cbcol,0:cbrow].T.reshape(-1,2)
22
23# Arrays to store object points and image points from all the images.
24objpoints = [] # 3d point in real world space
25imgpoints = [] # 2d points in image plane.
26
27# Have to calibrate our cameras in real life.
28
29def calibrate_camera(image):
30    #last_time = time.time()
31    img = image
32    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
33
34    # Find the chess board corners
35    ret, corners = cv2.findChessboardCorners(gray, (cbcol,cbrow), None)
36    print(loop)
37
38    # If found, add object points, image points (after refining them)
39    if ret == True:
40        objpoints.append(objp)
41
42        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
43        imgpoints.append(corners2)
44
45        # Draw and display the corners
46        img = cv2.drawChessboardCorners(img, (cbcol,cbrow), corners2, ret)
47
48    #print('Frame took {} seconds'.format(time.time() - last_time))
49    
50
51    return img
52
53# Add light threshold to select white pixels only
54# Increase frames from ~3 FPS to ~20 FPS
55
56def select_white(image):
57    # Keep a safe copy of the original image
58    original_image = image
59
60    # Convert image to HSL
61    processed_img = cv2.cvtColor(image, cv2.COLOR_BGR2HLS)
62
63    # white color mask
64    lower = np.uint8([0,160,0])
65    upper = np.uint8([255,255,255])
66    white_mask = cv2.inRange(processed_img, lower, upper)
67
68    # return mask
69    return cv2.bitwise_and(original_image, original_image, mask = white_mask)
70
71def draw_skeleton(image):
72    # Keep a safe copy of the original image
73    original_image = image
74
75    # Set size of array (800 x 800 pixels each with one grayscale value)
76    size = np.size(image)
77
78    # Set array of zeros for final skeleton image
79    skel = np.zeros(image.shape, np.uint8)
80
81    # Pass shape and size of Morphological kernel
82    element = cv2.getStructuringElement(cv2.MORPH_CROSS, (3,3))
83
84    # Set initial condition to false
85    done = False
86
87    # Repeat until finished
88    while(not done):
89        # Erode boundaries of foreground object
90        eroded = cv2.erode(image, element)
91
92        # Opposite of erosion -- Increases boundaries of foreground object
93        dilated = cv2.dilate(eroded, element)
94
95        # Calculate per-element difference
96        temp = cv2.subtract(image, dilated)
97
98        # Only adds skeleton to original skel array
99        skel = cv2.bitwise_or(skel, temp)
100
101        # ???
102        image = eroded.copy()
103
104        zeros = size - cv2.countNonZero(image)
105        if zeros == size:
106            done = True
107
108    return skel
109
110## Processes image and returns image with drawn lines
111#  @type image: Mat
112#  @param image input image to process
113#  
114#  @return image with detected edges
115def process_img(image):
116    # Track frames per second
117    # last_time = time.time()
118
119    # Keep a safe copy of the original image
120    original_image = image
121
122    # Debugging
123    cv2.imshow('original image', original_image)
124
125    # Retrieve image after applying white masking
126    white_img = select_white(image)
127    
128    # convert the image to Grayscale
129    gray_img = cv2.cvtColor(white_img, cv2.COLOR_BGR2GRAY)
130
131    # Edge detection: cv2.Canny(image to run detection, lower bound of intensity to ignore, upper bound of intensity to consider an edge)
132    edgeIntensityMin = 100
133    edgeIntensityMax = 200
134    canny_img = cv2.Canny(gray_img, threshold1 = edgeIntensityMin, threshold2 = edgeIntensityMax)
135    
136    # Gaussian Blur: cv2.GaussianBlur(image to run blur on, kernel size, sigma)
137    processed_img = cv2.GaussianBlur(canny_img, (5,5), 0)
138
139    # Define a blank mask
140    masking = np.zeros_like(processed_img)
141    
142    # Helper variables
143    rows = image.shape[0]
144    cols = image.shape[1]
145    
146    # Define points of the region of interest, in this case we defined a trapezoid for the lower half of the screen
147    # It needs to be 32 bit for masking to work correctly
148    roi = np.array([[[0, rows/2], [cols, rows/2], [cols, rows], [0, rows]]], dtype = np.int32)
149    # roi = np.array([[[cols/3, rows/2], [cols*2/3, rows/2], [cols*12, rows/1.465], [cols/12, rows/1.465]]], dtype = np.int32)
150    
151    # Actually draw the polygon now
152    cv2.fillPoly(masking, roi, 255)
153    # cv2.imshow("mask", masking) # debugging code
154
155    # Apply mask to the original image we mask here so we don't get weird edges created by masking the original image
156    processed_img = cv2.bitwise_and(processed_img, processed_img, mask=masking)
157
158    # Apply homography with perspective transform
159    warp_img = apply_perspective(processed_img)
160
161    # Hough Line Transforms
162    lines = cv2.HoughLinesP(warp_img, 1, np.pi/180, 180, np.array([]), 150, 5)
163
164    # Remove similar lines
165    updated_lines = discard_parallel_lines(lines)
166
167    # Draw the lines on the image
168    draw_lines(warp_img, updated_lines)
169
170    # Used for debugging purposes
171    cv2.imshow("Warped Image", warp_img)
172
173    # Convert to polar Coordinates
174    polar_image = convert_to_polar(warp_img)
175
176    return polar_image
177
178def new_process_img(image):
179    # Track frames per second
180    # last_time = time.time()
181
182    # Keep a safe copy of the original image
183    original_image = image
184
185    # Debugging
186    cv2.imshow('original image', original_image)
187
188    # Retrieve image after applying white masking
189    white_img = select_white(image)
190    cv2.imshow("White Image", white_img)
191    
192    # convert the image to Grayscale
193    gray_img = cv2.cvtColor(white_img, cv2.COLOR_BGR2GRAY)
194    cv2.imshow("Gray Image", gray_img)
195
196    # Apply homography with perspective transform
197    warp_img = apply_perspective(gray_img)
198    cv2.imshow("Warped Image", warp_img)
199
200    # Draw skeleton
201    #skel_img = draw_skeleton(warp_img)
202    #cv2.imshow("Skeleton Image", skel_img)
203
204    # Convert to polar Coordinates
205    polar_image = convert_to_polar(warp_img)
206    # cv2.imshow("Polar image", polar_image)
207
208    return polar_image
209
210
211## Helper function to draw lines on an image
212# @param image the image to draw on
213# @param lines output from Hough Line Transform to draw onto image
214def draw_lines(image, lines):
215    # Prevents some errors if there were no lines detected
216    if lines is None:
217        return
218
219    # Draw detected lines on the image (passed by reference)
220
221    for line in lines:
222        coords = line[0]
223        cv2.line(image, (coords[0], coords[1]), (coords[2], coords[3]), 255, 3)
224
225def discard_parallel_lines(lines):
226    if lines is None:
227        return
228
229    previous_slopes = {}
230    counter = 0
231
232    number_of_decimals = 3          # Number of places to right of decimal place
233
234    #print(precision)
235
236    new_lines = lines
237    original_length = new_lines.size
238
239    for line in lines:
240        dead_zone = float(0.1)          # +/- initial range (i.e Slope of -4.5 to 5.5 are all parallel slopes)
241
242        precision = float(dead_zone / (10**(-number_of_decimals)))      # Iterate through each increment
243                                                                        # of decimal places within range
244
245        coords = line[0]
246        #print(coords)
247        if(coords[2] - coords[0] != 0):
248            slope = (float(coords[3]) - float(coords[1])) / (float(coords[2]) - float(coords[0]))
249        else:
250            slope = 9999        # Abnormally large slope for vertical line to not divide by zero
251        
252        rounded_slope = round(slope, number_of_decimals)
253        #print("looping: ", counter)
254        #print(slope)
255
256        if(rounded_slope in previous_slopes):
257            #print(counter)
258            #print(previous_slopes)
259            #print(new_lines)
260            #print("deleting center: ", rounded_slope, new_lines[counter, 0])
261            new_lines = np.delete(new_lines, counter, 0)
262            counter -= 1
263        else:
264            #print(counter)
265            while(precision >= 1):
266                dead_zone = precision*(10**(-number_of_decimals))       # Reset dead zone to close in on center value
267                #print(dead_zone)
268                #print round(rounded_slope - dead_zone, number_of_decimals)
269                # print (previous_slopes, round(rounded_slope - dead_zone, number_of_decimals))
270
271                if(round(rounded_slope - dead_zone, number_of_decimals) in previous_slopes):
272                    new_lines = np.delete(new_lines, counter, 0)
273                    #print("deleting negative: ", rounded_slope, new_lines[counter, 0])
274                    counter -= 1
275                    break
276                elif(round(rounded_slope + dead_zone, number_of_decimals) in previous_slopes):
277                    new_lines = np.delete(new_lines, counter, 0)
278                    #print("deleting positive: ", rounded_slope, new_lines[counter, 0])
279                    counter -= 1
280                    break
281                else:
282                    previous_slopes[rounded_slope] = 1
283
284                precision -= 1
285        #print(counter)
286
287        counter += 1
288        #print(previous_slopes)
289    #print(previous_slopes)
290    #print("finished for loop")
291
292    length = new_lines.size
293    # print (original_length, length)
294    return new_lines
295
296
297
298def apply_perspective(image):
299    # Save copy of image just in case
300    original_image = image
301
302    # Obtain row/column data from image
303    rows = image.shape[0]
304    cols = image.shape[1]
305
306    ###
307    # To get source points follow these steps:
308    # 1) With a properly mounted camera (must be static) fill the bottom edge
309    #    of the raw image window with your rectilinear chessboard
310    # 2) Hover over the top pixels corresponding to the top two corners of the
311    #    chessboard and record these (x,y) values
312    # 3) Calculate x and y distances from the center of the frame and change
313    #    the values (0.125) and (5) for x and y correspondingly
314    ###
315
316    # Set source points (trapezoidal)
317    # src = np.float32([
318    #     [(cols/2) - 0.125*cols, (rows/2) + 5],
319    #     [(cols/2) - 0.5*cols, (rows)],
320    #     [(cols/2) + 0.5*cols, (rows)],
321    #     [(cols/2) + 0.125*cols, (rows/2) + 5]])
322
323    src = np.float32([
324        [(cols/2) - 0.189873*cols, (rows/2)],
325        [(cols/2) - 0.5*cols, (rows) - 200],
326        [(cols/2) + 0.5*cols, (rows) - 200],
327        [(cols/2) + 0.189873*cols, (rows/2)]])
328
329    # Set destination points -- What do you want to transform your image to? 
330    # In this case skewed 3D -> Planar 2D
331    dst = np.float32([
332        [0, 0],
333        [0, rows],
334        [(cols), rows],
335        [(cols), 0]])
336
337    # First retrieve perspective transform matrix
338    M = cv2.getPerspectiveTransform(src, dst)
339
340    # Apply matrix to original image
341    warp_img = cv2.warpPerspective(image, M, (cols, rows))
342
343    # Used for debugging purposes
344    # cv2.imshow("Warped Image", warp_img)
345
346    return warp_img
347
348
349def convert_to_polar(image):
350    # Save copy of image just in case
351    original_image = image
352
353    # Obtain row/column data from image
354    rows = image.shape[0]
355    cols = image.shape[1]
356
357    # Convert to linear polar format 
358    polar_img = cv2.linearPolar(image, (rows / 2, cols / 2), 800, cv2.WARP_FILL_OUTLIERS)
359
360    # Debugging purposes
361    # cv2.imshow("Log Polar image", log_polar_img)
362
363    return polar_img
364
365
366## ROS callback for subscriber image topic
367# @param msg the image advertised from another node
368# 
369def image_callback(msg):
370    # Set timer
371    last_time = time.time()
372
373    # Set img to msg data
374    img = msg
375
376    # Convert ROS image to CV format
377    cv_image = bridge.imgmsg_to_cv2(img, desired_encoding="passthrough")
378    cv2.imshow("Image window", new_process_img(cv_image))
379
380    # Convert back to CV and publish
381    ros_image = bridge.cv2_to_imgmsg(new_process_img(cv_image), encoding="passthrough")
382    log_image_pub.publish(ros_image)
383
384    # If 'q' is detected, quit
385    if(chr(cv2.waitKey(3)&255) == 'q'):
386        rospy.signal_shutdown("Quit detected")
387
388    # Print FPS
389    print("{} fps".format(1/(time.time() - last_time)))
390
391bridge = CvBridge()
392
393# Initialize a node in ROS
394rospy.init_node('image_listener')
395
396# Create publisher to publish log polar image
397log_image_pub = rospy.Publisher('/mybot/camera1/image_log_polar', Image, queue_size = 10)
398
399# Create a subscriber to read the published camera image
400rospy.Subscriber("/mybot/camera1/image_raw", Image, image_callback)
401
402# Loop until we die
403try:
404    rospy.spin()
405except KeyboardInterrupt:
406    print("Shutting down")
407cv2.destroyAllWindows()