LoginSignup
8

More than 1 year has passed since last update.

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

Last updated at Posted at 2022-06-05

デプス画像から三次元点群を構築の高速化(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で完成して

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

初期化



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

MommyTalk1654420680707.png

説明のため、以下の形で略書きします
$$
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だけが変化します
すなわち

QQ图片20220605182100.png


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

ではこんな形で計算する
MommyTalk1654421277897.png

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



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

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()関数に対して書き直しました

実行時間をテスト
image.png

結果の一致性をテスト
image.png

    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]

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
8