1# ----------------------------------------------------------------------------
2# - Open3D: www.open3d.org -
3# ----------------------------------------------------------------------------
4# Copyright (c) 2018-2023 www.open3d.org
5# SPDX-License-Identifier: MIT
6# ----------------------------------------------------------------------------
7"""Demonstrate RealSense camera discovery and frame capture. An RS bag file is
8used if a RealSense camera is not available. Captured frames are
9displayed as a live point cloud. Also frames are saved to ./capture/{color,depth}
10folders.
11
12Usage:
13
14 - Display live point cloud from RS camera:
15 python realsense_io.py
16
17 - Display live point cloud from RS bag file:
18 python realsense_io.py rgbd.bag
19
20 If no device is detected and no bag file is supplied, uses the Open3D
21 example JackJackL515Bag dataset.
22"""
23
24import sys
25import os
26from concurrent.futures import ThreadPoolExecutor
27import open3d as o3d
28import open3d.t.io as io3d
29from open3d.t.geometry import PointCloud
30import open3d.visualization.gui as gui
31import open3d.visualization.rendering as rendering
32
33DEPTH_MAX = 3
34
35
36def point_cloud_video(executor, rgbd_frame, mdata, timestamp, o3dvis):
37 """Update window to display the next point cloud frame."""
38 app = gui.Application.instance
39 update_flag = rendering.Scene.UPDATE_POINTS_FLAG | rendering.Scene.UPDATE_COLORS_FLAG
40
41 executor.submit(io3d.write_image,
42 f"capture/color/{point_cloud_video.fid:05d}.jpg",
43 rgbd_frame.color)
44 executor.submit(io3d.write_image,
45 f"capture/depth/{point_cloud_video.fid:05d}.png",
46 rgbd_frame.depth)
47 print(f"Frame: {point_cloud_video.fid}, timestamp: {timestamp * 1e-6:.3f}s",
48 end="\r")
49 if point_cloud_video.fid == 0:
50 # Start with a dummy max sized point cloud to allocate GPU buffers
51 # for update_geometry()
52 max_pts = rgbd_frame.color.rows * rgbd_frame.color.columns
53 pcd = PointCloud(o3d.core.Tensor.zeros((max_pts, 3)))
54 pcd.paint_uniform_color([1., 1., 1.])
55 app.post_to_main_thread(o3dvis,
56 lambda: o3dvis.add_geometry("Point Cloud", pcd))
57 pcd = PointCloud.create_from_rgbd_image(
58 rgbd_frame,
59 # Intrinsic matrix: Tensor([[fx, 0., cx], [0., fy, cy], [0., 0., 1.]])
60 mdata.intrinsics.intrinsic_matrix,
61 depth_scale=mdata.depth_scale,
62 depth_max=DEPTH_MAX)
63 # GUI operations MUST run in the main thread.
64 app.post_to_main_thread(
65 o3dvis, lambda: o3dvis.update_geometry("Point Cloud", pcd, update_flag))
66 point_cloud_video.fid += 1
67
68
69point_cloud_video.fid = 0
70
71
72def pcd_video_from_camera(executor, o3dvis):
73 """Show point cloud video coming from a RealSense camera. Save frames to
74 disk in capture/{color,depth} folders.
75 """
76 rscam = io3d.RealSenseSensor()
77 rscam.start_capture()
78 mdata = rscam.get_metadata()
79 print(mdata)
80 os.makedirs("capture/color")
81 os.makedirs("capture/depth")
82 rgbd_frame_future = executor.submit(rscam.capture_frame)
83
84 def on_window_close():
85 nonlocal rscam, executor
86 executor.shutdown()
87 rscam.stop_capture()
88 return True # OK to close window
89
90 o3dvis.set_on_close(on_window_close)
91
92 while True:
93 rgbd_frame = rgbd_frame_future.result()
94 # Run each IO operation in the threadpool
95 rgbd_frame_future = executor.submit(rscam.capture_frame)
96 point_cloud_video(executor, rgbd_frame, mdata, rscam.get_timestamp(),
97 o3dvis)
98
99
100def pcd_video_from_bag(rsbagfile, executor, o3dvis):
101 """Show point cloud video coming from a RealSense bag file. Save frames to
102 disk in capture/{color,depth} folders.
103 """
104 rsbag = io3d.RSBagReader.create(rsbagfile)
105 if not rsbag.is_opened():
106 raise RuntimeError(f"RS bag file {rsbagfile} could not be opened.")
107 mdata = rsbag.metadata
108 print(mdata)
109 os.makedirs("capture/color")
110 os.makedirs("capture/depth")
111
112 def on_window_close():
113 nonlocal rsbag, executor
114 executor.shutdown()
115 rsbag.close()
116 return True # OK to close window
117
118 o3dvis.set_on_close(on_window_close)
119
120 rgbd_frame = rsbag.next_frame()
121 while not rsbag.is_eof():
122 # Run each IO operation in the threadpool
123 rgbd_frame_future = executor.submit(rsbag.next_frame)
124 point_cloud_video(executor, rgbd_frame, mdata, rsbag.get_timestamp(),
125 o3dvis)
126 rgbd_frame = rgbd_frame_future.result()
127
128
129def main():
130 if os.path.exists("capture"):
131 raise RuntimeError(
132 "Frames saving destination folder 'capture' already exists. Please move it."
133 )
134
135 # Initialize app and create GUI window
136 app = gui.Application.instance
137 app.initialize()
138 o3dvis = o3d.visualization.O3DVisualizer("Open3D: PointCloud video", 1024,
139 768)
140 o3dvis.show_axes = True
141 # set view: fov, eye, lookat, up
142 o3dvis.setup_camera(45, [0., 0., 0.], [0., 0., -1.], [0., -1., 0.])
143 app.add_window(o3dvis)
144
145 have_cam = io3d.RealSenseSensor.list_devices()
146 have_bag = (len(sys.argv) == 2)
147
148 with ThreadPoolExecutor(max_workers=4) as executor:
149 # Run IO and compute in threadpool
150 if have_bag:
151 executor.submit(pcd_video_from_bag, sys.argv[1], executor, o3dvis)
152 elif have_cam:
153 executor.submit(pcd_video_from_camera, executor, o3dvis)
154 else:
155 executor.submit(pcd_video_from_bag,
156 o3d.data.JackJackL515Bag().path, executor, o3dvis)
157
158 app.run() # GUI runs in the main thread.
159
160
161if __name__ == "__main__":
162 main()