【Python】Open3Dの点群データをNumPyで扱う

はじめに

点群データを扱うライブラリの代表例として、PCL(Point Cloud Library)とOpen3Dが挙げられると思います。

私が主に使ってきたのはC/C++のPCLですが、Pythonで点群データを扱う必要が出てきたため、Python版のOpen3Dを最近使い始めました。

しかし、Open3D初心者、かつPythonにもあまり慣れていない私にとって、Open3Dのデータ型は不便でした。

そこで本記事では、自分用に作成した、Open3Dにおけるデータ型の変換とそれに付随する基本的な機能のラッパーを紹介します。

サンプルデータの作成

Open3Dで入手できるopen3d.data.BunnyMesh()を使っても良いのですが、個人的にPCLの名残でPCD形式を使いたいので、サンプルデータを作成します。

点群データがお手元にない方も、これでカラフルな立方体を作ることができます。

create_sample_data.py

# 以降のプログラムを使う際にもこの二つをインポートしてください
import open3d as o3d
import numpy as np

def createSamplePointCloud(output_pcd_path):
    points_list = []
    colors_list = []
    for x in range(0, 10):
        for y in range(0, 10):
            for z in range(0, 10):
                points_list.append([x, y, z])
                colors_list.append([x/10.0, y/10.0, z/10.0])
    cloud_o3d = o3d.geometry.PointCloud()
    cloud_o3d.points = o3d.utility.Vector3dVector(points_list)
    cloud_o3d.colors = o3d.utility.Vector3dVector(colors_list)
    o3d.io.write_point_cloud(output_pcd_path, cloud_o3d)

ラッパー

点群データの入出力

入力にはopen3d.io.read_point_cloud、出力にはopen3d.io.write_point_cloudを使います。

名前が長いですし、（私の環境では）入力アシストも働いてくれないので、ラッパーを作りました。

入力はそのままですが、出力ではひと手間加わっています。
その理由は次項で分かります。

point_cloud_io.py

def loadXYZRGB(pcd_path):
    cloud = o3d.io.read_point_cloud(pcd_path)
    return cloud

def savePCDFile(pcd_path, cloud):
    cloud_type = type(cloud)
    if cloud_type == o3d.cuda.pybind.geometry.PointCloud:
        o3d.io.write_point_cloud(pcd_path, cloud)
    elif cloud_type == np.ndarray:
        o3d.io.write_point_cloud(pcd_path, cvtNumpy2Cloud(cloud))
    else:
        print(f"cloud_type={cloud_type} is not supported!")

データ型の変換

open3d.io.read_point_cloudで読み込まれたデータは、Open3Dで定義されたクラスに属しています。

そして、データにアクセスするためにはNumPyの配列に変換する必要があります。

これが私にとっては非常に扱いづらく、本記事の執筆に至った一番の要因です。

以下のプログラムでは、Open3Dのデータ型からNumPyへの変換、その逆変換の関数を定義しています。

cvt_cloud_type.py

##################################################
# arg
#  1. pcd: point cloud data read by loadXYZRGB
#  2. bool: color data ranges [0,1] if False, [0,255] if True
# 
# return
#  1. numpy array: point cloud data of (x,y,z,r,g,b)
def cvtCloud2Numpy(cloud_o3d, is_color_8bit=False):
    points = np.asarray(cloud_o3d.points).copy()
    colors = np.asarray(cloud_o3d.colors).copy()
    if is_color_8bit:
        colors *= 255.0
    cloud_np = np.concatenate([points, colors], 1)
    return cloud_np

##################################################
# arg
#  1. numpy array: data made by cvtCloud2Numpy
#
# return
#  1. pcd: point cloud data for open3d
def cvtNumpy2Cloud(cloud_np):
    # get data
    points = cloud_np[:, 0:3].copy()
    colors = cloud_np[:, 3:6].copy()

    if 1.0 < np.max(colors):
        # normalize colors
        colors /= 255.0
    
    # make cloud_o3d
    cloud_o3d = o3d.geometry.PointCloud()
    cloud_o3d.points = o3d.utility.Vector3dVector(points)
    cloud_o3d.colors = o3d.utility.Vector3dVector(colors)
    return cloud_o3d

データ数の取得

点群データのサンプル数に応じて配列を作りたい場合など、データ数が欲しいことも多々あると思います。

Open3Dのデータ型と、上記プログラムで作成したNumPyのデータ型ではデータ数の取得方法が違うので、どちらが渡されても良いように関数を設計しておくのが好ましいでしょう。

同じような理念でプログラムを作っていくとコーディングが若干面倒になりますが、一度ラップしてしまえば十分なので、最初だけは我慢するしかありません。

get_cloud_size.py

##################################################
# arg
#  1. pcd or numpy array: point cloud data
#
# return
#  1. int: size of point cloud
def getCloudWidth(cloud):
    cloud_type = type(cloud)
    if cloud_type == o3d.cuda.pybind.geometry.PointCloud:
        return np.asarray(cloud.points).shape[0]
    elif cloud_type == np.ndarray:
        return cloud.shape[0]
    else:
        print(f"cloud_type={cloud_type} is not supported!")
        return 0

点群データの可視化

Open3Dで最も手軽にデータを可視化できるのはopen3d.draw_geometriesだと思いますが、背景が白色で目に優しくありません。

そこで、こちらの記事を参考にして、可視化関数もラップします。

visualize_point_cloud.py

##################################################
# arg
#  1. pcd list: list of point cloud data
#  2. list of 3 floats: background color [default: [0.0, 0.0, 0.20]]
def visualize(cloud_list, background_color=[0.0, 0.0, 0.20]):
    # convert data type
    vis_list = []
    for cloud in cloud_list:
        cloud_type = type(cloud)
        if cloud_type == o3d.cuda.pybind.geometry.PointCloud:
            vis_list.append(cloud)
        elif cloud_type == np.ndarray:
            cloud_vis = cvtNumpy2Cloud(cloud)
            vis_list.append(cloud_vis)
        else:
            print(f"cloud_type={cloud_type} is not supported!")
    
    # visualize
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    render_option = vis.get_render_option()
    render_option.background_color = np.asarray(background_color)
    for geometry in vis_list:
        vis.add_geometry(geometry)
    while vis.poll_events():
        vis.update_renderer()
    vis.destroy_window()

ラッパーを使ったサンプルコード

上記のラッパーを組み合わせて、点群データの色を変えて表示するプログラムを書いてみます。

cvtCloud2NumpyはNumPyの配列を返すので、データへのアクセスが直感的にできますね。

change_color.py

if __name__=="__main__":

    cloud_path = "/mnt/d/workspace/data/sample.pcd"
    createSamplePointCloud(cloud_path)
    cloud_np = cvtCloud2Numpy(loadXYZRGB(cloud_path))
    cloud_np[:, 3:6] = [0.0, 1.0, 1.0]   # 全ての色を変更
    visualize([cloud_np])

おわりに

本記事では、Open3Dにおけるデータ型の変換と、それに付随する基本的な機能のラッパーを紹介しました。

質問や疑問などは、コメント欄かX(Twitter)でご連絡ください。

本記事が少しでもお役に立てば幸いです。