LoginSignup
27

More than 5 years have passed since last update.

RealSense D435 で PointCloud 作って表示する(Python3)

Last updated at Posted at 2018-10-27

修正

2018/10/29 19:34
カメラパラメータをプログラムで取得するように変更した。
2018/10/29 19:17
下記プログラム中のカメラに関するパラメータ設定を間違えていたので修正した。
誤:PinholeCameraIntrinsic(1280, 720, 640, 640, 640, 360)
正:PinholeCameraIntrinsic(1280, 720, 925, 925, 640, 360)

カラー画像

pc_color.png

PointCloud表示1

pc_color_3d.png

PointCloud表示2

pc_color_3d_2.png

Pythonソース

pc_capt_color.py
import pyrealsense2 as rs
import numpy as np
import cv2
from open3d import *

if __name__=="__main__":
    align = rs.align(rs.stream.color)

    config = rs.config()
    config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 6)
    config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 6)
    pipeline = rs.pipeline()
    profile = pipeline.start(config)

    # get camera intrinsics
    intr = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()
    print(intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)
    pinhole_camera_intrinsic = PinholeCameraIntrinsic(intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)

    while True:
        frames = pipeline.wait_for_frames()
        aligned_frames = align.process(frames)

        color_frame = aligned_frames.get_color_frame()
        color_image = np.asanyarray(color_frame.get_data())

        cv2.namedWindow('color image', cv2.WINDOW_AUTOSIZE)
        cv2.imshow('color image', cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR))

        if cv2.waitKey(1) != -1:
            print('finish')
            break

    depth_frame = aligned_frames.get_depth_frame()
    depth = Image(np.asanyarray(depth_frame.get_data()))
    color = Image(color_image)

    rgbd = create_rgbd_image_from_color_and_depth(color, depth, convert_rgb_to_intensity = False);
    pcd = create_point_cloud_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]])

    pipeline.stop()
    write_point_cloud('./pc_color.pcd', pcd)
    draw_geometries([pcd])

まずはD435からのカラー画像をOpenCVで表示し続ける。
何かキーを押すとその時点でのカラー画像と深度画像からPointCloudを作り、pc_color.pcdファイルとして保存してからOpen3Dを使って表示する。(マウスで拡大縮小や視線変更可能)
Open3Dウィンドウの終了ボタンを押すと終了。

実行

$ python pc_capt_color.py

保存されたpc_color.pcdをOpen3Dで表示

pc_disp_color.py
from open3d import *

if __name__=="__main__":
    pcd = read_point_cloud('./pc_color.pcd')
    draw_geometries([pcd])

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
27