Realsenseの点群をリアルタイム表示(Python)
Realsenseの取得点群表示を気軽にPythonで実行したい。C++とPCLライブラリで表示しても良いが、あまりPCLに依存せずできる方法としてOpen3DのViewerを利用できないか調べていたOpen3D公式のExampleを見ているとカメラから取得した点群を表示する活用方法は紹介されていなかったが、issueを見てみると参考になりそうな記述があったので試してみた。
参考
https://github.com/intel-isl/Open3D/issues/473
コード
<環境設定> 使用センサ:Realsense D435 Open3D version = 0.8.0 Python version = 3.5.2
下記はissueに記述のあったコード。そのままでは動作しなかったので一部書き換えている。 コードに関しては一度geometoryにinputしたpointcloudを取得した点群データの最新のものに置き換え
vis.add_geometry(pointcloud)
下記描画処理のところでupdateを掛けて表示を更新している。
vis.update_geometry() vis.poll_events() vis.update_renderer()
#!/usr/bin/env python3 # -*- coding: utf-8 -*- 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=1280, height=720) 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()) img_depth = o3d.geometry.Image(depth_image) img_color = o3d.geometry.Image(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]]) pointcloud.points = pcd.points pointcloud.colors = pcd.colors if geom_added == False: vis.add_geometry(pointcloud) geom_added = True vis.update_geometry() 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())) pipeline.stop() cv2.destroyAllWindows() vis.destroy_window() del vis
テスト結果
ベイマックス君を撮影対象にしてみた。十分使えそうな感じであったので、次は描画設定の所を好みな感じにいじれるようにしたい。