def construct_abstract_scene(self, scene_msg):
scene = Scene(3)
for i, (obj, name) in enumerate(zip(scene_msg.bboxes,
scene_msg.names)):
BB = BoundingBox(
[point_to_vec(obj.points[0]),
point_to_vec(obj.points[1])])
polygon, _ = self.get_image_polygon(obj)
if name == 'table':
l = Landmark(name, RectangleRepresentation(BB), None,
ObjectClass.TABLE)
else:
for center, color, category in zip(
self.object_centers_msg.centers,
self.object_centers_msg.color_labels,
self.object_centers_msg.category_labels):
if polygon.contains_point(
Vec2(center.pixel.x, center.pixel.y)):
l = Landmark(
name,
RectangleRepresentation(BB, landmarks_to_get=[]),
None, category.upper(), color.upper())
l.representation.alt_representations = []
scene_msg.colors[i] = color.upper()
scene_msg.categories[i] = category.upper()
break
scene.add_landmark(l)
self.scene_lmk_to_bbox[l] = obj
self.scene_pub.publish(scene_msg)
rospy.loginfo('Constructed a scene')
return scene
def handle_scene(self, msg):
def get_image_polygon(obj):
minps = PointStamped()
minps.header.stamp = rospy.Time(0)
minps.header.frame_id = 'base_footprint'
minps.point.x = obj.points[0].y
minps.point.y = -obj.points[0].x
minps.point.z = obj.points[0].z
minp = self.tf.transformPoint('kinect_rgb_optical_frame', minps).point
minps.point.x = obj.points[1].y
minps.point.y = -obj.points[1].x
minps.point.z = obj.points[1].z
maxp = self.tf.transformPoint('kinect_rgb_optical_frame', minps).point
ct = np.array( [(minp.x,minp.y,minp.z), (maxp.x,minp.y,minp.z), (maxp.x,maxp.y,maxp.z), (minp.x,maxp.y,maxp.z)] )
corners = cv.fromarray(ct)
out = cv.fromarray(np.zeros((4,2)))
cam_mat = np.array( [(525, 0, 319.5), (0, 525, 239.5), (0, 0, 1)] )
cv.ProjectPoints2(corners,
cv.fromarray(np.zeros((1,3))),
cv.fromarray(np.zeros((1,3))),
cv.fromarray(cam_mat),
cv.fromarray(np.zeros((1,4))),
out)
vs = []
for l in np.asarray(out):
vs.append( Vec2(*l) )
return Polygon(vs)
obj_centers = rospy.wait_for_message('/object_centers', ObjectCenters)
scene = Scene(3)
for obj,name in zip(msg.bboxes, msg.names):
BB = BoundingBox([point_to_vec(obj.points[0]), point_to_vec(obj.points[1])])
polygon = get_image_polygon(obj)
if name == 'table':
l = Landmark(name,
RectangleRepresentation( BB ),
None,
ObjectClass.TABLE)
else:
for center,color,category in zip(obj_centers.centers, obj_centers.color_labels, obj_centers.category_labels):
if polygon.contains_point( Vec2(center.pixel.x,center.pixel.y) ):
l = Landmark(name,
RectangleRepresentation( BB, landmarks_to_get=[] ),
None,
category.upper(),
color.upper())
l.representation.alt_representations= []
break
scene.add_landmark(l)
self.last_scene = scene
rospy.loginfo('Constructed a scene')