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