RealSense D435iで3Dスキャナもどきの実装

Depthカメラで3Dスキャナもどきの実装（Open3Dの探索をかねて。）

前回に続いてRealSense D435iで遊んでいきます。

概要

• D435iでRGBDデータを作成する
• RGBDデータから点群データを生成する
• 点群データから法線を自動計算して、Meshを貼る

環境

• MacBook Air
• Intel RealSense D435i
• Jupyter-notebook
• Python
• Open3D

参考

• Open3Dドキュメント http://www.open3d.org/docs/release/
• 前回記事 https://qiita.com/tishihara/items/90ce0f0f722deddea34a
• メッシュ作成アルゴリズム https://stackoverflow.com/questions/56965268/how-do-i-convert-a-3d-point-cloud-ply-into-a-mesh-with-faces-and-vertices

手順

• ライブラリのインポート

realsenseのSDKとopen3dをインポートします。

import pyrealsense2 as rs
import numpy as np
import open3d as o3d

• 画角を合わせつつ、ストリーミング情報を取得する

RGBDを作成するためには、DepthとRGBの画角をあわせる必要があります。
画角をあわせるためにはalignオブジェクトを使います。SDKにこういったメソッドがあるのは便利ですね。

# ストリーム(Depth/Color)の設定
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

# ストリーミング開始
pipeline = rs.pipeline()
profile = pipeline.start(config)

# Alignオブジェクト生成
align_to = rs.stream.color
align = rs.align(align_to)

• 画角が同じRGB画像とDepth画像の２つから、RGBD画像を生成

RGBD画像作成のメソッドはcreate_from_color_and_depthです。

try:
    while True:
        # フレーム待ち(Color & Depth)
        frames = pipeline.wait_for_frames()
        aligned_frames = align.process(frames)
        color_frame = aligned_frames.get_color_frame()
        depth_frame = aligned_frames.get_depth_frame()
        if not depth_frame or not color_frame:
            continue

        color_image = o3d.geometry.Image(np.asanyarray(color_frame.get_data()))
        depth_image = o3d.geometry.Image(np.asanyarray(depth_frame.get_data()))

        rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(color_image, depth_image);

• RGBD画像からPoint Cloudを生成

RGBD画像があればPointCloud.create_from_rgbd_imageを使って一発でPoint Cloudを作成できます。

        pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image,
                                                             o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))

※PrimeSenseDefaultを指定していますが、本来ここはRealSense D435iでカメラキャリブレーションを行い、内部パラメータを求めた上で、内部パラメータ(instric)の行列を与えるべきかと思います。今回はキャリブレーションまで行えてないので、仮で上記を設定しました。

• 回転、法線計算、ダウンサンプリングなどの処理

メッシュを貼る前に、点群が細かすぎると表示がいまいちなので、ダウンサンプリングします。

        # 回転する
        pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])

        # 法線計算
        pcd.estimate_normals()

        # 指定したvoxelサイズでダウンサンプリング
        voxel_down_pcd = pcd.voxel_down_sample(voxel_size=0.01)

• メッシュを貼る

すみません、このあたりには明るくないです。
ドキュメントをみていたらcreate_from_point_cloud_ball_pivotingというメソッドがあったので、これを使ってみたらPCDからメッシュデータを生成することができました。
一部メッシュが貼られないのは、半径の設定の問題なのでしょうか？
アルゴリズムについてはこちらに記載があります。
https://ieeexplore.ieee.org/document/817351

        distances = voxel_down_pcd.compute_nearest_neighbor_distance()
        avg_dist = np.mean(distances)
        radius = 1.5 * avg_dist

        # メッシュ化
        radii = [radius, radius * 2]
        mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting(voxel_down_pcd, o3d.utility.DoubleVector(radii))

• メッシュデータの表示、保存など

        # 再度法線計算
        mesh.compute_vertex_normals()

        # 一応データ保存
        o3d.io.write_triangle_mesh("output.ply", mesh)

        # メッシュデータの表示
        o3d.visualization.draw_geometries([mesh])

finally:
    # ストリーミング停止
    pipeline.stop()

できたのはこちら。

終わりに

Open3DとRealSenseSDKの探索をかねて、3Dスキャナのようなものを作ってみました。
点群データを各方向から生成し、Mergeした上で本処理を実行すれば、もう少し3Dデータらしいものを生成できると思います。
それでは。