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)