深度相機生成點雲數據的原理

2019年8月4日 16:26:40

原理

RGB-D 圖像中的rgb圖片提供了像素坐標系下的x，y坐標，而深度圖直接提供了相機坐標系下的\(Z\)坐標，也就是相機與點的距離。

根據 RGB-D 圖像的信息和相機的內參，可以計算出任何一個像素點在相機坐標系下的坐標。

根據 RGB-D 圖像的信息和相機的內參與外參，可以計算出任何一個像素點在世界坐標系下的坐標。

相機視野范圍內，相機坐標系下的障礙物點的坐標，就是點雲傳感器數據，也就是相機坐標系下的點雲數據。點雲傳感器數據可以根據 RGB-D 圖像提供的坐標與相機內參算出來。

所有世界坐標系下的障礙物點的坐標，就是點雲地圖數據，也就是世界坐標系下的點雲數據。點雲地圖數據可以根據RGB-D 圖像提供的坐標與相機內參和外參算出來。

點雲傳感器數據-相機坐標系下的點雲數據

相機坐標系下的點雲數據是根據rgb圖像提供的像素坐標系下的x，y坐標（即公式中的u，v）和相機內參就能求出相機坐標系下的\(X\),\(Y\)坐標值。同時深度圖直接提供相機坐標系下的\(Z\)坐標值。進而得到相機坐標系下的坐標\(P=\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}\)，相機坐標系下的障礙物點的坐標，就是點雲傳感器數據，也就是相機坐標系下的點雲數據。

相機坐標系\(P\)與像素坐標系\(P_{uv}\)下的點的坐標的關系公式:

\[ZP_{uv}=Z\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\begin{bmatrix} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{bmatrix}\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}=KP \]

將上式整理后的具體求解公式如下：

\[X=Z(u-c_x)/f_x\\ Y= Z(v-c_y)/f_y\\ Z=d \]

一般來說，公式中點在相機坐標系下的\(Z\)值就是相機測出的深度值\(d\)，即就是真實點到相機平面的距離。如果不是可以加倍數補償。

如果在對應點的位置再加入彩色信息就能夠構成彩色點雲。

點雲地圖數據-世界坐標系下的點雲數據

根據世界坐標系到像素坐標系下點的坐轉換公式：

\[ZP_{uv}=Z\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=KP=K\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}=K(R\begin{bmatrix} X_w\\ Y_w\\ Z_w \end{bmatrix}+t)\\或者\\ZP_{uv}=Z\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=KP=K\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}=KT\begin{bmatrix} X_w\\ Y_w\\ Z_w\\ 1 \end{bmatrix}（隱含齊次坐標轉非齊次坐標） \]

\[K=\begin{bmatrix} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{bmatrix} \]

上述描述了世界坐標系\(P_w\)到像素坐標系\(P_{uv}\)下的點的坐標關系。因此這里利用上式，輸入像素坐標\([u,v]\)和深度\(Z\)之后，就可以求得世界坐標系下點的坐標\(p_w\)，所有世界坐標系下的障礙物點的坐標，就是點雲地圖數據。

深度圖能夠直接提供相機坐標系下某個點的\(Z\)坐標值，作為式子中的\(Z\)值。rgb圖提供了像素坐標系下點的坐標\(P_{uv}=[u,v]\)。

根據內參公式就可以計算出該點在相機坐標系下的三維坐標\(P=[X,Y,Z]\)。再根據有相機的齊次變換矩陣\(T\)或者旋轉矩陣和平移向量\(R,t\)，就可以求出該點在世界坐標系下的三維坐標\(P_w=[X_w,Y_w,Z_w]\)。\(P_w=[X_w,Y_w,Z_w]\)就是在世界坐標系下標定的點雲。也就是點雲地圖。

總結：根據RGBD圖求解點雲地圖數據，就是將像素坐標系下點的坐標\(P_{uv}=[u,v]\)和相機坐標系下的點的\(Z\)坐標值代入公式求出在世界坐標系下的三維坐標\(P_w=[X_w,Y_w,Z_w]\)，\(P_w\)就是最終要求的世界坐標系的點雲。根據上式一旦確定了相機內參矩陣K和外參旋轉矩陣R和平移矩陣t，就可以根據RGBD圖像完成世界坐標系下的點雲生成。

如果設定世界坐標系和相機坐標系是重合的，即沒有旋轉和平移，這時實質就是以相機坐標系建立的點雲。這時相機坐標系和世界坐標系的相對位姿的旋轉矩陣R就為單位矩陣，位移向量t就是零向量。

說明：

這里提到了要根據相機的位姿將所有像素的坐標在相機坐標系下轉換到世界坐標系下。這個過程只有在想要建立周圍環境的點雲地圖時才是必不可少的，因為要先建立整個環境的點雲就需要點雲的坐標系是靜止的。如果不使用相機位姿也可以建立以相機坐標系為參考的點雲數據，但是此時的點雲信息就只能顯示相機所視范圍內圖片的點雲數據，所有點都是相對於相機而言的，一旦變換相機位置，將重新建立相機所視范圍內圖片的點雲數據，因為此時點雲坐標系都變了，之前的點雲數據不再有意義。如果要使用多張圖片生成點雲那么就要把像素轉化到世界坐標系中。

參考資料

《機器人學、機器視覺與控制》

https://www.cnblogs.com/gaoxiang12/p/4652478.html

http://www.cnblogs.com/cv-pr/p/5719350.html