def cb(msg):
    out_msg = ClusterPointIndices()
    out_msg.header = msg.header
    for _ in range(len(msg.polygons)):
        indices = PointIndices()
        out_msg.cluster_indices.append(indices)
    pub.publish(out_msg)
コード例 #2
0
 def indices_publisher(self, msg):
     msg_indices = ClusterPointIndices(header=msg.header)
     for rect in msg.rects:
         H = self.image.height
         W = self.image.width
         bbox = [rect.y, rect.x, rect.height + rect.y, rect.width + rect.x]
         indices = np.arange(H * W).reshape(H,
                                            W)[bbox[0]:bbox[2],
                                               bbox[1]:bbox[3]].reshape(-1)
         indices_msg = PointIndices(header=msg.header, indices=indices)
         msg_indices.cluster_indices.append(indices_msg)
     self.pub_indices.publish(msg_indices)
コード例 #3
0
def callback(box_array, cluster_indices):
    if len(box_array.boxes) > 0:
        min_index = 0
        min_distance = 100000
        for box, i in zip(box_array.boxes, range(len(box_array.boxes))):
            distance = math.sqrt(box.pose.position.x * box.pose.position.x +
                                 box.pose.position.y * box.pose.position.y +
                                 box.pose.position.z * box.pose.position.z)
            if min_distance > distance:
                min_distance = distance
                min_index = i
        result_box_array = BoundingBoxArray()
        result_box_array.header = box_array.header
        result_box_array.boxes.append(box_array.boxes[min_index])
        pub_box_array.publish(result_box_array)
        pub_indices.publish(cluster_indices.cluster_indices[min_index])
        result_cluster_indices = ClusterPointIndices()
        result_cluster_indices.header = cluster_indices.header
        result_cluster_indices.cluster_indices = [cluster_indices.cluster_indices[min_index]]
        pub_cluster_indices.publish(result_cluster_indices)
    else:
        rospy.logwarn("No bounding box input")
コード例 #4
0
def callback(box_array, cluster_indices):
    if len(box_array.boxes) > 0:
        min_index = 0
        min_distance = 100000
        for box, i in zip(box_array.boxes, range(len(box_array.boxes))):
            distance = math.sqrt(box.pose.position.x * box.pose.position.x +
                                 box.pose.position.y * box.pose.position.y +
                                 box.pose.position.z * box.pose.position.z)
            if min_distance > distance:
                min_distance = distance
                min_index = i
        result_box_array = BoundingBoxArray()
        result_box_array.header = box_array.header
        result_box_array.boxes.append(box_array.boxes[min_index])
        pub_box_array.publish(result_box_array)
        pub_indices.publish(cluster_indices.cluster_indices[min_index])
        result_cluster_indices = ClusterPointIndices()
        result_cluster_indices.header = cluster_indices.header
        result_cluster_indices.cluster_indices = [
            cluster_indices.cluster_indices[min_index]
        ]
        pub_cluster_indices.publish(result_cluster_indices)
    else:
        rospy.logwarn("No bounding box input")
コード例 #5
0
    def callback(self, imgmsg):
        bridge = cv_bridge.CvBridge()
        img = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='rgb8')
        img_chw = img.transpose(2, 0, 1)  # C, H, W

        bboxes, masks, labels, scores = self.model.predict([img_chw])
        bboxes = bboxes[0]
        masks = masks[0]
        labels = labels[0]
        scores = scores[0]

        msg_indices = ClusterPointIndices(header=imgmsg.header)
        msg_labels = LabelArray(header=imgmsg.header)
        # -1: label for background
        lbl_cls = -np.ones(img.shape[:2], dtype=np.int32)
        lbl_ins = -np.ones(img.shape[:2], dtype=np.int32)
        for ins_id, (mask, label) in enumerate(zip(masks, labels)):
            indices = np.where(mask.flatten())[0]
            indices_msg = PointIndices(header=imgmsg.header, indices=indices)
            msg_indices.cluster_indices.append(indices_msg)
            class_name = self.fg_class_names[label]
            msg_labels.labels.append(Label(id=label, name=class_name))
            lbl_cls[mask] = label
            lbl_ins[mask] = ins_id  # instance_id
        self.pub_indices.publish(msg_indices)
        self.pub_labels.publish(msg_labels)

        msg_lbl_cls = bridge.cv2_to_imgmsg(lbl_cls)
        msg_lbl_ins = bridge.cv2_to_imgmsg(lbl_ins)
        msg_lbl_cls.header = msg_lbl_ins.header = imgmsg.header
        self.pub_lbl_cls.publish(msg_lbl_cls)
        self.pub_lbl_ins.publish(msg_lbl_ins)

        if self.pub_viz.get_num_connections() > 0:
            n_fg_class = len(self.fg_class_names)
            captions = [
                '{:d}: {:s}'.format(l, self.fg_class_names[l]) for l in labels
            ]
            viz = chainer_mask_rcnn.utils.draw_instance_bboxes(
                img,
                bboxes,
                labels + 1,
                n_class=n_fg_class + 1,
                masks=masks,
                captions=captions)
            msg_viz = bridge.cv2_to_imgmsg(viz, encoding='rgb8')
            msg_viz.header = imgmsg.header
            self.pub_viz.publish(msg_viz)
コード例 #6
0
 def _convert(self, rects_msg, img_height=None, img_width=None):
     H = self.img_height if img_height is None else img_height
     W = self.img_width if img_width is None else img_width
     cpi_msg = ClusterPointIndices(header=rects_msg.header)
     for rect in rects_msg.rects:
         indices_msg = PointIndices(header=rects_msg.header)
         ymin = max(0, int(np.floor(rect.y)))
         xmin = max(0, int(np.floor(rect.x)))
         ymax = min(H, int(np.ceil(rect.y + rect.height)))
         xmax = min(W, int(np.ceil(rect.x + rect.width)))
         indices = [
             range(W * y + xmin, W * y + xmax) for y in range(ymin, ymax)
         ]
         indices_msg.indices = np.array(indices, dtype=np.int32).flatten()
         cpi_msg.cluster_indices.append(indices_msg)
     self.pub.publish(cpi_msg)
    def test_conversion(self):
        self.dynparam.update_configuration({'index': 1})

        cluster_indices_msg = ClusterPointIndices()
        while self.msg is None:
            cluster_indices_msg.header.stamp = rospy.Time.now()
            for indices in self.cluster_indices:
                cluster_indices_msg.cluster_indices.append(
                    PointIndices(
                        header=cluster_indices_msg.header,
                        indices=indices,
                    )
                )
            self.pub.publish(cluster_indices_msg)
            rospy.sleep(0.1)

        self.assertTupleEqual(self.msg.indices, self.cluster_indices[1])
コード例 #8
0
    def image_cb(self, msg):
        if self.profiling:
            rospy.loginfo(
                "callback start: incomming msg is %s msec behind" %
                ((rospy.Time.now() - msg.header.stamp).to_sec() * 1000.0))
        tprev = time.time()
        try:
            # transform image to RGB, float, CHW
            img = self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8")
            img = np.asarray(img, dtype=np.float32)
            img = img.transpose(2, 0, 1)  # (H, W, C) -> (C, H, W)
        except Exception as e:
            rospy.logerr("Failed to convert image: %s" % str(e))
            return
        if self.profiling:
            tcur = time.time()
            rospy.loginfo("%s: elapsed %f msec" % ("convert",
                                                   (tcur - tprev) * 1000))
            tprev = tcur

        if self.gpu >= 0:
            chainer.cuda.get_device_from_id(self.gpu).use()
        bboxes, labels, scores = self.model.predict([img])
        bboxes, labels, scores = bboxes[0], labels[0], scores[0]

        if self.profiling:
            tcur = time.time()
            rospy.loginfo("%s: elapsed %f msec" % ("predict",
                                                   (tcur - tprev) * 1000))
            tprev = tcur

        labels_msg = LabelArray(header=msg.header)
        for l in labels:
            l_name = self.label_names[l]
            labels_msg.labels.append(Label(id=l, name=l_name))

        if self.profiling:
            tcur = time.time()
            rospy.loginfo("%s: elapsed %f msec" % ("make labels msg",
                                                   (tcur - tprev) * 1000))

        cluster_indices_msg = ClusterPointIndices(header=msg.header)
        H = img.shape[1]
        W = img.shape[2]
        for bbox in bboxes:
            ymin = max(0, int(np.floor(bbox[0])))
            xmin = max(0, int(np.floor(bbox[1])))
            ymax = min(H, int(np.ceil(bbox[2])))
            xmax = min(W, int(np.ceil(bbox[3])))
            indices = [
                range(W * y + xmin, W * y + xmax) for y in range(ymin, ymax)
            ]
            indices = np.array(indices, dtype=np.int32).flatten()
            indices_msg = PointIndices(header=msg.header, indices=indices)
            cluster_indices_msg.cluster_indices.append(indices_msg)

        if self.profiling:
            tcur = time.time()
            rospy.loginfo("%s: elapsed %f msec" % ("make cluster_indices msg",
                                                   (tcur - tprev) * 1000))
            tprev = tcur

        rect_msg = RectArray(header=msg.header)
        for bbox in bboxes:
            rect = Rect(x=bbox[1],
                        y=bbox[0],
                        width=bbox[3] - bbox[1],
                        height=bbox[2] - bbox[0])
            rect_msg.rects.append(rect)

        if self.profiling:
            tcur = time.time()
            rospy.loginfo("%s: elapsed %f msec" % ("make rect msg",
                                                   (tcur - tprev) * 1000))
            tprev = tcur

        cls_msg = ClassificationResult(
            header=msg.header,
            classifier=self.classifier_name,
            target_names=self.label_names,
            labels=labels,
            label_names=[self.label_names[l] for l in labels],
            label_proba=scores,
        )

        if self.profiling:
            tcur = time.time()
            rospy.loginfo("%s: elapsed %f msec" % ("make cls msg",
                                                   (tcur - tprev) * 1000))
            tprev = tcur

        self.pub_labels.publish(labels_msg)
        self.pub_indices.publish(cluster_indices_msg)
        self.pub_rects.publish(rect_msg)
        self.pub_class.publish(cls_msg)

        if self.profiling:
            tcur = time.time()
            rospy.loginfo("%s: elapsed %f msec" % ("publish msg",
                                                   (tcur - tprev) * 1000))
            tprev = tcur

        if self.visualize:
            self.publish_bbox_image(img, bboxes, labels, scores, msg.header)

        if self.profiling:
            tcur = time.time()
            rospy.loginfo("%s: elapsed %f msec" % ("callback end",
                                                   (tcur - tprev) * 1000))
            tprev = tcur
コード例 #9
0
    def callback(self, imgmsg):
        bridge = cv_bridge.CvBridge()
        img = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='rgb8')
        img_chw = img.transpose(2, 0, 1)  # C, H, W

        if self.gpu >= 0:
            chainer.cuda.get_device_from_id(self.gpu).use()
        bboxes, masks, labels, scores = self.model.predict([img_chw])

        bboxes = bboxes[0]
        masks = masks[0]
        labels = labels[0]
        scores = scores[0]

        msg_indices = ClusterPointIndices(header=imgmsg.header)
        msg_labels = LabelArray(header=imgmsg.header)
        # -1: label for background
        lbl_cls = -np.ones(img.shape[:2], dtype=np.int32)
        lbl_ins = -np.ones(img.shape[:2], dtype=np.int32)
        for ins_id, (mask, label) in enumerate(zip(masks, labels)):
            indices = np.where(mask.flatten())[0]
            indices_msg = PointIndices(header=imgmsg.header, indices=indices)
            msg_indices.cluster_indices.append(indices_msg)
            class_name = self.fg_class_names[label]
            msg_labels.labels.append(Label(id=label, name=class_name))
            lbl_cls[mask] = label
            lbl_ins[mask] = ins_id  # instance_id
        self.pub_indices.publish(msg_indices)
        self.pub_labels.publish(msg_labels)

        msg_lbl_cls = bridge.cv2_to_imgmsg(lbl_cls)
        msg_lbl_ins = bridge.cv2_to_imgmsg(lbl_ins)
        msg_lbl_cls.header = msg_lbl_ins.header = imgmsg.header
        self.pub_lbl_cls.publish(msg_lbl_cls)
        self.pub_lbl_ins.publish(msg_lbl_ins)

        cls_msg = ClassificationResult(
            header=imgmsg.header,
            classifier=self.classifier_name,
            target_names=self.fg_class_names,
            labels=labels,
            label_names=[self.fg_class_names[l] for l in labels],
            label_proba=scores,
        )

        rects_msg = RectArray(header=imgmsg.header)
        for bbox in bboxes:
            rect = Rect(x=bbox[1],
                        y=bbox[0],
                        width=bbox[3] - bbox[1],
                        height=bbox[2] - bbox[0])
            rects_msg.rects.append(rect)
        self.pub_rects.publish(rects_msg)
        self.pub_class.publish(cls_msg)

        if self.pub_viz.get_num_connections() > 0:
            n_fg_class = len(self.fg_class_names)
            captions = [
                '{:d}: {:s}'.format(l, self.fg_class_names[l]) for l in labels
            ]
            viz = chainer_mask_rcnn.utils.draw_instance_bboxes(
                img,
                bboxes,
                labels + 1,
                n_class=n_fg_class + 1,
                masks=masks,
                captions=captions)
            msg_viz = bridge.cv2_to_imgmsg(viz, encoding='rgb8')
            msg_viz.header = imgmsg.header
            self.pub_viz.publish(msg_viz)
コード例 #10
0
 def _decompose(self, *cpi_msgs):
     out_msg = ClusterPointIndices()
     out_msg.header = cpi_msgs[0].header
     for cpi_msg in cpi_msgs:
         out_msg.cluster_indices.extend(cpi_msg.cluster_indices)
     self._pub.publish(out_msg)