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

この記事について

初めまして、KEROと申します。
ロボット工学や画像工学などこと研究してます。
外人ですか、もっと日本語で技術のことを話しできるため、日本語で技術blogを書き始めた

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

画像は一枚だけで場合なら、コード例が見つけやすい
例えばこの記事
もしくはopen3d中の関数

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

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

テストとしては 960,720 サイズのデプス画像を三次元点群に構築1000回ループして、処理時間は17秒

[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

コード

まずコードを上げて、その下で説明補足します

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

テスト用例

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

for i in range(1000):
    pointcloud = depth2pcd.get_pcd(depth)

説明

Pythonで高速計算したいなら
できる限りPythonで計算、ループしなく、numpyで完成して

そのため、計算手順はできる限り行列式計算でいく

初期化

まずカメラ座標系から画像座標系で変換方程式は

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

中では[P]は画像座標系の座標すなわちpixelの位置　u,v
[C]はカメラ内部パラメータ
[D]がカメラ座標系の座標、すなわち点群座標、今回計算の目的

だから計算のため、先ずは[C] カメラパラメータ行列式を導入

 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パラメータを用意しました
画像サイズが調整する場合、カメラパラメータも合わせて調整が必要

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

self.pixel = np.array([0,0,1]).reshape(3,-1)

今回は逆変換、つまり上の方程式では　[D]　は求めている答え

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

[C]の逆行列を計算

self.resize_camera = np.matrix(self.resize_camera).I

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

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

self.pcd_list = np.zeros((720//self.resize_scale,960//self.resize_scale,3))

np.nditer 用いてトラバーサル、画像すべてのpixelの[K]を計算して、結果をpcd_list中で対応した位置に保存します

    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()

これでは初期化の準備が完了

変換計算

先色々準備した結果、本番の変換計算はとても簡単

    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画像を導入、サイズ調整

    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()が必要

vector_array = self.pcd_list.copy()

numpy.arrayとnumpy.arrayの　*　掛けでは、対応位置のデータを掛け

つまり

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

ではこんな形で計算する

そうすると一気にデプスから点群の変化を完成します

残るのは形を調整して、通用の形になるだけ

pointcloud = vector_array.reshape(-1,1,3)

Extra:左手座標系、右手座標系

カメラ座標系は左手座標系であるので、もしROSなど右手座標系ツールに入力するため、左手系と右手系の変換は必要
そのため、初期化関数に対しては以下コードを追加する


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]を計算する時

np.nditer 用いてトラバーサルするちょっと速くなったけど、本質としてはまだPythonでループしているから
テスト用例1000回の処理で17秒の時間では、約7秒は初期化、実際にデプス画像を処理する時間は十秒だけ

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

2022/09/16　アルゴリズム更新

get_depth_vector()関数に対して書き直しました

実行時間をテスト

結果の一致性をテスト

    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]