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
def _initStructures(self): """ :return: the _kdtree, the centre to indices map and the box list, in this order :rtype: tuple Initialize the data structures used inside the detector """ data = [] boxes = [] centres_to_indices = {} for i, rect in enumerate(self._rectangles): centres_to_indices[rect[0]] = i boxes.append(geom.sort_rectangle_corners(cv2.boxPoints(rect))) data.append(rect[0]) self._filtered_rectangle_centres = data if len(data) > 0: return KDTree(data), centres_to_indices, boxes else: return None, None, None
def record(self, marker, pitch, yaw): """ :param marker: the detected marker for the hole :type: hampy's Hamming Marker :param pitch: the pitch of NAO's head :param yaw: the yaw of NAO's head record the result into the lists """ # Variables set corners = np.array(geom.sort_rectangle_corners(marker.contours)).ravel().tolist() variables = [pitch, yaw] variables.extend(corners) variables = np.array(variables) # Result set self.shoulder_pitch_array.append(np.append(variables, self.left_arm_angles[0])) self.shoulder_roll_array.append(np.append(variables, self.left_arm_angles[1])) self.elbow_yaw_array.append(np.append(variables, self.left_arm_angles[2])) self.elbow_roll_array.append(np.append(variables, self.left_arm_angles[3])) self.wrist_yaw_array.append(np.append(variables, self.left_arm_angles[4]))