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 filterConnections(self):
"""
For every existing connection, check that there's minimum "min_similar_vectors" other vectors with the same
values (Same value following the threshold). If it's True, keep the connection, otherwise discard it
"""
filtered_arc_vectors = []
filtered_arc_indices = []
if self._min_similar_vectors > len(self._original_arc_vectors):
raise self._exception
if len(self._original_arc_vectors) > 0:
space_tree = KDTree(self._original_arc_vectors)
for j, connection in enumerate(self._original_arc_vectors):
nearest_neighbours = space_tree.query(connection, self._min_similar_vectors)[1]
if (geom.point_distance(connection, self._original_arc_vectors[nearest_neighbours
[(self._min_similar_vectors - 1)]])) <= self._pixel_error_margin:
filtered_arc_vectors.append(connection)
filtered_arc_indices.append(self._original_arc_indices[j])
self._filtered_arc_vectors = filtered_arc_vectors
self._filtered_arc_indices = filtered_arc_indices
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)))
