def _recognize(self, imgmsg, mask_msg=None):
bridge = cv_bridge.CvBridge()
bgr = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='bgr8')
if mask_msg is not None:
mask = bridge.imgmsg_to_cv2(mask_msg)
if mask.shape != bgr.shape[:2]:
logerr_throttle(10,
'Size of input image and mask is different')
return
elif mask.size == 0:
logerr_throttle(10, 'Size of input mask is 0')
return
bgr[mask < 128] = self.mean_bgr
input_msg = bridge.cv2_to_imgmsg(bgr.astype(np.uint8), encoding='bgr8')
input_msg.header = imgmsg.header
self.pub_input.publish(input_msg)
blob = (bgr - self.mean_bgr).transpose((2, 0, 1))
x_data = np.array([blob], dtype=np.float32)
if self.gpu != -1:
x_data = chainer.cuda.to_gpu(x_data, device=self.gpu)
x = Variable(x_data, volatile=True)
self.model.train = False
self.model(x)
proba = chainer.cuda.to_cpu(self.model.proba.data)[0]
cls_msg = ClassificationResult(header=imgmsg.header,
labels=None,
label_names=None,
label_proba=None,
probabilities=proba,
target_names=self.target_names)
self.pub.publish(cls_msg)
def _convert(self, msg):
polygon_msg = PolygonStamped()
polygon_msg.header = msg.header
if self.index < 0:
return
elif self.index < len(msg.polygons):
polygon_msg = msg.polygons[self.index]
self.pub.publish(polygon_msg)
else:
logerr_throttle(
10, 'Invalid index {} is specified '
'for polygon array whose size is {}'.format(
self.index, len(msg.polygons)))
def _convert(self, msg):
pose_msg = PoseStamped()
pose_msg.header = msg.header
if self.index < 0:
return
elif self.index < len(msg.poses):
pose_msg.pose = msg.poses[self.index]
self.pub.publish(pose_msg)
else:
logerr_throttle(10,
'Invalid index {} is specified '
'for pose array whose size is {}'
.format(self.index, len(msg.poses)))
def _convert(self, msg):
polygon_msg = PolygonStamped()
polygon_msg.header = msg.header
if self.index < 0:
return
elif self.index < len(msg.polygons):
polygon_msg = msg.polygons[self.index]
self.pub.publish(polygon_msg)
else:
logerr_throttle(
10,
"Invalid index {} is specified "
"for polygon array whose size is {}".format(self.index, len(msg.polygons)),
)
def _recognize(self, imgmsg, mask_msg=None):
bridge = cv_bridge.CvBridge()
bgr = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='bgr8')
if mask_msg is not None:
mask = bridge.imgmsg_to_cv2(mask_msg)
if mask.shape != bgr.shape[:2]:
logerr_throttle(10,
'Size of input image and mask is different')
return
elif mask.size == 0:
logerr_throttle(10, 'Size of input mask is 0')
return
bgr[mask == 0] = self.mean_bgr
bgr = skimage.transform.resize(bgr, (self.insize, self.insize),
preserve_range=True)
input_msg = bridge.cv2_to_imgmsg(bgr.astype(np.uint8), encoding='bgr8')
input_msg.header = imgmsg.header
self.pub_input.publish(input_msg)
blob = (bgr - self.mean_bgr).transpose((2, 0, 1))
x_data = np.array([blob], dtype=np.float32)
if self.gpu != -1:
x_data = cuda.to_gpu(x_data, device=self.gpu)
if LooseVersion(chainer.__version__) < LooseVersion('2.0.0'):
x = Variable(x_data, volatile=True)
self.model.train = False
self.model(x)
else:
with chainer.using_config('train', False), \
chainer.no_backprop_mode():
x = Variable(x_data)
self.model(x)
proba = cuda.to_cpu(self.model.pred.data)[0]
label = np.argmax(proba)
label_name = self.target_names[label]
label_proba = proba[label]
cls_msg = ClassificationResult(
header=imgmsg.header,
labels=[label],
label_names=[label_name],
label_proba=[label_proba],
probabilities=proba,
classifier=self.model_name,
target_names=self.target_names,
)
self.pub.publish(cls_msg)
def _recognize(self, imgmsg, mask_msg=None):
bridge = cv_bridge.CvBridge()
bgr = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='bgr8')
if mask_msg is not None:
mask = bridge.imgmsg_to_cv2(mask_msg)
if mask.shape != bgr.shape[:2]:
logerr_throttle(10,
'Size of input image and mask is different')
return
elif mask.size == 0:
logerr_throttle(10, 'Size of input mask is 0')
return
bgr[mask == 0] = self.mean_bgr
bgr = skimage.transform.resize(
bgr, (self.insize, self.insize), preserve_range=True)
input_msg = bridge.cv2_to_imgmsg(bgr.astype(np.uint8), encoding='bgr8')
input_msg.header = imgmsg.header
self.pub_input.publish(input_msg)
blob = (bgr - self.mean_bgr).transpose((2, 0, 1))
x_data = np.array([blob], dtype=np.float32)
if self.gpu != -1:
x_data = cuda.to_gpu(x_data, device=self.gpu)
if LooseVersion(chainer.__version__) < LooseVersion('2.0.0'):
x = Variable(x_data, volatile=True)
self.model.train = False
self.model(x)
else:
with chainer.using_config('train', False), \
chainer.no_backprop_mode():
x = Variable(x_data)
self.model(x)
proba = cuda.to_cpu(self.model.pred.data)[0]
label = np.argmax(proba)
label_name = self.target_names[label]
label_proba = proba[label]
cls_msg = ClassificationResult(
header=imgmsg.header,
labels=[label],
label_names=[label_name],
label_proba=[label_proba],
probabilities=proba,
classifier=self.model_name,
target_names=self.target_names,
)
self.pub.publish(cls_msg)
def callback(self, img_input, depth_input, cam_info, mode):
assert mode in ['rgb', 'label']
# From tf, generate camera pose w.r.t heightmap_frame
try:
trans, rot \
= self.listener.lookupTransform(self.heightmap_frame,
img_input.header.frame_id,
rospy.Time(0))
except Exception as e:
logerr_throttle(10, e)
return
cam_pose = self.tft.fromTranslationRotation(trans, rot)
# Generate other data
cam_intrinsics = np.array(cam_info.K).reshape(3, 3)
if mode == 'rgb':
color_img = self.br.imgmsg_to_cv2(
img_input, desired_encoding='rgb8'
)
color_img = color_img.astype(float) / 255 # Convert to range [0,1]
label_img = np.zeros(
(color_img.shape[0], color_img.shape[1]), dtype=np.int32
)
else:
label_img = self.br.imgmsg_to_cv2(
img_input, desired_encoding='passthrough'
)
# this should be filled by 1 for bg subtraction in get_heightmap
color_img = np.ones(
(label_img.shape[0], label_img.shape[1], 3), dtype=float
)
depth_img = self.br.imgmsg_to_cv2(
depth_input, desired_encoding='passthrough'
)
# Replace nan element to zero
depth_img = np.where(np.isnan(depth_img), 0, depth_img)
if depth_input.encoding == '16UC1':
depth_img = depth_img.astype(float) / 1000.0 # Convert mm to m
elif depth_input.encoding != '32FC1':
enc = depth_input.encoding
logerr_throttle(10, 'Unsupported depth encoding: %s' % enc)
return
# Generate heightmap w.r.t heightmap_frame
heightmap_color, heightmap, missing_heightmap, heightmap_label \
= grasp_fusion.utils.get_heightmap(
color_img=color_img,
depth_img=depth_img,
bg_color_img=np.zeros_like(color_img),
bg_depth_img=np.zeros_like(depth_img),
cam_intrinsics=cam_intrinsics,
cam_pose=cam_pose,
grid_origin=np.array([0, 0, 0]),
grid_rot=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
suction_img=label_img,
voxel_size=self.voxel_size,
suction_cval=self._bg_label,
)
color_data, depth_data \
= grasp_fusion.utils.heightmap_postprocess(
heightmap_color,
heightmap,
missing_heightmap,
)
# it is scaled in postprocess
depth_data = (depth_data / 10000.).astype(np.float32)
heightmap_label = heightmap_label.reshape(
heightmap.shape[0], heightmap.shape[1],
)
# Consider pixels whose height is 0 as background
heightmap_label[heightmap == 0] = self._bg_label
label_data = np.full((224, 320), self._bg_label, dtype=label_img.dtype)
label_data[12:212, 10:310] = heightmap_label
# For debug
# depth = grasp_fusion_lib.image.colorize_depth(depth_data,
# min_value=0, max_value=1.5)
# viz = grasp_fusion_lib.image.tile([color_data, depth], (1, 2))
# grasp_fusion_lib.io.imshow(viz)
# grasp_fusion_lib.io.waitkey()
if mode == 'rgb':
rgb_output = self.br.cv2_to_imgmsg(color_data, encoding='rgb8')
rgb_output.header = img_input.header
self.pub_rgb.publish(rgb_output)
else:
assert mode == 'label'
label_output = self.br.cv2_to_imgmsg(label_data)
label_output.header = img_input.header
self.pub_label.publish(label_output)
depth_output = self.br.cv2_to_imgmsg(
depth_data, encoding='passthrough'
)
depth_output.header = img_input.header
self.pub_depth.publish(depth_output)
