def line_detection(LineDetectorClass, bgr): detector = LineDetectorClass() detector.setImage(bgr) image_with_lines = bgr.copy() # detect lines and normals white = detector.detectLines("white") yellow = detector.detectLines("yellow") red = detector.detectLines("red") # draw lines drawLines(image_with_lines, white.lines, (0, 0, 0)) drawLines(image_with_lines, yellow.lines, (255, 0, 0)) drawLines(image_with_lines, red.lines, (0, 255, 0)) # elif isinstance(detector, LineDetector2): # # detect lines and normals # lines_white, normals_white, centers_white, area_white = detector.detectLines2('white') # lines_yellow, normals_yellow, centers_yellow, area_yellow = detector.detectLines2('yellow') # lines_red, normals_red, centers_red, area_red = detector.detectLines2('red') # # # draw lines # drawLines(image_with_lines, lines_white, (0, 0, 0)) # drawLines(image_with_lines, lines_yellow, (255, 0, 0)) # drawLines(image_with_lines, lines_red, (0, 255, 0)) # # draw normals # detector.drawNormals2(centers_white, normals_white, (0, 0, 0)) # detector.drawNormals2(centers_yellow, normals_yellow, (255, 0, 0)) # detector.drawNormals2(centers_red, normals_red, (0, 255, 0)) res = {} res["annotated"] = image_with_lines res["area_white"] = white.area res["area_red"] = red.area res["area_yellow"] = yellow.area res["edges"] = detector.edges return res
def line_detection(LineDetectorClass, bgr): detector = LineDetectorClass() detector.setImage(bgr) image_with_lines = bgr.copy() # detect lines and normals white = detector.detectLines('white') yellow = detector.detectLines('yellow') red = detector.detectLines('red') # draw lines drawLines(image_with_lines, white.lines, (0, 0, 0)) drawLines(image_with_lines, yellow.lines, (255, 0, 0)) drawLines(image_with_lines, red.lines, (0, 255, 0)) # elif isinstance(detector, LineDetector2): # # detect lines and normals # lines_white, normals_white, centers_white, area_white = detector.detectLines2('white') # lines_yellow, normals_yellow, centers_yellow, area_yellow = detector.detectLines2('yellow') # lines_red, normals_red, centers_red, area_red = detector.detectLines2('red') # # # draw lines # drawLines(image_with_lines, lines_white, (0, 0, 0)) # drawLines(image_with_lines, lines_yellow, (255, 0, 0)) # drawLines(image_with_lines, lines_red, (0, 255, 0)) # # draw normals #detector.drawNormals2(centers_white, normals_white, (0, 0, 0)) #detector.drawNormals2(centers_yellow, normals_yellow, (255, 0, 0)) #detector.drawNormals2(centers_red, normals_red, (0, 255, 0)) res = {} res['annotated'] = image_with_lines res['area_white'] = white.area res['area_red'] = red.area res['area_yellow'] = yellow.area res['edges'] = detector.edges return res
def processImage_(self, image_msg): self.stats.processed() if self.intermittent_log_now(): self.intermittent_log(self.stats.info()) self.stats.reset() tk = TimeKeeper(image_msg) self.intermittent_counter += 1 # Decode from compressed image with OpenCV try: image_cv = bgr_from_jpg(image_msg.data) except ValueError as e: self.loginfo('Could not decode image: %s' % e) return tk.completed('decoded') # Resize and crop image hei_original, wid_original = image_cv.shape[0:2] if self.image_size[0] != hei_original or self.image_size[ 1] != wid_original: # image_cv = cv2.GaussianBlur(image_cv, (5,5), 2) image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]), interpolation=cv2.INTER_NEAREST) image_cv = image_cv[self.top_cutoff:, :, :] tk.completed('resized') # Set the image to be detected self.detector_used.setImage(image_cv) # Detect lines and normals #rospy.loginfo('segment_max_threshold: %s' % self.segment_max_threshold) white = self.detector_used.detectLines('white', self.segment_max_threshold) yellow = self.detector_used.detectLines('yellow', self.segment_max_threshold) red = self.detector_used.detectLines('red', self.segment_max_threshold) tk.completed('detected') # SegmentList constructor segmentList = SegmentList() segmentList.header.stamp = image_msg.header.stamp # Convert to normalized pixel coordinates, and add segments to segmentList arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff)) arr_ratio = np.array( (1. / self.image_size[1], 1. / self.image_size[0], 1. / self.image_size[1], 1. / self.image_size[0])) if len(white.lines) > 0: lines_normalized_white = ((white.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_white, white.normals, Segment.WHITE)) if len(yellow.lines) > 0: lines_normalized_yellow = ((yellow.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_yellow, yellow.normals, Segment.YELLOW)) if len(red.lines) > 0: lines_normalized_red = ((red.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_red, red.normals, Segment.RED)) self.intermittent_log( '# segments: white %3d yellow %3d red %3d' % (len(white.lines), len(yellow.lines), len(red.lines))) tk.completed('prepared') # Publish segmentList #rospy.loginfo('segmentList: %s' % (len(white.lines)+len(yellow.lines)+len(red.lines))) self.pub_lines.publish(segmentList) tk.completed('--pub_lines--') # VISUALIZATION only below if self.verbose: # print('line_detect_node: verbose is on!') # Draw lines and normals image_with_lines = np.copy(image_cv) drawLines(image_with_lines, white.lines, (0, 0, 0)) drawLines(image_with_lines, yellow.lines, (255, 0, 0)) drawLines(image_with_lines, red.lines, (0, 255, 0)) tk.completed('drawn') # Publish the frame with lines image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8") image_msg_out.header.stamp = image_msg.header.stamp self.pub_image.publish(image_msg_out) tk.completed('pub_image') # if self.verbose: colorSegment = color_segment(white.area, red.area, yellow.area) edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector_used.edges, "mono8") colorSegment_msg_out = self.bridge.cv2_to_imgmsg( colorSegment, "bgr8") self.pub_edge.publish(edge_msg_out) self.pub_colorSegment.publish(colorSegment_msg_out) tk.completed('pub_edge/pub_segment') self.intermittent_log(tk.getall())
def cbImage(self, image_msg): # self.sub_rect.unregister() image_cv = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8") hei_original, wid_original = image_cv.shape[0:2] if self.image_size[0] != hei_original or self.image_size[ 1] != wid_original: # image_cv = cv2.GaussianBlur(image_cv, (5,5), 2) image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]), interpolation=cv2.INTER_NEAREST) image_cv = image_cv[self.top_cutoff:, :, :] # Set the image to be detected self.detector_used.setImage(image_cv) # Detect lines and normals white = self.detector_used.detectLines('white') yellow = self.detector_used.detectLines('yellow') red = self.detector_used.detectLines('red') # SegmentList constructor segmentList = SegmentList() segmentList.header.stamp = image_msg.header.stamp # Convert to normalized pixel coordinates, and add segments to segmentList arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff)) arr_ratio = np.array( (1. / self.image_size[1], 1. / self.image_size[0], 1. / self.image_size[1], 1. / self.image_size[0])) if len(white.lines) > 0: lines_normalized_white = ((white.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_white, white.normals, Segment.WHITE)) if len(yellow.lines) > 0: lines_normalized_yellow = ((yellow.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_yellow, yellow.normals, Segment.YELLOW)) if len(red.lines) > 0: lines_normalized_red = ((red.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_red, red.normals, Segment.RED)) rospy.loginfo('# segments: white %3d yellow %3d red %3d' % (len(white.lines), len(yellow.lines), len(red.lines))) # Publish segmentList self.pub_lines.publish(segmentList) if self.verbose: # print('line_detect_node: verbose is on!') # Draw lines and normals image_with_lines = np.copy(image_cv) drawLines(image_with_lines, white.lines, (0, 0, 0)) drawLines(image_with_lines, yellow.lines, (255, 0, 0)) drawLines(image_with_lines, red.lines, (0, 255, 0)) # Publish the frame with lines image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8") image_msg_out.header.stamp = image_msg.header.stamp self.pub_image.publish(image_msg_out) colorSegment = color_segment(white.area, red.area, yellow.area) edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector_used.edges, "mono8") colorSegment_msg_out = self.bridge.cv2_to_imgmsg( colorSegment, "bgr8") self.pub_edge.publish(edge_msg_out) self.pub_colorSegment.publish(colorSegment_msg_out)
def cbImage(self,image_msg): # self.sub_rect.unregister() image_cv = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8") hei_original, wid_original = image_cv.shape[0:2] if self.image_size[0] != hei_original or self.image_size[1] != wid_original: # image_cv = cv2.GaussianBlur(image_cv, (5,5), 2) image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]), interpolation=cv2.INTER_NEAREST) image_cv = image_cv[self.top_cutoff:,:,:] # Set the image to be detected self.detector_used.setImage(image_cv) # Detect lines and normals white = self.detector_used.detectLines('white') yellow = self.detector_used.detectLines('yellow') red = self.detector_used.detectLines('red') # print(white.lines) white_c = len(white.lines) yellow_c = len(yellow.lines) red_c = len(red.lines) # white_lane = np.zeros(3) # yellow_lane = np.zeros(3) # red_lane = np.zeros(3) white_lane = [0,0,0] yellow_lane = [0,0,0] red_lane = [0,0,0] if white_c > 0: white_lane = self.detectLane2NoWeights(white) if yellow_c > 0 : yellow_lane = self.detectLane2NoWeights(yellow) if red_c > 0 : red_lane = self.detectLane2NoWeights(red) self.intermittent_counter += 1 # self.drive(white_lane,yellow_lane,red_lane) print('white d:%6f phi:%6f n:%3d , yellow d:%6f phi:%6f n:%3d , red d:%6f phi:%6f n:%3d ' % (white_lane[0],white_lane[1], white_c,yellow_lane[0],yellow_lane[1], yellow_c, red_lane[0],red_lane[1] , red_c)) if self.verbose: # print('line_detect_node: verbose is on!') # Draw lines and normals image_with_lines = np.copy(image_cv) drawLines(image_with_lines, white.lines, (0, 0, 0)) drawLines(image_with_lines, yellow.lines, (255, 0, 0)) drawLines(image_with_lines, red.lines, (0, 255, 0)) # Publish the frame with lines image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8") image_msg_out.header.stamp = image_msg.header.stamp self.pub_image.publish(image_msg_out) colorSegment = color_segment(white.area, red.area, yellow.area) edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector_used.edges, "mono8") colorSegment_msg_out = self.bridge.cv2_to_imgmsg(colorSegment, "bgr8") self.pub_edge.publish(edge_msg_out) self.pub_colorSegment.publish(colorSegment_msg_out)