TOF深度相机数据到点云转换(一)

一、深度图的概念

1.深度图像的像素值反映场景中物体到相机的传感器平面的距离，获取深度图像的方法=被动测距传感+主动深度传感。
2.深度图像是物体的三维表示形式，一般通过立体照相机或者TOF照相机获取。如果具备照相机的内标定参数，可将深度图像转换为点云。
3.深度图像的获取方法有激光雷达深度成像法、计算机立体视觉成像、坐标测量机法、莫尔条纹法、结构光法等。
4.深度图往往以灰度形式或者伪彩色形式表现，但它和传统相机的到灰度图以及RGB图有本质的区别。

二、深度图和点云的数据存储

1.3D相机数据的数据存储形式，存储次序往往和传感器的像素排列一致

2.点云的数据存储形式，每行对应一个点的X/Y/Z坐标

三、TOF 3D相机数据中距离Z的计算关系

1.相机和光源位置重合

2.相机和光源位置不重合

对于原始的TOF相机数据，距离值指光源经过物体反射到达相机的距离L，光源和相机位置不重合时，按上面的方式计算物体的Z坐标。

四、 深度图和点云的转换代码示例（考虑光源位置的情况）

五、点云重投影到深度图

• 地面“调平”——通过点云数据调整，修正相机镜头和地面不平行
• 识别图像的视角归一化
• RGBD融合

1.使用点云重投影的运算过程
(1)原始深度图>>>点云
(2)点云通过坐标变换重投影到深度图
2.注意问题
(1)遮挡问题
由于遮挡，映射到相同像素坐标的物体在深度图中只保留最近物体
(1)透射问题
点云稀疏出发生“透射”现象,造成错误结果

3.解决方法

# !/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()