python 深度图转点云 + 基于点云的相机测距

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), 见代码中的"相机测距"部分