点云数据深度图与点云转换（二）
一、根据深度图数据生成点云
深度图转为点云说白了其实就是坐标系的变换：图像坐标系转换为世界坐标系。变换的约束条件就是相机内参。其中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=0),(v 二、点云重新投影生成深度图
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()