def show(self, align_color_img, depth_img, body_joint2D=None):
'''
:param align_color_img: <class 'tuple'>: (424, 512, 3),uint8
:param depth_img: <class 'tuple'>: (424, 512),uint16
:param body_joint2D:
:return:
'''
self.image_pcd.points, self.image_pcd.colors = open3d_tools.create_color_point_cloud(align_color_img,
depth_img,
self.camera_conf.depth_scale,
self.camera_conf.clipping_distance_in_meters,
self.camera_conf.intrinsic)
self.image_pcd.transform(self.flip_transform)
# pointcloud = pcl_tools.open3d2DepthColor2Cloud(np.asarray(self.image_pcd.points), np.asarray(self.image_pcd.colors))
# pcl_tools.view_cloud(pointcloud) # 显示点云
if body_joint2D is not None:
# body_joint3D = utils.map_body_joint2D_3D(body_joint, depth_img, config.intrinsic, config.depth_scale)
body_joint3D = open3d_tools.convert_point2D_3D_list(body_joint2D,
depth_img,
self.camera_conf.intrinsic,
self.camera_conf.depth_scale)
joint3D, orientation = open3d_tools.get_single_joint3D_orientation(body_joint3D, body_orientation=None,
joint_count=joint_count)
self.bone_line_pcd.points = open3d.Vector3dVector(joint3D)
self.bone_line_pcd.transform(self.camera_conf.flip_transform)
# self.draw_geometries()
# Display 2D images using OpenCV
align_color_img = open3d_tools.draw_joint2D_in_image(body_joint2D, align_color_img,self.camera_conf.joint_lines)
# depth_colormap = utils.draw_key_point_in_image(body_joint, depth_colormap)
# d = geometry_tools.compute_joint3D_distance(joint3D, index=JointType_Head)
d = open3d_tools.compute_point2area_distance(self.desktop_3d_point, joint3D[joint_head])
points_2d, _ = open3d_tools.convert_point3D_2D(joint3D[joint_head], self.camera_conf.intrinsic,
self.camera_conf.depth_scale)
print("points:{},distance:{}".format(points_2d,d))
align_color_img = image_processing.show_points_text(align_color_img, points=points_2d, texts=[str(d)],
color=None)
self.vis.update_geometry()
self.vis.poll_events()
self.vis.update_renderer()
image_processing.addMouseCallback("depth_colormap", param=depth_img, callbackFunc=self.default_callbackFunc)
image_processing.addMouseCallback("align_color_img", param=depth_img, callbackFunc=self.default_callbackFunc)
align_color_img = image_processing.draw_point_line("align_color_img", align_color_img,
self.triangle_points_2d.tolist(),
line=True, waitKey=1)
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_img, alpha=255 / self.camera_conf.clipping_distance),
cv2.COLORMAP_JET)
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)
def show_depth(depth_img, align_color_img, body_joint2D):
align_color_img2 = utils.draw_joint2D_in_image(body_joint2D,
align_color_img)
depth_img2 = utils.draw_joint2D_in_image(body_joint2D, depth_img)
# Resize (1080, 1920, 4) into half (540, 960, 4)
# Scale to display from 0 mm to 1500 mm
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_img2, alpha=255 / 1500), cv2.COLORMAP_JET)
# Scale from uint16 to uint8
image_processing.addMouseCallback("align_color_img",
align_color_img,
callbackFunc=None)
image_processing.addMouseCallback("depth", depth_img, callbackFunc=None)
image_processing.cv_show_image('align_color_img',
align_color_img2,
type="bgr",
waitKey=30) # (424, 512)
image_processing.cv_show_image('depth', depth_colormap,
type="bgr") # (424, 512)
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)
def start_capture(self):
show_origin = True # 原点必须显示
show_axis = True # 显示关节朝向
show_image = False # 显示图像点云
show_bone_line = True # 显示关节点连接线
##
# Create Open3D Visualizer
self.vis = open3d.Visualizer()
self.vis.create_window('Open3D_1',
width=self.depth_width,
height=self.depth_height,
left=10,
top=10)
self.vis.get_render_option().point_size = 3
if show_origin:
self.vis.add_geometry(self.origin_point) # 添加原始点到Visualizer
if show_image:
self.vis.add_geometry(self.image_pcd) # 添加图像点云到Visualizer
if show_bone_line:
self.vis.add_geometry(self.bone_line_pcd) # 添加24个关节点连接线到Visualizer
if show_axis:
for i in range(self.joint_count): # 添加25个关节点的朝向到Visualizer
self.vis.add_geometry(self.axis_pcd[i])
# self.vis.add_geometry(self.joint_pcd) # 添加25个关节点到Visualizer
while True:
# Get images from camera
if self.kinect.has_new_body_frame(
) and self.kinect.has_new_color_frame(
) and self.kinect.has_new_depth_frame():
body_frame = self.kinect.get_last_body_frame()
color_frame = self.kinect.get_last_color_frame()
depth_frame = self.kinect.get_last_depth_frame()
# Reshape from 1D frame to 2D image
color_img = color_frame.reshape(
((self.color_height, self.color_width,
4))).astype(np.uint8)
depth_img = depth_frame.reshape(
((self.depth_height, self.depth_width))).astype(np.uint16)
align_color_img = utils.get_align_color_image(
self.kinect, color_img)
# Useful functions in utils_PyKinectV2.py
self.image_pcd.points, self.image_pcd.colors = utils.create_color_point_cloud(
align_color_img, depth_img, self.depth_scale,
self.clipping_distance_in_meters, self.intrinsic)
self.image_pcd.transform(self.flip_transform)
# joint3D, orientation = utils.get_single_joint3D_and_orientation(self.kinect, body_frame, depth_img,
# self.intrinsic,
# self.depth_scale)
body_joint2D, body_orientation = utils.get_body_joint2D(
body_frame, self.kinect, map_space="depth_space")
body_joint3D = utils.map_body_joint2D_3D(
body_joint2D, depth_img, self.intrinsic, self.depth_scale)
joint3D, orientation = utils.get_single_joint3D_orientation(
body_joint3D, body_orientation)
self.bone_line_pcd.points = open3d.Vector3dVector(joint3D)
self.bone_line_pcd.transform(self.flip_transform)
if show_axis:
self.add_orientation(joint3D, orientation)
self.vis.update_geometry()
self.vis.poll_events()
self.vis.update_renderer()
# self.draw_geometries()
if show_axis:
self.inverse_transformation(joint3D, orientation)
# Display 2D images using OpenCV
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_img,
alpha=255 / self.clipping_distance),
cv2.COLORMAP_JET)
image_processing.addMouseCallback("depth_img", param=depth_img)
image_processing.addMouseCallback("align_color_img",
param=align_color_img)
cv2.imshow('depth_img', depth_colormap) # (424, 512)
cv2.imshow("align_color_img", align_color_img)
key = cv2.waitKey(30)
if key == 27: # Press esc to break the loop
break
color_width, color_height = kinect.color_frame_desc.Width, kinect.color_frame_desc.Height # Default: 1920, 1080
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
clipping_distance_in_meters = 4.080 # Set the maximum distance to display the point cloud data
clipping_distance = clipping_distance_in_meters / depth_scale # Convert dist in mm to unit
while True:
if kinect.has_new_color_frame() and kinect.has_new_depth_frame():
body_frame = kinect.get_last_body_frame()
color_frame = kinect.get_last_color_frame()
depth_frame = kinect.get_last_depth_frame()
# Reshape from 1D frame to 2D image ###
color_img = color_frame.reshape(((color_height, color_width, 4))).astype(np.uint8)
depth_img = depth_frame.reshape(((depth_height, depth_width))).astype(np.uint16)
# Useful functions in utils_PyKinectV2.py
align_color_img = utils.get_align_color_image(kinect, color_img)
rgbd_image = utils.get_rgbd_image(align_color_img, depth_img, depth_scale, clipping_distance_in_meters)
# depth_img1 = np.uint8(depth_img.clip(1, 4080) / 16.) # 转换为uint8时，需要避免溢出255*16=4080
depth_img1 = np.float32(depth_img * depth_scale).clip(0, clipping_distance_in_meters) # 转换为uint8时，需要避免溢出255*16=4080
depth_img2 = np.asarray(rgbd_image.depth)
color_img2 = np.asarray(rgbd_image.color)
image_processing.addMouseCallback("depth_img", depth_img1)
image_processing.addMouseCallback("depth_img2", depth_img2)
cv2.imshow('depth_img', depth_img1)
cv2.imshow('align_color_img', align_color_img)
cv2.imshow('depth_img2', depth_img2)
cv2.imshow('color_img2', color_img2)
cv2.waitKey(2)