Open3D RGBD图显示彩色点云

# 引入python库numpy和open3d，可以通过pip install命令安装
import os
import numpy as np
import open3d as o3d
from openni import openni2
import cv2
# 打开文件路径

openni2.initialize()

dev = openni2.Device.open_any()
print(dev.get_device_info())

depth_stream = dev.create_depth_stream()
depth_stream.start()

cap = cv2.VideoCapture(0)
step = 0
innermtx = np.load(r"./mtx.npy")

vis = o3d.visualization.Visualizer()
# 创建播放窗口
vis.create_window()
pointcloud = o3d.geometry.PointCloud()
to_reset = True
vis.add_geometry(pointcloud)

while True:
    #  depth
    frame = depth_stream.read_frame()
    dframe_data = np.array(frame.get_buffer_as_triplet()).reshape([480, 640, 2])
    dpt1 = np.asarray(dframe_data[:, :, 0], dtype='float32')
    dpt2 = np.asarray(dframe_data[:, :, 1], dtype='float32')

    dpt2 *= 255
    dpt = dpt1 + dpt2
    dpt = dpt/1000
    print(np.max(dpt))
    dpt = cv2.flip(dpt, 2)
    dpt_temp = dpt.reshape(-1)
    idx_value = np.where(dpt_temp != 0.)[0]

    # colors
    ret, frame_img = cap.read()

    # pointcloud
    frame = o3d.geometry.Image(frame_img)
    
    dpt = o3d.geometry.Image(dpt)
    rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(frame, dpt, convert_rgb_to_intensity=False)

    maxtrix = o3d.camera.PinholeCameraIntrinsic(frame_img.shape[1], frame_img.shape[0], innermtx[0][0], innermtx[1][1], innermtx[0][2], innermtx[1][2])
    pcd = pointcloud.create_from_rgbd_image(rgbd_image, maxtrix)
    
    pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
    pointcloud.points = o3d.utility.Vector3dVector(pcd. points)
    pointcloud.colors = o3d.utility.Vector3dVector(pcd. colors)

    vis.update_geometry(pointcloud)
    print(pointcloud.has_points(), pointcloud.has_colors())

    if to_reset:
        vis.reset_view_point(True)
        to_reset = False
    vis.poll_events()
    vis.update_renderer()