深度圖像轉換為點雲數據計算原理及代碼實現

轉載：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()