collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
d809afd
camera
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 15sensitivity = 20 #variable sensitivity value (can be tuned further down the line) 16whiteLower = (0 ,0 ,255 - sensitivity ) 17whiteUpper = (255 ,sensitivity ,255 ) 18 19#Read image 20start = timeit .default_timer ()#clock algorithm start 21img = cv2 .resize (img , (640 ,360 ))#resize image to decrease overhead 22#cv2.imshow('original',img) #debugging 23 24# Create the basic black image 25white_rec = np .zeros (img .shape ,dtype = "uint8" ) 26# Draw a white, filled rectangle on the mask image 27cv2 .rectangle (white_rec , (0 ,100 ), (720 ,500 ), (255 ,255 ,255 ),- 1 ) 28 29#cv2.imshow('white rectangle',white_rec) #debugging 30 31imgROI = cv2 .bitwise_and ( 32img ,white_rec )#get rid of the top part to decrease overhead (O(1)) 33#cv2.imshow('masked img',imgROI) #debugging 34 35imgHSV = cv2 .cvtColor ( 36imgROI ,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 40mask = cv2 .inRange (imgHSV ,whiteLower , 41whiteUpper )#time complexity estimate O(N^3) 42 43#helper variables for masking the part of the image immediately in front of the robot 44rows = imgHSV .shape [0 ] 45cols = imgHSV .shape [1 ] 46offset = 50 #offset for drawing the polygon 47mask_immediate_front = np .array ( 48 [[[cols / 5 ,rows / 2 ], [cols * 4 / 5 ,rows / 2 ], 49 [cols - offset ,rows ], [offset ,rows ]]], 50dtype = np .int32 ) 51 52cv2 .fillPoly (mask ,mask_immediate_front , 530 )#create the polygon on the mask 54#cv2.imshow('mask',mask) #debugging 55 56res = cv2 .bitwise_and ( 57imgROI ,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 62low_threshold = 100 63high_threshold = 200 64 65#run canny edge detection algorithm 66edges = cv2 .Canny ( 67res ,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 74rho = 1 # distance resolution in pixels of the Hough grid 75theta = np .pi / 180 # angular resolution in radians of the Hough grid 76threshold = 15 # minimum number of votes (intersections in Hough grid cell) 77min_line_length = 20 # minimum number of pixels making up a line 78max_line_gap = 50 # maximum gap in pixels between connectable line segments 79line_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 **** 83lines = cv2 .HoughLinesP (edges ,rho ,theta ,threshold ,np .array ([]), 84min_line_length ,max_line_gap ) 85 86stop = 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 90if (lines is not None ): 91for line in lines : 92for x1 ,y1 ,x2 ,y2 in line : 93cv2 .line (line_image , (x1 ,y1 ), (x2 ,y2 ), (255 ,50 ,50 ),5 ) 94 95#add the lines on top of the original image for display 96lines_edges = cv2 .addWeighted (img ,0.8 ,line_image ,1 ,0 ) 97 98#cv2.imshow('hough image', res) # debugging code 99 100time = str ((stop - start )* 1000 ) 101time + ' ms' ) 102 103return 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 113original_image = image 114 115# convert the image to Grayscale 116processed_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) 119edgeIntensityMin = 200 120edgeIntensityMax = 300 121processed_img = cv2 .Canny ( 122processed_img , 123threshold1 = edgeIntensityMin , 124threshold2 = edgeIntensityMax ) 125 126# Gaussian Blur: cv2.GaussianBlur(image to run blur on, kernel size, sigma) 127processed_img = cv2 .GaussianBlur (processed_img , (5 ,5 ),0 ) 128 129# Define a blank mask 130masking = np .zeros_like (processed_img ) 131 132# Helper variables 133rows = image .shape [0 ] 134cols = 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 138roi = np .array ( 139 [[[cols / 5 ,rows / 2 ], [cols * 4 / 5 ,rows / 2 ], [cols ,rows ], 140 [0 ,rows ]]], 141dtype = np .int32 ) 142 143# Actually draw the polygon now 144cv2 .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 148processed_img = cv2 .bitwise_and (processed_img ,processed_img ,mask = masking ) 149 150# Hough Line Transforms 151lines = cv2 .HoughLinesP (processed_img ,1 ,np .pi / 180 ,180 ,20 ,15 ) 152 153# Draw the lines on the image 154draw_lines (original_image ,lines ) 155 156return 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 164if lines is None : 165return 166 167# Draw detected lines on the image (passed by reference) 168for line in lines : 169coords = line [0 ] 170cv2 .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 ): 178img = msg 179cv_image = bridge .imgmsg_to_cv2 (img ,desired_encoding = "passthrough" ) 180cv2 .imshow ("Image window" ,process_img_Daniel (cv_image )) 181# If 'q' is detected, quit 182if (chr (cv2 .waitKey (3 )& 255 )== 'q' ): 183rospy .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 : 196rospy .spin () 197except KeyboardInterrupt : 198"Shutting down" ) 199cv2 .destroyAllWindows ()