def create_pcd_from_rgbd(rgb_img, depth_img):
"""
*Compute point cloud from depth*
"""
#depth_img 单位um
t1 = time.time()
depth = depth_img
rgb = rgb_img#[:, :, ::-1]
h,w,ch=rgb_img.shape
xmap = np.arange(w)
ymap = np.arange(h)
xmap, ymap = np.meshgrid(xmap, ymap)
points_z = depth*1000
points_x = xmap*10
points_y = ymap*10
pcd = np.stack([points_x, points_y, points_z], axis=-1)
pcd.shape = (h*w,3)
point_cloud = o3.geometry.PointCloud()
pcd_rgb=rgb_img.copy()
pcd_rgb.shape= (h*w,3)
point_cloud.points = o3.utility.Vector3dVector(pcd)
point_cloud.colors = o3.utility.Vector3dVector(pcd_rgb)
t2 = time.time()
print('create_pcd_from_rgbd time: {}'.format(t2 - t1))
return point_cloud
要注意一点,
rgb_img需要是浮点数,0~1之间
depth_img也是浮点数,且大小与rgb_img保持一致。
最终的点云图就可以通过o3.visualization.draw_geometries([point_cloud])来显示了。