前言
本文主要记录一下如何可视化相机位姿，如何用Blender得到的深度图反投影到3D空间，得到相应的点云。

Refernce

https://github.com/colmap/colmap/issues/1106
https://github.com/IntelRealSense/librealsense/issues/12090
https://medium.com/yodayoda/from-depth-map-to-point-cloud-7473721d3f
https://stackoverflow.com/questions/59590200/generate-point-cloud-from-depth-image
https://github.com/isl-org/Open3D/issues/481
https://stackoverflow.com/questions/31265245/extracting-3d-coordinates-given-2d-image-points-depth-map-and-camera-calibratio
https://github.com/vitalemonate/depth2Cloud

1 可视化相机位姿

import open3d as o3d
import numpy as np

focal = 346.4101498574051
img_w = img_h = 400.0
intrinsic = np.array([[focal, 0., -img_w / 2],
                      [0., -focal, -img_h / 2],
                      [0., 0., -1.]])
print(intrinsic)
pcds = []
# 创建相机线集并添加到列表中
for pose in poses:
#     extrinsic = pose
    extrinsic = np.eye(4)
    R, t = pose[:3, :3], pose[:3, 3]
    extrinsic[:3, :3] = R.T
    extrinsic[:3, 3] = -np.dot(R.T, t)
    print(extrinsic)
    cam_pcd = o3d.geometry.LineSet()
    cam_pcd = cam_pcd.create_camera_visualization(view_width_px=400,
                                                  view_height_px=400,
                                                  intrinsic=intrinsic,
                                                  extrinsic=extrinsic)
#     cam_pcd.paint_uniform_color(color)
#     cam_pcd.colors[4] = 0.5 * color
    cam_pcd.scale(scale=1., center=t)
    pcds.append(cam_pcd)

# 初始化Open3D的可视化窗口
vis = o3d.visualization.Visualizer()
vis.create_window()

vis.add_geometry()

# 添加相机线集到可视化窗口
for cam_pcd in pcds:
    vis.add_geometry(cam_pcd)

# 设置视图参数
vis.get_render_option().background_color = [0.5, 0.5, 0.5]  # 设置背景颜色为灰色
vis.get_render_option().point_show_normal = True  # 显示法线

# 更新可视化窗口
# vis.update_geometry()

# 运行可视化窗口
while True:
    vis.poll_events()
    vis.update_renderer()
    

# 关闭可视化窗口
vis.destroy_window()

2 Depth2PointCloud

2.1 depth map反投影至三维空间

# 将depth map反投影至三维空间
def depth_image_to_point_cloud(rgb, depth, scale, K, pose):
    u = range(0, rgb.shape[1])
    v = range(0, rgb.shape[0])

    u, v = np.meshgrid(u, v)
    u = u.astype(float)
    v = v.astype(float)
    
    # K为内参矩阵3*3
    # 图片坐标转相机坐标
    Z = depth.astype(float) / scale
    X = (u - K[0, 2]) * Z / K[0, 0]
    Y = (v - K[1, 2]) * Z / K[1, 1]
    X = np.ravel(X)
    Y = -np.ravel(Y)   # Blender的坐标系为[x, -y, -z]
    Z = -np.ravel(Z)

    valid = Z < 0
    X = X[valid]
    Y = Y[valid]
    Z = Z[valid]
    position = np.vstack((X, Y, Z, np.ones(len(X))))
    
    # 相机坐标转世界坐标
    transform = np.array([[1, 0, 0, 0],
                          [0, -1, 0, 0],
                          [0, 0, -1, 0],
                          [0, 0, 0, 1]])
    pose = np.dot(transform, pose)
    position = np.dot(pose, position)
    
    R = np.ravel(rgb[:, :, 0])[valid]
    G = np.ravel(rgb[:, :, 1])[valid]
    B = np.ravel(rgb[:, :, 2])[valid]
    
    print(position.shape, R.shape)
    points = np.transpose(np.vstack((position[:3, :], R, G, B))).tolist()

    return points

2.2 将点云保存至ply文件

import os
import numpy as np
import cv2
from path import Path
from tqdm import tqdm

# 将点云写入ply文件
def write_point_cloud(ply_filename, points):
    formatted_points = []
    for point in points:
        formatted_points.append("%f %f %f %d %d %d 0\n" % (point[0], point[1], point[2], point[3], point[4], point[5]))

    out_file = open(ply_filename, "w")
    out_file.write('''ply
    format ascii 1.0
    element vertex %d
    property float x
    property float y
    property float z
    property uchar blue
    property uchar green
    property uchar red
    property uchar alpha
    end_header
    %s
    ''' % (len(points), "".join(formatted_points)))
    out_file.close()
    
    
# image_files: XXXXXX.png (RGB, 24-bit, PNG)
# depth_files: XXXXXX.png (16-bit, PNG)
# poses: camera-to-world, 4×4 matrix in homogeneous coordinates
def build_point_cloud(dataset_path, scale, view_ply_in_world_coordinate, poses):
    K = np.fromfile(os.path.join(dataset_path, "K.txt"), dtype=float, sep="\n ")
    K = np.reshape(K, newshape=(3, 3))
    print(K)
    print(poses)
    image_files = sorted(Path(os.path.join(dataset_path, "images")).files('*.png'))
    depth_files = sorted(Path(os.path.join(dataset_path, "depth_maps")).files('*.png'))
    sum_points_3D = []
    for i in tqdm(range(0, len(image_files))):
        image_file = image_files[i]
        depth_file = depth_files[i]

        rgb = cv2.imread(image_file)
        depth = cv2.imread(depth_file, -1).astype(np.uint16)

        if view_ply_in_world_coordinate:
            current_points_3D = depth_image_to_point_cloud(rgb, depth, scale=scale, K=K, pose=poses[i])
        else:
            current_points_3D = depth_image_to_point_cloud(rgb, depth, scale=scale, K=K, pose=poses[i])
#             print(len(current_points_3D), current_points_3D[0])
        save_ply_name = os.path.basename(os.path.splitext(image_files[i])[0]) + ".ply"
        save_ply_path = os.path.join(dataset_path, "point_clouds")

        if not os.path.exists(save_ply_path):  # 判断是否存在文件夹如果不存在则创建为文件夹
            os.mkdir(save_ply_path)
        write_point_cloud(os.path.join(save_ply_path, save_ply_name), current_points_3D)
        sum_points_3D.extend(current_points_3D)
    write_point_cloud(os.path.join(save_ply_path, 'all.ply'), sum_points_3D)