def __init__(self): # Kinect runtime object self.joint_count = PyKinectV2.JointType_Count # 25 self.kinect = PyKinectRuntime.PyKinectRuntime( PyKinectV2.FrameSourceTypes_Body | PyKinectV2.FrameSourceTypes_Color | PyKinectV2.FrameSourceTypes_Depth) self.depth_width, self.depth_height = self.kinect.depth_frame_desc.Width, self.kinect.depth_frame_desc.Height # Default: 512, 424 self.color_width, self.color_height = self.kinect.color_frame_desc.Width, self.kinect.color_frame_desc.Height # Default: 1920, 1080 # User defined variables self.depth_scale = 0.001 # Default kinect depth scale where 1 unit = 0.001 m = 1 mm # depth_scale = 1.0 # Default kinect depth scale where 1 unit = 0.001 m = 1 mm self.clipping_distance_in_meters = 1.5 # Set the maximum distance to display the point cloud data self.clipping_distance = self.clipping_distance_in_meters / self.depth_scale # Convert dist in mm to unit # Hardcode the camera intrinsic parameters for backprojection # width=depth_width; height=depth_height; ppx=258.981; ppy=208.796; fx=367.033; fy=367.033 # Hardcode the camera intrinsic parameters for backprojection ppx = 260.166 ppy = 205.197 fx = 367.535 fy = 367.535 # Open3D visualisation self.intrinsic = open3d.PinholeCameraIntrinsic(self.depth_width, self.depth_height, fx, fy, ppx, ppy) # To convert [x,y,z] -> [x.-y,-z] self.flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]] # 定义图像点云 self.image_pcd = open3d.PointCloud() # self.joint_pcd = open3d.PointCloud() # 定义原点 self.origin_point = open3d.geometry.create_mesh_coordinate_frame( size=0.5, origin=[0, 0, 0]) # 定义关节点坐标点云 self.axis_pcd = [] for i in range(self.joint_count): # 25 axes for 25 joints # XYZ axis length of 0.1 m pre_axis = open3d.create_mesh_coordinate_frame(size=0.1, origin=[0, 0, 0]) self.axis_pcd.append(pre_axis) # 定义关节点点云连接线:24关节点连接线 self.bone_line_pcd = utils.create_line_set_bones( np.zeros((24, 3), dtype=np.float32))
ppy = 205.197 fx = 367.535 fy = 367.535 # Hardcode the camera intrinsic parameters for backprojection # width=depth_width; height=depth_height; ppx=258.981; ppy=208.796; fx=367.033; fy=367.033 # Hardcode the camera intrinsic parameters for backprojection ############################ ### Open3D visualisation ### ############################ intrinsic = open3d.camera.PinholeCameraIntrinsic(width, height, fx, fy, ppx, ppy) flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]] # To convert [x,y,z] -> [x.-y,-z] # Define the objects to be drawn obj_pcd = open3d.geometry.PointCloud() obj_bone = utils.create_line_set_bones(np.zeros( (24, 3), dtype=np.float32)) # 24 bones connecting 25 joints obj_axis = [] for i in range(PyKinectV2.JointType_Count): # 25 axes for 25 joints obj_axis.append( open3d.geometry.TriangleMesh.create_coordinate_frame( size=0.1, origin=[0, 0, 0])) # XYZ axis length of 0.1 m # Create Open3D Visualizer vis = open3d.visualization.Visualizer() vis.create_window(width=width, height=height) vis.get_render_option().point_size = 3 vis.add_geometry(obj_bone) for i in range(PyKinectV2.JointType_Count): # 25 axes for 25 joints vis.add_geometry(obj_axis[i]) first_loop = True while True:
def rgbd_test(depth_img, align_color_img, body_joint2D): show_origin = True # 原点必须显示 show_image = True # 显示图像点云 show_bone_line = True # 显示关节点连接线 flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]] # 定义图像点云 image_pcd = open3d.PointCloud() # self.joint_pcd = open3d.PointCloud() # 定义原点 origin_point = open3d.geometry.create_mesh_coordinate_frame( size=0.5, origin=[0, 0, 0]) # 定义关节点点云连接线:24关节点连接线 bone_line_pcd = utils.create_line_set_bones( np.zeros((24, 3), dtype=np.float32)) # Create Open3D Visualizer vis = open3d.Visualizer() vis.create_window('Open3D_1', width=config.depth_width, height=config.depth_height, left=10, top=10) vis.get_render_option().point_size = 3 if show_origin: vis.add_geometry(origin_point) # 添加原始点到Visualizer if show_image: vis.add_geometry(image_pcd) # 添加图像点云到Visualizer if show_bone_line: vis.add_geometry(bone_line_pcd) # 添加24个关节点连接线到Visualizer while True: image_pcd.points, image_pcd.colors = utils.create_color_point_cloud( align_color_img, depth_img, config.depth_scale, config.clipping_distance_in_meters, config.intrinsic) image_pcd.transform(flip_transform) body_joint3D = utils.map_body_joint2D_3D(body_joint2D, depth_img, config.intrinsic, config.depth_scale) joint3D, orientation = utils.get_single_joint3D_orientation( body_joint3D, body_orientation=None) bone_line_pcd.points = open3d.Vector3dVector(joint3D) bone_line_pcd.transform(config.flip_transform) vis.update_geometry() vis.poll_events() vis.update_renderer() # self.draw_geometries() # Display 2D images using OpenCV depth_colormap = cv2.applyColorMap( cv2.convertScaleAbs(depth_img, alpha=255 / config.clipping_distance), cv2.COLORMAP_JET) align_color_img = utils.draw_joint2D_in_image(body_joint2D, align_color_img) # depth_colormap = utils.draw_key_point_in_image(body_joint, depth_colormap) d = posture.compute_joint3D_distance(joint3D, index=PyKinectV2.JointType_Head) # print("d={}".format(d)) image_processing.addMouseCallback("depth_colormap", param=depth_img) image_processing.addMouseCallback("align_color_img", param=depth_img) cv2.imshow('depth_img', depth_img) # (424, 512) cv2.imshow('depth_colormap', depth_colormap) # (424, 512) cv2.imshow("align_color_img", align_color_img) cv2.waitKey(30)