def find_yellow_circle(split, color, distance, erode_kernel, erode_iterations, dilate_kernel, dilate_iterations, min_contour_size, min_circularity, radius_offset): mask, _ = thresh_color_distance(split, color, distance, weights=[0.5, 2, 2]) mask = erode(mask, rect_kernel(erode_kernel), iterations=erode_iterations) mask = dilate(mask, rect_kernel(dilate_kernel), iterations=dilate_iterations) # mask = erode(mask, rect_kernel(self.options['circle_erode_kernel']), # iterations=self.options['circle_dilate_iterations']-self.options['circle_erode_iterations']) contours = outer_contours(mask) contours = filter_contour_size(contours, min_contour_size) contours = filter_shapularity( lambda c: pi * cv2.minEnclosingCircle(c)[1]**2, contours, min_circularity) def circle_with_offset(contour): c = cv2.minEnclosingCircle(contour) return (int(c[0][0]), int(c[0][1])), max(int((1 - radius_offset) * c[1]), 0) return [{'contour': c, 'circle': circle_with_offset(c)} for c in contours]
def find_yellow_rectangle(self, split, color, distance, erode_kernel, erode_iterations, dilate_kernel, dilate_iterations, min_contour_size, min_rectangularity, padding_offset): # mask = thresh_color_distance(split, color, distance, use_first_channel=False) # t = time.perf_counter() mask, _ = thresh_color_distance(split, color, distance, ignore_channels=[0]) mask = erode(mask, rect_kernel(erode_kernel), iterations=erode_iterations) mask = dilate(mask, rect_kernel(dilate_kernel), iterations=dilate_iterations) self.post('mask', mask) # tt = time.perf_counter() # print('f %f' % (tt-t)) contours = outer_contours(mask) contours = filter_contour_size(contours, min_contour_size) # ttt = time.perf_counter() # print('fo %f' % (ttt-tt)) def box_area(contour): r = cv2.minAreaRect(contour) return r[1][0] * r[1][1] contours = filter_shapularity(box_area, contours, min_rectangularity) # print('foo %f' % (time.perf_counter() - ttt)) def rectangle_with_offset(contour, offset=padding_offset): r = cv2.minAreaRect(contour) return r[0], (max(r[1][0] * (1-offset), 0), max(r[1][1] * (1-offset), 0)), r[2] return [{'contour': c, 'rectangle': rectangle_with_offset(c), 'rectangle_org': rectangle_with_offset(c, offset=0.2)} for c in contours]
def outline_mask(mask, simplify=True): ret = np.zeros(mask.shape, dtype=np.uint8) contour = outer_contours(mask) contour = max(contour, key=contour_area) if simplify: contour = contour_approx(contour, epsilon=contour_area(contour) * 0.01) cv2.drawContours(ret, [contour], -1, 255) return ret
def intersect_circles(circles, mask, min_size): for i in range(len(circles)): c = circles[i]['circle'] mask_c = np.zeros(mask.shape, dtype=np.uint8) mask_c = cv2.circle(mask_c, *c, 255, -1) intersect = cv2.bitwise_and(mask, mask_c) if any( map(lambda x: cv2.contourArea(x) > min_size, outer_contours(intersect))): return i, mask_c return None, None
def intersect_rectangles(self, rectangles, mask, min_size): ret = [] for i in range(len(rectangles)): c = rectangles[i]['rectangle'] mask_c = np.zeros(mask.shape, dtype=np.uint8) mask_c = cv2.fillPoly(mask_c, [np.int0(cv2.boxPoints(c))], color=255) # self.post('mask_%d'%i, mask_c) intersect = cv2.bitwise_and(mask, mask_c) # self.post('intersect_%d'%i, intersect) if any(map(lambda x: cv2.contourArea(x) > min_size, outer_contours(intersect))): ret.append((i, intersect)) return ret
def process(self, *mats): results = shm.gate_vision.get() h, w, _ = mats[0].shape h = int(h * self.options['resize_height_scale']) w = int(w * self.options['resize_width_scale']) results.img_height = h results.img_width = w mat = resize(mats[0], w, h) #print(np.mean(mat)) avg_brightness_ratio = np.mean(mat) / REFERENCE_BRIGHTNESS nonblack_thresh_dist = self.options['nonblack_thresh'] * avg_brightness_ratio lab, lab_split = bgr_to_lab(mat) median_a = np.median(lab_split[1]) median_b = np.median(lab_split[2]) median_filter_a = range_threshold(lab_split[1], median_a - self.options['water_a_thresh'], median_a + self.options['water_a_thresh']) median_filter_b = range_threshold(lab_split[2], median_b - self.options['water_b_thresh'], median_b + self.options['water_b_thresh']) if self.options['debug']: self.post('median filter a', median_filter_a) self.post('median filter b', median_filter_b) nonwater_mask, _ = gray_to_bgr(255 - (median_filter_a & median_filter_b)) self.post('nonwater', nonwater_mask) # Tuned for a 320x256 image vehicle_depth = shm.kalman.depth.get() reflection_cutoff = min(h, int(max(0, 3 - vehicle_depth)**2 * CUTOFF_SCALAR)) mat[:reflection_cutoff] *= 0 tmp = mat.copy() draw_text(tmp, 'Depth: {:.2f}'.format(vehicle_depth), (30, 30), 0.5, color=(255, 255, 255)) self.post('mat', tmp) #lab, lab_split = bgr_to_lab(mat) #nonblack_mask, _ = gray_to_bgr(np.uint8(255 * (lab_split[0] > self.options['nonblack_thresh']))) nonblack_mask, _ = gray_to_bgr(np.uint8(255 * (np.var(mat, axis=2) > nonblack_thresh_dist))) self.post('nonblack', nonblack_mask) mat &= nonblack_mask mat &= nonwater_mask mat = to_umat(mat) mat = simple_gaussian_blur(mat, to_odd(self.options['blur_kernel']), self.options['blur_std']) lab, lab_split = bgr_to_lab(mat) threshed, dists = thresh_color_distance([lab_split[0], lab_split[1], lab_split[2]], [self.options['lab_l_ref'], self.options['lab_a_ref'], self.options['lab_b_ref']], self.options['color_dist_thresh'], auto_distance_percentile=self.options['auto_distance_percentile'], ignore_channels=[0], weights=[2, 0, 15]) if self.options['debug']: self.post('threshed', threshed) self.post('dists', dists) dilated = dilate(threshed, rect_kernel(self.options['dilate_kernel'])) if self.options['debug']: self.post('dilated', dilated) eroded = erode(dilated, rect_kernel(self.options['erode_kernel'])) if self.options['debug']: self.post('eroded', eroded) contours = outer_contours(eroded) areas = [*map(contour_area, contours)] centroids = [*map(contour_centroid, contours)] xs = [c[0] for c in centroids] ys = [c[1] for c in centroids] rects = [*map(min_enclosing_rect, contours)] lengths = [max(r[1]) for r in rects] ratios = [max(r[1]) / (1e-30 + min(r[1])) for r in rects] vehicle_roll = shm.kalman.roll.get() lines = [cv2.fitLine(c, cv2.DIST_L2, 0, 0.01, 0.01) for c in contours] angles = [np.degrees(np.arctan2(line[1], line[0]))[0] for line in lines] angles = [min(abs(90 - a - vehicle_roll), abs(-90 - a - vehicle_roll)) for a in angles] rectangularities = [a / (1e-30 + rect[1][0] * rect[1][1]) for (c, a, rect) in zip(contours, areas, rects)] contours = [ContourFeats(*feats) for feats in zip(contours, areas, xs, ys, rectangularities, angles, lengths, ratios)] contours = [*filter(lambda c: c.area > self.options['min_contour_area'], contours)] self.post_contours('area', h, w, contours) contours = [*filter(lambda c: c.angle < self.options['max_angle_from_vertical'], contours)] self.post_contours('angle', h, w, contours) contours = [*filter(lambda c: c.length > self.options['min_length'], contours)] self.post_contours('length', h, w, contours) #contours = [*filter(lambda c: c.rect > self.options['min_contour_rect'], contours)] #self.post_contours('rect', h, w, contours) contours = [*filter(lambda c: c.ratio > self.options['min_contour_ratio'], contours)] self.post_contours('ratio', h, w, contours) contours = sorted(contours, key=lambda c: c.area)[:6] contours_by_x = sorted(contours, key=lambda c: c.x) contours_by_x = filter_duplicates_sorted_by_x(contours_by_x) leftmost = try_index(contours_by_x, 0) middle = try_index(contours_by_x, 1) rightmost = try_index(contours_by_x, 2) tmp = np.zeros((h, w, 3)) results.leftmost_visible = leftmost is not None results.middle_visible = middle is not None results.rightmost_visible = rightmost is not None draw_text(tmp, 'Roll: {:.2f}'.format(vehicle_roll), (30, 30), 0.5, color=(255, 255, 255)) if leftmost is not None: draw_contours(tmp, [leftmost.contour], color=(255, 0, 0), thickness=-1) draw_circle(tmp, (leftmost.x, leftmost.y), 5, color=(255, 255, 255), thickness=-1) results.leftmost_x = leftmost.x results.leftmost_y = leftmost.y results.leftmost_len = leftmost.length if middle is not None: draw_contours(tmp, [middle.contour], color=(0, 255, 0), thickness=-1) draw_circle(tmp, (middle.x, middle.y), 5, color=(255, 255, 255), thickness=-1) results.middle_x = middle.x results.middle_y = middle.y results.middle_len = middle.length if rightmost is not None: draw_contours(tmp, [rightmost.contour], color=(0, 0, 255), thickness=-1) draw_circle(tmp, (rightmost.x, rightmost.y), 5, color=(255, 255, 255), thickness=-1) results.rightmost_x = rightmost.x results.rightmost_y = rightmost.y results.rightmost_len = rightmost.length shm.gate_vision.set(results) self.post('contours', tmp)
def find_vampire(self, mat, split, color, distance): # mask = thresh_color_distance(split, color, distance) mask, _ = thresh_color_distance(split, color, distance, weights=[0.5, 2, 2]) self.post('purple', mask) rects = self.intersect_rectangles(self.rectangles, mask, self.options['intersection_size_min']) if self.rectangles: empty = max([r['rectangle'] for r in self.rectangles], key=lambda r: r[1][0] * r[1][1]) align_angle = empty[2] + 90 if empty[1][1] > empty[1][0] else empty[2] align_angle = 360 + align_angle if align_angle < 0 else align_angle shm.recovery_vampire.empty_visible.set(True) shm.recovery_vampire.empty_x.set(int(empty[0][0])) shm.recovery_vampire.empty_y.set(int(empty[0][1])) shm.recovery_vampire.empty_offset_x.set(int(empty[0][0] + min(empty[1]) * self.options['open_offset'])) shm.recovery_vampire.empty_offset_y.set(int(empty[0][1])) shm.recovery_vampire.empty_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], align_angle)) shm.recovery_vampire.empty_size.set(empty[1][0] * empty[1][1]) cv2.circle(mat, (int(empty[0][0]), int(empty[0][1])), 5, color=(255, 255, 255), thickness=-1) opened = [] closed = [] for j in range(len(rects)): i, mask_r = rects[j] self.post('rectangle_%d' % j, mask_r) purple = cv2.bitwise_and(mask, mask_r) purple_contours = outer_contours(purple) purple_center = contour_centroid(max(purple_contours, key=contour_area)) # print(purple_center) align_angle = self.rectangles[i]['rectangle'][2] if self.rectangles[i]['rectangle'][1][1] > self.rectangles[i]['rectangle'][1][0] else self.rectangles[i]['rectangle'][2] + 90 align_angle = 360 + align_angle if align_angle < 0 else align_angle def point_in_rectangle(point, rect): contour = np.float32([cv2.boxPoints(rect)]).reshape(-1, 1, 2) return cv2.pointPolygonTest(contour, point, measureDist=False) if point_in_rectangle(purple_center, self.rectangles[i]['rectangle_org']) > 0: color = (0, 0, 255) opened.append({'center': purple_center, 'align': align_angle, 'size': self.rectangles[i]['rectangle'][1][0] * self.rectangles[i]['rectangle'][1][1], 'offset': (int(min(self.rectangles[i]['rectangle'][1]) * self.options['open_offset']), int(max(self.rectangles[i]['rectangle'][1]) * self.options['vert_offset']))}) else: color = (255, 0, 0) direction = 1 if self.rectangles[i]['rectangle'][0][0] > purple_center[0] else -1 closed.append({'center': purple_center, 'align': ((align_angle + 180) % 360) if direction == 1 else align_angle, 'size': self.rectangles[i]['rectangle'][1][0] * self.rectangles[i]['rectangle'][1][1], 'offset': (int(min(self.rectangles[i]['rectangle'][1]) * self.options['closed_offset']), int(max(self.rectangles[i]['rectangle'][1]) * self.options['vert_offset'])), 'direction': direction}) # cv2.circle(mat, purple_center, 20, color=color, thickness=-1) # draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=purple_center), thickness=5) # draw_line(mat, *angle_to_line(align_angle, origin=purple_center), color=color, thickness=5) opened = max(opened, key=lambda x: x['size']) if opened else None closed = max(closed, key=lambda x: x['size']) if closed else None if opened: cv2.circle(mat, opened['center'], 5, color=(0, 0, 255), thickness=-1) draw_line(mat, *angle_to_line(opened['align'], origin=opened['center']), color=(0, 0, 255), thickness=5) draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=opened['center']), thickness=5) # print('nani %d' % opened['align']) shm.recovery_vampire.open_visible.set(True) shm.recovery_vampire.open_handle_x.set(opened['center'][0]) shm.recovery_vampire.open_handle_y.set(opened['center'][1]) shm.recovery_vampire.open_offset_x.set(opened['center'][0] + opened['offset'][0]) shm.recovery_vampire.open_offset_y.set(opened['center'][1] + opened['offset'][1]) shm.recovery_vampire.open_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], opened['align'])) shm.recovery_vampire.open_size.set(opened['size']) else: shm.recovery_vampire.open_visible.set(False) if closed: draw_line(mat, *angle_to_line(closed['align'], origin=closed['center']), color=(0, 255, 0), thickness=5) draw_line(mat, *angle_to_line(self.options['manipulator_angle'], origin=closed['center']), thickness=5) # print('what %d' % closed['align']) cv2.circle(mat, closed['center'], 5, color=(0, 255, 0), thickness=-1) shm.recovery_vampire.closed_visible.set(True) shm.recovery_vampire.closed_handle_x.set(closed['center'][0]) shm.recovery_vampire.closed_handle_y.set(closed['center'][1]) shm.recovery_vampire.closed_handle_direction.set(closed['direction']) shm.recovery_vampire.closed_offset_x.set(closed['center'][0] + closed['offset'][0]) shm.recovery_vampire.closed_offset_y.set(closed['center'][1]) shm.recovery_vampire.closed_angle_offset.set(heading_sub_degrees(self.options['manipulator_angle'], closed['align'])) shm.recovery_vampire.closed_size.set(closed['size']) else: shm.recovery_vampire.closed_visible.set(False) self.post('hmm', mat)
def process(self, mat): global DOWNWARD_CAM_WIDTH, DOWNWARD_CAM_HEIGHT curr_time = time.time() if curr_time - self.last_run < shm.vision_module_settings.time_between_frames.get( ): return self.last_run = curr_time DOWNWARD_CAM_WIDTH = DOWNWARD_CAM_WIDTH or mat.shape[1] DOWNWARD_CAM_HEIGHT = DOWNWARD_CAM_HEIGHT or mat.shape[0] mat = to_umat(mat) debug = self.options['debug'] try: ## Reset SHM output #for s in ALL_SHM: # s.reset() for s in ALL_SHM: s.visible = False lab, lab_split = bgr_to_lab(mat) # detect green section dist_from_green = elementwise_color_dist(lab, [185, 55, 196]) if debug: self.post('green_dist', np.abs(dist_from_green).astype('uint8')) green_threshed = range_threshold( dist_from_green, self.options["color_dist_min_green_funnel"], self.options["color_dist_max_green_funnel"], ) erode_kernel = rect_kernel(to_odd(self.options['erode_kernel'])) green_threshed = erode(green_threshed, erode_kernel) # detect red section red_threshed = range_threshold(lab_split[1], self.options['red_lab_a_min'], self.options['red_lab_a_max']) red_threshed = erode(red_threshed, erode_kernel) # detect black section black_threshed = range_threshold(lab_split[0], self.options['black_lab_l_min'], self.options['black_lab_l_max']) black_threshed = erode( black_threshed, erode_kernel, iterations=self.options['black_erode_iters']) if debug and POST_UMAT: self.post('green_threshed', green_threshed) self.post('red_threshed', red_threshed) self.post('black_threshed', black_threshed) #comp = red_threshed & ~green_threshed comp = green_threshed if debug: self.post('comp', comp) percent_red = cv2.countNonZero(red_threshed) / \ cv2.countNonZero(red_threshed | green_threshed | black_threshed) percent_red_thresh = self.options['percent_red_thresh'] # Find center using hough lines blurred = simple_gaussian_blur(comp, to_odd(self.options['blur_kernel']), 0) edges = simple_canny(blurred, use_mean=True) if debug and POST_UMAT: self.post('edges', edges) lines_cart, lines_polar = find_lines( edges, self.options['hough_lines_rho'], np.radians(self.options['hough_lines_theta']), self.options['hough_lines_thresh']) found_center = False thetas = [] THETA_DIFF = math.radians(15) if lines_cart: lines_groups_mat = mat # Group lines into bins bins = [] for line_polar, line_cart in zip(lines_polar, lines_cart): for (idx, bin) in enumerate(bins): # Multiple by 2 because we're in [0, 180], not [0, 360] if abs(angle_diff(line_polar[1] * 2, bin[0][1] * 2)) < THETA_DIFF * 2: bins[idx] = (bin[0], bin[1] + 1) break else: bins.append((line_polar, 1)) draw_line(lines_groups_mat, (line_cart[0], line_cart[1]), (line_cart[2], line_cart[3]), thickness=2) if debug: self.post('lines_groups', lines_groups_mat) # Pick top four - we sometimes get the ends of the bins as lines as well lines_unpicked = [ line for line, count in sorted( bins, key=lambda bin: bin[1], reverse=True)[:4] ] if len(lines_unpicked) >= 2: target_angle = math.radians(TARGET_ANGLE) # Find two lines that are about 30 degrees apart # Find the pairing of lines with the angle difference closest to 30 degrees pairs = itertools.combinations(lines_unpicked, 2) # We double angles because we're in [0, 180] and not [0, 360] lines = sorted( pairs, key=lambda pair: abs(target_angle * 2 - abs( angle_diff(pair[0][1] * 2, pair[1][1] * 2))))[0] delta = math.degrees( abs(target_angle * 2 - abs(angle_diff(lines[0][1] * 2, lines[1][1] * 2)))) MAX_ANGLE_DIFF = 30 if delta <= MAX_ANGLE_DIFF: line_equations = [] lines_mat = mat #mat.copy() for (rho, theta) in lines: thetas.append(theta) (x1, y1, x2, y2) = line_polar_to_cartesian(rho, theta) draw_line(lines_mat, (x1, y1), (x2, y2), (255, 0, 0), thickness=2) line_equations.append((x1, x2, y1, y2)) if debug and POST_UMAT: self.post('lines', lines_mat) found_center = len( line_equations ) >= 2 and percent_red >= percent_red_thresh if found_center: # calculate intersection of diameters of green section [x01, x02, y01, y02] = line_equations[0] [x11, x12, y11, y12] = line_equations[1] # This is stupid but it works if x02 == x01: x01 += 0.01 if x12 == x11: x11 += 0.01 b1 = (y02 - y01) / (x02 - x01) b2 = (y12 - y11) / (x12 - x11) if b1 == b2: print('ovelapping') found_center = False else: intersection_x = (b1 * x01 - b2 * x11 + y11 - y01) / (b1 - b2) if math.isinf(intersection_x): if abs(x02 - x01) < 0.2: intersection_x = x02 elif abs(x12 - x11) < 0.2: intersection_x = x12 intersection_y = (b1 * (intersection_x - x01) + y01) intersection_x = int(intersection_x) intersection_y = int(intersection_y) center_x, center_y = intersection_x, intersection_y else: found_center = False if found_center: center_mat = mat # mat.copy() draw_circle(center_mat, (center_x, center_y), 7, (255, 255, 255), thickness=-1) if POST_UMAT: self.post('center', center_mat) ROULETTE_BOARD.visible = True (ROULETTE_BOARD.center_x, ROULETTE_BOARD.center_y) = (center_x, center_y) if len(thetas) == 2: x = 0 y = 0 # We multiply angle by 2 for calculating average because # we want it to be in the range [0,180] instead of [0,360] for theta in thetas: if theta > math.pi: theta -= math.pi * 2 x += math.cos(theta * 2) y += math.sin(theta * 2) avg_heading = math.atan2(y, x) * 180 / math.pi / 2 GREEN_BINS[0].angle = avg_heading # draw centroids of green sections and predict location ~3 seconds later contours = outer_contours(green_threshed) contours = sorted(contours, key=contour_area, reverse=True) bin_index = 0 for contour in contours[:len(GREEN_BINS)]: centroid_x, centroid_y = contour_centroid(contour) draw_contours(mat, [contour], (0, 255, 0), thickness=2) draw_circle(mat, (centroid_x, centroid_y), 7, (255, 255, 255), thickness=-1) # #self.post('centroids', mat) # GREEN_BINS[bin_index].visible = True # GREEN_BINS[bin_index].centroid_x = centroid_x # GREEN_BINS[bin_index].centroid_y = centroid_y # if found_center: # predicted_x, predicted_y = predict_xy(center_x, center_y, centroid_x, centroid_y) # if within_camera(predicted_x, predicted_y): # cv2.circle(mat, (predicted_x, predicted_y), 7, (255, 0, 0), -1) # GREEN_BINS[bin_index].predicted_location = True # GREEN_BINS[bin_index].predicted_x = predicted_x # GREEN_BINS[bin_index].predicted_y = predicted_y # bin_index += 1 assign_bins(contours[:len(GREEN_BINS)], GREEN_BINS, self) if debug and POST_UMAT: self.post('centroids', mat) # # draw centroids for red sections and predict location ~3 seconds later # _, contours, _ = cv2.findContours(red_threshed.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # contours = sorted(contours, key=lambda cont: cv2.contourArea(cont), reverse=True) # bin_index = 0 # for contour in contours[:len(RED_BINS)]: # centroid_x, centroid_y = contour_centroid(contour) # cv2.drawContours(mat, [contour], -1, (0, 255, 0), 2) # cv2.circle(mat, (centroid_x, centroid_y), 7, (255, 255, 255), -1) # assign_bins(contours[:len(RED_BINS)], RED_BINS, self) # if POST_UMAT: # self.post('centroids', mat) # # draw centroids for black sections and predict location ~3 seconds later # _, contours, _ = cv2.findContours(black_threshed.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # contours = sorted(contours, key=lambda cont: cv2.contourArea(cont), reverse=True) # bin_index = 0 # for contour in contours[:len(BLACK_BINS)]: # centroid_x, centroid_y = contour_centroid(contour) # cv2.drawContours(mat, [contour], -1, (0, 255, 0), 2) # cv2.circle(mat, (centroid_x, centroid_y), 7, (255, 255, 255), -1) # assign_bins(contours[:len(BLACK_BINS)], BLACK_BINS, self) # if POST_UMAT: self.post('centroids', mat) except Exception: traceback.print_exc(file=sys.stdout) finally: for s in ALL_SHM: s.commit()