之前也有用python和numpy寫過深度圖轉(zhuǎn)點云的代碼，但是速度慢出天際 無法實時處理視頻流

本來以為慢的理由是畢竟python本來就慢，打算找時間用C++重寫一遍

不過重寫之前先在之前python寫的基礎(chǔ)上優(yōu)化一下提提速，結(jié)果發(fā)現(xiàn)其實優(yōu)化好后一點也不慢，慢的竟是我自己

作為測試，輸入一張（960，720）大小的深度圖，轉(zhuǎn)成點云循環(huán)一千次，程序用時17秒

其中7秒是參數(shù)初始化時間，真正處理圖片的地方只用了10秒

考慮到實際運用的時候可以無視初始化時間，以及不會用到這么大的深度圖大?。ㄌ罅酥髣e的程序也處理不動這點云）

遠遠超過使用需求了（1秒20幀以上）

代碼：

測試用例：

說明：

首先python計算想要快，那就得盡可能別在python里面進行計算和循環(huán)

這次優(yōu)化就是盡可能讓numpy來完成所有的計算和循環(huán)，在python里面循環(huán)越少 那么就越快

還有一些是對本來的計算方法的一些優(yōu)化，特別運用場景是實時視頻流的場合

為了用numpy來計算，那就全部用矩陣計算來搞定

首先是投影矩陣? 相機坐標系——圖像坐標系

為了描述方便，簡寫成

其中[P]為畫像坐標系，也就是圖像上每一個像素的坐標

[C]為相機內(nèi)參矩陣

[D]為相機坐標系的三維坐標，因為不考慮相機外參（這部分轉(zhuǎn)換交給ROS更方便），所以[D]就是這次求解的目標，然后相機坐標系的Z 也就是深度圖里面的depth

這樣的話[D]的求解就應(yīng)該為

所以初始化的時候首先導(dǎo)入相機內(nèi)參矩陣，這里用的是我的攝像頭標定的數(shù)據(jù)，為了后期調(diào)整大小方便引入了一個縮放系數(shù)

當輸入圖像縮放的時候，相機內(nèi)參矩陣也需要按照這樣子同比例縮放

numpy初始化一個[P]矩陣

計算相機矩陣的逆矩陣 [C]逆

重新再看一遍投影方程

會發(fā)現(xiàn)，如果輸入的是一個視頻流? 在圖像尺寸以及相機內(nèi)參都不會發(fā)生改變的情況下

那么隨時間變化Z是會改變的，但是[P]和[C]逆不會變

也就是

所以避免重復(fù)計算，我們先行計算這個[K]=[P][C]逆

先來numpy初始化一個尺寸和畫像大小相同，但是一個像素位里面有三個格子空間的數(shù)組來存放數(shù)據(jù)，其實就和普通的RGB圖像是一個維度 只不過這里我們用來存放XYZ而不是RGB

然后用numpy.nditer迭代一個和深度圖同樣大小的數(shù)組，計算所有位置的[K]，保存到上面創(chuàng)建的容器的對應(yīng)坐標里面

計算完成后初始化準備工作就完成了

接下來開始實際計算點云坐標

先拿到深度圖，需要縮放的話就縮放

為了避免計算改變初始化好的那個容器，需要用copy來復(fù)制一個新的

有了之前的準備，現(xiàn)在我們只需要把深度圖上每一個像素的深度值Z乘以這個vector_array對應(yīng)的每一個像素里面的[XYZ]就可以

而這個對應(yīng)位置相乘直接用numpy.array和numpy.array的*乘就可以搞定

也就是

運算效果上是

至此點云的計算就完成了，最后只是改成標準形狀

如果是要發(fā)布到ROS里面，還涉及一個左手系到右手系的變換

放以前的話我會在循環(huán)后面這么寫

但是這么一來也是多做了無用的計算，開頭的測試用例的話會從17秒增加到27秒

雖然還是綽綽有余，但是更好的方式顯然是在初始化的時候就完成坐標變換

在初始化函數(shù)最后加上

運行測試用例，完成了坐標系變換同時 運行時間還是17秒? 可喜可賀 可喜可賀