深度图像转换为点云数据计算原理及代码实现

转载：https://blog.csdn.net/CH_monsy/article/details/113312644

转载：https://blog.csdn.net/cszn6666/article/details/107400778

转载：https://blog.csdn.net/lixianjun913/article/details/10032019

转载：https://zhuanlan.zhihu.com/p/360320545

深度图转换为点云

深度图转换为点云是3D点投影到2D平面的逆过程，有以下两个基础知识需要了解

深度图

深度图中的单个像素值是空间中物体的某个点到垂直于镜头光轴并通过镜头光心（深度相机光学零点）平面的垂直距离。注意不是物体到相机的直线距离，如果是直线距离，则需要近距离到深度的转换；

/*!
\brief TOF距离转深度。
@param [in] image 距离图。
@param [in] fx TOF相机内参X方向焦距。
@param [in] fy TOF相机内参Y方向焦距。
@param [in] cx TOF相机内参X方向中心。
@param [in] cy TOF相机内参Y方向中心。
@return  深度图
@remarks
*/
cv::Mat distance2depth(const cv::Mat& image, float fx, float fy, float cx, float cy) {
    cv::Mat depthImg = image.clone();
    depthImg.convertTo(depthImg, CV_32F); //convert the image data to float type
    for (int i = 0; i < image.rows; i++) {
         for (int j = 0; j < image.cols; j++){
             if (depthImg.at<float>(i, j) >= MAX_DISTANCE) {
                  depthImg.at<float>(i, j) = 0;
             }else {
                  float ax = (j - cx)*(j - cx) / (fx*fx);
                  float ay = (i - cy)*(i - cy) / (fy*fy);
                  depthImg.at<float>(i, j) = depthImg.at<float>(i, j) / sqrt(ax + ay + 1);
             }
         }
    }
    return depthImg;
}　

深度图与比例因子（scale_factor）

深度图对应的尺度因子是深度图中存储的值与真实深度（单位为m）的比例

depth_map_value / real_depth = scale_factor

通常情况下，深度值以毫米为单位存储在16位无符号整数中(0~65535)，因此要获得以米为单位的z值，深度图的像素需要除以比例因子1000。不过不同相机的的比例因子可能不同，比如TUM数据集的比例因子为5000，即深度图中存储的深度值的单位为200毫米。

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

首先，要了解下世界坐标到图像的映射过程，考虑世界坐标点M(Xw,Yw,Zw)映射到图像点m(u,v)的过程，如下图所示：

形式化表示如下：

u,v分别为图像坐标系下的任意坐标点。u0,v0分别为图像的中心坐标。xw,yw,zw表示世界坐标系下的三维坐标点。zc表示相机坐标的z轴值，即目标到相机的距离。R,T分别为外参矩阵的3x3旋转矩阵和3x1平移矩阵。

对外参矩阵的设置：由于世界坐标原点和相机原点是重合的，即没有旋转和平移，所以

相机坐标系和世界坐标系的坐标原点重合，因此相机坐标和世界坐标下的同一个物体具有相同的深度，即zc=zw.于是公式可进一步简化为：

从以上的变换矩阵公式，可以计算得到图像点[u,v]T 到世界坐标点[xw,yw,zw]T的变换公式:

这里Zc是深度值，相机内参为：fx=f/dx, fy= f/dy, cx=u0, cy = v0;

代码实现

投影的逆过程

#!/usr/bin/python3
def depth_image_to_point_cloud(rgb, depth, scale, K, pose):
    u = range(0, rgb.shape[1])
    v = range(0, rgb.shape[0])

    u, v = np.meshgrid(u, v)
    u = u.astype(float)
    v = v.astype(float)

    Z = depth.astype(float) / scale
    X = (u - K[0, 2]) * Z / K[0, 0]
    Y = (v - K[1, 2]) * Z / K[1, 1]

    X = np.ravel(X)
    Y = np.ravel(Y)
    Z = np.ravel(Z)

    valid = Z > 0

    X = X[valid]
    Y = Y[valid]
    Z = Z[valid]

    position = np.vstack((X, Y, Z, np.ones(len(X))))
    position = np.dot(pose, position)

    R = np.ravel(rgb[:, :, 0])[valid]
    G = np.ravel(rgb[:, :, 1])[valid]
    B = np.ravel(rgb[:, :, 2])[valid]

    points = np.transpose(np.vstack((position[0:3, :], R, G, B))).tolist()

    return points

将点云保存为ply格式，使用CloudCompare查看

#!/usr/bin/python3
def write_point_cloud(ply_filename, points):
    formatted_points = []
    for point in points:
        formatted_points.append("%f %f %f %d %d %d 0\n" % (point[0], point[1], point[2], point[3], point[4], point[5]))

    out_file = open(ply_filename, "w")
    out_file.write('''ply
    format ascii 1.0
    element vertex %d
    property float x
    property float y
    property float z
    property uchar blue
    property uchar green
    property uchar red
    property uchar alpha
    end_header
    %s
    ''' % (len(points), "".join(formatted_points)))
    out_file.close()