コード例 #1
0
    def connectCircles(self):
        """
        Private method.
        Computes a list with connections between circle centres.
        For every couple of centres (a, b) such that a is not b:
               if there's no centre c such that ((dist(a,c) < dist(a,b)) and (dist(c, b) < dist(a, c)))
               then add the vector (a,b) to the connection with the indices of the centres a and b
        self.prepareGraph must be called before this one
        """
        keypoints = self._circles
        circles_dict = {}
        vectors_dict = {}

        subdiv = cv2.Subdiv2D()
        subdiv.initDelaunay(self._bounds)
        for i in range(len(keypoints)):
            keypoint = keypoints[i]
            if not self._noise_circles[i]:  # If the circle i is not a noisy circle
                key = (float(keypoint[0]), float(keypoint[1]))
                circles_dict[key] = i
                subdiv.insert(key)

        triangle_list = subdiv.getTriangleList()
        edge_list = subdiv.getEdgeList()
        # An edge is the coordinates of two points. 1st coordinate = (edge[0], edge[1]), 2nd = (edge[2], edge[3])
        for edge in edge_list:
            pt1 = (edge[0], edge[1])
            pt2 = (edge[2], edge[3])
            if self.checkInBounds(pt1) and self.checkInBounds(pt2):
                vectors_dict[(pt1, pt2)] = geom.vectorize(pt1, pt2)
                vectors_dict[(pt2, pt1)] = geom.vectorize(pt2, pt1)

        for triangle in triangle_list:
            pt1 = (triangle[0], triangle[1])
            pt2 = (triangle[2], triangle[3])
            pt3 = (triangle[4], triangle[5])
            if self.checkInBounds(pt1) and self.checkInBounds(pt2) and self.checkInBounds(pt3):
                dist1 = geom.point_distance(pt1, pt2)
                dist2 = geom.point_distance(pt2, pt3)
                dist3 = geom.point_distance(pt3, pt1)
                max_dist = max(dist1, dist2, dist3)
                if max_dist == dist1:
                    if (pt1, pt2) in vectors_dict and (pt2, pt1) in vectors_dict:
                        vectors_dict.pop((pt1, pt2))
                        vectors_dict.pop((pt2, pt1))
                elif max_dist == dist2:
                    if (pt2, pt3) in vectors_dict and (pt3, pt2) in vectors_dict:
                        vectors_dict.pop((pt2, pt3))
                        vectors_dict.pop((pt3, pt2))
                else:
                    if (pt1, pt3) in vectors_dict and (pt3, pt1) in vectors_dict:
                        vectors_dict.pop((pt3, pt1))
                        vectors_dict.pop((pt1, pt3))
        vectors = []
        indices = []
        for (pt1, pt2) in vectors_dict:
            vectors.append(vectors_dict[(pt1, pt2)])
            indices.append((circles_dict[pt1], circles_dict[pt2]))
        self._original_arc_vectors = vectors
        self._original_arc_indices = indices
コード例 #2
0
 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
コード例 #3
0
ファイル: upper_hole.py プロジェクト: Angeall/pyConnect4NAO
 def _findHamcodesRectangles(self):
     hamcodes_id_to_rect_centres = {}
     for hamcode in self._hamcodes:
         hamcode.contours = geom.sort_rectangle_corners(hamcode.contours)
         ham_long_vector = geom.vectorize(hamcode.contours[0], hamcode.contours[1])
         for rect_centre in self._filtered_rectangle_centres:
             ((long_vector, long_norm), (_, _)) = geom.get_box_info(self._boxes[self._centres_to_indices[rect_centre]])
             if geom.are_vectors_parallel(long_vector, ham_long_vector, 0.25):
                 hamcode_vector = geom.vectorize(hamcode.contours[0], hamcode.contours[1])
                 centers_vector = geom.vectorize(hamcode.center, rect_centre)
                 model_ratio = (self._model.hamcode_v_margin + (self._model.hamcode_side / 2.)
                                - self._model.hole_v_margin - (self._model.hole_width / 2.)) / self._model.hamcode_side
                 image_ratio = np.linalg.norm(centers_vector) / np.linalg.norm(hamcode_vector)
                 if geom.are_ratio_similar(image_ratio, model_ratio, 1.3):
                     hamcodes_id_to_rect_centres[hamcode.id] = rect_centre
     return hamcodes_id_to_rect_centres
コード例 #4
0
ファイル: motion.py プロジェクト: Angeall/pyConnect4NAO
 def compareToLeftHandPosition(self, coord, must_print=False):
     """
     :param coord: the 6D coordinates to compare with the left hand position
     :type coord: list
     :return: the difference of position from the hand to the given coordinates [x-axis, y-axis]
     :rtype: np.array
     """
     hand_coord = self.getLeftHandPosition()
     if must_print:
         print coord
         print hand_coord
     return geom.vectorize(hand_coord[0:2], coord[0:2])
コード例 #5
0
ファイル: loop.py プロジェクト: Angeall/pyConnect4NAO
 def inverseKinematicsConvergence(self, hole_index):
     """
     :param hole_index: the number of the hole above which we want to move NAO's hand
     :return:
     """
     self.nao_motion.moveHead(self.nao_motion.DEFAULT_HEAD_PITCH, self.nao_motion.DEFAULT_HEAD_YAW, True)
     max_tries = 4  # If we don't see any marker after 2 tries, we move NAO's head
     i = 0
     stable = False
     while not stable:
         while i < max_tries:
             try:
                 hole_coord = self.c4_handler \
                     .getUpperHoleCoordinatesUsingMarkers(hole_index,
                                                          self.nao_motion.getCameraBottomPositionFromTorso(),
                                                          data.CAM_MATRIX, data.CAM_DISTORSION,
                                                          tries=self.min_detections)
                 self.resetHead()
                 if abs(hole_coord[5] + 0.505) > self.rA:  # If the board is sloped from NAO, we need to rotate NAO
                     self.nao_motion.moveAt(0, 0, (hole_coord[5] + 0.505)/3)
                     continue
                 dist_from_optimal = geom.vectorize((0.161, 0.113), (hole_coord[0], hole_coord[1]))
                 if abs(dist_from_optimal[0]) > self.ppA or abs(dist_from_optimal[1]) > 2 * self.ppA:
                     self.nao_motion.moveAt(dist_from_optimal[0], dist_from_optimal[1], hole_coord[5] + 0.505)
                     continue
                 self.estimated_distance = hole_coord[0]
                 i = 0
                 self.nao_motion.setLeftHandPosition(hole_coord, mask=63)
                 diff = self.nao_motion.compareToLeftHandPosition(hole_coord)
                 if abs(diff[0]) < self.cA and abs(diff[1]) < 2 * self.cA:
                     stable = True
                     self.nao_motion.playDisc(hole_coord)
                     break
                 else:
                     self.nao_motion.setLeftArmRaised()
                     self.nao_motion.moveAt(diff[0], diff[1], hole_coord[5] + 0.505)
                     i += 1
             except NotEnoughLandmarksException:
                 i += 1
         if not stable:
             # self.tts.say("Je ne trouve pas les marqueurs dans mon champ de vision")
             self.moveHeadToNextPosition()
             time.sleep(0.500)
             i = 0
コード例 #6
0
ファイル: upper_hole.py プロジェクト: Angeall/pyConnect4NAO
    def _filterOtherRectangles(self):
        filtered_rectangle_centres = []
        contains_map = {}
        # The ratio between the hole length + the horizontal space and the hole length
        model_hole_space_ratio = (self._model.hole_length + self._model.hole_h_space) / self._model.hole_length
        model_length_width_ratio = (self._model.hole_length / self._model.hole_width)
        for centre in self._filtered_rectangle_centres:
            if not contains_map.get(centre, False):
                box = self._boxes[self._centres_to_indices[centre]]
                ((box_vector, box_length), (_, box_width)) = geom.get_box_info(box)
                max_distance = 0.5 * box_length
                # If the rectangle is not too small and the length/width ratio is the ratio expected by the _model
                if box_length > 30 and geom.are_ratio_similar(box_length / box_width, model_length_width_ratio, 1.75):
                    for other_centre in self._filtered_rectangle_centres:
                        if other_centre is not centre:
                            other_box = self._boxes[self._centres_to_indices[other_centre]]
                            ((other_box_vector, other_box_length),(_, other_box_width)) = geom.get_box_info(other_box)
                            centres_vector = geom.vectorize(centre, other_centre)
                            # If the other rectangle is not too small and the length/width ratio
                            #   is the ratio expected by the _model
                            #   and : the two rectangle have the same length
                            #   and : the distance between the _rectangles is the distance expected by the _model
                            #   and : the vector of the long_side of the rectangle and the vector that binds the
                            #       two centres is approximately parallel
                            if other_box_length > 30 \
                                    and geom.are_ratio_similar(other_box_length / other_box_width,
                                                               model_length_width_ratio, 1.75) \
                                    and geom.are_vectors_similar(box_vector, other_box_vector, max_distance,
                                                                 signed=False) \
                                    and geom.are_ratio_similar(np.linalg.norm(centres_vector) / box_length,
                                                               model_hole_space_ratio, 0.25) \
                                    and geom.are_vectors_parallel(box_vector, centres_vector, 0.4):

                                if not contains_map.get(centre, False):
                                    filtered_rectangle_centres.append(centre)
                                    contains_map[centre] = True
                                if not contains_map.get(other_centre, False):
                                    filtered_rectangle_centres.append(other_centre)
                                    contains_map[other_centre] = True
        return filtered_rectangle_centres