深度图+rgb转点云

import open3d as o3d
import numpy as np

# 1. 读取图像和全局点云
image = o3d.io.read_image("/media/kj/2B9747BF3C0EC4D0/SLAMData/lidar_rgbd_bedroom/image/000000.jpg")
depth = o3d.io.read_image("/media/kj/2B9747BF3C0EC4D0/SLAMData/lidar_rgbd_bedroom/depth/000000.png")
all_pcd = o3d.io.read_point_cloud("/media/kj/2B9747BF3C0EC4D0/SLAMData/lidar_rgbd_bedroom/scan_02.ply")

# 2. 设置相机内参
intrinsic = o3d.camera.PinholeCameraIntrinsic(
    width=640,
    height=480,
    fx=525.0,
    fy=525.0,
    cx=319.5,
    cy=239.5
)

# 3. 定义位姿矩阵 (4x4)
pose = np.array([
    [1.0, -2e-08, 1e-08, 2.0000000529999999],
    [2e-08, 1.0, 1e-08, 1.999999973],
    [-1e-08, -1e-08, 1.0, -0.29999995000000002],
    [0.0, 0.0, 0.0, 1.0]
])

# 4. 创建RGBD图像
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color=image,
    depth=depth,
    convert_rgb_to_intensity=False
)

# 5. 使用相机内参将RGBD图像转换为点云
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
    rgbd_image,
    intrinsic
)

# 6. 将点云从相机坐标系转换到全局坐标系
pcd.transform(pose)

# 7. 显示转换后的点云
# o3d.visualization.draw_geometries([pcd, all_pcd], window_name="Image PCD with Global PCD")

# 8. 保存生成的点云
o3d.io.write_point_cloud("/media/kj/2B9747BF3C0EC4D0/SLAMData/lidar_rgbd_bedroom/generated_pcd.ply", pcd)