之前寫了python的深度圖轉(zhuǎn)點云的文章

這次文章說的是它的逆變換 點云——到深度圖

測試用例

本文用KITTI數(shù)據(jù)集來作為測試用例

以下的圖片以及LiDAR數(shù)據(jù)取自2011_09_26_drive_0005

代碼的colab在線版：https://colab.research.google.com/drive/1uq2wKbfqYI9LMkPRL-Zm22ljwfLutYln?usp=sharing

數(shù)據(jù)導(dǎo)入

圖像在kitti數(shù)據(jù)集中的路徑為

kitti/2011_09_26/2011_09_26_drive_0005_sync/image_00/0000000150.png　

LiDAR的二進(jìn)制文件路徑為

kitti/2011_09_26/2011_09_26_drive_0005_sync/velodyne_points/data/0000000150.bin

colab里面直接上傳到了根目錄

kitti數(shù)據(jù)集的LiDAR數(shù)據(jù)有4位 分別是 X Y Z 反射率，反射率本次暫時用不到 所以只取前三位

?? ?

投影變換 1

想要做出深度圖，首先得將點云給投影到圖像上，取得像素坐標(biāo)值

一個簡單的方法是利用opencv的函數(shù)?CV2.projectPoints

函數(shù)需要的參數(shù)有

1. 點云

2. 點云坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣

3. 點云坐標(biāo)系到相機(jī)坐標(biāo)系的平移向量

4. 相機(jī)內(nèi)參

5. 相機(jī)畸變參數(shù)

其實就是所謂的 外參（2+3）和內(nèi)參（4+5）

LiDAR和相機(jī)1的外參可以在數(shù)據(jù)集中

/kitti/2011_09_26/calib_velo_to_cam.txt

里找到

calib_time: 15-Mar-2012 11:37:16

R: 7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02

T: -4.069766e-03 -7.631618e-02 -2.717806e-01

相機(jī)內(nèi)參則是參考ORB_SLAM3的kitti示例配置文件

并且，相機(jī)的視角是朝前方的 后面的點云自然不可能投影到相機(jī)上，所以這里把點云中在后方的點去掉，也就是X為負(fù)的部分

接著把上面的參數(shù)全部傳入CV2.projectPoints

就可以得到點云投影在畫像上的像素坐標(biāo)，因為像素坐標(biāo)是只有整數(shù) 所以轉(zhuǎn)為整數(shù)型

剛剛?cè)コ薒iDAR中心后半部分的點

因為相機(jī)有FOV 視野的限制，所以前半部分的點也并不是全部都能投影到畫像上

所以這里把坐標(biāo)超出畫像的無效點給去除

畫出來觀察一下

看起來沒毛病。雖然仔細(xì)看有的點也并不是和物體嚴(yán)絲合縫，比如右手邊的胖大叔

不過這應(yīng)該是LIDAR數(shù)據(jù)的采集時間和拍照時間有細(xì)微的時間差

數(shù)據(jù)集里面有時間戳，可以看得出兩個數(shù)據(jù)之間略微有時間差

投影變換 2

剛才利用了opencv的函數(shù)一下就完成了點云到畫像的投影。

不過很可惜這樣只能知道點云對應(yīng)的畫像坐標(biāo)，并不能達(dá)到我們的目的——做出深度圖

為了做出深度圖 除了點云的畫像坐標(biāo)以外，我們還需要每個點的深度

而深度——depth的定義，是一個點在相機(jī)坐標(biāo)系下的Z值

剛才opencv的函數(shù)拿了相機(jī)和LiDAR的外參以及內(nèi)參

一口氣完成了如下的變換

LiDAR坐標(biāo)系——外參——相機(jī)坐標(biāo)系——內(nèi)參——畫像坐標(biāo)系

而為了得到深度值，我們需要點群在相機(jī)坐標(biāo)系下的坐標(biāo)

所以先來手動寫一下前面這部分變換

在齊次坐標(biāo)系下 坐標(biāo)系和坐標(biāo)系的變換可以寫成

中間的那個矩陣也就是相機(jī)的外參矩陣 也叫三維空間幾何變換矩陣

其中 左上角的3x3為旋轉(zhuǎn)矩陣 而右邊下3x1為平移向量

重新導(dǎo)入我們的LiDAR點云，這次不刪掉第四位 而是將其全部設(shè)置成1 讓XYZ變成齊次坐標(biāo)XYZ1

然后把之前的旋轉(zhuǎn)矩陣以及平移向量拼接一下 得到變換矩陣

7.533745e-03 -9.999714e-01 -6.166020e-04 -4.069766e-03
1.480249e-02 7.280733e-04 -9.998902e-01 -7.631618e-02
9.998621e-01 7.523790e-03 1.480755e-02 -2.717806e-01
0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00

接下來用矩陣乘以點云里所有的點，就完成了點云從LiDAR坐標(biāo)系到相機(jī)坐標(biāo)系的變換

之后是相機(jī)坐標(biāo)系到畫像坐標(biāo)系的變換

因為相機(jī)背面的點不會投影到相機(jī)上，所以我們刪掉背面的點

而相機(jī)坐標(biāo)系下，背面指的也就Z值小于0的部分

和前面的坐標(biāo)變換一樣，在有內(nèi)參矩陣的情況下，乘以所有點就可以完成變換

但是這里偷個懶，繼續(xù)使用我們的CV2.projectPoints

不過因為我們已經(jīng)自己完成了到相機(jī)坐標(biāo)系的變換，所以這時候外參就應(yīng)該等于0

將旋轉(zhuǎn)以及平移都設(shè)為0 0 0

接下來一樣是處理一下畫面外的點以及可視化

當(dāng)然，兩次的結(jié)果肯定是完全一致的，區(qū)別就是這次我們?nèi)〉昧它c云在相機(jī)坐標(biāo)系下的數(shù)據(jù)?

可以來做我們的深度圖了

深度圖作成

上一步的最后我們用一個filter來去除了超出圖片范圍之外的無效點，同理用這個filter也刪除對應(yīng)點云里的點。

這樣兩邊剩下的點就能按順序一一對應(yīng)

而我們想要的深度值是Z，那便取XYZ點云的第三位

接著用numpy創(chuàng)建一個和畫像長寬一致的二維矩陣

把先前計算的像素坐標(biāo)pixel里面填入對應(yīng)的深度值，一張稀疏的深度圖就做好了

用opencv保存后

RGB點云

取得深度圖以后當(dāng)然用途很多，不過你可以選擇再一次將他投射成點云

不過這一次因為你知道了每個點對應(yīng)圖像的坐標(biāo)值，可以從彩色圖像中獲取RGB值

也就可以投影出所謂的RGB點云