深度图-＞点云图-＞深度图

此变化都基于相机坐标系

后面再做一个，基于相机坐标系的深度图到基于世界坐标系的点云图。基于世界坐标系的点云图到基于相机坐标系的深度图 

import os

import numpy as np
from PIL import Image

import torch
import  cv2

def get_intrinsic(w, h, fx, fy, cx, cy):
    cx = cx if cx > 0 else w / 2
    cy = cy if cy > 0 else h / 2
    intrinstic = torch.tensor([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=torch.float32)
    return intrinstic



# compute_vertex_map 函数将二维深度图转换为三维点云坐标，使用相机内参矩阵K
# 计算每个像素在相机坐标系下的三维坐标。通过这种方法，可以从深度图生成相应的三维点云
def compute_vertex_map(depth, K):
    H, W = depth.shape[:2]
    fx, fy, cx, cy = K[0, 0], K[1, 1], K[0, 2], K[1, 2]
    device = depth.device

    i, j = torch.meshgrid(torch.linspace(0, W - 1, W), torch.linspace(0, H - 1, H))  # pytorch's meshgrid has indexing='ij'
    i = i.t().to(device)  # [h, w]
    j = j.t().to(device)  # [h, w]

    vertex = torch.stack([(i - cx) / fx, (j - cy) / fy, torch.ones_like(i)], -1).to(device) * depth  # [h, w, 3]
    return vertex

def PILtoTorch(pil_image, resolution, method=Image.BILINEAR):
    resized_image_PIL = pil_image.resize(resolution, method)
    resized_image = torch.from_numpy(np.array(resized_image_PIL))
    if len(resized_image.shape) == 3:
        return resized_image.permute(2, 0, 1)
    else:
        return resized_image.unsqueeze(dim=-1).permute(2, 0, 1)

# 假设 depth_map 是一个 HxW 的深度图
def show_and_save_depth_map(depth_map, depth_scale,file_path='depth_map.png'):
    # 确保 depth_map 是一个 NumPy 数组
    if isinstance(depth_map, torch.Tensor):
        depth_map = depth_map.cpu().numpy()  # 移动到 CPU 并转换为 NumPy 数组

    # 确保 depth_map 是一个 2D 数组
    if depth_map.ndim == 3:
        depth_map = depth_map[:, :, 0]  # 如果是 3D 数组，取第一个通道

    depth_map=(depth_map*depth_scale).astype(np.uint16)
    cv2.imwrite(file_path, depth_map)

    # 显示深度图
    # cv2.imshow('Depth Map', depth_map_normalized)
    cv2.imwrite(file_path, depth_map)  # 保存为图片
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()

if __name__=="__main__":
    h=680
    w=1200
    fx=600
    fy=600
    cx=599.5
    cy=339.5
    depth_scale=6553.5
    resolution = (w, h)  # 目标分辨率

    # 读取深度图
    file_path=r"/media/kj/2B9747BF3C0EC4D0/Project/RTG-SLAM-Test/data/Replica/office4/results/depth000000.png"
    file_folder=r"/media/kj/2B9747BF3C0EC4D0/Project/RTG-SLAM-Test/data/Replica/office4/results"
    out_depth_img=r"/media/kj/2B9747BF3C0EC4D0/Project/RTG-SLAM-Test/data/Replica/office4/out_depth_img"

    for filename in os.listdir(file_folder):
        if filename.endswith(".png"):
            file_path=os.path.join(file_folder,filename)
            depth_image = Image.open(file_path).convert("F")
            image_depth = (
                    np.asarray(Image.open(file_path), dtype=np.float32) / depth_scale
            )
            depth_map=Image.fromarray(image_depth)
            resized_image_depth = PILtoTorch(depth_map, resolution, Image.NEAREST)


            depth_map = resized_image_depth.permute(1, 2, 0)
            intrinstic = get_intrinsic(w, h, fx, fy, cx, cy)
            vertex_map_c = compute_vertex_map(depth_map, intrinstic)

            # 顶点转换为深度图
            my_depth_map = np.zeros((depth_map.shape[0], depth_map.shape[1]), dtype=np.float32)
            for v in range(h):
                for u in range(w):
                    x, y, z = vertex_map_c[v, u]
                    my_depth_map[v, u] = z
            out_path=os.path.join(out_depth_img,filename)
            show_and_save_depth_map(my_depth_map,depth_scale,out_path)

 下面的代码还没有验证，但是应该差不多可以

为了将基于世界坐标系的点云转换到相机坐标系下的深度图，我们需要以下步骤：

1. **将世界坐标系的点云转换到相机坐标系**：
   使用给定的位姿矩阵将点云从世界坐标系转换到相机坐标系。

2. **将相机坐标系的点云投影到图像平面**：
   使用相机内参将点云投影到图像平面，生成深度图。

### 具体步骤

1. **将世界坐标系的点云转换到相机坐标系**：
   位姿矩阵提供了从世界坐标系到相机坐标系的变换。

2. **将相机坐标系的点云投影到图像平面**：
   使用相机内参将点云的三维坐标转换为二维像素坐标，同时生成深度值。

### 代码示例

import numpy as np
import cv2

def transform_point_cloud(point_cloud, pose):
    """
    将点云从世界坐标系转换到相机坐标系
    :param point_cloud: h*w*3 的点云，基于世界坐标系
    :param pose: 4x4 的位姿矩阵，将世界坐标系转换到相机坐标系
    :return: 转换后的点云，基于相机坐标系
    """
    h, w, _ = point_cloud.shape
    point_cloud_homogeneous = np.concatenate([point_cloud, np.ones((h, w, 1))], axis=-1)  # 转换为齐次坐标
    point_cloud_camera = np.einsum('ij,hwj->hwi', pose, point_cloud_homogeneous)[..., :3]  # 应用位姿矩阵
    return point_cloud_camera

def project_to_image_plane(point_cloud_camera, intrinsic):
    """
    将相机坐标系的点云投影到图像平面，生成深度图
    :param point_cloud_camera: h*w*3 的点云，基于相机坐标系
    :param intrinsic: 3x3 的相机内参矩阵
    :return: 深度图
    """
    fx, fy, cx, cy = intrinsic[0, 0], intrinsic[1, 1], intrinsic[0, 2], intrinsic[1, 2]
    h, w, _ = point_cloud_camera.shape
    depth_map = np.zeros((h, w), dtype=np.float32)

    # 投影到图像平面
    x = point_cloud_camera[..., 0]
    y = point_cloud_camera[..., 1]
    z = point_cloud_camera[..., 2]
    u = (x * fx / z + cx).astype(np.int32)
    v = (y * fy / z + cy).astype(np.int32)

    # 只处理有效的深度值
    valid_mask = (z > 0) & (u >= 0) & (u < w) & (v >= 0) & (v < h)
    depth_map[v[valid_mask], u[valid_mask]] = z[valid_mask]

    return depth_map

# 示例数据
pose = np.array([
    [6.894134520130755783e-01, -7.495637506797080130e-02, 7.204794473266287191e-01, 1.167238776983348769e+00],
    [-7.243680640278219451e-01, -7.133933127677465069e-02, 6.857124817513298165e-01, -2.976526525695861736e-01],
    [1.218072541897872703e-16, -9.946317115644626172e-01, -1.034782989343548815e-01, 6.015583338982717354e-01],
    [0.000000000000000000e+00, 0.000000000000000000e+00, 0.000000000000000000e+00, 1.000000000000000000e+00]
])

h, w = 680, 1200
fx, fy, cx, cy = 600, 600, 599.5, 339.5
intrinsic = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])

# 假设 point_cloud 是 h*w*3 的点云，基于世界坐标系
point_cloud = np.random.rand(h, w, 3)  # 这是一个示例，需要替换为实际点云数据

# 将点云从世界坐标系转换到相机坐标系
point_cloud_camera = transform_point_cloud(point_cloud, pose)

# 投影到图像平面，生成深度图
depth_map = project_to_image_plane(point_cloud_camera, intrinsic)

# 归一化并保存深度图
depth_map_normalized = np.clip(depth_map, a_min=0, a_max=np.max(depth_map))
depth_map_normalized = (depth_map_normalized / np.max(depth_map_normalized) * 255).astype(np.uint8)
cv2.imwrite('depth_map_normalized.png', depth_map_normalized)

# 保存为浮点型深度图
depth_map_scaled = (depth_map * 6553.5).astype(np.float32)
cv2.imwrite('depth_map_float.png', depth_map_scaled)

### 代码说明
1. **transform_point_cloud**: 将点云从世界坐标系转换到相机坐标系。
2. **project_to_image_plane**: 使用相机内参将点云投影到图像平面，生成深度图。
3. **示例数据**: 包括位姿矩阵、相机内参和假设的点云数据。
4. **生成并保存深度图**: 归一化并保存为 `.png` 文件，同时保存为浮点型深度图。

这个过程会生成两个深度图文件，一个是归一化后的8位深度图，一个是浮点型深度图。根据实际需求选择保存的格式和精度。