RealSense D435 で PointCloud 作って表示する（Python3）

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

#カラー画像

#PointCloud表示1

#PointCloud表示2

#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ウィンドウの終了ボタンを押すと終了。

#実行

.sh

$ 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])