diff --git a/launch/tic_tac_toe.launch b/launch/tic_tac_toe.launch
index 222220348d26c664eedca919dc0c66d360101d1d..6479fdb133162af2e9e186e772a107b660a9e88b 100644
--- a/launch/tic_tac_toe.launch
+++ b/launch/tic_tac_toe.launch
@@ -58,6 +58,7 @@
<node name="node_coords_to_board" pkg="socrates_workshop" type="node_coords_to_board.py" />
<node name="node_soundplay" pkg="sound_play" type="soundplay_node.py" />
<node name="node_coords" pkg="socrates_workshop" type="obj_recogkinect.py" />
+ <node name="node_kineckt" pkg="freenect_camera" type="freenect_node" />
</launch>
diff --git a/scripts/node_coords_to_board.py b/scripts/node_coords_to_board.py
index 7154e765e254f45e2c0f08579270a2af38c1d395..518d1ebea1b8182cf47d9eb9b15e501d5d719132 100755
--- a/scripts/node_coords_to_board.py
+++ b/scripts/node_coords_to_board.py
@@ -27,11 +27,18 @@ int32 id_ai
int32[] board
"""
def closest_point(node, nodes):
- nodes = np.asarray(node)
+ node = np.asarray(node)
nodes = np.asarray(nodes)
- dist_2 = np.sum((nodes - node)**2)
+ dist_2 = np.sum((nodes - node)**2, axis=1)
return np.argmin(dist_2)
+
+def dists(node, nodes):
+ node = np.asarray(node)
+ nodes = np.asarray(nodes)
+ dist_2 = np.sum((nodes - node)**2, axis=1)
+ return dist_2
+
"""
Returns the cell number (in 0-8) given its point
"""
@@ -64,8 +71,10 @@ def handleCoordsMessage(msg):
cells_points = [[cells[i].x, cells[i].y] for i in range(n_cells)]
- pieces_dict = [[getCellNo(closest_point(pieces_points[i], cells_points), cells_points, i), getPlayerId(pieces_labels[i])] for i in range(n_pieces)]
+ pieces_dict = [[closest_point(pieces_points[i], cells_points), getPlayerId(pieces_labels[i])] for i in range(n_pieces)]
+
+ rospy.loginfo(str(pieces_dict))
this.current_board = [0, 0, 0, 0, 0, 0, 0, 0, 0]
diff --git a/scripts/obj_recogkinect.py b/scripts/obj_recogkinect.py
index 46275aa0e35c23eaaf0aa02ec8858226fd76caf6..849cfbb8450f7c51f72cd780f187d1f566eb07bc 100755
--- a/scripts/obj_recogkinect.py
+++ b/scripts/obj_recogkinect.py
@@ -12,36 +12,32 @@ import sys
import numpy as np
from socrates_workshop.msg import PiecesCoords, LabeledPoint
+def common_filtering(c):
+ area = cv2.contourArea(c)
+ return area > 500 and area < 20000
+def in_bound(var, v1, v2):
+ return var >= v1 and var <= v2
-def filter_cell(c):
+def filter_square(c):
perimeter = cv2.arcLength(c, True)
area = cv2.contourArea(c)
- approx = cv2.approxPolyDP(c, 0.04 * perimeter, True)
- if len(approx) == 4 and area < 10000:
- return True
- else:
- return False
+ approx = cv2.approxPolyDP(c, 0.02 * perimeter, True)
+ return common_filtering(c) and in_bound(len(approx), 4, 4) and in_bound(area, 3000, 20000)
-def filter_circle(contour):
- perimeter = cv2.arcLength(contour, True)
- area = cv2.contourArea(contour)
- approx = cv2.approxPolyDP(contour, 0.04 * perimeter, True)
- if len(approx) > 2 and len(approx) <= 8 and area < 5000:
- return True
- else:
- return False
+def filter_circle(c):
+ perimeter = cv2.arcLength(c, True)
+ area = cv2.contourArea(c)
+ approx = cv2.approxPolyDP(c, 0.02 * perimeter, True)
+ return common_filtering(c) and in_bound(len(approx), 7, 9) and in_bound(area, 1500, 1800)
-def filter_cross(contour):
- perimeter = cv2.arcLength(contour, True)
- area = cv2.contourArea(contour)
- approx = cv2.approxPolyDP(contour, 0.03 * perimeter, True)
- if len(approx) > 6 and area > 2000:
- return True
- else:
- return False
+def filter_cross(c):
+ perimeter = cv2.arcLength(c, True)
+ area = cv2.contourArea(c)
+ approx = cv2.approxPolyDP(c, 0.03 * perimeter, True)
+ return common_filtering(c) and in_bound(area, 0, 1000) and in_bound(len(approx), 7, 12)
def filter_contours(contours, filter_function):
@@ -66,6 +62,54 @@ def find_center_point(c, ratio):
+def index(h, c):
+ return h.index[c]
+
+def rh(h):
+ return range(len(h))
+
+def has_children(h, i):
+ return h[i][2] != -1
+
+
+def find_brothers_next(h, i):
+ if h[i][0] == -1:
+ return [i]
+ else:
+ return [i] + find_brothers_next(h, h[i][0])
+
+def get_children(h, i):
+ if not has_children(h, i):
+ return []
+
+ return find_brothers_next(h, h[i][2])
+
+
+def filter_hierarchy(h, c, f, i=0):
+ #[Next, Previous, First_Child, Parent]
+
+ res = []
+
+ if f(h, c, i):
+ return [c[i]]
+
+ for j in get_children(h, i):
+ res = res + filter_hierarchy(h, c, f, i=j)
+ return res
+
+
+def no_children_and_circle(h, c, i):
+ return filter_circle(c[i]) and not any([common_filtering(c[j]) for j in get_children(h, i)])
+
+
+def no_children_and_cross(h, c, i):
+ return filter_cross(c[i]) and not any([common_filtering(c[j]) for j in get_children(h, i)])
+
+def no_square_children_and_square(h, c, i):
+ return filter_square(c[i]) and not any([filter_square(c[j]) for j in get_children(h, i)])
+
+
+
def print_contour_stats(img, c, color):
ratio = img.shape[0] / float(img.shape[0])
c = c.astype("float")
@@ -75,13 +119,13 @@ def print_contour_stats(img, c, color):
M = cv2.moments(c)
cX = int((M["m10"] / max(M["m00"], 0.001)) * ratio)
cY = int((M["m01"] / max(M["m00"], 0.001)) * ratio)
- cv2.putText(img,str(cv2.contourArea(c)),(cX-20,cY-20),cv2.FONT_HERSHEY_SIMPLEX, 0.5, color,2)
+ cv2.putText(img,str(cv2.contourArea(c)),(cX-40,cY-25),cv2.FONT_HERSHEY_SIMPLEX, 0.5, color,1)
perimeter = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, 0.03 * perimeter, True)
- cv2.putText(img, str(len(approx)),(cX-20,cY-10),cv2.FONT_HERSHEY_SIMPLEX, 0.5, color,2)
+ cv2.putText(img, str(len(approx)),(cX-40,cY-15),cv2.FONT_HERSHEY_SIMPLEX, 0.5, color,1)
@@ -90,7 +134,7 @@ class image_converter:
def __init__(self):
img_tpc = "/rgb/image_raw"
- self.image_pub = rospy.Publisher("img_proc/contours_stats", Image, queue_size=10)
+ self.image_pub = rospy.Publisher("/ttt/img_proc/contours_stats", Image, queue_size=10)
self.pub = rospy.Publisher("/ttt/coords",PiecesCoords, queue_size=10)
self.image_sub = rospy.Subscriber(img_tpc, Image, self.img_callback)
self.bridge = CvBridge()
@@ -103,17 +147,33 @@ class image_converter:
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
- blur = cv2.GaussianBlur(gray, (5, 5), 0)
+ blur = cv2.medianBlur(gray,5)
+ blur = cv2.GaussianBlur(gray,(7,3),0)
+ blur = cv2.bilateralFilter(gray, 13, 25, 25)
- (ret, thresh) = cv2.threshold(blur, 100, 250, cv2.THRESH_BINARY)
+ thresh = cv2.adaptiveThreshold(blur,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 15, 5)
+ thresh1 = cv2.cvtColor(thresh, cv2.COLOR_GRAY2BGR)
ratio = img.shape[0] / float(img.shape[0])
- (im2, contours, hierarchy) = cv2.findContours(thresh, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
+ (im2, contours, hierarchy) = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+
+ #[cells, c1] = filter_contours(contours, filter_square)
+ #[circles, c2] = filter_contours(c1, filter_circle)
+ #[crosses, c3] = filter_contours(c2, filter_cross)
+
+ h = hierarchy[0]
+
+ cells = []
+ circles = []
+ crosses = []
+
+ roots = [i for i in range(len(h)) if h[i][3] == -1 ]
- [cells, c1] = filter_contours(contours, filter_cell)
- [circles, c2] = filter_contours(c1, filter_circle)
- [crosses, c3] = filter_contours(c2, filter_cross)
+ for i in roots:
+ cells = cells + filter_hierarchy(h, contours, no_square_children_and_square, i)
+ circles = circles + filter_hierarchy(h, contours, no_children_and_circle, i)
+ crosses = crosses + filter_hierarchy(h, contours, no_children_and_cross, i)
self.msg = PiecesCoords()
@@ -122,7 +182,7 @@ class image_converter:
self.msg.pieces = self.msg.pieces + [LabeledPoint(find_center_point(c, ratio), "X") for c in crosses]
- self.msg.cells = sorted(self.msg.cells, key=lambda p: p.x + 100*p.y)
+ self.msg.cells = sorted(self.msg.cells, key=lambda p: p.x + 1000*p.y)
[print_contour_stats(img, c, (255,0,0)) for c in cells]
[print_contour_stats(img, c, (0,255,0)) for c in circles]