add resize and timer_rate option in tile_image to reduce cpu load

jsk_perception/node_scripts/tile_image.py

@@ -53,6 +53,8 @@ def __init__(self):
             rospy.logerr('need to specify input_topics')
             sys.exit(1)
         self._shape = rospy.get_param('~shape', None)
+        self._resize = rospy.get_param('~resize', False)
+        self._timer_rate = rospy.get_param('~timer_rate', 10.0)
         if self._shape:
             if not (isinstance(self._shape, collections_Sequence) and
                     len(self._shape) == 2):
@@ -79,7 +81,7 @@ def subscribe(self):
         if self.no_sync:
             self.input_imgs = {}
             self.sub_img_list = [rospy.Subscriber(topic, Image, self.simple_callback(topic), queue_size=1) for topic in self.input_topics]
-            rospy.Timer(rospy.Duration(0.1), self.timer_callback)
+            rospy.Timer(rospy.Duration(1.0 / self._timer_rate), self.timer_callback)
         else:
             queue_size = rospy.get_param('~queue_size', 10)
             slop = rospy.get_param('~slop', 1)
@@ -121,7 +123,20 @@ def _append_images(self, imgs):
             return
         # convert tile shape: (Y, X) -> (X, Y)
         # if None, shape is automatically decided to be square AMAP.
-        shape_xy = self._shape[::-1] if self._shape else None
+        if self._shape is None:
+            shape_xy = get_tile_shape(len(imgs))
+        else:
+            shape_xy = self._shape[::-1]
+        # resize to shrink output image
+        if self._resize:
+            resized_imgs = []
+            for img in imgs:
+                w, h = img.shape[:2][::-1]
+                resized_w = int(w / max(shape_xy))
+                resized_h = int(h / max(shape_xy))
+                resized_imgs.append(cv2.resize(img, (resized_w, resized_h)))
+            imgs = resized_imgs
+
         if self.cache_img is None:
             out_bgr = jsk_recognition_utils.get_tile_image(
                 imgs, tile_shape=shape_xy)
@@ -134,7 +149,6 @@ def _append_images(self, imgs):
                 out_bgr = jsk_recognition_utils.get_tile_image(
                     imgs, tile_shape=shape_xy)
                 self.cache_img = out_bgr
-
         encoding = 'bgr8'
         stamp = rospy.Time.now()
         if self.pub_img.get_num_connections() > 0:

jsk_recognition_utils/python/jsk_recognition_utils/__init__.py

@@ -73,6 +73,7 @@
 
 centerize = visualize.centerize
 colorize_cluster_indices = visualize.colorize_cluster_indices
+get_tile_shape = visualize.get_tile_shape
 get_tile_image = visualize.get_tile_image
 
 get_overlap_of_aabb = geometry.get_overlap_of_aabb

jsk_recognition_utils/python/jsk_recognition_utils/visualize.py

@@ -62,6 +62,14 @@ def _tile_images(imgs, tile_shape, concatenated_image, margin_color=None):
     return concatenated_image
 
 
+def get_tile_shape(img_num):
+    x_num = 0
+    y_num = int(math.sqrt(img_num))
+    while x_num * y_num < img_num:
+        x_num += 1
+    return x_num, y_num
+
+
 def get_tile_image(imgs, tile_shape=None, result_img=None, margin_color=None,
                    min_size=50):
     """Concatenate images whose sizes are different.
@@ -70,13 +78,6 @@ def get_tile_image(imgs, tile_shape=None, result_img=None, margin_color=None,
     @param tile_shape: shape for which images should be concatenated
     @param result_img: numpy array to put result image
     """
-    def get_tile_shape(img_num):
-        x_num = 0
-        y_num = int(math.sqrt(img_num))
-        while x_num * y_num < img_num:
-            x_num += 1
-        return x_num, y_num
-
     imgs = [img for img in imgs if img is not None]
     if tile_shape is None:
         tile_shape = get_tile_shape(len(imgs))