深度图与RGB图生成点云

深度图与RGB图转点云代码示例importopen3daso3dimportcv2importnumpyasnpdefgenerate_pcd(rgb_img:np.ndarray,depth_img:np.ndarray,cam_k:np.ndarray,depth_scale:float1.0,depth_trunc:float10.0,):ifdepth_img.ndim2:img_h,img_wdepth_img.shapeelse:img_h,img_wdepth_img.shape[:2]depth_img_o3do3d.geometry.Image(depth_img.astype(np.float32))fxcam_k[0,0]fycam_k[1,1]cxcam_k[0,2]cycam_k[1,2]intrinsico3d.camera.PinholeCameraIntrinsic(img_w,img_h,fx,fy,cx,cy)ifrgb_imgisnotNone:ifrgb_img.ndim2:rgb_imgcv2.cvtColor(rgb_img,cv2.COLOR_GRAY2RGB)elifrgb_img.shape[2]4:rgb_imgcv2.cvtColor(rgb_img,cv2.COLOR_BGRA2RGB)else:rgb_imgcv2.cvtColor(rgb_img,cv2.COLOR_BGR2RGB)rgb_img_o3do3d.geometry.Image(rgb_img.astype(np.uint8))rgbd_imgo3d.geometry.RGBDImage.create_from_color_and_depth(rgb_img_o3d,depth_img_o3d,depth_scaledepth_scale,depth_truncdepth_trunc,convert_rgb_to_intensityFalse,)pcdo3d.geometry.PointCloud.create_from_rgbd_image(rgbd_img,intrinsic)else:pcdo3d.geometry.PointCloud.create_from_depth_image(depth_img_o3d,intrinsic,depth_scaledepth_scale,depth_truncdepth_trunc,)returnpcddefmain():rgb_imgcv2.imread(examples/1/rgb.jpg,cv2.IMREAD_COLOR)ifrgb_imgisNone:raiseFileNotFoundError(RGB image not found: examples/1/rgb.jpg)depth_imgcv2.imread(examples/1/raw_depth.png,cv2.IMREAD_UNCHANGED)ifdepth_imgisNone:raiseFileNotFoundError(Depth image not found: examples/1/raw_depth.png)depth_imgdepth_img.astype(np.float32)/1000.0cam_knp.loadtxt(examples/1/intrinsics.txt)print(cam_k)pcdgenerate_pcd(rgb_img,depth_img,cam_k,depth_scale1.0)o3d.io.write_point_cloud(point_cloud.ply,pcd)print(Saved point_cloud.ply)if__name____main__:main()