import numpy as np
def depth2xyz(depth_map,depth_cam_matrix,flatten=False,depth_scale=1000):
fx,fy = depth_cam_matrix[0,0],depth_cam_matrix[1,1]
cx,cy = depth_cam_matrix[0,2],depth_cam_matrix[1,2]
h,w=np.mgrid[0:depth_map.shape[0],0:depth_map.shape[1]]
z=depth_map/depth_scale
x=(w-cx)*z/fx
y=(h-cy)*z/fy
xyz=np.dstack((x,y,z)) if flatten==False else np.dstack((x,y,z)).reshape(-1,3)
#xyz=cv2.rgbd.depthTo3d(depth_map,depth_cam_matrix)
return xyz
if __name__ == '__main__':
# 随便生成一个 分辨率为(720, 1280) -> (高为720, 宽为1280)的深度图, 注意深度图shape为(720, 1280)即深度图为单通道, 维度为2
#而不是类似于shape为(720, 1280, 3)维度为3的这种
depth_map = np.random.randint(0,10000,(720, 1280))
depth_cam_matrix = np.array([[540, 0, 640],
[0, 540,360],
[0, 0, 1]])
pc = depth2xyz(depth_map, depth_cam_matrix) # 此时pc即为点云(point cloud)
pc_flatten = pc.reshape(-1, 3) # 等价于 pc = depth2xyz(depth_map, depth_cam_matrix, flatten=True)
'''
################### 相机测距 ##################
置 flatten=False 此时的pc是具有二维信息的 既shape为(720, 1280, 3) 否则为(720 * 1280, 3)
此时 rgb 图片和点云的shape是一样的, 都为(720, 1280, 3)
假设此时欲想测图片中的一个箱子的在真实世界中的长度, 箱子长边的一角a在rgb图片中的像素坐标为 (500, 100) -> (纵坐标, 横坐标), 长边的另一角b的像素坐标为(600, 200) -> (纵坐标, 横坐标)
则a, b两点在 pc 中的xyz坐标就是已知的, 因此此时只需求取a, b两点对应的xyz坐标的欧氏距离就是该箱子的长度. 至于a, b两像素点的来源可能是你在rgb图片上对着箱子手动标出的, 也可能是算法得出的比如角点检测
note1: 基于点云的测距不受相机角度影响, 只要能保证像素点对应的xyz精度够高, 相机可以从任何角度拍摄并得到正确的距离
note2: 相机测距如果想测得准则对深度相机的精度要求比较高, 如果深度相机精度不高 测出来的长度会十分糟糕(在depth2xyz中可以看出xy的计算均与z有关), 本人在realense 435, 415, 515 上使用过测距功能, 精度上 515 > 415 > 435, 因此测距功能 515 > 415 > 435
当然 工业级深度相机的精度大部分是要远高于realsense这类消费级深度相机的, 价钱也更贵一些
'''
'''
# 代码如下:
a = pc[500, 100] # (500, 100)为上面提到的像素坐标
b = pc[600, 200] # (600, 200)为上面提到的像素坐标
ax, ay, az = a
bx, by, bz = b
# 此时 distance 就是箱子的长度
distance = ((ax - bx) ** 2 + (ay - by) ** 2 + (az - bz) ** 2) ** 0.5
# distance = np.linalg.norm(a - b)
'''
输入参数:
1. depth_map: 深度图, 个人采用realsense D415, 采集出来的depth_map为数据类型为uint16;
2. depth_cam_matrix: 深度相机内参矩阵, 应为二维矩阵, 并沿用以下格式
[[ fx, 0, cx],
[ 0, fy, cy],
[ 0, 0, 1]]
3. flatten(bool) : 是否铺平, 如为False则保持二维信息 shape (H, W, 3), 为True则铺平点云 shape (H * W, 3), 其中 H, W为深度图的 高与宽, 若铺平则与 cv2.rgbd.depthTo3d(depth_map,depth_cam_matrix)结果相同
4. depth_scale: 深度缩放因子,realsense 415的depth_scale 为1000, 指的是毫米到米的变化, 415采集出来的depth_map单位是毫米, 部分相机比较特殊depth_scale 可以到相机文档中找到
输出参数:
xyz : 点云
-----------------------------------补充----------------------------------
有被问到几个问题, 我额外开了几个博客回答.
1. 单个或多个像素坐标 到 点云坐标的变换
见 python 像素坐标转点云坐标_tycoer的博客-CSDN博客
2. 点云从相机坐标系到世界坐标系的变换 或者 点云从世界坐标系到相机坐标系的变换
见 python 点云从相机坐标系到世界坐标系/ 世界坐标系到相机坐标系下的变换_tycoer的博客-CSDN博客
3. 相机测距
最典型的就是苹果手机上的相机测距功能. 这要求相机需带有同时载有彩色相机和深度相机(典型如 Realsense), 见代码中的"相机测距"部分