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)