collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
6ac3dce
simulation
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() 31img = image 32gray = cv2 .cvtColor (img ,cv2 .COLOR_BGR2GRAY ) 33 34# Find the chess board corners 35ret ,corners = cv2 .findChessboardCorners (gray , (cbcol ,cbrow ),None ) 36loop ) 37 38# If found, add object points, image points (after refining them) 39if ret == True : 40objpoints .append (objp ) 41 42corners2 = cv2 .cornerSubPix (gray ,corners ,(11 ,11 ),(- 1 ,- 1 ),criteria ) 43imgpoints .append (corners2 ) 44 45# Draw and display the corners 46img = cv2 .drawChessboardCorners (img , (cbcol ,cbrow ),corners2 ,ret ) 47 48#print('Frame took {} seconds'.format(time.time() - last_time)) 49 50 51return 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 58original_image = image 59 60# Convert image to HSL 61processed_img = cv2 .cvtColor (image ,cv2 .COLOR_BGR2HLS ) 62 63# white color mask 64lower = np .uint8 ([0 ,160 ,0 ]) 65upper = np .uint8 ([255 ,255 ,255 ]) 66white_mask = cv2 .inRange (processed_img ,lower ,upper ) 67 68# return mask 69return cv2 .bitwise_and (original_image ,original_image ,mask = white_mask ) 70 71def draw_skeleton (image ): 72# Keep a safe copy of the original image 73original_image = image 74 75# Set size of array (800 x 800 pixels each with one grayscale value) 76size = np .size (image ) 77 78# Set array of zeros for final skeleton image 79skel = np .zeros (image .shape ,np .uint8 ) 80 81# Pass shape and size of Morphological kernel 82element = cv2 .getStructuringElement (cv2 .MORPH_CROSS , (3 ,3 )) 83 84# Set initial condition to false 85done = False 86 87# Repeat until finished 88while (not done ): 89# Erode boundaries of foreground object 90eroded = cv2 .erode (image ,element ) 91 92# Opposite of erosion -- Increases boundaries of foreground object 93dilated = cv2 .dilate (eroded ,element ) 94 95# Calculate per-element difference 96temp = cv2 .subtract (image ,dilated ) 97 98# Only adds skeleton to original skel array 99skel = cv2 .bitwise_or (skel ,temp ) 100 101# ??? 102image = eroded .copy () 103 104zeros = size - cv2 .countNonZero (image ) 105if zeros == size : 106done = True 107 108return 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 120original_image = image 121 122# Debugging 123cv2 .imshow ('original image' ,original_image ) 124 125# Retrieve image after applying white masking 126white_img = select_white (image ) 127 128# convert the image to Grayscale 129gray_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) 132edgeIntensityMin = 100 133edgeIntensityMax = 200 134canny_img = cv2 .Canny (gray_img ,threshold1 = edgeIntensityMin ,threshold2 = edgeIntensityMax ) 135 136# Gaussian Blur: cv2.GaussianBlur(image to run blur on, kernel size, sigma) 137processed_img = cv2 .GaussianBlur (canny_img , (5 ,5 ),0 ) 138 139# Define a blank mask 140masking = np .zeros_like (processed_img ) 141 142# Helper variables 143rows = image .shape [0 ] 144cols = 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 148roi = 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 152cv2 .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 156processed_img = cv2 .bitwise_and (processed_img ,processed_img ,mask = masking ) 157 158# Apply homography with perspective transform 159warp_img = apply_perspective (processed_img ) 160 161# Hough Line Transforms 162lines = cv2 .HoughLinesP (warp_img ,1 ,np .pi / 180 ,180 ,np .array ([]),150 ,5 ) 163 164# Remove similar lines 165updated_lines = discard_parallel_lines (lines ) 166 167# Draw the lines on the image 168draw_lines (warp_img ,updated_lines ) 169 170# Used for debugging purposes 171cv2 .imshow ("Warped Image" ,warp_img ) 172 173# Convert to polar Coordinates 174polar_image = convert_to_polar (warp_img ) 175 176return 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 183original_image = image 184 185# Debugging 186cv2 .imshow ('original image' ,original_image ) 187 188# Retrieve image after applying white masking 189white_img = select_white (image ) 190cv2 .imshow ("White Image" ,white_img ) 191 192# convert the image to Grayscale 193gray_img = cv2 .cvtColor (white_img ,cv2 .COLOR_BGR2GRAY ) 194cv2 .imshow ("Gray Image" ,gray_img ) 195 196# Apply homography with perspective transform 197warp_img = apply_perspective (gray_img ) 198cv2 .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 205polar_image = convert_to_polar (warp_img ) 206# cv2.imshow("Polar image", polar_image) 207 208return 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 216if lines is None : 217return 218 219# Draw detected lines on the image (passed by reference) 220 221for line in lines : 222coords = line [0 ] 223cv2 .line (image , (coords [0 ],coords [1 ]), (coords [2 ],coords [3 ]),255 ,3 ) 224 225def discard_parallel_lines (lines ): 226if lines is None : 227return 228 229previous_slopes = {} 230counter = 0 231 232number_of_decimals = 3 # Number of places to right of decimal place 233 234#print(precision) 235 236new_lines = lines 237original_length = new_lines .size 238 239for line in lines : 240dead_zone = float (0.1 )# +/- initial range (i.e Slope of -4.5 to 5.5 are all parallel slopes) 241 242precision = float (dead_zone / (10 ** (- number_of_decimals )))# Iterate through each increment 243# of decimal places within range 244 245coords = line [0 ] 246#print(coords) 247if (coords [2 ]- coords [0 ]!= 0 ): 248slope = (float (coords [3 ])- float (coords [1 ]))/ (float (coords [2 ])- float (coords [0 ])) 249else : 250slope = 9999 # Abnormally large slope for vertical line to not divide by zero 251 252rounded_slope = round (slope ,number_of_decimals ) 253#print("looping: ", counter) 254#print(slope) 255 256if (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]) 261new_lines = np .delete (new_lines ,counter ,0 ) 262counter -= 1 263else : 264#print(counter) 265while (precision >= 1 ): 266dead_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 271if (round (rounded_slope - dead_zone ,number_of_decimals )in previous_slopes ): 272new_lines = np .delete (new_lines ,counter ,0 ) 273#print("deleting negative: ", rounded_slope, new_lines[counter, 0]) 274counter -= 1 275break 276elif (round (rounded_slope + dead_zone ,number_of_decimals )in previous_slopes ): 277new_lines = np .delete (new_lines ,counter ,0 ) 278#print("deleting positive: ", rounded_slope, new_lines[counter, 0]) 279counter -= 1 280break 281else : 282previous_slopes [rounded_slope ]= 1 283 284precision -= 1 285#print(counter) 286 287counter += 1 288#print(previous_slopes) 289#print(previous_slopes) 290#print("finished for loop") 291 292length = new_lines .size 293# print (original_length, length) 294return new_lines 295 296 297 298def apply_perspective (image ): 299# Save copy of image just in case 300original_image = image 301 302# Obtain row/column data from image 303rows = image .shape [0 ] 304cols = 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 323src = 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 331dst = np .float32 ([ 332 [0 ,0 ], 333 [0 ,rows ], 334 [(cols ),rows ], 335 [(cols ),0 ]]) 336 337# First retrieve perspective transform matrix 338M = cv2 .getPerspectiveTransform (src ,dst ) 339 340# Apply matrix to original image 341warp_img = cv2 .warpPerspective (image ,M , (cols ,rows )) 342 343# Used for debugging purposes 344# cv2.imshow("Warped Image", warp_img) 345 346return warp_img 347 348 349def convert_to_polar (image ): 350# Save copy of image just in case 351original_image = image 352 353# Obtain row/column data from image 354rows = image .shape [0 ] 355cols = image .shape [1 ] 356 357# Convert to linear polar format 358polar_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 363return 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 371last_time = time .time () 372 373# Set img to msg data 374img = msg 375 376# Convert ROS image to CV format 377cv_image = bridge .imgmsg_to_cv2 (img ,desired_encoding = "passthrough" ) 378cv2 .imshow ("Image window" ,new_process_img (cv_image )) 379 380# Convert back to CV and publish 381ros_image = bridge .cv2_to_imgmsg (new_process_img (cv_image ),encoding = "passthrough" ) 382log_image_pub .publish (ros_image ) 383 384# If 'q' is detected, quit 385if (chr (cv2 .waitKey (3 )& 255 )== 'q' ): 386rospy .signal_shutdown ("Quit detected" ) 387 388# Print FPS 389"{} 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 : 404rospy .spin () 405except KeyboardInterrupt : 406"Shutting down" ) 407cv2 .destroyAllWindows ()