Esempio n. 1
0
 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
Esempio n. 2
0
    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")
 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')