def callback(self, data): print("Call back") print(data) try: myimage = rgb_from_ros(data) print("RGB Image = ", myimage) #cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print("Hark! I am an error", e) # (rows,cols,channels) = cv_image.shape # if cols > 60 and rows > 60 : # cv2.circle(cv_image, (50,50), 10, 255) print("About to write image") image_to_write = cv2.cvtColor(myimage, cv2.COLOR_RGB2BGR) cv2.imwrite("./duckies.jpg", image_to_write) print("Ctrl C now") cv2.imshow("Image window", myimage) cv2.waitKey(3) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) except CvBridgeError as e: print(e)
def callback(self, data): center_circle = (0, 0) myimage = rgb_from_ros(data) #Crop image to limit window of duckiebot myimage = myimage[100:500, 80:800] #Output image for debug purposes cv2.imwrite("cropped_ducks.jpg", myimage) frame = myimage #Convert RGB image to BGR frame = cv2.cvtColor(myimage, cv2.COLOR_RGB2BGR) #Convert the image to gray gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) gray = cv2.medianBlur(gray, 5) rows = gray.shape[0] #Implement the HoughCircles algorithm to find the duckie(s)'s heads, which are circles #We found better performance when using our object detection algorithms on the transformed image compared to the original circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, rows / 8, param1=100, param2=30, minRadius=1, maxRadius=60) circles_frame = frame.copy() #Here, we ensure at least some circles were found, and output an image containign the circles around the original objects if circles is not None: circles = np.uint16(np.around(circles)) for i in circles[0, :]: center = (i[0], i[1]) center_circle = center # circle center cv2.circle(circles_frame, center, 1, (0, 100, 100), 3) # circle outline radius = i[2] cv2.circle(circles_frame, center, radius, (255, 0, 255), 3) cv2.imwrite("circle.jpg", circles_frame) # Here, we ensure at least some circles were found and implement the stop and swerve logic if circles is not None: print("Found duckies") if self.obj_detected == 0: #If we detect an object, we stop, move right, move a little forward, move left, and move forward again, to simulate swerving #We move forward just a bit, even if we detect a duckie, to simulate real-life conditions, where cars can't stop right away #before they try to swerve. self.robot.forward(75, 1.0) self.robot.stop() self.robot.right(90, 0.5) self.robot.forward(75, 1.0) self.robot.left(100, 0.3) self.robot.forward(75, 0.5) #Set the flag, so even if we detect the same duckie again, we try not to repeat the same logic self.obj_detected = 1 else: #If no duckies are detected, we just move forward print("No duckies detected") self.robot.forward(75, 5.5)
def callback(self, image): rectified_img = CompressedImage() rectified_img.header.stamp = image.header.stamp rectified_img.format = "jpeg" rectified_img.data = self.image_rectifier.process_image( rgb_from_ros(image)) # publish new message self.publisher.publish(rectified_img)
def go(self): vehicle_name = dtu.get_current_robot_name() if dtu.on_duckiebot( ) else "default" output = self.options.output if output is None: output = 'out-pipeline' # + dtu.get_md5(self.options.image)[:6] self.info('No --output given, using %s' % output) if self.options.image is not None: image_filename = self.options.image if image_filename.startswith('http'): image_filename = dtu.get_file_from_url(image_filename) bgr = dtu.bgr_from_jpg_fn(image_filename) else: print("Validating using the ROS image stream...") import rospy from sensor_msgs.msg import CompressedImage topic_name = os.path.join('/', vehicle_name, 'camera_node/image/compressed') print('Let\'s wait for an image. Say cheese!') # Dummy to get ROS message rospy.init_node('single_image') img_msg = None try: img_msg = rospy.wait_for_message(topic_name, CompressedImage, timeout=10) print('Image captured!') except rospy.ROSException as e: print( '\n\n\nDidn\'t get any message!: %s\n MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER!\n\n\n' % (e, )) bgr = dtu.bgr_from_rgb(dtu.rgb_from_ros(img_msg)) self.info('Picture taken: %s ' % str(bgr.shape)) gp = GroundProjection(vehicle_name) dtu.DuckietownConstants.show_timeit_benchmarks = True res, _stats = run_pipeline( bgr, gp, line_detector_name=self.options.line_detector, image_prep_name=self.options.image_prep, anti_instagram_name=self.options.anti_instagram, lane_filter_name=self.options.lane_filter) self.info('Resizing images..') res = dtu.resize_small_images(res) self.info('Writing images..') dtu.write_bgr_images_as_jpgs(res, output)
def processMsg(self, msg_in): # Get the image from the message im_rgb = du.rgb_from_ros(msg_in) im_bgr = cv2.cvtColor(im_rgb, cv2.COLOR_RGB2BGR) # Apply filters im_mod_bgr = self.applyFilters(im_bgr, self.filters) # Convert to message msg_out = msg_in msg_out.data = du.d8_compressed_image_from_cv_image(im_mod_bgr, msg_in).data return msg_out
def cbFilter(self, msg): filter = self.setupParameter("~filter") image = dt.rgb_from_ros(msg) image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) if filter == 'flip-vertical': image = np.fliplr(image) if filter == 'flip-horizontal': image = np.flipud(image) if filter == 'greyscale': image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if filter == 'sepia': image_copy = np.empty(image.shape, 'float') image = cv2.transform(image, M_Sepia, image_copy) new_msg = dt.d8_compressed_image_from_cv_image(image) self.pub_topic_a.publish(new_msg)
def callback(self,data): print("Call back") center_circle = (0,0) myimage = rgb_from_ros(data) # Shantha: Full image size i captured is 1280 x 720. # We want a 1200 x 300 cropped view in the bottom center of the full screenshot myimage = myimage[40:1240, 420:720] cv2.imwrite("cropped_ducks.jpg", myimage) frame = myimage im = frame frame = cv2.cvtColor(myimage, cv2.COLOR_RGB2BGR) # Convert BGR to HSV hsv_blue = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # define range of yellow color in HSV lower_blue = np.array([51, 97, 100]) upper_blue = np.array([56, 86, 100]) # Threshold the HSV image to get only blue colors mask_blue = cv2.inRange(hsv_blue, lower_blue, upper_blue) # Bitwise-AND mask and original image res = cv2.bitwise_and(frame, frame, mask_blue) res = cv2.cvtColor(res, cv2.COLOR_HSV2BGR) cv2.imwrite('frame.jpg',frame) cv2.imwrite('mask.jpg',mask_blue) hsv_blue[mask_blue > 0] = ([56, 86, 100]) cv2.imwrite('hsv_yellow.jpg',hsv_blue) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) gray = cv2.medianBlur(gray, 5) rows = gray.shape[0] circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, rows / 8, param1=100, param2=30, minRadius=1, maxRadius=60) circles_frame = frame.copy() # ensure at least some circles were found if circles is not None: self.num_obj_detected = 1 print("Done! Found atleast one ducky") if circles is None: self.num_obj_detected = 0 print("Done! No ducky found")
def callback(self, image): #print "HELLO CALLLBACK HERE" obst_image = CompressedImage() obst_image.header.stamp = image.header.stamp obst_image.format = "jpeg" # you should write the following function in your class obst_image.data = self.detector.process_image(rgb_from_ros(image)) #spaeter: anstatt obst_image.data eher #1. EXTRACT OBSTACLES #2. SEND THEM #3. VISUALIE THEM! # publish new message self.publisher.publish(obst_image.data)
def callback(self, obst_list, image): self.publisher_bblinelist.publish( self.bbmarker) # since a single publish is likely to get lost # print "CALLBACK HERE" if (self.show_marker): marker_list = self.visualizer.visualize_marker(obst_list) self.publisher_marker.publish(marker_list) # 4. EVENTUALLY DISPLAY IMAGE if (self.show_image): obst_image = CompressedImage() obst_image.header.stamp = image.header.stamp obst_image.format = "jpeg" obst_image.data = self.visualizer.visualize_image( rectify(rgb_from_ros(image), self.intrinsics), obst_list) self.publisher_img.publish(obst_image.data)
def on_image_received(self, compressed_image): detections = self.object_detector.detect( rgb_from_ros(compressed_image)) if len(detections) > 0: detection_list_msg = ObjectDetectionList() for detection in detections: detection_list_msg.detections.append( Detection(class_label=detection['class_label'], class_id=detection['class_id'], xmin=detection['xmin'], xmax=detection['xmax'], ymin=detection['ymin'], ymax=detection['ymax'], score=detection['score'])) self.pub_detection.publish(detection_list_msg)
def ProcessRawImage(self, img_raw): # rectify image img_undistorted = dt.rectify(dt.rgb_from_ros(img_raw), self.intrinsics) # compute grayscale image img_gray = cv2.cvtColor(img_undistorted, cv2.COLOR_RGB2GRAY) # detect edges img_canny = cv2.Canny(img_gray, self.canny_lower_threshold, self.canny_upper_threshold, apertureSize=self.canny_aperture_size) # apply mask img_mask = cv2.bitwise_and(img_canny,self.mask_visible) # pad image to avoid running into borders img_processed = cv2.copyMakeBorder(img_mask, self.line_search_length, self.line_search_length, self.line_search_length, self.line_search_length, cv2.BORDER_CONSTANT, value=0) return img_processed, img_gray
gen = cv2.cvtColor(gen, cv2.COLOR_BGR2GRAY).reshape( (gen.shape[0], gen.shape[1], 1)) gen = np.concatenate([gen, gen, gen], axis=2) elif f == "sepia": sepia_kernel = np.array([[.272, .534, .131], [.349, .686, .168], [.393, .769, .189]]) gen = cv2.transform(gen, sepia_kernel) else: raise Exception('filter not found') return gen bag_file = sys.argv[1] topic_sel = sys.argv[2] filters = sys.argv[3].split(":") bag_out = rosbag.Bag(sys.argv[4], mode="w") bag = rosbag.Bag(bag_file) for topic, msg, t in bag.read_messages(): if topic == topic_sel: img = rgb_from_ros(msg) img = instagram(img, filters) msg = d8_compressed_image_from_cv_image(img) bag_out.write(topic=topic, msg=msg, t=t) bag.close() bag_out.close()
def callback(self, image): if not self.active: return if not self.thread_lock.acquire(False): return #start = time.time() obst_list = PoseArray() marker_list = MarkerArray() # pass RECTIFIED IMAGE TO DETECTOR MODULE #1. EXTRACT OBSTACLES and return the pose array obst_list = self.detector.process_image( rectify(rgb_from_ros(image), self.intrinsics)) obst_list.header.stamp = image.header.stamp #for synchronization #interessant um zu schauen ob stau oder nicht!!!! #print image.header.stamp.to_sec() self.publisher_arr.publish(obst_list) #EXPLANATION: (x,y) is world coordinates of obstacle, z is radius of obstacle #QUATERNION HAS NO MEANING!!!! #3. VISUALIZE POSE ARRAY IN TF if (self.show_marker): marker_list = self.visualizer.visualize_marker(obst_list) self.publisher_marker.publish(marker_list) #4. EVENTUALLY DISPLAY !!!!CROPPED!!!!!! IMAGE if (self.show_image): obst_image = CompressedImage() obst_image.header.stamp = image.header.stamp obst_image.format = "jpeg" obst_image.data = self.visualizer.visualize_image( rectify(rgb_from_ros(image), self.intrinsics), obst_list) #here i want to display cropped image rgb_image = rgb_from_ros(obst_image.data) obst_image.data = d8_compressed_image_from_cv_image( rgb_image[self.detector.crop:, :, ::-1]) #THIS part only to visualize the cropped version -> somehow a little inefficient but keeps #the visualizer.py modular!!! self.publisher_img.publish(obst_image.data) if (self.show_bird_perspective): bird_perspective_image = CompressedImage() bird_perspective_image.header.stamp = image.header.stamp bird_perspective_image.format = "jpeg" bird_perspective_image.data = self.visualizer.visualize_bird_perspective( rectify(rgb_from_ros(image), self.intrinsics), obst_list) #here i want to display cropped image rgb_image = rgb_from_ros(obst_image.data) obst_image.data = d8_compressed_image_from_cv_image(rgb_image) #THIS part only to visualize the cropped version -> somehow a little inefficient but keeps #the visualizer.py modular!!! self.publisher_img_bird_perspective.publish(obst_image.data) #end = time.time() #print "OBST DETECTION TOOK: s" #print(end - start) self.r.sleep() self.thread_lock.release()
def callback(msg): img = rgb_from_ros(msg) gen = instagram(img, filter_lst) newmsg = d8_compressed_image_from_cv_image(gen) publisher.publish(newmsg)
while (True): filename = im_path + '/' + str(nummer) + '.jpg' im1 = cv2.imread(filename) #reads BGR if (im1 is None): #stop it! break else: #START MODIFYING THE IMAGE!!! #-------------HERE GOES THE REAL CODE----------------------------------------------------------- #----------------------------------------------------------------------------------------------- obst_list = detector.process_image(rectify(im1[:, :, ::-1], intrinsics)) obst_image = CompressedImage() obst_image.format = "jpeg" obst_image.data = visualizer.visualize_image( rectify(im1[:, :, ::-1], intrinsics), obst_list) #here i want to display cropped image image = rgb_from_ros(obst_image.data) image = image[crop:, :, :] #THIS part only to visualize the cropped version -> somehow a little inefficient but keeps #the visualizer.py modular!!! #plt.imshow(image[detector.crop:,:,:]);plt.show() #the cropped image #plt.imshow(image);plt.show() #normal sized image #SAVE THE IMAGE conv = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) cv2.imwrite(dir_path + '/' + str(nummer) + '.jpg', conv) nummer += 1 print "FERTIG" os.system("rosnode kill obstacle_detection_node")
def callback(self, data): print("Call back") center_circle = (0, 0) myimage = rgb_from_ros(data) frame = myimage #myimage = myimage[100:350, 100:1000] cv2.imwrite("cropped_ducks.jpg", myimage) #im = Image.open("Shantha_duck.jpg") im = frame frame = cv2.cvtColor(myimage, cv2.COLOR_RGB2BGR) # Convert BGR to HSV hsv_blue = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # define range of yellow color in HSV lower_blue = np.array([51, 97, 100]) upper_blue = np.array([56, 86, 100]) # Threshold the HSV image to get only blue colors mask_blue = cv2.inRange(hsv_blue, lower_blue, upper_blue) # Bitwise-AND mask and original image res = cv2.bitwise_and(frame, frame, mask_blue) res = cv2.cvtColor(res, cv2.COLOR_HSV2BGR) cv2.imwrite('frame.jpg', frame) cv2.imwrite('mask.jpg', mask_blue) hsv_blue[mask_blue > 0] = ([56, 86, 100]) cv2.imwrite('hsv_yellow.jpg', hsv_blue) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) gray = cv2.medianBlur(gray, 5) rows = gray.shape[0] circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, rows / 8, param1=100, param2=30, minRadius=1, maxRadius=60) circles_frame = frame.copy() # ensure at least some circles were found if circles is not None: circles = np.uint16(np.around(circles)) for i in circles[0, :]: center = (i[0], i[1]) center_circle = center # circle center cv2.circle(circles_frame, center, 1, (0, 100, 100), 3) # circle outline radius = i[2] cv2.circle(circles_frame, center, radius, (255, 0, 255), 3) def find_marker(image): # convert the image to grayscale, blur it, and detect edges try: gray = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) except: pass gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) gray = cv2.GaussianBlur(image, (5, 5), 0) edged = cv2.Canny(gray, 35, 125) cnts = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) print("I found {} matching shapes".format(len(cnts))) c = max(cnts, key=cv2.contourArea) return cv2.minAreaRect(c) def find_marker_altered(image): # blur image, and detect edges gray = cv2.GaussianBlur(image, (5, 5), 0) edged = cv2.Canny(gray, 35, 125) cnts = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) print("I found {} matching shapes".format(len(cnts))) return cnts def find_distance(c): M = cv2.moments(c) cX = int(M["m10"] / M["m00"]) cY = int(M["m01"] / M["m00"]) immat = im.load() (X, Y) = im.size m = np.zeros((X, Y)) #Is this needed? m = np.sum(np.asarray(im), -1) < 255 * 3 m = m / np.sum(np.sum(m)) dx = np.sum(m, 0) dy = np.sum(m, 1) # expected values new_frame = frame.copy() middle_of_image_x = np.sum(dx * np.arange(X)) middle_of_image_y = np.sum(dy * np.arange(Y)) print("x, y", cX, cY) print(middle_of_image_x, middle_of_image_y) print("X, Y") obj_center_dist = dist.euclidean( (cX, cY), (middle_of_image_x, middle_of_image_y)) print(obj_center_dist) # draw the contour and center of the shape on the image cv2.drawContours(new_frame, [c], -1, (0, 255, 0), 2) cv2.circle(new_frame, (cX, cY), 7, (255, 255, 255), -1) cv2.putText( new_frame, str(cX - middle_of_image_x), (cX - 20, cY - 20), #str(obj_center_dist) + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) cv2.imwrite("new_distance_image" + str(c[1]) + ".jpg", new_frame) # show the image def distance_to_camera(knownWidth, focalLength, perWidth): return (knownWidth * focalLength) / perWidth KNOWN_DISTANCE = 12.0 KNOWN_WIDTH = 1.5 image = cv2.imread("duckie_calibration.jpg") marker = find_marker(image) focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH ############### cnts = find_marker_altered(hsv_blue) cnts = list(sorted(cnts, key=cv2.contourArea, reverse=True))[:30] #22 detected_obj_list = [] for c in cnts: bbox = cv2.boundingRect(c) x, y, w, h = bbox if w < 20 or h < 45 or w * h < 20: # h < 50 or w > 1000 do h < 35 to get perfect detection on test_ducks, h < 50 for Shantha_duck pass else: peri = cv2.arcLength(c, True) approx = cv2.approxPolyDP(c, 0.02 * peri, True) print("Hi", len(approx)) if len(approx) == 4: pass if len(approx) > 4 and len(approx) <= 12: distance_param = cv2.pointPolygonTest( c, center_circle, False) print(distance_param) #if distance_param == 1.0: marker = approx #else: #See if you can refine this further!! detected_obj_list.append(marker) inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0]) cv2.drawContours(frame, [marker], -1, (0, 255, 0), 2) #0, 255, 0 print([marker]) #find_distance(c) self.num_obj_detected = len(detected_obj_list) #self.num_obj_detected = num_obj_detected #robot = Robot.Robot(left_trim=0, right_trim=0) #if num_ob_detected >=1: # robot.forward(150, 1.0) #rospy.spin() #rate = rospy.Rate(10) #print("num obj detected", self.num_obj_detected) #if self.num_obj_detected == 0: #robot.forward(100, 30.5) #if self.num_obj_detected >= 1: #robot.stop() #image_to_write = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) #try: # os.remove("hi_newest_edge_detected.jpg") #except: # pass #cv2.imwrite( "hi_newest_edge_detected.jpg", image_to_write) print("Done!")
import rosbag # Example usage: ./sample_images.py path_to_bag /scbb/camera_rectifier/image/compressed 500 if __name__ == '__main__': parser = argparse.ArgumentParser( description='Sample images from ROS Bags.') parser.add_argument('bag_path', help='path to the ROS Bag') parser.add_argument('topic', help='the topic for image sampling') parser.add_argument('n_imgs', help='the approximate number of images to extract') parser.add_argument('--output_dir', default='./sampled_images', help='output directory for the sampled images') args = parser.parse_args() if os.path.exists(args.output_dir): shutil.rmtree(args.output_dir, ignore_errors=True) os.makedirs(args.output_dir) rb = rosbag.Bag(args.bag_path) n_imgs = float(args.n_imgs) p = n_imgs / rb.get_message_count(args.topic) img_no = 0 for msg in rb.read_messages(args.topic): if numpy.random.rand() < p: cv2.imwrite( os.path.join( args.output_dir, 'sample_{{:0{}d}}.png'.format( int(np.floor(np.log10(n_imgs))) + 1).format(img_no)), duckietown_utils.rgb_from_ros(msg.message)[..., ::-1]) img_no += 1
def get_the_image(data): myimage = rgb_from_ros(data)
msg_out.data = du.d8_compressed_image_from_cv_image(im_mod_bgr, msg_in).data return msg_out ### --- Testing --- # if __name__ == "__main__": # Load im = cv2.imread("sunset.jpg", cv2.IMREAD_COLOR) msg_in = CompressedImage() # Not manipulated msg_in.data = du.d8_compressed_image_from_cv_image(im, msg_in).data inst = Instagram("") msg_out = inst.processMsg(msg_in) im_out_rgb = du.rgb_from_ros(msg_out) im_out_bgr = cv2.cvtColor(im_out_rgb, cv2.COLOR_RGB2BGR) cv2.imwrite("untouched.jpg", im_out_bgr) # Flip vertically msg_in.data = du.d8_compressed_image_from_cv_image(im, msg_in).data inst = Instagram("flip-vertical") msg_out = inst.processMsg(msg_in) im_out_rgb = du.rgb_from_ros(msg_out) im_out_bgr = cv2.cvtColor(im_out_rgb, cv2.COLOR_RGB2BGR) cv2.imwrite("vflip.jpg", im_out_bgr) # Flip horizontally msg_in.data = du.d8_compressed_image_from_cv_image(im, msg_in).data inst = Instagram("flip-horizontal") msg_out = inst.processMsg(msg_in)
def callback(self, data): #print("Call back") center_circle = (0, 0) myimage = rgb_from_ros(data) cv2.imwrite("original_ducks.jpg", myimage) # Shantha: Full image size i captured is 1280 x 720. # We want a 1200 x 300 cropped view in the bottom center of the full screenshot myimage = myimage[100:500, 80:800] cv2.imwrite("cropped_ducks.jpg", myimage) frame = myimage im = frame frame = cv2.cvtColor(myimage, cv2.COLOR_RGB2BGR) # Convert BGR to HSV hsv_blue = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # define range of yellow color in HSV lower_blue = np.array([51, 97, 100]) upper_blue = np.array([56, 86, 100]) # Threshold the HSV image to get only blue colors mask_blue = cv2.inRange(hsv_blue, lower_blue, upper_blue) # Bitwise-AND mask and original image res = cv2.bitwise_and(frame, frame, mask_blue) res = cv2.cvtColor(res, cv2.COLOR_HSV2BGR) cv2.imwrite('frame.jpg', frame) cv2.imwrite('mask.jpg', mask_blue) hsv_blue[mask_blue > 0] = ([56, 86, 100]) cv2.imwrite('hsv_yellow.jpg', hsv_blue) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) gray = cv2.medianBlur(gray, 5) rows = gray.shape[0] circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, rows / 8, param1=100, param2=30, minRadius=1, maxRadius=60) circles_frame = frame.copy() # ensure at least some circles were found if circles is not None: circles = np.uint16(np.around(circles)) for i in circles[0, :]: center = (i[0], i[1]) center_circle = center # circle center cv2.circle(circles_frame, center, 1, (0, 100, 100), 3) # circle outline radius = i[2] cv2.circle(circles_frame, center, radius, (255, 0, 255), 3) cv2.imwrite("new_circle.jpg", circles_frame) def find_marker(image): # convert the image to grayscale, blur it, and detect edges gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) gray = cv2.GaussianBlur(gray, (5, 5), 0) edged = cv2.Canny(gray, 35, 125) # find the contours in the edged image and keep the largest one; # we'll assume that this is our piece of paper in the image cnts = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) c = max(cnts, key=cv2.contourArea) # compute the bounding box of the of the paper region and return it return cv2.minAreaRect(c) def distance_to_camera(knownWidth, focalLength, perWidth): # compute and return the distance from the maker to the camera return (knownWidth * focalLength) / perWidth def duckie_distance_detection(): # initialize the known distance from the camera to the object, which # in this case is 24 inches KNOWN_DISTANCE = 12.0 # initialize the known object width, which in this case, the piece of # paper is 12 inches wide KNOWN_WIDTH = 1.5 # load the furst image that contains an object that is KNOWN TO BE 2 feet # from our camera, then find the paper marker in the image, and initialize # the focal length image = cv2.imread("duckie_calibration.jpg") marker = find_marker(image) focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH # loop over the images # load the image, find the marker in the image, then compute the # distance to the marker from the camera marker = find_marker(circles_frame) inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0]) # draw a bounding box around the image and display it box = cv2.cv.BoxPoints( marker) if imutils.is_cv2() else cv2.boxPoints(marker) box = np.int0(box) cv2.drawContours(circles_frame, [box], -1, (0, 255, 0), 2) cv2.putText( circles_frame, "%.2fft" % (inches / 12), (circles_frame.shape[1] - 200, circles_frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 2.0, (0, 255, 0), 3) cv2.imwrite("distance_image.jpg", circles_frame) return inches / 12 # ensure at least some circles were found distance_from_obj_ft = duckie_distance_detection() if circles is not None: self.num_obj_detected = 1 print("Done! Found duckies") #sys.exit() #self.robot.stop() if distance_from_obj_ft <= 5.0: self.robot.right(100, 0.5) else: self.robot.left(100, 3.5) #self.robot.forward(100, 5.5) #sys.exit() else: self.num_obj_detected = 0 self.robot.forward(75, 5.5) #self.robot.stop() print("Done! no ducky")
def process_log(self, bag_in, prefix_in, bag_out, prefix_out, utils): #@UnusedVariable log_name = utils.get_log().log_name vehicle_name = dtu.which_robot(bag_in) dtu.logger.info('Vehicle name: %s' % vehicle_name) gp = GroundProjection(vehicle_name) topic = dtu.get_image_topic(bag_in) bgcolor = dtu.ColorConstants.BGR_DUCKIETOWN_YELLOW sequence = bag_in.read_messages_plus(topics=[topic]) for _i, mp in enumerate(sequence): bgr = dtu.bgr_from_rgb(dtu.rgb_from_ros(mp.msg)) res, stats = run_pipeline(bgr, gp=gp, line_detector_name=self.line_detector, image_prep_name=self.image_prep, lane_filter_name=self.lane_filter, anti_instagram_name=self.anti_instagram, all_details=self.all_details) rect = (480, 640) if not self.all_details else (240, 320) res = dtu.resize_images_to_fit_in_rect(res, rect, bgcolor=bgcolor) print('abs: %s window: %s fron log: %s' % (mp.time_absolute, mp.time_window, mp.time_from_physical_log_start)) headers = [ "Robot: %s log: %s time: %.2f s" % (vehicle_name, log_name, mp.time_from_physical_log_start), "Algorithms | color correction: %s | preparation: %s | detector: %s | filter: %s" % ( self.anti_instagram, self.image_prep, self.line_detector, self.lane_filter, ) ] res = dtu.write_bgr_images_as_jpgs(res, dirname=None, bgcolor=bgcolor) cv_image = res['all'] for head in reversed(headers): max_height = 35 cv_image = dtu.add_header_to_bgr(cv_image, head, max_height=max_height) otopic = "all" omsg = dtu.d8_compressed_image_from_cv_image( cv_image, same_timestamp_as=mp.msg) t = rospy.Time.from_sec(mp.time_absolute) # @UndefinedVariable print('written %r at t = %s' % (otopic, t.to_sec())) bag_out.write(prefix_out + '/' + otopic, omsg, t=t) for name, value in stats.items(): utils.write_stat(prefix_out + '/' + name, value, t=t)
def go(self): robot_name = dtu.get_current_robot_name() output = self.options.output if output is None: output = 'out-calibrate-extrinsics' # + dtu.get_md5(self.options.image)[:6] self.info('No --output given, using %s' % output) if self.options.input is None: print("{}\nCalibrating using the ROS image stream...\n".format( "*" * 20)) import rospy from sensor_msgs.msg import CompressedImage topic_name = os.path.join('/', robot_name, 'camera_node/image/compressed') print('Topic to listen to is: %s' % topic_name) print('Let\'s wait for an image. Say cheese!') # Dummy for getting a ROS message rospy.init_node('calibrate_extrinsics') img_msg = None try: img_msg = rospy.wait_for_message(topic_name, CompressedImage, timeout=10) print('Image captured!') except rospy.ROSException as e: print( '\n\n\nDidn\'t get any message!: %s\n MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER!\n\n\n' % (e, )) bgr = dtu.bgr_from_rgb(dtu.rgb_from_ros(img_msg)) self.info('Picture taken: %s ' % str(bgr.shape)) else: self.info('Loading input image %s' % self.options.input) bgr = dtu.bgr_from_jpg_fn(self.options.input) if bgr.shape[1] != 640: interpolation = cv2.INTER_CUBIC bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation) self.info('Resized to: %s ' % str(bgr.shape)) # Disable the old calibration file disable_old_homography(robot_name) camera_info = get_camera_info_for_robot(robot_name) homography_dummy = get_homography_default() gpg = GroundProjectionGeometry(camera_info, homography_dummy) res = OrderedDict() try: bgr_rectified = gpg.rectify(bgr, interpolation=cv2.INTER_CUBIC) res['bgr'] = bgr res['bgr_rectified'] = bgr_rectified _new_matrix, res['rectified_full_ratio_auto'] = gpg.rectify_full( bgr, ratio=1.65) result = estimate_homography(bgr_rectified) dtu.check_isinstance(result, HomographyEstimationResult) if result.bgr_detected_refined is not None: res['bgr_detected_refined'] = result.bgr_detected_refined if not result.success: raise Exception(result.error) save_homography(result.H, robot_name) msg = ''' To check that this worked well, place the robot on the road, and run: rosrun complete_image_pipeline single_image Look at the produced jpgs. ''' self.info(msg) finally: dtu.write_bgr_images_as_jpgs(res, output)
def go(self): robot_name = dtu.get_current_robot_name() output = self.options.output if output is None: output = 'out-calibrate-extrinsics' # + dtu.get_md5(self.options.image)[:6] self.info('No --output given, using %s' % output) if self.options.input is None: print("{}\nCalibrating using the ROS image stream...\n".format( "*" * 20)) import rospy from sensor_msgs.msg import CompressedImage topic_name = os.path.join('/', robot_name, 'camera_node/image/compressed') print('Topic to listen to is: %s' % topic_name) print('Let\'s wait for an image. Say cheese!') # Dummy for getting a ROS message rospy.init_node('calibrate_extrinsics') img_msg = None try: img_msg = rospy.wait_for_message(topic_name, CompressedImage, timeout=10) print('Image captured!') except rospy.ROSException as e: print( '\n\n\n' 'Didn\'t get any message!: %s\n ' 'MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER!' '\n\n\n' % (e, )) bgr = dtu.bgr_from_rgb(dtu.rgb_from_ros(img_msg)) print('Picture taken: %s ' % str(bgr.shape)) else: print('Loading input image %s' % self.options.input) bgr = dtu.bgr_from_jpg_fn(self.options.input) if bgr.shape[1] != 640: interpolation = cv2.INTER_CUBIC bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation) print('Resized to: %s ' % str(bgr.shape)) # Disable the old calibration file print("Disableing old homography") disable_old_homography(robot_name) print("Obtaining camera info") try: camera_info = get_camera_info_for_robot(robot_name) except Exception as E: print("Error on obtaining camera info!") print(E) print("Get default homography") try: homography_dummy = get_homography_default() except Exception as E: print("Error on getting homography") print(E) print("Rectify image") try: rect = Rectify(camera_info) except Exception as E: print("Error rectifying image!") print(E) print("Calculate GPG") try: gpg = GroundProjectionGeometry(camera_info.width, camera_info.height, homography_dummy.reshape((3, 3))) except Exception as E: print("Error calculating GPG!") print(E) print("Ordered Dict") res = OrderedDict() try: bgr_rectified = rect.rectify(bgr, interpolation=cv2.INTER_CUBIC) res['bgr'] = bgr res['bgr_rectified'] = bgr_rectified _new_matrix, res['rectified_full_ratio_auto'] = rect.rectify_full( bgr, ratio=1.65) (result_gpg, status) = gpg.estimate_homography(bgr_rectified) if status is not None: raise Exception(status) save_homography(result_gpg.H, robot_name) msg = ''' To check that this worked well, place the robot on the road, and run: rosrun complete_image_pipeline single_image Look at the produced jpgs. ''' print(msg) except Exception as E: print(E) finally: dtu.write_bgr_images_as_jpgs(res, output)
def callback(self,data): #print("Call back") center_circle = (0,0) myimage = rgb_from_ros(data) myimage = myimage[100:500, 80:800] # Shantha: Full image size i captured is 1280 x 720. # We want a 1200 x 300 cropped view in the bottom center of the full screenshot #myimage = myimage[40:1240, 420:720] cv2.imwrite("cropped_ducks.jpg", myimage) frame = myimage im = frame frame = cv2.cvtColor(myimage, cv2.COLOR_RGB2BGR) # Convert BGR to HSV hsv_blue = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # define range of yellow color in HSV lower_blue = np.array([51, 97, 100]) upper_blue = np.array([56, 86, 100]) # Threshold the HSV image to get only blue colors mask_blue = cv2.inRange(hsv_blue, lower_blue, upper_blue) # Bitwise-AND mask and original image res = cv2.bitwise_and(frame, frame, mask_blue) res = cv2.cvtColor(res, cv2.COLOR_HSV2BGR) cv2.imwrite('frame.jpg',frame) cv2.imwrite('mask.jpg',mask_blue) hsv_blue[mask_blue > 0] = ([56, 86, 100]) cv2.imwrite('hsv_yellow.jpg',hsv_blue) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) gray = cv2.medianBlur(gray, 5) rows = gray.shape[0] circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, rows / 8, param1=100, param2=30, minRadius=1, maxRadius=60) circles_frame = frame.copy() # ensure at least some circles were found if circles is not None: circles = np.uint16(np.around(circles)) for i in circles[0, :]: center = (i[0], i[1]) center_circle = center # circle center cv2.circle(circles_frame, center, 1, (0, 100, 100), 3) # circle outline radius = i[2] cv2.circle(circles_frame, center, radius, (255, 0, 255), 3) cv2.imwrite( "circle.jpg", circles_frame) # ensure at least some circles were found if circles is not None: self.num_obj_detected = 1 print("Done! Found duckies") #sys.exit() #self.robot.stop() self.robot.right(100, 0.5) #self.robot.forward(100, 5.5) #sys.exit() else: self.num_obj_detected = 0 self.robot.forward(75, 5.5) #self.robot.stop() print("Done! no ducky")