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データらしいものを生成できると思います。
それでは。