def skeleton_motion_data_collect(skeleton_file_list): train_data = [] total_frame = 0 fc_list = [] # frame count list index = 0 while index < len(skeleton_file_list): # ----------------------- # -----[ Data Load ]----- # ----------------------- print('list number: ', index) print('skeleton: ', skeleton_file_list[index]) file = open(skeleton_file_list[index]) frameCount = np.fromstring(file.readline(), sep=' ', dtype='int32')[0] skel = Skeleton() for f in range(frameCount): # frameCount bodyCount = np.fromstring(file.readline(), sep=' ', dtype='int32')[0] body = Body_struct() for b in range(bodyCount): bodyInfo = np.fromstring(file.readline(), sep=' ', dtype='float64') jointCount = np.fromstring(file.readline(), sep=' ', dtype='int32')[0] body.set_body_info(bodyInfo, jointCount, f * 1.0 / frameCount) for j in range(jointCount): jointInfo = np.fromstring(file.readline(), sep=' ', dtype='float64') joint = Joint_struct(jointInfo) body.joints.append(joint) skel.append_body(f, b, body) # print('phase: ', body.phase) train_data.append(skel) file.close() # print(skel.print_skeleton_info()) index += 1 total_frame += len(skel.iBody) fc_list.append(len(skel.iBody)) print('total number of frames: ', total_frame) print('train data len: ', len(train_data)) return train_data, total_frame, fc_list
skel = Skeleton() for f in range(frameCount): # frameCount bodyCount = np.fromstring(file.readline(), sep=' ', dtype='int32')[0] for b in range(bodyCount): body = Body_struct() bodyInfo = np.fromstring(file.readline(), sep=' ', dtype='float64') jointCount = np.fromstring(file.readline(), sep=' ', dtype='int32')[0] body.set_body_info(bodyInfo, jointCount, f*1.0/frameCount) for j in range(jointCount): jointInfo = np.fromstring(file.readline(), sep=' ', dtype='float64') joint = Joint_struct(jointInfo) body.joints.append(joint) try: skel.append_body(f, b, body) except IndexError: exception_avi_list.append(video_file_list[index]) exception_skel_list.append(skeleton_file_list[index]) print('Skeleton was not found..') file.close() # print(skel.print_skeleton_info()) # ---------------------------------------- # -----[ Display Image and Skeleton ]----- # ---------------------------------------- fCount = 0 # frame count while(cap.isOpened()): ret, frame = cap.read()
def run(self): # -----[ For socket Communication, Server ] ----- if socket_comm: conn = SocketCom('localhost', tcp_port) print('Port is Opened and Wait for the connection..') if retarget_mode == 'Analytic': # direct connect to the NAO agent nao = NAO_AMR() conn.socket_connect() conn.write_socket({ 'header': 'start Kinect!', 'data': [] }) # start signal else: # indirect connect to the NAO by C-3PO model demo = MR_Demo(target_robot=target_robot, target_env=target_env, tcp_port=tcp_port) if target_env == 'CHREO': conn.socket_connect() # -------- Main Program Loop ----------- demo_body_idx = -1 # only for the firstly detected human socket_count = 0 while not self._done: # --- Main event loop for event in pygame.event.get(): # User did something if event.type == pygame.QUIT: # If user clicked close self._done = True # Flag that we are done so we exit this loop elif event.type == pygame.VIDEORESIZE: # window resized self._screen = pygame.display.set_mode( event.dict['size'], pygame.HWSURFACE | pygame.DOUBLEBUF | pygame.RESIZABLE, 32) # --- Game logic should go here # --- Getting frames and drawing # --- Woohoo! We've got a color frame! Let's fill out back buffer surface with frame's data if self._kinect.has_new_color_frame(): frame = self._kinect.get_last_color_frame() self.draw_color_frame(frame, self._frame_surface) frame = None # --- Cool! We have a body frame, so can get skeletons if self._kinect.has_new_body_frame(): self._bodies = self._kinect.get_last_body_frame() # --- draw skeletons to _frame_surface if self._bodies is not None: # Get the index of the first body only if demo_body_idx < 0: # no body index # Get new body index for i in range(0, self._kinect.max_body_count): body = self._bodies.bodies[i] if body.is_tracked: demo_body_idx = i break else: # already have a body index body = self._bodies.bodies[demo_body_idx] if not body.is_tracked: demo_body_idx = -1 if demo_body_idx >= 0: # print('Demo body index: ', demo_body_idx) body = self._bodies.bodies[demo_body_idx] joints = body.joints joint_points = self._kinect.body_joints_to_color_space( joints) # get a skeleton frame from Kinect skel = Skeleton() body_struct = Body_struct() bodyInfo = np.array([ demo_body_idx, 0, 0, 0, 0, 0, 0, 0, 0, body.is_tracked ]) jointCount = PyKinectV2.JointType_Count body_struct.set_body_info(bodyInfo, jointCount, phase=0) for j in range(jointCount): # 25 body joints jointInfo = np.array([ joints[j].Position.x, joints[j].Position.y, joints[j].Position.z, 0, 0, 0, 0, 0, 0, 0, 0, body.is_tracked ]) joint = Joint_struct(jointInfo) body_struct.joints.append(joint) skel.append_body(0, demo_body_idx, body_struct) if socket_comm: socket_count += 1 if socket_count >= 1: # 6, for sync of socket delay socket_count = 0 _from = _to = 'None' skel_data = skeleton_coordinate_transform( [skel], 1) # numpy data if retarget_mode == 'Analytic': data = skel_data[0].tolist()[:-1] lMotors = nao.left_arm_solve(data) rMotors = nao.right_arm_solve(data) motors = lMotors + rMotors conn.write_socket({ 'header': 'Analytic', 'data': motors, 'from': _from, 'to': _to }) else: if target_env == 'CHREO': retargeted_skel = demo.do_retargeting( skel_data[0].tolist()[:-1]) conn.write_socket({ 'header': 'SetMotor', 'data': retargeted_skel, 'from': _from, 'to': _to }) elif target_env == 'VREP': ret = demo.do_retargeting_vrep( skel_data[0].tolist()[:-1]) conn.flush() print('Send Success') self.draw_body(joints, joint_points, SKELETON_COLORS[demo_body_idx]) # --- copy back buffer surface pixels to the screen, resize it if needed and keep aspect ratio # --- (screen size may be different from Kinect's color frame size) h_to_w = float(self._frame_surface.get_height( )) / self._frame_surface.get_width() # (w: 1920, h: 1080) target_height = int(h_to_w * self._screen.get_width()) surface_to_draw = pygame.transform.scale( self._frame_surface, (self._screen.get_width(), target_height)) self._screen.blit(surface_to_draw, (0, 0)) surface_to_draw = None pygame.display.update() # --- Go ahead and update the screen with what we've drawn. pygame.display.flip() # --- Limit to 60 frames per second self._clock.tick(60) # Close our Kinect sensor, close the window and quit. self._kinect.close() pygame.quit()