def analyze(self): rate = rospy.Rate(5) while self.image_ready == False and not rospy.is_shutdown(): rate.sleep() final_image = self.image_raw final_image = self.edit_video(final_image) ############### # Process openpose ############ # Process image datum = op.Datum() datum.cvInputData = final_image self.opWrapper.emplaceAndPop(op.VectorDatum([datum])) # Display Image frame = datum.poseKeypoints cv2.imshow("Frame", datum.cvOutputData) ############ # Classify current passenger state. ###### prediction = self.safety_check(frame) return prediction, frame, datum.cvOutputData
def hand_skel(self, image_path, is_left=False): from openpose import pyopenpose as op # Read image and face rectangle locations img = cv2.imread(image_path) if img is None: return None height, width = img.shape[:2] top, bottom, left, right = get_square_padding(img) img = squarify(img, (top, bottom, left, right)) hand_rect = op.Rectangle(0.0, 0.0, img.shape[0], img.shape[0]) null_rect = op.Rectangle(0.0, 0.0, 0.0, 0.0) if is_left: hand_rects = [[hand_rect, null_rect]] else: hand_rects = [[null_rect, hand_rect]] datum = op.Datum() datum.cvInputData = img datum.handRectangles = hand_rects self.op_wrap.emplaceAndPop(op.VectorDatum([datum])) if is_left: kps = datum.handKeypoints[0][0] else: kps = datum.handKeypoints[1][0] if left: kps[:, 0] -= left elif top: kps[:, 1] -= top return kps, width, height
def processPic(self): # ============================= 启动openPose =================================== datum = op.Datum() datum.cvInputData = cv2.imread(self.picPaths) # 输入 opWrapper.emplaceAndPop(op.VectorDatum([datum])) # 输出 keyPoints = datum.poseKeypoints.tolist() dstPicPath = "../dataset/marked_pic/p_" + self.picPaths.split('/')[ -1] # 处理后的图片 cv2.imwrite(dstPicPath, datum.cvOutputData) # ============================= 写骨骼数据文件 =================================== with open("../dataset/bone_dataSet.data", "a+") as dataSet: dataSet.writelines( str( self.pointDistance(keyPoints[0]) + self.pointAngle(keyPoints[0]) + [int(self.lineEdit.text())])) dataSet.write("\n") # ============================= 写骨骼图片文件 =================================== bone_img = datum.cvOutputData height, width, channel = bone_img.shape pixmap = QPixmap.fromImage( QImage(bone_img.data, width, height, 3 * width, QImage.Format_RGB888).rgbSwapped()) self.label_3.setPixmap(pixmap)
def get_sample_heatmaps(): # These parameters are globally set. You need to unset variables set here if you have a new OpenPose object. See * params = dict() params["model_folder"] = os.path.join(os.path.expanduser("~"), 'catkin_ws/src/openpose/models/', '') params["heatmaps_add_parts"] = True params["heatmaps_add_bkg"] = True params["heatmaps_add_PAFs"] = True params["heatmaps_scale"] = 3 params["upsampling_ratio"] = 1 params["body"] = 1 # Starting OpenPose opWrapper = op.WrapperPython() opWrapper.configure(params) opWrapper.start() # Process Image and get heatmap datum = op.Datum() imageToProcess = cv2.imread(args[0].image_path) datum.cvInputData = imageToProcess opWrapper.emplaceAndPop(op.VectorDatum([datum])) poseHeatMaps = datum.poseHeatMaps.copy() opWrapper.stop() return poseHeatMaps
def detect_poses(self, image): # `op` added to globals in the constructor datum = op.Datum() # pylint: disable=undefined-variable # noqa: F821 datum.cvInputData = image self._openpose_wrapper.emplaceAndPop(op.VectorDatum([datum])) keypoints = datum.poseKeypoints overlayed_image = datum.cvOutputData recognitions = [] if keypoints is not None and len( keypoints.shape ) == 3: # If no detections, keypoints will be None num_persons, num_bodyparts, _ = keypoints.shape for person_id in range(0, num_persons): for body_part_id in range(0, num_bodyparts): body_part = self._model["body_parts"][body_part_id] x, y, probability = keypoints[person_id][body_part_id] if probability > 0: recognitions.append( Recognition(group_id=person_id, roi=RegionOfInterest(width=1, height=1, x_offset=int(x), y_offset=int(y)), categorical_distribution= CategoricalDistribution(probabilities=[ CategoryProbability( label=body_part, probability=float(probability)) ]))) return recognitions, overlayed_image
def scan_image_without_normalize(self, img_path): # Opening image in OpenCV imageToProcess = cv2.imread(img_path) # Get data points (datum) datum = op.Datum() datum.cvInputData = imageToProcess self.opWrapper.emplaceAndPop(op.VectorDatum([datum])) # Get output image output_image = datum.cvOutputData arr = [] try: pop_all(arr) x_high = 0 x_low = 9999 y_high = 0 y_low = 9999 # Get highest and lowest keypoints for count, x in enumerate(datum.poseKeypoints[0]): # Avoid x=0 and y=0 because some keypoints that are not discovered. # This "if" is to define the LOWEST and HIGHEST discovered keypoint. if x[0] != 0 and x[1] != 0: if x_high < x[0]: x_high = x[0] if x_low > x[0]: x_low = x[0] if y_high < x[1]: y_high = x[1] if y_low > x[1]: y_low = x[1] # Add pose keypoints to a dictionary KP = {'x': x[0], 'y': x[1]} # Append dictionary to array arr.append(KP) # Find the highest and lowest position of x and y # (Used to draw rectangle) if y_high - y_low > x_high - x_low: height = y_high - y_low width = x_high - x_low else: height = x_high - x_low width = y_high - y_low # Draw rectangle (get width and height) y_high = int(y_high + height / 40) y_low = int(y_low - height / 12) x_high = int(x_high + width / 5) x_low = int(x_low - width / 5) return arr, x_low, y_low, output_image except Exception as e: print(end="")
def hand_pose(model_dir, left=0): params = {'model_folder': model_dir} params['net_resolution'] = '240x192' #params['disable_blending'] = True #params['number_people_max'] = 1 params['hand'] = True #params['face'] = True params['body'] = 0 params['hand_detector'] = 2 hands_boxes = [ # left & right hands: only 1-person [pyop.Rectangle(0, 0, 0, 0), pyop.Rectangle(0, 0, 640, 640)] ] #opWrapper = pyop.WrapperPython(pyop.ThreadManagerMode.Synchronous) #opWrapper.configure(params); opWrapper.execute() # webcam '''opWrapper = pyop.WrapperPython(pyop.ThreadManagerMode.AsynchronousOut) opWrapper.configure(params); opWrapper.start() # webcam while cv2.waitKey(5)!=27: # faster t = time(); x = pyop.VectorDatum() if not opWrapper.waitAndPop(x): continue x = x[0]; im = x.cvOutputData; fps = 'FPS=%.1f'%(1/(time()-t)) im = cv2.putText(im, fps, (5,20), 4, 0.7, (0,255,255), 1, 16) cv2.imshow('OpenPose', im) # hand_standalone unsupported cv2.destroyAllWindows()#''' opWrapper = pyop.WrapperPython() opWrapper.configure(params) opWrapper.start() x = pyop.Datum() cap = cv2.VideoCapture(-1) hand_boxes = hand_box(cap, left) while cv2.waitKey(5) != 27: # slower t = time() _, im = cap.read() x.cvInputData = im if params['hand_detector'] == 2: x.handRectangles = hand_boxes opWrapper.emplaceAndPop(pyop.VectorDatum([x])) im = x.cvOutputData fps = 'FPS=%.1f' % (1 / (time() - t)) im = cv2.putText(im, fps, (5, 20), 4, 0.7, (0, 255, 255), 1, 16) cv2.imshow('OpenPose', RSZ(im, 1.5)) # cv2.LINE_AA=16 cv2.destroyAllWindows() cap.release() #'''
def __init__(self, frame_id, no_depth, pub_topic, color_topic, depth_topic, cam_info_topic, op_wrapper, display): self.pub = rospy.Publisher(pub_topic, Frame, queue_size=10) self.frame_id = frame_id self.no_depth = no_depth self.bridge = CvBridge() self.op_wrapper = op_wrapper self.display = display self.frame = None # Populate necessary K matrix values for 3D pose computation. cam_info = rospy.wait_for_message(cam_info_topic, CameraInfo) self.fx = cam_info.K[0] self.fy = cam_info.K[4] self.cx = cam_info.K[2] self.cy = cam_info.K[5] # Obtain depth topic encoding encoding = rospy.wait_for_message(depth_topic, Image).encoding self.mm_to_m = 0.001 if encoding == "16UC1" else 1. # Function wrappers for OpenPose version discrepancies if OPENPOSE1POINT7_OR_HIGHER: self.emplaceAndPop = lambda datum: self.op_wrapper.emplaceAndPop( op.VectorDatum([datum])) self.detect = lambda kp: kp is not None else: self.emplaceAndPop = lambda datum: self.op_wrapper.emplaceAndPop( [datum]) self.detect = lambda kp: kp.shape != () image_sub = message_filters.Subscriber(color_topic, Image) depth_sub = message_filters.Subscriber(depth_topic, Image) self.ts = message_filters.ApproximateTimeSynchronizer( [image_sub, depth_sub], 1, 0.01) self.ts.registerCallback(self.callback) """ OpenPose skeleton dictionary
def process_frame(img, op_wrapper): """Function to take a Frame and return keypoints and output image with keypoints Args: img: Image to work with op_wrapper: The openpose wrapper """ datum = op.Datum() if not img.dtype == np.uint8: img = np.uint8(img) tqdm.write("Wrong image dtype: Changing to np.uint8") # To be resolved img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) datum.cvInputData = img op_wrapper.emplaceAndPop(op.VectorDatum([datum])) # tqdm.write(datum.poseKeypoints.shape) #cv2.imwrite('output/test.jpg', datum.cvOutputData) # return datum.cvOutputData, datum.poseKeypoints, datum.handKeypoints return datum.getPoseKeypoints()
def showCapture(self): try: frame = self.video.captureFrame() datum = op.Datum() datum.cvInputData = frame opWrapper.emplaceAndPop(op.VectorDatum([datum])) resPic = datum.cvOutputData # cv2.putText(resPic, "OpenPose", (25, 25), # cv2.FONT_HERSHEY_COMPLEX, 1.0, (0, 0, 222)) pic = cv2.cvtColor(resPic, cv2.COLOR_BGR2RGB) pic = QImage(pic, pic.shape[1], pic.shape[0], QtGui.QImage.Format_RGB888) self.label_3.setPixmap(QPixmap.fromImage(pic)) keyPoints = datum.poseKeypoints.tolist() self.label_4.setText(pos[predict_result( pointDistance(keyPoints[0]) + pointAngle(keyPoints[0]))]) except TypeError: print("No frame")
def classify_image(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) flip_image = cv2.flip(cv_image, -1) # flip the zed image right side up print(flip_image.shape) # Process image datum = op.Datum() datum.cvInputData = flip_image self.opWrapper.emplaceAndPop(op.VectorDatum([datum])) # Display Image poseKeypoints = datum.poseKeypoints # print("pose keypoints") print(poseKeypoints) print("left shoulder ", poseKeypoints[0][2]) print("right shoulder ", poseKeypoints[0][5]) cv2.imshow("Frame", datum.cvOutputData) cv2.waitKey(3)
def upload(): if request.method == 'POST': # Get the file from post request f = request.files['file'] # Save the file to ./uploads basepath = os.path.dirname(__file__) file_path = os.path.join( basepath, secure_filename(f.filename)) f.save(file_path) dir_path = os.path.dirname(os.path.realpath(__file__)) sys.path.append('/usr/local/python') from openpose import pyopenpose as op parser = argparse.ArgumentParser() nameOfUpload = f.filename print(nameOfUpload) parser.add_argument("--image_path", default=nameOfUpload, help="Process an image. Read all standard formats (jpg, png, bmp, etc.).") args = parser.parse_known_args() # Custom Params (refer to include/openpose/flags.hpp for more parameters) params = dict() params["model_folder"] = "/Users/timothy/openpose/models" params["write_json"] = "../temp" params["keypoint_scale"] = "3" # Add others in path? for i in range(0, len(args[1])): curr_item = args[1][i] if i != len(args[1])-1: next_item = args[1][i+1] else: next_item = "1" if "--" in curr_item and "--" in next_item: key = curr_item.replace('-','') if key not in params: params[key] = "1" elif "--" in curr_item and "--" not in next_item: key = curr_item.replace('-','') if key not in params: params[key] = next_item # Construct it from system arguments # op.init_argv(args[1]) # oppython = op.OpenposePython() # Starting OpenPose opWrapper = op.WrapperPython() opWrapper.configure(params) opWrapper.start() print('here') print(args[0].image_path) # Process Image datum = op.Datum() imageToProcess = cv2.imread(args[0].image_path) datum.cvInputData = imageToProcess opWrapper.emplaceAndPop(op.VectorDatum([datum])) # Display Image print("Body keypoints: \n" + str(datum.poseKeypoints)) cv2.imshow("OpenPose 1.7.0 - Tutorial Python API", datum.cvOutputData) #saves output as outputName = 'poseSkeleton' + args[0].image_path cv2.imwrite(outputName,datum.cvOutputData) cv2.waitKey(1) bridge = {"version":1.3,"people":[{"person_id":[-1],"pose_keypoints_2d":[0.255244,0.603225,0.853665,0.3113,0.66861,0.611142,0.32568,0.706593,0.807634,0.426866,0.712084,0.641127,0.517195,0.717605,0.256783,0.302272,0.6304,0.369653,0,0,0,0,0,0,0.479215,0.510503,0.315729,0.475625,0.502358,0.327412,0.639993,0.447886,0.737408,0.632795,0.706738,0.700472,0.477412,0.521413,0.28034,0.632712,0.453244,0.332464,0.618279,0.665819,0.568486,0.239083,0.622133,0.949109,0.240877,0.603191,0.23209,0.242691,0.684896,0.808952,0,0,0,0.697774,0.733872,0.147229,0.701321,0.723011,0.143883,0.605658,0.684873,0.465871,0.697786,0.733884,0.716619,0.686923,0.739437,0.644811,0.623708,0.72306,0.68213],"face_keypoints_2d":[],"hand_left_keypoints_2d":[],"hand_right_keypoints_2d":[],"pose_keypoints_3d":[],"face_keypoints_3d":[],"hand_left_keypoints_3d":[],"hand_right_keypoints_3d":[]}]} fileNameForPhoto = '0' +'_keypoints.json' with open(fileNameForPhoto) as f: data = json.load(f) print(data["people"][0]["pose_keypoints_2d"]) accuracy = comparePoints(1, data["people"][0]["pose_keypoints_2d"]) # Output: {'name': 'Bob', 'languages': ['English', 'Fench']} # assume data contains your decoded image encoded = base64.b64encode(open(outputName , "rb").read()) # Make prediction # preds = model_predict(file_path, model) # Process your result for human # # pred_class = preds.argmax(axis=-1) # Simple argmax # pred_class = decode_predictions(preds, top=1) # ImageNet Decode # result = str(pred_class[0][0][1]) # Convert to string # return result #return encoded return json.dumps(accuracy)
def player_thread(client, opWrapper, mqtt_client, mqtt_channel, debug, addr): data = b'' fps_time = 0 epoch = time.time() detect_move = False reg = False while True: payload_size = struct.calcsize("L") while len(data) < payload_size: #data += client.recv(90456) data += client.recv(4096) packed_msg_size = data[:payload_size] data = data[payload_size:] msg_size = struct.unpack("L", packed_msg_size)[0] #print(str(msg_size)) str_msg = str(msg_size) #print("Size with last digit player" +str_msg); player_num = int(str_msg[-1]) msg_size = int(str_msg[:-1]) player = "player" + str(player_num) print("Received input from " + player) while len(data) < msg_size: data += client.recv(4096) frame_data = data[:msg_size] data = data[msg_size:] frame = pickle.loads(frame_data) now = datetime.now() current_time = now.strftime("%M:%S") print("\tTime: " + current_time + " -- " + str(frame.size)) datum = op.Datum() datum.cvInputData = frame #imageToProcess print("\tPost process " + current_time + " -- " + str(frame.size)) stats = opWrapper.emplaceAndPop(op.VectorDatum([datum])) #print(str(stats)); poseModel = op.PoseModel.BODY_25 #print(op.getPoseBodyPartMapping(poseModel)) #print("Body keypoints: \n" + str(datum.poseKeypoints)) movement = move(datum.poseKeypoints) #####MQTT SEND IT####### if movement == "hook" and detect_move == False: detect_move = True action = 'h' if movement == "blocking" and detect_move == False: detect_move = True action = 'o' #else: #message = json.dumps({"player": player, "action": ""}) #print(""); print(time.time()) if int(time.time()) % 5 == 0: if reg == True: if detect_move == True: message = json.dumps({ "playerID": player_num, "action": action }) detect_move = False else: message = json.dumps({ "playerID": player_num, "action": "" }) mqtt_client.publish(mqtt_channel, message, qos=1) reg = False if int(time.time()) % 6 == 0: reg = True detect_move = False if debug: cv2.putText(datum.cvOutputData, "FPS: %f" % (1.0 / (time.time() - fps_time)), (10, 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) cv2.imshow(player, datum.cvOutputData) print("FPS: %f" % (1.0 / (time.time() - fps_time))) fps_time = time.time() if cv2.waitKey(1) == 27: break
if key not in params: params[key] = next_item # Construct it from system arguments # op.init_argv(args[1]) # oppython = op.OpenposePython() # Starting OpenPose opWrapper = op.WrapperPython(op.ThreadManagerMode.AsynchronousOut) opWrapper.configure(params) opWrapper.start() # Main loop userWantsToExit = False while not userWantsToExit: # Pop frame s = time.time() datumProcessed = op.VectorDatum() if opWrapper.waitAndPop(datumProcessed): if not args[0].no_display: # Display image userWantsToExit = display(datumProcessed) printKeypoints(datumProcessed) else: break e = time.time() print('FPS:%5.2f' % (1 / (e - s))) except Exception as e: print(e) sys.exit(-1)
for input_path in args.files: print(f"Video: {input_path}") video = Video(input_path=input_path) tracker = Tracker( distance_function=keypoints_distance, distance_threshold=DISTANCE_THRESHOLD, detection_threshold=DETECTION_THRESHOLD, hit_counter_max=HIT_COUNTER_MAX, initialization_delay=INITIALIZATION_DELAY, pointwise_hit_counter_max=POINTWISE_HIT_COUNTER_MAX, ) KEYPOINT_DIST_THRESHOLD = video.input_height / 40 for frame in video: datum.cvInputData = frame detector(op.VectorDatum([datum])) detected_poses = datum.poseKeypoints if detected_poses is not None: openpose_detections = ([] if not detected_poses.any() else [ Detection(p, scores=s, label=0) for (p, s) in zip( detected_poses[:, :, :2], detected_poses[:, :, 2]) ]) else: openpose_detections = [] yolo_out = model(frame, conf_threshold=args.conf_thres, iou_threshold=args.iou_thresh, image_size=args.img_size, classes=args.classes)
def get_keypoints_and_id_from_img_without_normalize(self, img): # KP ordering of body parts NECK = 1 R_SHOULDER = 2 R_ELBOW = 3 R_WRIST = 4 L_SHOULDER = 5 L_ELBOW = 6 L_WRIST = 7 MID_HIP = 8 R_HIP = 9 R_KNEE = 10 R_ANKLE = 11 L_HIP = 12 L_KNEE = 13 L_ANKLE = 14 # Define bodyparts to get the selected keypoints BODY_PARTS = [ NECK, R_SHOULDER, R_ELBOW, R_WRIST, L_SHOULDER, L_ELBOW, L_WRIST, MID_HIP, R_HIP, R_KNEE, R_ANKLE, L_HIP, L_KNEE, L_ANKLE ] # Set tracker max_cosine_distance = 0.2 nn_budget = 100 metric = nn_matching.NearestNeighborDistanceMetric( "cosine", max_cosine_distance, nn_budget) tracker = Tracker(metric) # Get data points (datum) datum = op.Datum() datum.cvInputData = img self.opWrapper.emplaceAndPop(op.VectorDatum([datum])) # Initialize lists arr = [] boxes = [] list_of_pose_temp = [] list_of_pose_and_id = [] try: # Get highest and lowest keypoints for kp_idx, keypoint in enumerate(datum.poseKeypoints): pop_all(arr) x_high = 0 x_low = 9999 y_high = 0 y_low = 9999 for count, x in enumerate(keypoint): # Avoid x=0 and y=0 because some keypoints that are not discovered. # This "if" is to define the LOWEST and HIGHEST discovered keypoint. if x[0] != 0 and x[1] != 0: if x_high < x[0]: x_high = x[0] if x_low > x[0]: x_low = x[0] if y_high < x[1]: y_high = x[1] if y_low > x[1]: y_low = x[1] # Add pose keypoints to a dictionary if count in BODY_PARTS: KP = {'x': x[0], 'y': x[1]} # Append dictionary to array arr.append(KP) # Find the highest and lowest position of x and y # (Used to draw rectangle) if y_high - y_low > x_high - x_low: height = y_high - y_low width = x_high - x_low else: height = x_high - x_low width = y_high - y_low # Draw rectangle (get width and height) y_high = int(y_high + height / 40) y_low = int(y_low - height / 12) x_high = int(x_high + width / 5) x_low = int(x_low - width / 5) # # Normalize keypoint list_of_pose_temp.append(arr) # Make the box boxes.append([x_low, y_low, width, height]) # Encode the features inside the designated box features = self.encoder(datum.cvOutputData, boxes) # For a non-empty item add to the detection array def nonempty(xywh): return xywh[2] != 0 and xywh[3] != 0 detections = [ Detection(bbox, 1.0, feature) for bbox, feature in zip(boxes, features) if nonempty(bbox) ] # Run non-maxima suppression. np_boxes = np.array([d.tlwh for d in detections]) scores = np.array([d.confidence for d in detections]) indices = preprocessing.non_max_suppression( np_boxes, self.nms_max_overlap, scores) detections = [detections[i] for i in indices] # Update tracker. tracker.predict() tracker.update(detections) # Make pose and person ID list if kp_idx == len(datum.poseKeypoints) - 1: for track_idx, track in enumerate(tracker.tracks): bbox = track.to_tlwh() list_of_pose_and_id.append({ "Keypoints": list_of_pose_temp[track_idx], "ID": track.track_id }) return list_of_pose_and_id except Exception as e: print(end="")
if key not in params: params[key] = next_item # Construct it from system arguments # op.init_argv(args[1]) # oppython = op.OpenposePython() # Starting OpenPose opWrapper = op.WrapperPython() opWrapper.configure(params) opWrapper.start() # Process Image datum = op.Datum() imageToProcess = cv2.imread(args[0].image_path) datum.cvInputData = imageToProcess opWrapper.emplaceAndPop(op.VectorDatum([datum])) human_count = len(datum.poseKeypoints) # Display Image for human in range(human_count): print("%dth person body keypoints:\n" % human + str(datum.poseKeypoints[human])) print("%dth person face keypoints:\n" % human + str(datum.faceKeypoints[human])) print("%dth person Left Hand keypoints:\n" % human + str(datum.handKeypoints[0][human])) print("%dth person Right Hand keypoints:\n" % human + str(datum.handKeypoints[1][human])) print("Total %d human detected" % human_count) cv2.imshow("OpenPose 1.7.0 - Tutorial Python API", datum.cvOutputData)
def scan_video_without_normalize(self, video_path, keypoints_to_extract): # Opening OpenCV stream stream = cv2.VideoCapture(video_path) # Define list of pose, x low, and y low list_of_pose = [] list_of_x_low = [] list_of_y_low = [] while True: try: # Stream ret, imageToProcess = stream.read() datum = op.Datum() datum.cvInputData = imageToProcess except Exception as e: # Break at end of frame break # Find keypoints self.opWrapper.emplaceAndPop(op.VectorDatum([datum])) # Get output image processed by Openpose output_image = datum.cvOutputData # Define keypoints array and binding box array arr = [] boxes = [] try: # Loop each of the 17 keypoints for keypoint in datum.poseKeypoints: pop_all(arr) x_high = 0 x_low = 9999 y_high = 0 y_low = 9999 # Get highest and lowest keypoints for count, x in enumerate(keypoint): # Check which keypoints to extract if count in keypoints_to_extract: # Avoid x=0 and y=0 because some keypoints that are not discovered. # This "if" is to define the LOWEST and HIGHEST discovered keypoint. if x[0] != 0 and x[1] != 0: if x_high < x[0]: x_high = x[0] if x_low > x[0]: x_low = x[0] if y_high < x[1]: y_high = x[1] if y_low > x[1]: y_low = x[1] # Add pose keypoints to a dictionary KP = {'x': x[0], 'y': x[1]} # Append dictionary to array arr.append(KP) # Find the highest and lowest position of x and y # (Used to draw rectangle) if y_high - y_low > x_high - x_low: height = y_high - y_low width = x_high - x_low else: height = x_high - x_low width = y_high - y_low # Draw rectangle (get width and height) y_high = int(y_high + height / 40) y_low = int(y_low - height / 12) x_high = int(x_high + width / 5) x_low = int(x_low - width / 5) # Append list of pose, x low, and y low list_of_pose.append(arr) list_of_x_low.append(x_low) list_of_y_low.append(y_low) except Exception as e: print(end="") return list_of_pose, list_of_x_low, list_of_y_low
def callback(self, ros_image, ros_depth): # Don't process if we have not obtained K matrix yet if not (self.fx and self.cx and self.fy and self.cy): return # Construct a frame with current time !before! pushing to OpenPose fr = Frame() fr.header.frame_id = self.frame_id fr.header.stamp = rospy.Time.now() # Convert images to cv2 matrices image = depth = None try: image = self.bridge.imgmsg_to_cv2(ros_image, "bgr8") depth = self.bridge.imgmsg_to_cv2(ros_depth, "32FC1") except CvBridgeError as e: rospy.logerr("CvBridge Error: {0}".format(e)) # Push data to OpenPose and block while waiting for results datum = op.Datum() datum.cvInputData = image if OPENPOSE1POINT7_OR_HIGHER: self.op_wrapper.emplaceAndPop(op.VectorDatum([datum])) else: self.op_wrapper.emplaceAndPop([datum]) pose_kp = datum.poseKeypoints lhand_kp = datum.handKeypoints[0] rhand_kp = datum.handKeypoints[1] # Set number of people detected if pose_kp is not None: if pose_kp.shape == (): num_persons = 0 body_part_count = 0 else: num_persons = pose_kp.shape[0] body_part_count = pose_kp.shape[1] else: return # Check to see if hands were detected lhand_detected = False rhand_detected = False hand_part_count = 0 if lhand_kp is not None: if lhand_kp.shape != (): lhand_detected = True hand_part_count = lhand_kp.shape[1] if rhand_kp is not None: if rhand_kp.shape != (): rhand_detected = True hand_part_count = rhand_kp.shape[1] # Handle body points fr.persons = [Person() for _ in range(num_persons)] try: for person in range(num_persons): fr.persons[person].bodyParts = [BodyPart() for _ in range(body_part_count)] fr.persons[person].leftHandParts = [BodyPart() for _ in range(hand_part_count)] fr.persons[person].rightHandParts = [BodyPart() for _ in range(hand_part_count)] detected_hands = [] if lhand_detected: detected_hands.append((lhand_kp, fr.persons[person].leftHandParts)) if rhand_detected: detected_hands.append((rhand_kp, fr.persons[person].rightHandParts)) # Process the body for bp in range(body_part_count): u, v, s = pose_kp[person, bp] x, y, z = self.convert_to_3d(u, v, depth) arr = fr.persons[person].bodyParts[bp] arr.pixel.x = u arr.pixel.y = v arr.score = s arr.point.x = x arr.point.y = y arr.point.z = z # Process left and right hands for hp in range(hand_part_count): for kp, harr in detected_hands: u, v, s = kp[person, hp] x, y, z = self.convert_to_3d(u, v, depth) arr = harr[hp] arr.pixel.x = u arr.pixel.y = v arr.score = s arr.point.x = x arr.point.y = y arr.point.z = z except IndexError as e: rospy.logerr("Indexing error occured: {}".format(e)) # return if self.display: self.frame = datum.cvOutputData.copy() self.pub.publish(fr)