デプス画像から三次元点群を構築の高速化(Python,numpy)

<script type="text/javascript" async src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.7/MathJax.js?config=TeX-MML-AM_CHTML">
</script>
<script type="text/x-mathjax-config">
 MathJax.Hub.Config({
 tex2jax: {
 inlineMath: [['$', '$'] ],
 displayMath: [ ['$$','$$'], ["\\[","\\]"] ]
 }
 });
</script>
# デプス画像から三次元点群を構築の高速化(Python,numpy) <br>
## この記事について
---
初めまして、KEROと申します。
ロボット工学や画像工学などこと研究してます。
外人ですか、もっと日本語で技術のことを話しできるため、日本語で技術blogを書き始めた<br>

<br>
今日はPyhonでデプス画像から三次元点群を構築のことついて話します。<br>
<br>

画像は一枚だけで場合なら、コード例が見つけやすい
例えば[この記事](https://qiita.com/iwatake2222/items/bbd4ccdacfcf8f846a0a)
もしくは[open3d中の関数](http://www.open3d.org/docs/0.7.0/python_api/open3d.geometry.create_point_cloud_from_depth_image.html)<br>

以前Pythonとnumpyでこんな計算行って、点群構築のコード書きましたが、計算めちゃくちゃ遅い。
自分の目的はロボットのナビゲーションので、連続の画像をリアルタイムで処理なければならない
本来は遅くの理由としては、計算量が多いもしくはPython自体は遅いと思うから、C++で書き直し予定ですが。

その前計算アルゴリズムを見直しため、もう一回Pythonとnumpyで書き直して
結果としては処理速度が大幅上げて、Pyhonでもリアルタイム処理できました

テストとしては **960,720** サイズのデプス画像を三次元点群に構築**1000**回ループして、処理時間は**17**秒<br>
```
[Running] python -u "g:\test\test.py"

[Done] exited with code=0 in 17.162 seconds exited with code=0 in 17.162 seconds
```
### 2022/09/16
アルゴリズム更新するため、現在の実行時間は11秒
```
[Running] python -u "g:\test\test.py"

[Done] exited with code=0 in 10.914 seconds
```



## コード
---
まずコードを上げて、その下で説明補足します
```python
import numpy as np
import cv2

class depth_to_pcd(object):

    def __init__(self, resize_scale):
        self.resize_scale = resize_scale
        self.resize_camera = np.array((910.072 / self.resize_scale, 0, 485.523 / self.resize_scale, 0,
                                       0, 914.094 / self.resize_scale, 336.718 / self.resize_scale, 0,
                                       0, 0, 1, 0), dtype="float").reshape(3, 4)

        self.pixel = np.array([0,0,1]).reshape(3,-1)
        self.resize_camera = np.matrix(self.resize_camera).I
        
        self.pcd_list = np.zeros((720//self.resize_scale,960//self.resize_scale,3))
        # self.get_depth_vector()
        self.get_depth_vector_v2()
        return

    #before
    # def get_depth_vector(self):
    #     fake_depth =np.zeros((720//self.resize_scale,960//self.resize_scale))
    #     it = np.nditer(fake_depth, flags=['multi_index'])
    #     with it:
    #         while not it.finished:
    #             self.pixel[0] = it.multi_index[1]
    #             self.pixel[1] = it.multi_index[0]
    #             point = np.dot(self.resize_camera, self.pixel)          
    #             self.pcd_list[it.multi_index] = point[0:3].T[0]
    #             it.iternext()


    #new
    def get_depth_vector_v2(self):
        u = 960//self.resize_scale
        v = 720//self.resize_scale
        u_vector = np.ones((1,u,3))
        v_vector = np.ones((u,1,3))

        for i in range(u):
            self.pixel[0] = self.pixel[1] = i
            point = np.dot(self.resize_camera, self.pixel)
            u_vector[0,i,0] = point[0]
            v_vector[i,0,1] = point[1]

        v_vector = v_vector[:v,:,:]
        v_vector = np.tile(v_vector,(u,1))
        u_vector = np.tile(u_vector ,(v,1,1))
        self.pcd_list = v_vector * u_vector

    def get_pcd(self, depth):
        self.depth = cv2.resize(depth, (int(960 / self.resize_scale), int(720 / self.resize_scale)),interpolation=cv2.INTER_NEAREST)
        vector_array = self.pcd_list.copy()
        vector_array *= self.depth.reshape((int(720 / self.resize_scale), int(960 / self.resize_scale),1))
        
            
        pointcloud = vector_array.reshape(-1,1,3)
        return pointcloud
```
<br>
テスト用例

```python
depth = np.load("ai_depth.npy")
depth2pcd = depth_to_pcd(1)

for i in range(1000):
    pointcloud = depth2pcd.get_pcd(depth)
```
## 説明
---
Pythonで高速計算したいなら
できる限りPythonで計算、ループしなく、numpyで完成して

そのため、計算手順はできる限り行列式計算でいく<BR>
### 初期化
---
<BR>

まず[カメラ座標系](https://mem-archive.com/2018/10/13/post-682/)から画像座標系で変換方程式は

![MommyTalk1654420680707.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/ed8c6d9f-65e1-5302-c2a8-70647bb49d26.png)

説明のため、以下の形で略書きします
$$
Z\begin{bmatrix}P\end{bmatrix}=\begin{bmatrix}C\end{bmatrix}\begin{bmatrix}D\end{bmatrix}
$$

中では[P]は画像座標系の座標 すなわちpixelの位置　u,v
[C]は[カメラ内部パラメータ](https://mem-archive.com/2018/02/21/post-157/)
[D]がカメラ座標系の座標、すなわち点群座標、今回計算の目的

だから計算のため、先ずは[C] カメラパラメータ行列式を導入
```python
 def __init__(self, resize_scale):
        self.resize_scale = resize_scale
        self.resize_camera = np.array((910.072 / self.resize_scale, 0, 485.523 / self.resize_scale, 0,
                                       0, 914.094 / self.resize_scale, 336.718 / self.resize_scale, 0,
                                       0, 0, 1, 0), dtype="float").reshape(3, 4)
```

こちらはのパラメータ行列式は自分ロボット用いたカメラをcalibrationした結果
さらに、処理画像のサイズを調整するため、、resize_scaleパラメータを用意しました
画像サイズが調整する場合、カメラパラメータも合わせて調整が必要

<BR>
[P]　pixel座標行列式を初期化

```python
self.pixel = np.array([0,0,1]).reshape(3,-1)
```
<BR>
今回は逆変換、つまり上の方程式では　[D]　は求めている答え

$$
\begin{bmatrix}D\end{bmatrix}=\begin{bmatrix}P\end{bmatrix}\begin{bmatrix}C\end{bmatrix}^{-1} Z
$$

[C]の逆行列を計算

```python
self.resize_camera = np.matrix(self.resize_camera).I
```
<BR>

もう一回この方程式を見直しすると
$$
\begin{bmatrix}D\end{bmatrix}=\begin{bmatrix}P\end{bmatrix}\begin{bmatrix}C\end{bmatrix}^{-1} Z
$$
同じカメラの同じサイズの画像が連続入力する場合[P]と[C]は変わらない、Zだけが変化します
すなわち

![QQ图片20220605182100.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/2edb1d41-caf7-ffdf-9c47-bd3e80f7977d.png)


<br>
ここで一気に画像すべてのpixelの[K]を計算します<br>
結果保存するため、先ずはデプス画像と同じサイズ、ひとつpixel中では　1ｘ3大きさがあるの行列を作り<br>

```python
self.pcd_list = np.zeros((720//self.resize_scale,960//self.resize_scale,3))
```
[np.nditer](https://snowtree-injune.com/2020/06/29/nditer-z009/) 用いてトラバーサル、画像すべてのpixelの[K]を計算して、結果をpcd_list中で対応した位置に保存します

```python
    def get_depth_vector(self):
        fake_depth =np.zeros((720//self.resize_scale,960//self.resize_scale))
        it = np.nditer(fake_depth, flags=['multi_index'])
        with it:
            #loop
            while not it.finished:
                #今の座標対して[P] pixel行列更新
                self.pixel[0] = it.multi_index[1]
                self.pixel[1] = it.multi_index[0]
                #[K]の計算
                point = np.dot(self.resize_camera, self.pixel)
                #結果を保存          
                self.pcd_list[it.multi_index] = point[0:3].T[0]
                it.iternext()
```

これでは初期化の準備が完了<BR>
### 変換計算
---
先色々準備した結果、本番の変換計算はとても簡単

```python
    def get_pcd(self, depth):
        self.depth = cv2.resize(depth, (int(960 / self.resize_scale), int(720 / self.resize_scale)),interpolation=cv2.INTER_NEAREST)
        vector_array = self.pcd_list.copy()
        vector_array *= self.depth.reshape((int(720 / self.resize_scale), int(960 / self.resize_scale),1))
        
            
        pointcloud = vector_array.reshape(-1,1,3)
        return pointcloud
```

先ずはdepth画像を導入、サイズ調整
```python
    def get_pcd(self, depth):
        self.depth = cv2.resize(depth, (int(960 / self.resize_scale), int(720 / self.resize_scale)),interpolation=cv2.INTER_NEAREST)
```

初期化時計算の[K]行列を取る、元行列の数値を変換させないため、copy()が必要

```python
vector_array = self.pcd_list.copy()
```

numpy.arrayとnumpy.arrayの　*　掛けでは、**対応位置のデータを掛け**<br>
つまり

```python
vector_array *= self.depth.reshape((int(720 / self.resize_scale), int(960 / self.resize_scale),1))
```

ではこんな形で計算する
![MommyTalk1654421277897.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/acf652b5-fea3-33ed-d82b-9c8293753b90.png)

そうすると一気にデプスから点群の変化を完成します<br>
<br>
残るのは形を調整して、通用の形になるだけ
```python
pointcloud = vector_array.reshape(-1,1,3)
```
### Extra:左手座標系、右手座標系
カメラ座標系は左手座標系であるので、もしROSなど右手座標系ツールに入力するため、左手系と右手系の変換は必要
そのため、初期化関数に対しては以下コードを追加する
```python

self.pcd_list = self.pcd_list.reshape(-1,1,3)
self.pcd_list[:,:,0],self.pcd_list[:,:,1],self.pcd_list[:,:,2]=self.pcd_list[:,:,2],-self.pcd_list[:,:,0],-self.pcd_list[:,:,1]
self.pcd_list = self.pcd_list.reshape(720//self.resize_scale,960//self.resize_scale,3)
```


## 最後
---
そうのように、計算やループはできるだけnumpyを任せすれば、連続計算の場合計算速度はめちゃくちゃ速くなる
今一番遅くなる部分は初期化時[K]を計算する時<br>
np.nditer 用いてトラバーサルするちょっと速くなったけど、本質としてはまだPythonでループしているから
テスト用例1000回の処理で17秒の時間では、約7秒は初期化、実際にデプス画像を処理する時間は十秒だけ

本番なら初期化の時間が無視できるし、さらにデプス画像サイズをresizeして小さくなるため
合格線は一秒20フレーム対して今の計算速度を十分と思います

## 2022/09/16　アルゴリズム更新
get_depth_vector()関数に対して書き直しました

実行時間をテスト
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/10a57d2c-f831-eb9a-7e92-d18d21bf74ad.png)

結果の一致性をテスト
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/587ceb24-8f6d-76bb-f02f-f15765bdee69.png)

```python
    def get_depth_vector_v2(self):
        u = 960//self.resize_scale
        v = 720//self.resize_scale

        u_vector = np.ones((1,u,3)) # array 1*u*[1,1,1]
        v_vector = np.ones((u,1,3)) # array u*1*[1,1,1]

        for i in range(u):
            self.pixel[0] = self.pixel[1] = i # pixel = [u,u,1]
            point = np.dot(self.resize_camera, self.pixel) # point = [x,y,z] z=1
            u_vector[0,i,0] = point[0] #save x  [x,1,1]
            v_vector[i,0,1] = point[1] #save y  [1,y,1]

        v_vector = v_vector[:v,:,:] # array v*1*[1,y,1]

        v_vector = np.tile(v_vector,(u,1)) # array v*u*[1,y,1]

        u_vector = np.tile(u_vector ,(v,1,1)) # array v*u*[x,1,1]

        self.pcd_list = v_vector * u_vector # aray v*u*[x,y,1]
```