点云数据深度图与点云转换（二）

一、根据深度图数据生成点云

深度图转为点云说白了其实就是坐标系的变换：图像坐标系转换为世界坐标系。变换的约束条件就是相机内参。其中x,y,z是点云坐标系，x’,y’是图像坐标系，Ｄ为深度值。

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()

二、点云重新投影生成深度图

import numpy as np

# 加载数据
img=np.genfromtxt('dep640x480.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],'k.',markersize=0.05)
plt.title('point cloud')
plt.show()