-
Notifications
You must be signed in to change notification settings - Fork 0
/
rs_pcd_visualizer.py
65 lines (51 loc) · 2.02 KB
/
rs_pcd_visualizer.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
import pyrealsense2 as rs
import numpy as np
import cv2
import open3d as o3d
align = rs.align(rs.stream.color)
config = rs.config()
config.enable_stream(rs.stream.depth, 1024, 768, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1920, 1080, rs.format.rgb8, 30)
pipeline = rs.pipeline()
profile = pipeline.start(config)
intr = profile.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)
extrinsic = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]
def convert_rs_frames_to_pointcloud(rs_frames):
aligned_frames = align.process(rs_frames)
rs_depth_frame = aligned_frames.get_depth_frame()
np_depth = np.asanyarray(rs_depth_frame.get_data())
o3d_depth = o3d.geometry.Image(np_depth)
rs_color_frame = aligned_frames.get_color_frame()
np_color = np.asanyarray(rs_color_frame.get_data())
o3d_color = o3d.geometry.Image(np_color)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
o3d_color, o3d_depth, depth_scale=4000.0, convert_rgb_to_intensity=False)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd, pinhole_camera_intrinsic, extrinsic)
return pcd
def main():
rs_frames = pipeline.wait_for_frames()
pcd = convert_rs_frames_to_pointcloud(rs_frames)
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="Point Cloud Visualizer",
width=800, height=800)
vis.add_geometry(pcd)
render_opt = vis.get_render_option()
render_opt.point_size = 1.0
while True:
rs_frames = pipeline.wait_for_frames()
pcd_new = convert_rs_frames_to_pointcloud(rs_frames)
pcd.points = pcd_new.points
pcd.colors = pcd_new.colors
vis.update_geometry(pcd)
if vis.poll_events():
vis.update_renderer()
else:
break
vis.destroy_window()
pipeline.stop()
if __name__ == "__main__":
main()