def convert_to_ros_bounding_box(bounding_box): ros_bb = RegionOfInterest() ros_bb.x_offset = bounding_box['x'] ros_bb.y_offset = bounding_box['y'] ros_bb.width = bounding_box['width'] ros_bb.height = bounding_box['height'] return ros_bb
def publish(self, boxes, scores, classes, num, category_index, masks=None, fps=0): # init detection message msg = Detection() for i in range(boxes.shape[0]): if scores[i] > 0.5: if classes[i] in category_index.keys(): class_name = category_index[classes[i]]['name'] else: class_name = 'N/A' ymin, xmin, ymax, xmax = tuple(boxes[i].tolist()) box = RegionOfInterest() box.x_offset = xmin + (xmax-xmin)/2.0 box.y_offset = ymin + (ymax-ymin)/2.0 box.height = ymax - ymin box.width = xmax - xmin # fill detection message with objects obj = Object() obj.box = box obj.class_name = class_name obj.score = int(100*scores[i]) if masks is not None: obj.mask = self._bridge.cv2_to_imgmsg(masks[i], encoding="passthrough") msg.objects.append(obj) msg.fps = fps # publish detection message self.DetPub.publish(msg)
def camera_info(self): msg_header = Header() msg_header.frame_id = "f450/robot_camera_down" msg_roi = RegionOfInterest() msg_roi.x_offset = 0 msg_roi.y_offset = 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify = 0 msg = CameraInfo() msg.header = msg_header msg.height = 480 msg.width = 640 msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [ 1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0 ] msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg
def publish_roi(self): if not self.drag_start: if self.track_box is not None: roi_box = self.track_box elif self.detect_box is not None: roi_box = self.detect_box else: return try: roi_box = self.cvBox2D_to_cvRect(roi_box) except: return # Watch out for negative offsets roi_box[0] = max(0, roi_box[0]) roi_box[1] = max(0, roi_box[1]) try: ROI = RegionOfInterest() ROI.x_offset = int(roi_box[0]) ROI.y_offset = int(roi_box[1]) ROI.width = int(roi_box[2]) ROI.height = int(roi_box[3]) self.roi_pub.publish(ROI) except: rospy.loginfo("Publishing ROI failed")
def rosmsg(self): """:obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg """ msg_header = Header() msg_header.frame_id = self._frame msg_roi = RegionOfInterest() msg_roi.x_offset = 0 msg_roi.y_offset = 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify = 0 msg = CameraInfo() msg.header = msg_header msg.height = self._height msg.width = self._width msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [ self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0 ] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [ self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0, 0.0, 1.0, 0.0 ] msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg
def face_detect_cb(self, event): if self.image is not None and self.lock.acquire(False): cnn_results = self.recogniser.detect_faces(self.image, scale=self.cnn_scale) features = Features() features.features = [] for k, d in enumerate(cnn_results): padding = int(self.image.shape[0] * self.cnn_padding) feature = Feature() roi = RegionOfInterest() roi.x_offset = np.maximum(d.left() - padding, 0) roi.y_offset = np.maximum(d.top() - padding, 0) roi.height = np.minimum(d.bottom() - roi.y_offset + padding, self.image.shape[0]) roi.width = np.minimum(d.right() - roi.x_offset + padding, self.image.shape[1]) feature.roi = roi feature.crop = self.bridge.cv2_to_imgmsg( np.array( self.image[roi.y_offset:roi.y_offset + roi.height, roi.x_offset:roi.x_offset + roi.width, :])) features.features.append(feature) self.lock.release() self.faces_pub.publish(features)
def getResult(predictions, classes): boxes = predictions.pred_boxes if predictions.has("pred_boxes") else None if predictions.has("pred_masks"): masks = np.asarray(predictions.pred_masks) #print(type(masks)) else: return result_msg = Result() # result_msg.header = self._header result_msg.class_ids = predictions.pred_classes if predictions.has( "pred_classes") else None result_msg.class_names = np.array(classes)[result_msg.class_ids.numpy()] result_msg.scores = predictions.scores if predictions.has( "scores") else None for i, (x1, y1, x2, y2) in enumerate(boxes): mask = np.zeros(masks[i].shape, dtype="uint8") mask[masks[i, :, :]] = 255 mask = br.cv2_to_imgmsg(mask) result_msg.masks.append(mask) box = RegionOfInterest() box.x_offset = np.uint32(x1) box.y_offset = np.uint32(y1) box.height = np.uint32(y2 - y1) box.width = np.uint32(x2 - x1) result_msg.boxes.append(box) return result_msg
def imgCallback(self, image): num = 0 try: cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8") except CvBridgeError as e: print(e) # 如果存在人 roi = RegionOfInterest() try: position = self.detectWave(cv_image, self.check_gender) cv2.rectangle(cv_image, (position[1], position[3]), (position[0], position[2]), (0, 0, 255)) roi.x_offset = position[0] roi.y_offset = position[2] roi.width = position[1] - position[0] roi.height = position[3] - position[2] self.roi = roi self.roi_pub.publish(roi) if self.ispub == False: stringq = "I can tell one wave. Now I will recognize people. " self.word_pub.publish(stringq) stringq = "ok " self.face_pub.publish(stringq) self.ispub = True rospy.loginfo("One Wave!") num = 1 except: self.roi = roi if num == 0: self.roi_pub.publish(roi) cv_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") self.img_pub.publish(cv_image)
def callback(image_msg): global pub_Image global pub_BoxesImage global pub_Boxes global face_cascade global bridge global boxes_msg global faces global local_position_pub global goal_pose # Convert ros image into a cv2 image cv_img = bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough") cv_gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY) # Find the faces and draw a rectangle around them faces = face_cascade.detectMultiScale(cv_gray, 1.3, 5) for (x, y, w, h) in faces: cv_img = cv2.rectangle(cv_img, (x, y), (x + w, y + h), (255, 0, 0), 2) # For each image, we also want to publish the croped image crop_img = cv_img[y:y + h, x:x + w] crop_msg = bridge.cv2_to_imgmsg(crop_img, encoding="bgr8") # And the region of interest information pub_BoxesImage.publish(crop_msg) boxes_msg = RegionOfInterest() boxes_msg.x_offset = x boxes_msg.y_offset = y boxes_msg.height = h boxes_msg.width = w boxes_msg.do_rectify = False pub_Boxes.publish(boxes_msg) # Publish the original image with the rectangles mod_img = bridge.cv2_to_imgmsg(cv_img, encoding="bgr8") pub_Image.publish(mod_img)
def mouse_callback(self, event, x, y, flags, param): print 'Mouse callback status', self.status if self.status == 'initializing': if event == cv2.EVENT_MOUSEMOVE: self.color_image_drawing = np.copy(self.color_image) p1_x = max(0, x - self.x_width / 2) p1_y = max(0, y - self.y_width / 2) p2_x = min(self.img_size_x - 1, x + self.x_width / 2) p2_y = min(self.img_size_y - 1, y + self.y_width / 2) cv2.rectangle(self.color_image_drawing, (p1_x, p1_y), (p2_x, p2_y), color=(0, 255, 0), thickness=1) if event == cv2.EVENT_LBUTTONDOWN: self.status = 'tracking' init_track_msg = RegionOfInterest() init_track_msg.x_offset = max(0, x - self.x_width / 2) init_track_msg.y_offset = max(0, y - self.y_width / 2) init_track_msg.height = self.y_width init_track_msg.width = self.x_width self.init_tracking_pub.publish(init_track_msg)
def publish_roi(self): msg = RegionOfInterest() msg.x_offset = (self.center[0] - self.majorAxis) * self.overlayImgSize msg.y_offset = (self.center[1] - self.minorAxis) * self.overlayImgSize msg.width = int(self.majorAxis * 2 * self.overlayImgSize) msg.height = int(self.minorAxis * 2 * self.overlayImgSize) self.roiPub.publish(msg)
def to_roi(top_left, bottom_right): msg = RegionOfInterest() msg.x_offset = round(top_left[0]) msg.y_offset = round(top_left[1]) msg.width = round(abs(bottom_right[0] - top_left[0])) msg.height = round(abs(bottom_right[1] - top_left[1])) return msg
def publish(self, boxes, scores, classes, num, category_index, masks=None, fps=0): # init detection message msg = Detection() for i in range(boxes.shape[0]): if scores[i] > 0.5: if classes[i] in category_index.keys(): class_name = category_index[classes[i]]['name'] else: class_name = 'N/A' ymin, xmin, ymax, xmax = tuple(boxes[i].tolist()) box = RegionOfInterest() box.x_offset = xmin + (xmax - xmin) / 2.0 box.y_offset = ymin + (ymax - ymin) / 2.0 box.height = ymax - ymin box.width = xmax - xmin # fill detection message with objects obj = Object() obj.box = box obj.class_name = class_name obj.score = int(100 * scores[i]) if masks is not None: obj.mask = self._bridge.cv2_to_imgmsg( masks[i], encoding="passthrough") msg.objects.append(obj) msg.fps = fps # publish detection message self.DetPub.publish(msg)
def publish(self, boxes, scores, classes, num, image_shape, category_index, masks=None): # init detection message msg = Detection() for i in range(boxes.shape[0]): if scores[i] > 0.5: ymin, xmin, ymax, xmax = tuple(boxes[i].tolist()) box = RegionOfInterest() box.x_offset = np.asscalar(xmin) box.y_offset = np.asscalar(ymin) box.height = np.asscalar(ymax - ymin) box.width = np.asscalar(xmax - xmin) display_str = '##\nnumber {} {}: {}% at image coordinates (({}, {}) to ({}, {}))\n##'.format(i, class_name, int(100*scores[i]), left, top, right, bottom) print(display_str) # fill detection message with objects obj = Object() obj.box = box obj.class_name = class_name obj.score = int(scores[i]) obj.mask = masks[i] msg.objects.append(obj) # publish detection message self.DetPub.publish(msg)
def _build_result_msg(self, msg, result): result_msg = Result() result_msg.header = msg.header for i, (y1, x1, y2, x2) in enumerate(result['rois']): box = RegionOfInterest() box.x_offset = x1#np.asscalar(x1) box.y_offset = y1#np.asscalar(y1) box.height = y2-y1#np.asscalar(y2 - y1) box.width = x2-x1#np.asscalar(x2 - x1) result_msg.boxes.append(box) class_id = result['class_ids'][i] result_msg.class_ids.append(class_id) class_name = self._class_names[class_id] result_msg.class_names.append(class_name) score = result['scores'][i] result_msg.scores.append(score) mask = Image() mask.header = msg.header mask.height = result['masks'].shape[0] mask.width = result['masks'].shape[1] mask.encoding = "mono8" mask.is_bigendian = False mask.step = mask.width mask.data = (result['masks'][:, :, i] * 255).tobytes() result_msg.masks.append(mask) return result_msg
def main(): rospy.wait_for_service('detect_keypoints') detect_keypoint = rospy.ServiceProxy('detect_keypoints', MankeyKeypointDetection) # Get the test data path project_path = os.path.join(os.path.dirname(__file__), os.path.pardir) project_path = os.path.abspath(project_path) test_data_path = os.path.join(project_path, 'test_data') cv_rbg_path = os.path.join(test_data_path, '000000_rgb.png') cv_depth_path = os.path.join(test_data_path, '000000_depth.png') # Read the image cv_rgb = cv2.imread(cv_rbg_path, cv2.IMREAD_COLOR) cv_depth = cv2.imread(cv_depth_path, cv2.IMREAD_ANYDEPTH) # The bounding box roi = RegionOfInterest() roi.x_offset = 261 roi.y_offset = 194 roi.width = 327 - 261 roi.height = 260 - 194 # Build the request request = MankeyKeypointDetectionRequest() bridge = CvBridge() request.rgb_image = bridge.cv2_to_imgmsg(cv_rgb, 'bgr8') request.depth_image = bridge.cv2_to_imgmsg(cv_depth) request.bounding_box = roi response = detect_keypoint(request) print response
def getResult(self, predictions): boxes = predictions.pred_boxes if predictions.has( "pred_boxes") else None if predictions.has("pred_masks"): masks = np.asarray(predictions.pred_masks) else: return result_msg = Result() result_msg.header = self._header result_msg.class_ids = predictions.pred_classes if predictions.has( "pred_classes") else None result_msg.class_names = np.array( self._class_names)[result_msg.class_ids.numpy()] result_msg.scores = predictions.scores if predictions.has( "scores") else None for i, (x1, y1, x2, y2) in enumerate(boxes): mask = np.zeros(masks[i].shape, dtype="uint8") mask[masks[i, :, :]] = 255 mask = self._bridge.cv2_to_imgmsg(mask) mask.encoding = "mono8" # 参考mask_rcnn_node line164 result_msg.masks.append(mask) box = RegionOfInterest() box.x_offset = np.uint32(x1) box.y_offset = np.uint32(y1) box.height = np.uint32(y2 - y1) box.width = np.uint32(x2 - x1) result_msg.boxes.append(box) return result_msg
def face_detect_cb(self, event): if self.image is not None and self.lock.acquire(False): cnn_results = self.recogniser.detect_faces(self.image, scale=self.cnn_scale) features = Features() features.features = [] closest_face = None closest_face_img = None max_distance = float("inf") for k, d in enumerate(cnn_results): padding = int(self.image.shape[0] * self.cnn_padding) feature = Feature() roi = RegionOfInterest() roi.x_offset = np.maximum(d.left() - padding, 0) roi.y_offset = np.maximum(d.top() - padding, 0) roi.height = np.minimum(d.bottom() - roi.y_offset + padding, self.image.shape[0]) roi.width = np.minimum(d.right() - roi.x_offset + padding, self.image.shape[1]) feature.roi = roi feature.crop = self.bridge.cv2_to_imgmsg( np.array( self.image[roi.y_offset:roi.y_offset + roi.height, roi.x_offset:roi.x_offset + roi.width, :])) if self.depth_image is not None: xw = np.array(self.depth_image[ roi.y_offset:roi.y_offset + roi.height, roi.x_offset:roi.x_offset + roi.width]).mean() * self.rs_depth_scale else: xw = 2.0 xc = roi.x_offset + roi.width / 2.0 yc = roi.y_offset + roi.height / 2.5 x, y, z = self.get_world_coordinates(xw, self.image.shape[1], self.image.shape[0], xc, yc) distance = math.sqrt(x * x + y * y + z * z) if distance < max_distance: closest_face = x, y, z max_distance = distance features.features.append(feature) self.lock.release() self.faces_pub.publish(features) if closest_face is not None: x, y, z = closest_face self.last_face_pos = closest_face # self.face_debug.publish(closest_face_img) # print(x, y, z) self.send_face_transform(x, y, z)
def compute_roi_from_indices(indices, width, height, padding = 30): xvals = [ind - width*(math.floor(ind / width)) for ind in indices] yvals = [math.floor(ind / width) for ind in indices] roi = RegionOfInterest() roi.x_offset = max(0, min(xvals) - padding) roi.y_offset = max(0, min(yvals) - padding) roi.height = max(yvals) - roi.y_offset + padding if roi.height + roi.y_offset > height: roi.height = height - roi.y_offset roi.width = max(xvals) - roi.x_offset + padding if roi.width + roi.x_offset > width: roi.width = width - roi.x_offset roi.do_rectify = 0 return roi
def do_camshift(self, cv_image): """ Get the image size """ image_size = cv.GetSize(cv_image) image_width = image_size[0] image_height = image_size[1] """ Convert to HSV and keep the hue """ hsv = cv.CreateImage(image_size, 8, 3) cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV) self.hue = cv.CreateImage(image_size, 8, 1) cv.Split(hsv, self.hue, None, None, None) """ Compute back projection """ backproject = cv.CreateImage(image_size, 8, 1) """ Run the cam-shift algorithm """ cv.CalcArrBackProject( [self.hue], backproject, self.hist ) cv.ShowImage("Backproject", backproject) if self.track_window and is_rect_nonzero(self.track_window): crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1) (iters, (area, value, rect), track_box) = cv.CamShift(backproject, self.track_window, crit) self.track_window = rect """ If mouse is pressed, highlight the current selected rectangle and recompute the histogram """ if self.drag_start and is_rect_nonzero(self.selection): sub = cv.GetSubRect(cv_image, self.selection) save = cv.CloneMat(sub) cv.ConvertScale(cv_image, cv_image, 0.5) cv.Copy(save, sub) x,y,w,h = self.selection cv.Rectangle(cv_image, (x,y), (x+w,y+h), (255,255,255)) sel = cv.GetSubRect(self.hue, self.selection ) cv.CalcArrHist( [sel], self.hist, 0) (_, max_val, _, _) = cv.GetMinMaxHistValue(self.hist) if max_val != 0: cv.ConvertScale(self.hist.bins, self.hist.bins, 255. / max_val) elif self.track_window and is_rect_nonzero(self.track_window): """ The width and height on the returned track_box seem to be exchanged--a bug in CamShift? """ ((x, y), (w, h), angle) = track_box track_box = ((x, y), (h, w), angle) cv.EllipseBox(cv_image, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0) roi = RegionOfInterest() roi.x_offset = int(min(image_width, max(0, track_box[0][0] - track_box[1][0] / 2))) roi.y_offset = int(min(image_height, max(0, track_box[0][1] - track_box[1][1] / 2))) roi.width = int(track_box[1][0]) roi.height = int(track_box[1][1]) self.ROI.publish(roi) cv.ShowImage("Histogram", self.hue_histogram_as_image(self.hist)) if not self.backproject_mode: return cv_image else: return backproject
def extended_face_roi(self, face_roi, img_shape): extroi = RegionOfInterest() wext = int(face_roi.width * FACE_ROI_WIDTH_EXTENTION_K) hext = int(face_roi.height * FACE_ROI_HEIGHT_EXTENTION_K) extroi.x_offset = max(face_roi.x_offset - wext, 0) extroi.y_offset = max(face_roi.y_offset - hext, 0) extroi.width = min(face_roi.width + 2*wext, img_shape[1] - extroi.x_offset - 1) extroi.height = min(face_roi.height + 2*hext, img_shape[0] - extroi.y_offset - 1) return extroi
def pcl_segment_objects(self, cloud, non_planar_ratio, cluster_tol, min_size, max_size, visualize=False): # segments objects in pcl by clustering # npr is about how much of the cloud to filter out # cluster_tol distance between each point to be considered in 1 cluster. 0.02 is 2cm # last two are min and max number of points in a cluster try: rospy.loginfo('waiting for {}'.format('/segment_objects')) rospy.wait_for_service('/segment_objects') rospy.loginfo('connected to {}'.format('/segment_objects')) segmentation_req = SegmentObjectsRequest(cloud, non_planar_ratio, cluster_tol, min_size, max_size) segmentation_res = self.segment_objects_srv(segmentation_req) width = cloud.width rospy.loginfo('No of clusters found from segment objects : ') rospy.loginfo(len(segmentation_res.clusters)) bounding_boxes = [] for cluster in segmentation_res.clusters: left = cluster.indices[0] % width right = cluster.indices[0] % width top = int(cluster.indices[0] / width) bottom = int(cluster.indices[0] / width) for index in cluster.indices: row = int(index / width) column = index % width left = min(left, column) right = max(right, column) top = min(top, row) bottom = max(bottom, row) bounding_box = RegionOfInterest() bounding_box.x_offset = left bounding_box.y_offset = top bounding_box.width = right - left bounding_box.height = bottom - top bounding_boxes.append(bounding_box) if visualize: self.visualize_bounding_boxes(bounding_boxes, 'PCL segmented objects') return bounding_boxes, segmentation_res.objects, segmentation_res.whole except ServiceException as ex: rospy.logwarn(ex)
def converts_to_ObjectInBoxe(image_size, ymin, xmin, ymax, xmax, probability=(), object_name_list=(), use_normalized_coordinates=True, do_rectify=False): """Adds a bounding box to an image. Bounding box coordinates can be specified in either absolute (pixel) or normalized coordinates by setting the use_normalized_coordinates argument. Each string in display_str_list is displayed on a separate line above the bounding box in black text on a rectangle filled with the input 'color'. If the top of the bounding box extends to the edge of the image, the strings are displayed below the bounding box. Args: image_size: image_size. ymin: ymin of bounding box. xmin: xmin of bounding box. ymax: ymax of bounding box. xmax: xmax of bounding box. object_name_list: list of strings to display in box (each to be shown on its own line). use_normalized_coordinates: If True (default), treat coordinates ymin, xmin, ymax, xmax as relative to the image. Otherwise treat coordinates as absolute. """ im_height, im_width = image_size if use_normalized_coordinates: (left, right, top, bottom) = (xmin * im_width, xmax * im_width, ymin * im_height, ymax * im_height) else: (left, right, top, bottom) = (xmin, xmax, ymin, ymax) object = Object() object.object_name = object_name_list[0] object.probability = probability[0] roi = RegionOfInterest() roi.x_offset = int(left) roi.y_offset = int(top) roi.height = int(bottom - top) roi.width = int(right - left) roi.do_rectify = do_rectify object_in_box = ObjectInBox() object_in_box.object = object object_in_box.roi = roi return object_in_box
def publish_roi(self): try: ROI = RegionOfInterest() ROI.x_offset = int(self.x) ROI.y_offset = int(self.y) ROI.width = int(self.w) ROI.height = int(self.h) self.roi_pub.publish(ROI) except: rospy.loginfo("Publishing ROI failed")
def yoloCallback(self, msg): roi = RegionOfInterest() for data in msg.bounding_boxes: if data.Class == self.target: roi.x_offset = data.xmin roi.y_offset = data.ymin roi.width = data.xmax -data.xmin roi.height = data.ymax - data.ymin self.roi_pub.publish(roi) return 0
def publish_roi(self): try: roi = RegionOfInterest() roi.x_offset = 0 roi.y_offset = 0 roi.width = 0 roi.height = 0 self.roi_pub.publish(roi) except: rospy.loginfo("Publishing ROI failed")
def extended_face_roi(self, face_roi, img_shape): extroi = RegionOfInterest() wext = int(face_roi.width * FACE_ROI_WIDTH_EXTENTION_K) hext = int(face_roi.height * FACE_ROI_HEIGHT_EXTENTION_K) extroi.x_offset = max(face_roi.x_offset - wext, 0) extroi.y_offset = max(face_roi.y_offset - hext, 0) extroi.width = min(face_roi.width + 2 * wext, img_shape[1] - extroi.x_offset - 1) extroi.height = min(face_roi.height + 2 * hext, img_shape[0] - extroi.y_offset - 1) return extroi
def do_camshift(self, cv_image): """ Get the image size """ image_size = cv.GetSize(cv_image) image_width = image_size[0] image_height = image_size[1] """ Convert to HSV and keep the hue """ hsv = cv.CreateImage(image_size, 8, 3) cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV) self.hue = cv.CreateImage(image_size, 8, 1) cv.Split(hsv, self.hue, None, None, None) """ Compute back projection """ backproject = cv.CreateImage(image_size, 8, 1) """ Run the cam-shift algorithm """ cv.CalcArrBackProject( [self.hue], backproject, self.hist ) if self.track_window and is_rect_nonzero(self.track_window): crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1) (iters, (area, value, rect), track_box) = cv.CamShift(backproject, self.track_window, crit) self.track_window = rect """ If mouse is pressed, highlight the current selected rectangle and recompute the histogram """ if self.drag_start and is_rect_nonzero(self.selection): sub = cv.GetSubRect(cv_image, self.selection) save = cv.CloneMat(sub) cv.ConvertScale(cv_image, cv_image, 0.5) cv.Copy(save, sub) x,y,w,h = self.selection cv.Rectangle(cv_image, (x,y), (x+w,y+h), (255,255,255)) sel = cv.GetSubRect(self.hue, self.selection ) cv.CalcArrHist( [sel], self.hist, 0) (_, max_val, _, _) = cv.GetMinMaxHistValue(self.hist) if max_val != 0: cv.ConvertScale(self.hist.bins, self.hist.bins, 255. / max_val) elif self.track_window and is_rect_nonzero(self.track_window): cv.EllipseBox( cv_image, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0 ) roi = RegionOfInterest() roi.x_offset = int(min(image_width, max(0, track_box[0][0] - track_box[1][0] / 2))) roi.y_offset = int(min(image_height, max(0, track_box[0][1] - track_box[1][1] / 2))) roi.width = int(track_box[1][0]) roi.height = int(track_box[1][1]) self.ROI.publish(roi) cv.ShowImage("Histogram", self.hue_histogram_as_image(self.hist)) if not self.backproject_mode: return cv_image else: return backproject
def publish_track_roi(self): try: roi = RegionOfInterest() x,y,w,h = self.track_box roi.x_offset = x + 0.5 * w roi.y_offset = y + 0.5 * h roi.width = w roi.height = h self.roi_pub.publish(roi) except: rospy.loginfo("Publishing ROI failed")
def tracked_object_message(track): msg = TrackedObject() msg.id = track.track_id roi_msg = RegionOfInterest() roi_msg.x_offset = int(track.x1) roi_msg.y_offset = int(track.y1) roi_msg.height = int(track.get_height()) roi_msg.width = int(track.get_width()) msg.roi = roi_msg return msg
def main(visualize): rospy.wait_for_service('detect_keypoints') detect_keypoint = rospy.ServiceProxy('detect_keypoints', MankeyKeypointDetection) # Get the test data path project_path = os.path.join(os.path.dirname(__file__), os.path.pardir) project_path = os.path.abspath(project_path) test_data_path = os.path.join(project_path, 'test_data') cv_rbg_path = os.path.join(test_data_path, '000000_rgb.png') cv_depth_path = os.path.join(test_data_path, '000000_depth.png') # Read the image cv_rgb = cv2.imread(cv_rbg_path, cv2.IMREAD_COLOR) cv_depth = cv2.imread(cv_depth_path, cv2.IMREAD_ANYDEPTH) # The bounding box roi = RegionOfInterest() roi.x_offset = 261 roi.y_offset = 194 roi.width = 327 - 261 roi.height = 260 - 194 # Build the request request = MankeyKeypointDetectionRequest() bridge = CvBridge() request.rgb_image = bridge.cv2_to_imgmsg(cv_rgb, 'bgr8') request.depth_image = bridge.cv2_to_imgmsg(cv_depth) request.bounding_box = roi response = detect_keypoint(request) print response if visualize: import open3d as o3d vis_list = [] color = o3d.geometry.Image(cv_rgb) depth = o3d.geometry.Image(cv_depth) rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(color, depth) pcd = o3d.geometry.PointCloud.create_from_rgbd_image( rgbd, o3d.camera.PinholeCameraIntrinsic( o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)) vis_list.append(pcd) for keypoint in response.keypoints_camera_frame: keypoints_coords \ = o3d.geometry.TriangleMesh.create_coordinate_frame( size=0.1, origin=[keypoint.x, keypoint.y, keypoint.z]) vis_list.append(keypoints_coords) o3d.visualization.draw_geometries(vis_list)
def BuildMsg(self): bbox = RegionOfInterest() bbox.x_offset = self.bbox[0] bbox.y_offset = self.bbox[1] bbox.height = self.bbox[3] - self.bbox[1] bbox.width = self.bbox[2] - self.bbox[0] personLocation = PersonLocationMsg() personLocation.targetId = self.id personLocation.annotationType = self.annotationType personLocation.bbox = bbox return personLocation
def publishROI(self): # loop over the face bounding boxes and draw them for rect in self.rects: cv2.rectangle(self.frameClone, (rect[0], rect[1]), (rect[2], rect[3]), (0, 255, 0), 2) roi = RegionOfInterest() roi.x_offset = rect[0] roi.y_offset = rect[1] roi.width = rect[2] roi.height = rect[3] self.pub.publish(roi)
def publish(self, boxes, classes, labels, seg_map, fps=0): # init detection message msg = Segmentation() boxes = [] for i in range(boxes.shape[0]): class_name = label[i] ymin, xmin, ymax, xmax = tuple(boxes[i].tolist()) box = RegionOfInterest() box.x_offset = xmin + (xmax-xmin)/2.0 box.y_offset = ymin + (ymax-ymin)/2.0 box.height = ymax - ymin box.width = xmax - xmin # fill segmentation message msg.boxes.append(box) msg.class_names.append(class_name) msg.seg_map = self._bridge.cv2_to_imgmsg(seg_map, encoding="passthrough") msg.fps = fps # publish detection message self.SegPub.publish(msg)
def publish_roi(self): roi_box = self.track_window # roi_box = self.track_box try: roi_box = self.cvBox2D_to_cvRect(roi_box) except: return # Watch out for negative offsets roi_box[0] = max(0, roi_box[0]) roi_box[1] = max(0, roi_box[1]) try: roi = RegionOfInterest() roi.x_offset = int(roi_box[0]) roi.y_offset = int(roi_box[1]) roi.width = int(roi_box[2]) roi.height = int(roi_box[3]) self.roi_pub.publish(roi) except: rospy.loginfo("Publishing ROI failed")
def match_template(self, cv_image): frame = np.array(cv_image, dtype=np.uint8) W,H = frame.shape[1], frame.shape[0] w,h = self.template.shape[1], self.template.shape[0] width = W - w + 1 height = H - h + 1 # Make sure that the template image is smaller than the source if W < w or H < h: rospy.loginfo( "Template image must be smaller than video frame." ) return False if frame.dtype != self.template.dtype: rospy.loginfo("Template and video frame must have same depth and number of channels.") return False # Create copies of the images to modify frame_copy = frame.copy() template_copy = self.template.copy() # Down pyramid the images for k in range(self.numDownPyrs): # Start with the source image W = (W + 1) / 2 H = (H + 1) / 2 frame_small = np.array([H, W], dtype=frame.dtype) frame_small = cv2.pyrDown(frame_copy) # frame_window = "PyrDown " + str(k) # cv.NamedWindow(frame_window, cv.CV_NORMAL) # cv.ShowImage(frame_window, cv.fromarray(frame_small)) # cv.ResizeWindow(frame_window, 640, 480) # Prepare for next loop, if any frame_copy = frame_small.copy() #Next, do the target w = (w + 1) / 2 h = (h + 1) / 2 template_small = np.array([h, w], dtype=self.template.dtype) template_small = cv2.pyrDown(template_copy) # template_window = "Template PyrDown " + str(k) # cv.NamedWindow(template_window, cv.CV_NORMAL) # cv.ShowImage(template_window, cv.fromarray(template_small)) # cv.ResizeWindow(template_window, 640, 480) # Prepare for next loop, if any template_copy = template_small.copy() # Perform the match on the shrunken images small_frame_width = frame_copy.shape[1] small_frame_height = frame_copy.shape[0] small_template_width = template_copy.shape[1] small_template_height = template_copy.shape[0] result_width = small_frame_width - small_template_width + 1 result_height = small_frame_height - small_template_height + 1 result_mat = cv.CreateMat(result_height, result_width, cv.CV_32FC1) result = np.array(result_mat, dtype = np.float32) cv2.matchTemplate(frame_copy, template_copy, cv.CV_TM_CCOEFF_NORMED, result) cv2.imshow("Result", result) return (0, 0, 100, 100) # # Find the best match location # (minValue, maxValue, minLoc, maxLoc) = cv2.minMaxLoc(result) # # # Transform point back to original image # target_location = Point() # target_location.x, target_location.y = maxLoc # # return (target_location.x, target_location.y, w, h) # Find the top match locations locations = self.MultipleMaxLoc(result, self.numMaxima) foundPointsList = list() confidencesList = list() W,H = frame.shape[1], frame.shape[0] w,h = self.template.shape[1], self.template.shape[0] # Search the large images at the returned locations for currMax in range(self.numMaxima): # Transform the point to its corresponding point in the larger image #locations[currMax].x *= int(pow(2.0, self.numDownPyrs)) #locations[currMax].y *= int(pow(2.0, self.numDownPyrs)) locations[currMax].x += w / 2 locations[currMax].y += h / 2 searchPoint = locations[currMax] print "Search Point", searchPoint # If we are searching for multiple targets and we have found a target or # multiple targets, we don't want to search in the same location(s) again # if self.findMultipleTargets and len(foundPointsList) != 0: # thisTargetFound = False # numPoints = len(foundPointsList) # # for currPoint in range(numPoints): # foundPoint = foundPointsList[currPoint] # if (abs(searchPoint.x - foundPoint.x) <= self.searchExpansion * 2) and (abs(searchPoint.y - foundPoint.y) <= self.searchExpansion * 2): # thisTargetFound = True # break # # # If the current target has been found, continue onto the next point # if thisTargetFound: # continue # Set the source image's ROI to slightly larger than the target image, # centred at the current point searchRoi = RegionOfInterest() searchRoi.x_offset = searchPoint.x - w / 2 - self.searchExpansion searchRoi.y_offset = searchPoint.y - h / 2 - self.searchExpansion searchRoi.width = w + self.searchExpansion * 2 searchRoi.height = h + self.searchExpansion * 2 #print (searchRoi.x_offset, searchRoi.y_offset, searchRoi.width, searchRoi.height) # Make sure ROI doesn't extend outside of image if searchRoi.x_offset < 0: searchRoi.x_offset = 0 if searchRoi.y_offset < 0: searchRoi.y_offset = 0 if (searchRoi.x_offset + searchRoi.width) > (W - 1): numPixelsOver = (searchRoi.x_offset + searchRoi.width) - (W - 1) print "NUM PIXELS OVER", numPixelsOver searchRoi.width -= numPixelsOver if (searchRoi.y_offset + searchRoi.height) > (H - 1): numPixelsOver = (searchRoi.y_offset + searchRoi.height) - (H - 1) searchRoi.height -= numPixelsOver mask = (searchRoi.x_offset, searchRoi.y_offset, searchRoi.width, searchRoi.height) frame_mat = cv.fromarray(frame) searchImage = cv.CreateMat(searchRoi.height, searchRoi.width, cv.CV_8UC3) searchImage = cv.GetSubRect(frame_mat, mask) searchArray = np.array(searchImage, dtype=np.uint8) # Perform the search on the large images result_width = searchRoi.width - w + 1 result_height = searchRoi.height - h + 1 result_mat = cv.CreateMat(result_height, result_width, cv.CV_32FC1) result = np.array(result_mat, dtype = np.float32) cv2.matchTemplate(searchArray, self.template, cv.CV_TM_CCOEFF_NORMED, result) # Find the best match location (minValue, maxValue, minLoc, maxLoc) = cv2.minMaxLoc(result) maxValue *= 100 # Transform point back to original image target_location = Point() target_location.x, target_location.y = maxLoc target_location.x += searchRoi.x_offset - w / 2 + self.searchExpansion target_location.y += searchRoi.y_offset - h / 2 + self.searchExpansion if maxValue >= self.matchPercentage: # Add the point to the list foundPointsList.append(maxLoc) confidencesList.append(maxValue) # If we are only looking for a single target, we have found it, so we # can return if not self.findMultipleTargets: break if len(foundPointsList) == 0: rospy.loginfo("Target was not found to required confidence") return (target_location.x, target_location.y, w, h)