3 from __future__
import print_function
8 import itertools, pkg_resources
9 from distutils.version
import LooseVersion
10 if LooseVersion(pkg_resources.get_distribution(
"chainer").version) >= LooseVersion(
'7.0.0')
and \
11 sys.version_info.major == 2:
12 print(
'''Please install chainer < 7.0.0: 14 sudo pip install chainer==6.7.0 16 c.f https://github.com/jsk-ros-pkg/jsk_recognition/pull/2485 19 if [p
for p
in list(itertools.chain(*[pkg_resources.find_distributions(_)
for _
in sys.path]))
if "cupy-" in p.project_name] == []:
20 print(
'''Please install CuPy 22 sudo pip install cupy-cuda[your cuda version] 24 sudo pip install cupy-cuda91 29 from chainer
import cuda
30 import chainer.serializers
as S
31 from chainer
import Variable
33 from distutils.version
import LooseVersion
37 from dynamic_reconfigure.server
import Server
38 from jsk_perception.cfg
import FastRCNNConfig
as Config
39 from jsk_recognition_msgs.msg
import Rect, RectArray
40 from jsk_recognition_msgs.msg
import ClassificationResult
41 import jsk_recognition_utils
44 from jsk_recognition_utils.nms
import nms
45 from jsk_topic_tools
import ConnectionBasedTransport
46 import message_filters
49 from sensor_msgs.msg
import Image
53 img = orig_img.astype(np.float32, copy=
True)
55 im_size_min = np.min(img.shape[0:2])
56 im_size_max = np.max(img.shape[0:2])
57 im_scale = float(scale) / float(im_size_min)
58 if np.rint(im_scale * im_size_max) > max_size:
59 im_scale = float(max_size) / float(im_size_max)
60 img = cv2.resize(img,
None,
None, fx=im_scale, fy=im_scale,
61 interpolation=cv2.INTER_LINEAR)
62 return img.transpose([2, 0, 1]).astype(np.float32), im_scale
67 def __init__(self, model, target_names, pixel_means, use_gpu):
74 RectArray, queue_size=1)
76 ClassificationResult, queue_size=1)
91 use_async = rospy.get_param(
'~approximate_sync',
False)
92 queue_size = rospy.get_param(
'~queue_size', 100)
95 slop = rospy.get_param(
'~slop', 0.1)
96 sync = message_filters.ApproximateTimeSynchronizer(
97 subs, queue_size, slop)
100 sync.registerCallback(self.
_detect)
103 self._sub.unregister()
104 self._sub_rects.unregister()
107 bridge = cv_bridge.CvBridge()
108 im_orig = bridge.imgmsg_to_cv2(imgmsg, desired_encoding=
'bgr8')
111 if len(rects_orig) == 0:
113 rects = rects_orig * im_scale
114 scores, bbox_pred = self.
_im_detect(im, rects)
116 rects = RectArray(header=imgmsg.header)
120 _cls = scores[:, cls_id][:, np.newaxis]
121 _bbx = bbox_pred[:, cls_id * 4: (cls_id + 1) * 4]
122 dets = np.hstack((_bbx, _cls))
125 orig_rects = cuda.cupy.asnumpy(rects_orig)[keep, :]
127 inds = np.where(dets[:, -1] >= self.
conf_thresh)[0]
131 x1, y1, x2, y2 = orig_rects[i]
134 center_x = x1 + 0.5 * width
135 center_y = y1 + 0.5 * height
137 dx, dy, dw, dh = _bbox
138 _center_x = dx * width + center_x
139 _center_y = dy * height + center_y
140 _width = np.exp(dw) * width
141 _height = np.exp(dh) * height
143 x1 = _center_x - 0.5 * _width
144 y1 = _center_y - 0.5 * _height
145 x2 = _center_x + 0.5 * _width
146 y2 = _center_y + 0.5 * _height
147 rect = Rect(x=x1, y=y1, width=x2-x1, height=y2-y1)
148 rects.rects.append(rect)
149 labels.append(cls_id)
150 label_proba.append(dets[:, -1][i])
153 clss = ClassificationResult(
154 header=imgmsg.header,
159 label_proba=label_proba,
161 self._pub_rects.publish(rects)
162 self._pub_class.publish(clss)
165 xp = cuda.cupy
if self.
use_gpu else np
167 rects = xp.asarray(rects)
168 x_data = im[xp.newaxis, :, :, :]
170 batch_indices = xp.zeros((len(rects), 1), dtype=np.float32)
171 rects = xp.hstack((batch_indices, rects))
172 if LooseVersion(chainer.__version__).version[0] < 2:
173 x = Variable(x_data, volatile=
True)
174 rects_val = Variable(rects, volatile=
True)
175 self.model.train =
False 176 cls_score, bbox_pred = self.
model(x, rects_val)
178 with chainer.using_config(
'train',
False), \
179 chainer.no_backprop_mode():
181 rects_val = Variable(rects)
182 cls_score, bbox_pred = self.
model(x, rects_val)
184 scores = cuda.to_cpu(cls_score.data)
185 bbox_pred = cuda.to_cpu(bbox_pred.data)
186 return scores, bbox_pred
190 rospy.init_node(
'fast_rcnn_caffenet')
194 model_name = rospy.get_param(
'~model')
195 except KeyError
as e:
196 rospy.logerr(
'Unspecified rosparam: {0}'.format(e))
199 gpu = rospy.get_param(
'~gpu', -1)
200 use_gpu =
True if gpu >= 0
else False 203 PKG =
'jsk_perception' 204 rp = rospkg.RosPack()
205 data_path = osp.join(rp.get_path(PKG),
'trained_data')
206 if model_name ==
'vgg_cnn_m_1024':
207 model = VGG_CNN_M_1024()
208 chainermodel = osp.join(data_path,
'vgg_cnn_m_1024.chainermodel')
209 elif model_name ==
'vgg16':
210 model = VGG16FastRCNN()
211 chainermodel = osp.join(data_path,
'vgg16_fast_rcnn.chainermodel')
213 rospy.logerr(
'Unsupported model: {0}'.format(model_name))
215 rospy.loginfo(
'Loading chainermodel')
216 S.load_hdf5(chainermodel, model)
219 rospy.loginfo(
'Finished loading chainermodel')
245 pixel_means = [102.9801, 115.9465, 122.7717]
248 model=model, target_names=target_names,
249 pixel_means=pixel_means, use_gpu=use_gpu)
253 if __name__ ==
'__main__':
def img_preprocessing(orig_img, pixel_means, max_size=1000, scale=600)
def _im_detect(self, im, rects)
def _detect(self, imgmsg, rects_msg)
def __init__(self, model, target_names, pixel_means, use_gpu)
def configCallback(self, config, level)