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(self, contours, rgb, tile_id, 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)
for cont in contours:
resultuv = []
"""
#1,num,2,centeral point 3,for angular point uv ,4,clockwise direction
#caculating Area for tile selected just one tile
"""
if cv2.contourArea(cont) > 5000 and cv2.contourArea(cont) < 60000:
# print "cont----------", cont
# 获取轮廓长度
arc_len = cv2.arcLength(cont, True)
# 多边形拟合
approx = cv2.approxPolyDP(cont, 0.1 * arc_len, True)
if (len(approx) == 4):
IS_FOUND = 1
M = cv2.moments(cont)
# 获取图像质心坐标
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.putText(rgb, obj_desire, (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 = []
for i in range(len(approx.tolist())):
if i == 0:
cv2.circle(rgb, (approx.tolist()[i][0][0],
approx.tolist()[i][0][1]), 5,
(20, 60, 220), -1)
print "first point x,y,others use clockwise---", approx.tolist()[i][0][0], \
approx.tolist()[i][0][1]
angular_point.append([
approx.tolist()[i][0][0],
approx.tolist()[i][0][1]
])
cv2.putText(rgb, str(i),
(approx.tolist()[i][0][0],
approx.tolist()[i][0][1]),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 0), 2)
else:
cv2.circle(rgb, (approx.tolist()[i][0][0],
approx.tolist()[i][0][1]), 5,
(0, 255, 0), -1)
print "x,y", approx.tolist(
)[i][0][0], approx.tolist()[i][0][1]
angular_point.append([
approx.tolist()[i][0][0],
approx.tolist()[i][0][1]
])
print "chr(i)", str(i)
cv2.putText(rgb, str(i),
(approx.tolist()[i][0][0],
approx.tolist()[i][0][1]),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 0), 2)
resultuv.append([[tile_id], [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)
print "all info for tile------", resultuv
print "Now tile id", tile_id
tile_uv.tile_id = tile_id
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)
else:
pass
# count += 1
# cnt += 11
return rgb.copy()
def process_rgb_image(self, rgb_image):
central_list = []
sumuv = []
minuv = []
minuvuv = []
##################
DELAY = 0.02
USE_CAM = 1
IS_FOUND = 0
count = 0 #count feature tile numbers
cnt = 0
MORPH = 7
CANNY = 250
##################
_width = 480.0
_height = 640.0
_margin = 0.0
error = 331 + 229
cornerssuv = []
uvuv = uv()
tile_uv = tileuv()
##################
corners = np.array([
[[_margin, _margin]],
[[_margin, _height + _margin]],
[[_width + _margin, _height + _margin]],
[[_width + _margin, _margin]],
])
pts_dst = np.array(corners, np.float32)
rgb = rgb_image
#print "rgb_image\n",rgb
t = 0
if rgb_image is not None:
#转化为灰度图
# time.sleep(5)
gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY)
#双边滤波
gray = cv2.bilateralFilter(gray, 1, 75, 75)
#获取边缘信息
# edges = cv2.Canny( gray, 10, CANNY )
edges = cv2.Canny(gray, 50, 150, apertureSize=3)
# corners = cv2.goodFeaturesToTrack(edges, 25, 0.01, 10)
# # 返回的结果是 [[ 311., 250.]] 两层括号的数组。
# corners = np.int0(corners)
# for i in corners:
# x, y = i.ravel()
# cv2.circle(rgb_image, (x, y), 3, 255, -1)
minLineLength = 30
maxLineGap = 10
lines = cv2.HoughLinesP(edges, 1, np.pi / 180, 80, minLineLength,
maxLineGap)
print lines[0]
for x1, y1, x2, y2 in lines[0]:
cv2.circle(rgb, (x1, y1), 7, (0, 255, 255), -1)
cv2.circle(rgb, (x2, y2), 7, (0, 255, 255), -1)
print "(x1,y1)", x1, y1
print "(x2,y2)", x2, y2
# cv2.line(rgb_image, (x1, y1), (x2, y2), (0, 255, 0), 2)
cv2.namedWindow('crossline_edges', cv2.WINDOW_NORMAL)
cv2.imshow('crossline_edges', edges)
cv2.namedWindow('central_frame', cv2.WINDOW_NORMAL)
cv2.imshow('central_frame', rgb)
cv2.waitKey(8)
# # 再将opencv格式额数据转换成ros image格式的数据发布
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(rgb_image, "bgr8"))
except CvBridgeError as e:
print e
return central_list
def process_rgb_image(self, rgb_image):
central_list = []
##################
DELAY = 0.02
USE_CAM = 1
IS_FOUND = 0
count = 0 #count feature tile numbers
cnt = 0
MORPH = 7
CANNY = 250
##################
_width = 480.0
_height = 640.0
_margin = 0.0
uvuv = uv()
tile_uv = tileuv()
##################
corners = np.array([
[[_margin, _margin]],
[[_margin, _height + _margin]],
[[_width + _margin, _height + _margin]],
[[_width + _margin, _margin]],
])
pts_dst = np.array(corners, np.float32)
rgb = rgb_image
#print "rgb_image\n",rgb
if rgb_image is not None:
#转化为灰度图
# time.sleep(5)
gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY)
#双边滤波
gray = cv2.bilateralFilter(gray, 1, 10, 120)
#获取边缘信息
edges = cv2.Canny(gray, 10, CANNY)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (MORPH, MORPH))
closed = cv2.morphologyEx(edges.copy(), cv2.MORPH_CLOSE, kernel)
_, contours, h = cv2.findContours(closed.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
#print "contours",contours
print "len(contours)", len(contours)
print "h", h
for cont in contours:
resultuv = [
] #1,num,2,centeral point 3,for angular point uv ,4,clockwise direction
if cv2.contourArea(cont) > 5000:
#print "cont", cont
#获取轮廓长度
arc_len = cv2.arcLength(cont, True)
#多边形拟合
approx = cv2.approxPolyDP(cont, 0.1 * arc_len, True)
if (len(approx) == 4):
IS_FOUND = 1
M = cv2.moments(cont)
#获取图像质心坐标
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.putText(rgb, chr(ord('o') - cnt), (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 = []
for i in range(len(approx.tolist())):
if i == 0:
cv2.circle(rgb, (approx.tolist()[i][0][0],
approx.tolist()[i][0][1]), 5,
(20, 60, 220), -1)
print "first point x,y,others use clockwise---", approx.tolist(
)[i][0][0], approx.tolist()[i][0][1]
angular_point.append([
approx.tolist()[i][0][0],
approx.tolist()[i][0][1]
])
else:
cv2.circle(rgb, (approx.tolist()[i][0][0],
approx.tolist()[i][0][1]), 5,
(0, 255, 0), -1)
print "x,y", approx.tolist(
)[i][0][0], approx.tolist()[i][0][1]
angular_point.append([
approx.tolist()[i][0][0],
approx.tolist()[i][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), 1)
print "all info for tile------", resultuv
tile_uv.tile_id = count
tile_uv.obj_desire = chr(ord('o') - cnt)
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)
else:
pass
count += 1
cnt += 11
#cv2.imshow( 'closed', closed )
#cv2.imshow( 'gray', gray )
cv2.namedWindow('tile_edges', cv2.WINDOW_NORMAL)
cv2.imshow('tile_edges', edges)
cv2.namedWindow('tile_pixel_frame', cv2.WINDOW_NORMAL)
cv2.imshow('tile_pixel_frame', rgb)
cv2.waitKey(8)
# # 再将opencv格式额数据转换成ros image格式的数据发布
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(rgb_image, "bgr8"))
except CvBridgeError as e:
print e
return central_list
def process_rgb_image(self, rgb_image):
central_list = []
sumuv = []
minuv = []
minuvuv = []
##################
DELAY = 0.02
USE_CAM = 1
IS_FOUND = 0
count = 0 #count feature tile numbers
cnt = 0
MORPH = 7
CANNY = 250
##################
_width = 480.0
_height = 640.0
_margin = 0.0
error = 331 + 229
cornerssuv = []
uvuv = uv()
tile_uv = tileuv()
##################
corners = np.array([
[[_margin, _margin]],
[[_margin, _height + _margin]],
[[_width + _margin, _height + _margin]],
[[_width + _margin, _margin]],
])
pts_dst = np.array(corners, np.float32)
rgb = rgb_image
#print "rgb_image\n",rgb
t = 0
if rgb_image is not None:
#转化为灰度图
# time.sleep(5)
gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY)
#双边滤波
gray = cv2.bilateralFilter(gray, 1, 5, 200)
#获取边缘信息
edges = cv2.Canny(gray, 10, CANNY)
corners = cv2.goodFeaturesToTrack(gray, 3, 0.01, 1) #
corners = np.int0(corners)
for i in corners:
#print "i-------",i
x, y = i.ravel()
cornerssuv.append([x, y])
#cv2.circle(rgb, (x, y), 7, 255, -1)
sumuv.append([t, x + y])
#print "corners",(x,y)
t += 1
for i in range(len(sumuv)):
minuv.append([i, abs(sumuv[i][1] - error)])
print "minuv----", minuv, minuvuv
for i in range(len(sumuv)):
uv_u = [minuv[0][1], minuv[1][1], minuv[2][1]]
ii = uv_u.index(min(uv_u))
if len(self.resuv) == 100:
self.resuv = self.resuv[1:]
self.resuv.append(cornerssuv[ii])
else:
self.resuv.append(cornerssuv[ii])
if len(self.resuv) != 0:
cv2.circle(rgb, (self.resuv[-1][0], self.resuv[-1][1]), 7,
(0, 255, 255), -1)
self.cross_pub.publish([self.resuv[-1][0], self.resuv[-1][1]])
else:
print "wait point ok----"
print "resuv----", self.resuv
print "sumuv", sumuv
print "len(resuv)", len(self.resuv)
print "cornerssuv", cornerssuv
cv2.namedWindow('crossline_edges', cv2.WINDOW_NORMAL)
cv2.imshow('crossline_edges', edges)
cv2.namedWindow('central_frame', cv2.WINDOW_NORMAL)
cv2.imshow('central_frame', rgb)
cv2.waitKey(8)
# # 再将opencv格式额数据转换成ros image格式的数据发布
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(rgb_image, "bgr8"))
except CvBridgeError as e:
print e
return central_list