はじめに
前回は、Realsense 公式サイトのプログラムより、PointCloudViewer のプログラムを作成し、点群データの表示を行いました。
今回は、PointCloud ライブラリの一種である Open3D を使用して、点群データの確認を行います。
前提条件
前提条件は、以下の通りです。
- pyrealsense2 がインストールされている
- OpenCV ==4.6.0
- Python == 3.9.13
- Windows11
open3d のインストール
インストールは pip で簡単に可能です。
pip install open3dopen3D == 0.16.0 をインストールしました。
GPU上で動かしたい場合はビルドが必須となります。こちらを参照ください。
GPU上での動作はUbuntu20.04のみ私の方で確認しております。
PointCloud 表示・保存プログラムの実行
表示用のプログラムは、以下となります。
import pyrealsense2 as rs
import numpy as np
import cv2
import open3d as o3d
import datetime
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
pipeline.start(config)
align = rs.align(rs.stream.color)
vis = o3d.visualization.Visualizer()
vis.create_window('PCD', width=640, height=480)
pointcloud = o3d.geometry.PointCloud()
geom_added = False
while True:
dt0 = datetime.datetime.now()
frames = pipeline.wait_for_frames()
frames = align.process(frames)
profile = frames.get_profile()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
o3d_color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
img_depth = o3d.geometry.Image(depth_image)
img_color = o3d.geometry.Image(o3d_color_image)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(img_color, img_depth, convert_rgb_to_intensity=False)
intrinsics = profile.as_video_stream_profile().get_intrinsics()
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, pinhole_camera_intrinsic)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
voxel_down_pcd = pcd.voxel_down_sample(voxel_size=0.005)
alpha = 0.015
mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(voxel_down_pcd, alpha)
mesh.compute_vertex_normals()
pointcloud.points = pcd.points
pointcloud.colors = pcd.colors
if geom_added == False:
vis.add_geometry(pointcloud)
geom_added = True
vis.update_geometry(pointcloud)
vis.poll_events()
vis.update_renderer()
cv2.imshow('bgr', color_image)
key = cv2.waitKey(1)
if key == ord('q'):
break
process_time = datetime.datetime.now() - dt0
print("FPS: " + str(1 / process_time.total_seconds()))
o3d.io.write_triangle_mesh("output2.ply", mesh)
pipeline.stop()
cv2.destroyAllWindows()
vis.destroy_window()
del vis上記プログラムを動かすと、以下のような結果が得られます。
“bgr” window上で “q” を押して終了します。
“PCD” window上で3Dモデルを様々な角度から確認することができますが、
非常に処理が重いので確認は後にしましょう。
PointCloud 表示・保存コードの説明
コードについて説明していきます。
import pyrealsense2 as rs
import numpy as np
import cv2
import open3d as o3d
import datetime
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
pipeline.start(config)
align = rs.align(rs.stream.color)
こちらは以前解説しているので、割愛します。
Open3D Visualizer クラス
vis = o3d.visualization.Visualizer()
vis.create_window('PCD', width=640, height=480)
pointcloud = o3d.geometry.PointCloud()
geom_added = Falseo3d.visualization.Visualizer() をインスタンス化します。
Visualizer クラスはこちらの公式サイトに説明があります。
create_window は、ウィンドウを作成するとともに、GLFWを初期化します。
GLFW は、OpenGL よりマウス関連のイベントを処理するライブラリです。
GLFW を使用することで、前回の記事のマウス処理を簡潔に記述することができます。
o3d.geometry.PointCloud() に関しては、こちらに説明があります。
dt0 = datetime.datetime.now()
frames = pipeline.wait_for_frames()
frames = align.process(frames)
profile = frames.get_profile()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
o3d_color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)こちらも以前説明しているので、割愛します。
最終行の BGR → RGB の変換は、Open3D で PointCloud を作成するときの色調に合わせました。
Open3D 用の画像を作成 + RGBD画像作成
img_depth = o3d.geometry.Image(depth_image)
img_color = o3d.geometry.Image(o3d_color_image)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(img_color, img_depth, convert_rgb_to_intensity=False)o3d.geometry.Image() で、Open3D 用の画像を作成します。
o3d.geometry.RGBDImage.create_from_color_and_depth() で、色と深度画像を合成して、一つのRGBDImage を作成します。
convert_rgb_to_intensity は、色を強度画像に変換するかどうかを指定します。今回は不要です。
realsense のカメラ固有情報を Open3D に渡す
intrinsics = profile.as_video_stream_profile().get_intrinsics()profile は、realsense で取得した composit_frame クラスから、get_profile で取得した変数です。
as_video_stream_profile() は、video_stream_profile を取得します。
この video_stream_profile には、realsense の intrinsics(固有)情報 が含まれていますので、get_intrinsics() で固有情報を取得します。
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy)o3d.camera.PinholeCameraIntrinsic() に、先ほど realsense で取得した intrinsics情報 を渡します。公式ドキュメントはこちらにあります。
PinholeCameraIntrinsic() は、Open3D にカメラ固有情報を引き渡します。
rgbd 画像から PointCloud を作成
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, pinhole_camera_intrinsic)o3d.geometry.PointCloud.create_from_rgbd_image() は、関数名の通り、先ほど作成した rgbd 画像から PointCloud を作成します。
PointCloudクラス の説明は、こちらにあります。
PointCloud から Geometry3D への変換
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])pcd.transform は、geometry 座標 (4×4) へ変換します。
やっていることは、open3d.geometry.PointCloud タイプを open3d.geometry.Geometry3D へと変換します。 Geometry3Dタイプの説明はこちらにあります。
これを実行しないと、window上での表示が天地逆かつ左右逆になります。
ボクセルダウンサンプリング
voxel_down_pcd = pcd.voxel_down_sample(voxel_size=0.005)すべての点群を可視化するような PointCloud の出力は非常に処理が重くなるのでダウンサンプリングを行います。
ボクセルダウンサンプリングは、三次元空間を立方体のグリッドに分割します。
各グリッド内の複数の点群データからグリッドの重心を算出し、単一の点をグリッドの代表点として取り扱います。これを組み合わせて、点群データとして取り扱います。
voxel_size=0.005 は、立方体のサイズとなります。被写体のサイズに合わせて変更してください。
メッシュの貼付
現状の点群データをそのまま .ply に出力しても、Blender などの 3Dソフトで表示することができません。(形状データがないから…？) Open3D でも表示できません。
そこで、点群データにメッシュを貼付し、3Dソフトで読み取れるようにしていきます。
alpha = 0.015
mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(voxel_down_pcd, alpha)
mesh.compute_vertex_normals()o3d.geometry.TriangleMesh の公式ドキュメントはこちらにあります。
alpha の数値との関係性についてはこちらの公式ドキュメントを参照ください。
一般的に、alpha が小さくなるほど、形状を細部まで捉えることができます。
compute_vertex_normals() は、mesh を正規化し、頂点を算出します。これにより、立体的表現が可能となります。
メッシュ作成はここでいったん終了です。
空の PointCloud に現フレームから算出された PointCloud を登録
pointcloud.points = pcd.points
pointcloud.colors = pcd.colorsこちらの処理は、メモリ消費量の累積を防ぐ処理です。
Visualizer で点群データを表示
最後に、Visualizer で点群データを表示します。メッシュの表示ではありません。
if geom_added == False:
vis.add_geometry(pointcloud)
geom_added = True
vis.update_geometry(pointcloud)
vis.poll_events()
vis.update_renderer()add_geometry() で、点群データを Visualizer に登録します。
この処理は一度きりとなります。これを毎フレーム処理すると、メモリ消費量が増え、処理時間が逐次的に遅くなります。
こちらのサイトを参考にしました。ページ下部にもリンクを貼っておきます。
update_geometry() は、既に登録されている点群データを更新します。
poll_events() は、点群データの更新を待ちます。( 返り値 bool なので ret = … という形にすべき？)
update_renderer() は、レンダリングを実行します。
表示処理
cv2.imshow('bgr', color_image)
key = cv2.waitKey(1)
if key == ord('q'):
break
process_time = datetime.datetime.now() - dt0
print("FPS: " + str(1 / process_time.total_seconds()))opencv の表示処理なので割愛します。
メッシュファイルを保存
o3d.io.write_triangle_mesh("output2.ply", mesh)o3d.io.write_triangle_mesh() でメッシュファイルを保存します。
公式ドキュメントはこちらにあります。
終了処理
pipeline.stop()
cv2.destroyAllWindows()
vis.destroy_window()
del vispipeline の終了処理は確実に実行します。try ~ finally が望ましいです。
vis.destroy_window() で画面を削除した後、del vis で Visualizer クラスを削除します。
おわりに
長くなりましたのでひとまずここまでとします。
今回は realsense2 の色画像と深度画像を合成して、Open3D でリアルタイムに3Dイメージを確認する方法について説明しました。
次回は、保存した点群データの表示方法について説明していきます。
以下、参考にさせていただいたサイト様を掲載しますので、理解を深めるためにも、是非ご確認いただければと思います。
参考にさせていただいたサイト様
Open3D のメモリ消費量が累積していく issue を丁寧に解決しているサイト様です。
メッシュの貼り方について参考にさせていただきました
ボクセルダウンサンプリングについて詳細かつ分かりやすい説明があります
コメント