深度图与3D点云互转

深度图像和 3D 点云互转只涉及相机内参矩阵，其中

• fx，fy 分别为镜头 x，y 方向的焦距（成像平面到镜头的距离）
• cx，cy 分别是光心（镜头中心点在成像平面的投影）在图像坐标系（原点位于图像左上角，水平为 x，垂直为 y）下的坐标。

深度图到3D点

import numpy as np

# 加载深度数据
img = np.genfromtxt('img_dep_640x480.csv', delimiter=',').astype(np.float32)

# 参数
CAM_WID, CAM_HGT = 640, 480
CAM_FX, CAM_FY = 795.209, 793.957
CAM_CX, CAM_CY = 332.031, 231.308

# 转换
x, y = np.meshgrid(range(CAM_WID), range(CAM_HGT))
x = x.astype(np.float32) - CAM_CX
y = y.astype(np.float32) - CAM_CY

img_z = img.copy()
if False:  # 如果需要矫正视线到Z的转换的话使能
    f = (CAM_FX + CAM_FY) / 2.0
    img_z *= f / np.sqrt(x ** 2 + y ** 2 + f ** 2)

pc_x = img_z * x / CAM_FX  # X=Z*(u-cx)/fx
pc_y = img_z * y / CAM_FY  # Y=Z*(v-cy)/fy

pc = np.array([pc_x.ravel(), pc_y.ravel(), img_z.ravel()]).T

# 结果保存
np.savetxt('pc.csv', pc, fmt='%.18e', delimiter=',', newline='\n')

# 从CSV文件加载点云并显示
pc = np.genfromtxt('pc.csv', delimiter=',').astype(np.float32)

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

ax = plt.figure(1).gca(projection='3d')
ax.plot(pc[:, 0], pc[:, 1], pc[:, 2], 'b.', markersize=0.5)
plt.title('point cloud')
plt.show()

3D点到深度图

# !/usr/bin/python3
# coding=utf-8

import numpy as np

CAM_WID, CAM_HGT = 640, 480  # 重投影到的深度图尺寸
CAM_FX, CAM_FY = 795.209, 793.957  # fx/fy
CAM_CX, CAM_CY = 332.031, 231.308  # cx/cy

EPS = 1.0e-16

# 加载点云数据
pc = np.genfromtxt('pc_rot.csv', delimiter=',').astype(np.float32)

# 滤除镜头后方的点
valid = pc[:, 2] > EPS
z = pc[valid, 2]

# 点云反向映射到像素坐标位置
u = np.round(pc[valid, 0] * CAM_FX / z + CAM_CX).astype(int)
v = np.round(pc[valid, 1] * CAM_FY / z + CAM_CY).astype(int)

# 滤除超出图像尺寸的无效像素
valid = np.bitwise_and(np.bitwise_and((u >= 0), (u < CAM_WID)),
                       np.bitwise_and((v >= 0), (v < CAM_HGT)))
u, v, z = u[valid], v[valid], z[valid]

# 按距离填充生成深度图，近距离覆盖远距离
img_z = np.full((CAM_HGT, CAM_WID), np.inf)
for ui, vi, zi in zip(u, v, z):
    img_z[vi, ui] = min(img_z[vi, ui], zi)  # 近距离像素屏蔽远距离像素

# 小洞和“透射”消除
img_z_shift = np.array([img_z, \
                        np.roll(img_z, 1, axis=0), \
                        np.roll(img_z, -1, axis=0), \
                        np.roll(img_z, 1, axis=1), \
                        np.roll(img_z, -1, axis=1)])
img_z = np.min(img_z_shift, axis=0)

# 保存重新投影生成的深度图dep_rot
np.savetxt('dep_rot.csv', img_z, fmt='%.12f', delimiter=',', newline='\n')

# 加载刚保存的深度图dep_rot并显示
import matplotlib.pyplot as plt

img = np.genfromtxt('dep_rot.csv', delimiter=',').astype(np.float32)
plt.imshow(img, cmap='jet')
plt.show()