def __init__(self, next_img_func, cam_no=-1, model_type=DEFAULT_MODEL): if model_type == DEFAULT_MODEL: self.model = DefaultModel() else: raise Connect4ModelNotFound(model_type) self.cam_no = cam_no self.img = None self.next_img_func = next_img_func self.front_hole_detector = FrontHolesDetector(self.model) self.upper_hole_detector = UpperHolesDetector(self.model) self.tracker = Connect4Tracker(self.model) # Used for the detection self.front_holes_detection_prepared = False self.circles = [] self.param1 = None self.param2 = None self.min_radius = None self.max_radius = None self.min_dist = None self.sloped = False self.pixel_error_margin = None self.res = 320 self.distance = None
class Connect4Handler(object): def __init__(self, next_img_func, cam_no=-1, model_type=DEFAULT_MODEL): if model_type == DEFAULT_MODEL: self.model = DefaultModel() else: raise Connect4ModelNotFound(model_type) self.cam_no = cam_no self.img = None self.next_img_func = next_img_func self.front_hole_detector = FrontHolesDetector(self.model) self.upper_hole_detector = UpperHolesDetector(self.model) self.tracker = Connect4Tracker(self.model) # Used for the detection self.front_holes_detection_prepared = False self.circles = [] self.param1 = None self.param2 = None self.min_radius = None self.max_radius = None self.min_dist = None self.sloped = False self.pixel_error_margin = None self.res = 320 self.distance = None def estimateMinRadius(self, dist): """ Estimate the minimum radius to detect in an image for a front hole :param dist: The distance from the board in meters :return: The estimated minimum radius to detect """ return int(np.ceil(4.4143 * (dist ** (-1.1446)) / (self.model.circle_diameter / 0.046))) def estimateMaxRadius(self, dist): """ Estimate the maximum radius to detect in an image for a front hole :param dist: The distance from the board in meters :return: The estimated maximum radius to detect """ return int(round(8.5468 * (dist ** (-0.7126)) / (self.model.circle_diameter / 0.046))) def computeMaxRadiusRatio(self, distance): """ Computes the max ratio between the radius of the circle that is the farthest from the robot and the radius of the circle that is the closest from the robot. :param distance: The distance between the robot and the farthest circle in the Connect 4 grid in cm :return: The max radius ratio """ max_angle = np.pi / 2. ab = distance # The length of the vector between the robot and the # farthest point of the farthest vector bc = self.model.circle_diameter # The length of the vector of a circle ac = geom.al_kashi(b=ab, c=bc, angle=max_angle) # The length of the vector between the robot and the closest # point of the farthest vector beta = geom.al_kashi(a=bc, b=ab, c=ac) # Angle of vision of the robot to the farthest vector de = bc # de and bc are the same vectors bd = self.model.length - de # bd is the length of the game board minus one vector ad = geom.al_kashi(b=ab, c=bd, angle=max_angle) # The length of the vector between the robot and the farthest # point of the closest vector be = self.model.length # The length of the game board ae = geom.al_kashi(b=ab, c=be, angle=max_angle) # The length of the vector between the robot and the # closest point of the closest vector alpha = geom.al_kashi(a=de, b=ad, c=ae) # Angle of vision of the robot to the closest vector return alpha / beta def computeMinMaxRadius(self, distance, sloped=False, res=DEFAULT_RESOLUTION): """ Get the minimum and the maximum radius to detect during the detection :param res: The resolution (length) of the picture. :param distance: The distance between the robot and the farthest circle in the Connect 4 grid in meters :type distance: float :param sloped: True if the connect 4 can be considered as sloped for the robot vision :type sloped: bool :return: The minimum and the maximum radius to detect (min_radius, max_radius) :rtype: tuple """ min_radius = self.estimateMinRadius(distance) max_radius = self.estimateMaxRadius(distance) res_diff = res / DEFAULT_RESOLUTION min_radius *= res_diff max_radius *= res_diff if sloped: radius_ratio = self.computeMaxRadiusRatio(distance * 100) max_radius *= radius_ratio return min_radius, int(max_radius) def computeMaxPixelError(self, min_radius): """ Get the maximum error box to consider during the front hole detection :param min_radius: The minimum radius used during the detection :type min_radius: float :return: The maximum error in pixel to consider during the front hole detection :rtype: float """ cm_to_pixels = min_radius / (self.model.circle_diameter / 2) hor = (self.model.circle_h_space + self.model.circle_diameter) * cm_to_pixels ver = (self.model.circle_v_space + self.model.circle_diameter) * cm_to_pixels hyp = np.sqrt(hor ** 2 + ver ** 2) return 0.9 * hyp def prepareFrontHolesDetection(self, distance, sloped, res): """ Initialize The parameters for the detection :param distance: The distance between the robot and the farthest circle in the Connect 4 grid in meters :type distance: float :param sloped: True if the connect 4 can be considered as sloped for the robot vision :type sloped: bool :param res: The resolution (width in pixels) to consider during the detection :type res: int """ self.front_holes_detection_prepared = True self.res = res self.min_radius, self.max_radius = self.computeMinMaxRadius(distance, sloped, res) self.pixel_error_margin = self.computeMaxPixelError(self.min_radius) self.min_dist = int(self.min_radius * 2.391 * (self.res / 320)) self.param1 = 77 self.param2 = 9.25 if self.sloped: self.param2 = 8 def detectFrontHoles(self, distance, sloped=False, res=DEFAULT_RESOLUTION, tries=1, debug=False): """ :param distance: The distance between the robot and the connect4 :type distance: float :param sloped: True if the connect4 is sloped or in an unknown position :type sloped: bool :param res: the resolution of the image :type res: int :param tries: the number of success the algorithm must perform to mark the Connect 4 as detected :type tries: int :param debug: if True, displays image of the current detection :type debug: bool Detect the front holes in the next image of the "next_img_func". If the connect 4 is not found in one attempt, the whole detection fails (see the 'tries' parameter). """ last_0_0_coord = None if res == 640: i_res = 2 else: i_res = 1 for i in range(tries): if self.cam_no == -1: self.img = self.next_img_func(0, res=i_res) # We detect the front holes using the top camera else: self.img = self.next_img_func(self.cam_no, res=i_res) self.circles = [] if not self.front_holes_detection_prepared: self.prepareFrontHolesDetection(distance, sloped, res) elif self.distance != distance or self.sloped != sloped: self.min_radius, self.max_radius = self.computeMinMaxRadius(distance, sloped) gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY) gray = cv2.GaussianBlur(gray, (3, 3), 0) gray = cv2.medianBlur(gray, 3) circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, self.min_dist, param1=self.param1, param2=self.param2, minRadius=self.min_radius, maxRadius=self.max_radius) if circles is not None: self.circles = circles[0] self.front_hole_detector.runDetection(self.circles, pixel_error_margin=self.pixel_error_margin, img=self.img) if debug: img2 = draw_circles(self.img, self.circles) cv2.imshow("Circles detected", img2) cv2.imshow("Original picture", self.img) (cur_0_0_coord, cur_1_0_coord) = cv2.perspectiveTransform( np.float32(np.array([self.front_hole_detector.reference_mapping[(0, 0)], self.front_hole_detector.reference_mapping[(1, 0)]])).reshape(1, -1, 2), self.front_hole_detector.homography).reshape(-1, 2) if last_0_0_coord is not None: vertical_max_dist = geom.point_distance(cur_0_0_coord, cur_1_0_coord) # If the distance between two board detection exceed 3/4 * distance between two rows, # the detection is considered as unstable because two different grids have been detected if geom.point_distance(cur_0_0_coord, last_0_0_coord) > 0.75 * vertical_max_dist: raise FrontHolesGridNotFoundException( "The detection was not stable as it detected two different boards during {0} attempt(s)" .format(str(i))) last_0_0_coord = cur_0_0_coord if debug: cv2.imshow("Perspective", self.front_hole_detector.getPerspective()) if cv2.waitKey(1) == 27: print "Esc pressed : exit" cv2.destroyAllWindows() raise FrontHolesGridNotFoundException("The detection was interrupted") else: raise FrontHolesGridNotFoundException( "The detection was not stable as it lost the board after {0} attempt(s)".format(str(i))) def getUpperHoleCoordinatesUsingMarkers(self, index, camera_position, camera_matrix, camera_dist, tries=1, debug=False, res=640): """ :param res: The resolution length :param index: the index of the hole :type index: int :param camera_position: the 6D position of the camera used for the detection (the bottom one), from the robot torso :type camera_position: tuple :param camera_matrix: the camera distortion matrix :type camera_matrix: np.array :param camera_dist: the camera distortion coefficients vector :type camera_dist: np.array :param debug: if True, draw the detected markers :type debug: bool :param tries: the number of times the detection will be run. If one try fails, the whole detection is considered as failed :type tries: int Get an upper hole's coordinates using the Hamming markers on the Connect 4. This method assumes that the Hamming codes are visible on the image it will acquire using its "next_img_func". Otherwise, the detection fails """ max_nb_of_markers = 0 rvec = None tvec = None if res == 640: i_res = 2 else: i_res = 1 for i in range(tries): if self.cam_no == -1: img = self.next_img_func(1, res=i_res) # We get the image from the bottom camera else: img = self.next_img_func(self.cam_no, res=i_res) min_nb_of_codes = 2 markers = detect_markers(img) if markers is not None and len(markers) >= min_nb_of_codes: if debug: for m in markers: m.draw_contour(img) cv2.putText(img, str(m.id), tuple(int(p) for p in m.center), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 3) self.upper_hole_detector._hamcodes = markers self.upper_hole_detector.runDetection([], markers) if len(markers) > max_nb_of_markers: max_nb_of_markers = len(markers) rvec, tvec = self.upper_hole_detector.match3DModel(camera_matrix, camera_dist) else: if debug: cv2.imshow("Debug", img) if cv2.waitKey(100) == 27: raise NotEnoughLandmarksException("The detection was interrupted") raise NotEnoughLandmarksException("The model needs at least " + str(min_nb_of_codes) + " detected codes") return self._getUpperHoleCoordinates(rvec, tvec, index, camera_position) def getUpperHoleCoordinatesUsingFrontHoles(self, distance, sloped, index, camera_position, camera_matrix, camera_dist, debug=False, tries=1): """ :param distance: The distance between the robot and the connect4 :type distance: float :param sloped: True if the connect4 is sloped or in an unknown position :type sloped: bool :param index: the index of the hole :type index: int :param camera_position: the 6D position of the camera used for the detection (the top one), from the robot torso :type camera_position: tuple :param camera_matrix: the camera distortion matrix :type camera_matrix: np.array :param camera_dist: the camera distortion coefficients vector :type camera_dist: np.array :param debug: if True, draw the detected markers :type debug: bool :param tries: the number of times the detection will be run. If one try fails, the whole detection is considered as failed :type tries: int Get an upper hole's coordinates using the front holes of the Connect 4. This method assumes that the front holes are visible on the image it will acquire using its "next_img_func". Otherwise, the detection fails """ self.detectFrontHoles(distance, sloped, tries=tries, debug=debug) rvec, tvec = self.front_hole_detector.match3DModel(camera_matrix, camera_dist) return self._getUpperHoleCoordinates(rvec, tvec, index, camera_position) def _getUpperHoleCoordinates(self, rvec, tvec, index, camera_position): """ :param rvec: the rotation vector that will transform the 2D coordinates into 3D coordinates :type rvec: np.array :param tvec: the translation vector that will transform the 2D coordinates into 3D coordinates :type tvec: np.array :param index: the index of the hole :type index: int :param camera_position: the 6D position of the camera used for the detection, from the robot torso :type camera_position: tuple :return: The asked upper hole 3D coordinates :rtype: np.array Detect holes in the image using the Hamming codes. """ coords = self.tracker.getHoleCoordinates(rvec, tvec, camera_position, index) coords2 = self.tracker.getHoleCoordinates(rvec, tvec, camera_position, (index+1) % 7) sign = 1 if index == 6: sign = -1 # We invert it if we took a vector the other way vector = sign * geom.vectorize(coords[0:2], coords2[0:2], False) angle = np.arctan2(vector[1], vector[0]) - 1.56 sign = 1 if coords2[0] > coords[0]: sign = -1 angle *= sign coords[5] = angle coords[2] += 0.12 # So the hand of NAO is located above the connect 4, not on it coords[3:] = [-1.542, -0.0945, angle - 0.505] coords[1] += 0.028 coords[0] -= 0.01 return coords