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]))
