def pub_empty_uv_info(self, tile_id, obj_desire):
uvuv = uv()
tile_uv = tileuv()
tile_uv.tile_id = tile_id
tile_uv.obj_desire = obj_desire
tile_uv.cen_uv.uvinfo = [0, 0]
tile_uv.f1th_uv.uvinfo = [0, 0]
tile_uv.s2th_uv.uvinfo = [0, 0]
tile_uv.t3th_uv.uvinfo = [0, 0]
tile_uv.f4th_uv.uvinfo = [0, 0]
self.tile_pub.publish(tile_uv)
def draw_triangle_numbers(self, contours, rgb, obj_desire):
uvuv = uv()
tile_uv = tileuv()
alltile_uvlist, newrgb = self.Draw_triangle(contours, rgb, obj_desire)
newtileuv = self.Sort_tile_mass_num(alltile_uvlist)
for i in newtileuv:
# print "i",i
cv2.putText(newrgb, str(i[0][0]), (i[1][0], i[1][1]),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
tile_uv.tile_id = i[0][0]
tile_uv.obj_desire = obj_desire
tile_uv.cen_uv.uvinfo = [i[1][0], i[1][1]]
tile_uv.f1th_uv.uvinfo = i[2][0]
tile_uv.s2th_uv.uvinfo = i[2][1]
tile_uv.t3th_uv.uvinfo = i[2][2]
tile_uv.f4th_uv.uvinfo = i[2][3]
self.tile_pub.publish(tile_uv)
return newrgb.copy()
def Draw_triangle(self, contours, rgb, obj_desire):
##################
DELAY = 0.02
USE_CAM = 1
IS_FOUND = 0
count = 0 #count feature tile numbers
cnt = 0
central_list = []
uvuv = uv()
tile_uv = tileuv()
##################
_width = 480.0
_height = 640.0
_margin = 0.0
corners = np.array([
[[_margin, _margin]],
[[_margin, _height + _margin]],
[[_width + _margin, _height + _margin]],
[[_width + _margin, _margin]],
])
pts_dst = np.array(corners, np.float32)
latest_central = (0, 0)
resultuv = []
for cont in contours:
"""
#1,num,2,centeral point 3,for angular point uv ,4,clockwise direction
#caculating Area for tile selected just one tile
"""
# print "cont----------", cont
# 获取轮廓长度
arc_len = cv2.arcLength(cont, True)
# 多边形拟合
approx = cv2.approxPolyDP(cont, 0.1 * arc_len, True)
# print "cv2.contourArea(cont)",cv2.contourArea(cont)
# print "approx",len(np.array(approx).reshape(-1,2))
if cv2.contourArea(cont) > 5000 and cv2.contourArea(cont) < 20000:
# if cv2.contourArea(cont) > 3000:
if (len(approx) == 4):
# print "a"
IS_FOUND = 1
M = cv2.moments(cont)
# 获取图像质心坐标
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
now_central = (cX, cY)
if self.Judge_isnot_same_tile(latest_central,
now_central) != 1:
count += 1
# cv2.putText(rgb, str(count), (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
# print "CX,CY", [cX, cY]
central_list.append([cX, cY])
pts_src = np.array(approx, np.float32)
# print "pts_src", pts_src
cv2.circle(rgb, (cX, cY), 5, (0, 0, 0), -1)
# print approx.tolist()
angular_point = []
print "approx", approx.tolist()
new_approx = self.Sort_tile_feature(approx.tolist())
new_approx = sorted(new_approx,
reverse=True,
key=lambda k: [k[1], k[0]])
# new_approx=approx
print "new_approx", new_approx
for i in range(len(new_approx)):
if i == 0:
cv2.circle(rgb,
(new_approx[i][0], new_approx[i][1]), 5,
(20, 60, 220), -1)
angular_point.append(
[new_approx[i][0], new_approx[i][1]])
cv2.putText(rgb, str(i),
(new_approx[i][0], new_approx[i][1]),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 0), 1)
else:
cv2.circle(rgb,
(new_approx[i][0], new_approx[i][1]), 5,
(0, 255, 0), -1)
angular_point.append(
[new_approx[i][0], new_approx[i][1]])
cv2.putText(rgb, str(i),
(new_approx[i][0], new_approx[i][1]),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 0), 1)
resultuv.append([[count], [cX, cY], angular_point])
# draw trangle in image
h, status = cv2.findHomography(pts_src, pts_dst)
out = cv2.warpPerspective(rgb, h,
(int(_width + _margin * 2),
int(_height + _margin * 2)))
cv2.drawContours(rgb, [approx], -1, (0, 255, 255), 3)
# cv2.drawContours(rgb, [approx], -1, (20*count, 255, 0), -1, cv2.LINE_AA)
print "all info for tile------", resultuv
print "angular_point", angular_point
cv2.line(rgb,
((angular_point[0][0] + angular_point[2][0]) / 2,
(angular_point[0][1] + angular_point[2][1]) / 2),
(cX, cY), (255, 0, 0), 3)
cv2.line(rgb,
((angular_point[1][0] + angular_point[3][0]) / 2,
(angular_point[1][1] + angular_point[3][1]) / 2),
(cX, cY), (255, 0, 0), 3)
# print "Now tile id",count
# if count == 1:
# tile_uv.tile_id = count
# tile_uv.obj_desire = obj_desire
# tile_uv.cen_uv.uvinfo = [cX, cY]
# tile_uv.f1th_uv.uvinfo = angular_point[0]
# tile_uv.s2th_uv.uvinfo = angular_point[1]
# tile_uv.t3th_uv.uvinfo = angular_point[2]
# tile_uv.f4th_uv.uvinfo = angular_point[3]
# self.tile_pub.publish(tile_uv)
latest_central = now_central
else:
pass
# count += 1
# cnt += 11
# return rgb.copy()
return resultuv, rgb.copy()