1# ---------------------------------------------------------------------------- 2# - Open3D: www.open3d.org - 3# ---------------------------------------------------------------------------- 4# Copyright (c) 2018-2024 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 24importsys 25importos 26fromconcurrent.futuresimportThreadPoolExecutor 27importopen3daso3d 28importopen3d.t.ioasio3d 29fromopen3d.t.geometryimportPointCloud 30importopen3d.visualization.guiasgui 31importopen3d.visualization.renderingasrendering 32 33DEPTH_MAX=3 34 35 36defpoint_cloud_video(executor,rgbd_frame,mdata,timestamp,o3dvis): 37"""Update window to display the next point cloud frame.""" 38app=gui.Application.instance 39update_flag=rendering.Scene.UPDATE_POINTS_FLAG|rendering.Scene.UPDATE_COLORS_FLAG 40 41executor.submit(io3d.write_image, 42f"capture/color/{point_cloud_video.fid:05d}.jpg", 43rgbd_frame.color) 44executor.submit(io3d.write_image, 45f"capture/depth/{point_cloud_video.fid:05d}.png", 46rgbd_frame.depth) 47print(f"Frame: {point_cloud_video.fid}, timestamp: {timestamp*1e-6:.3f}s", 48end="\r") 49ifpoint_cloud_video.fid==0: 50# Start with a dummy max sized point cloud to allocate GPU buffers 51# for update_geometry() 52max_pts=rgbd_frame.color.rows*rgbd_frame.color.columns 53pcd=PointCloud(o3d.core.Tensor.zeros((max_pts,3))) 54pcd.paint_uniform_color([1.,1.,1.]) 55app.post_to_main_thread(o3dvis, 56lambda:o3dvis.add_geometry("Point Cloud",pcd)) 57pcd=PointCloud.create_from_rgbd_image( 58rgbd_frame, 59# Intrinsic matrix: Tensor([[fx, 0., cx], [0., fy, cy], [0., 0., 1.]]) 60mdata.intrinsics.intrinsic_matrix, 61depth_scale=mdata.depth_scale, 62depth_max=DEPTH_MAX) 63# GUI operations MUST run in the main thread. 64app.post_to_main_thread( 65o3dvis,lambda:o3dvis.update_geometry("Point Cloud",pcd,update_flag)) 66point_cloud_video.fid+=1 67 68 69point_cloud_video.fid=0 70 71 72defpcd_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 """ 76rscam=io3d.RealSenseSensor() 77rscam.start_capture() 78mdata=rscam.get_metadata() 79print(mdata) 80os.makedirs("capture/color") 81os.makedirs("capture/depth") 82rgbd_frame_future=executor.submit(rscam.capture_frame) 83 84defon_window_close(): 85nonlocalrscam,executor 86executor.shutdown() 87rscam.stop_capture() 88returnTrue# OK to close window 89 90o3dvis.set_on_close(on_window_close) 91 92whileTrue: 93rgbd_frame=rgbd_frame_future.result() 94# Run each IO operation in the threadpool 95rgbd_frame_future=executor.submit(rscam.capture_frame) 96point_cloud_video(executor,rgbd_frame,mdata,rscam.get_timestamp(), 97o3dvis) 98 99100defpcd_video_from_bag(rsbagfile,executor,o3dvis):101"""Show point cloud video coming from a RealSense bag file. Save frames to102 disk in capture/{color,depth} folders.103 """104rsbag=io3d.RSBagReader.create(rsbagfile)105ifnotrsbag.is_opened():106raiseRuntimeError(f"RS bag file {rsbagfile} could not be opened.")107mdata=rsbag.metadata108print(mdata)109os.makedirs("capture/color")110os.makedirs("capture/depth")111112defon_window_close():113nonlocalrsbag,executor114executor.shutdown()115rsbag.close()116returnTrue# OK to close window117118o3dvis.set_on_close(on_window_close)119120rgbd_frame=rsbag.next_frame()121whilenotrsbag.is_eof():122# Run each IO operation in the threadpool123rgbd_frame_future=executor.submit(rsbag.next_frame)124point_cloud_video(executor,rgbd_frame,mdata,rsbag.get_timestamp(),125o3dvis)126rgbd_frame=rgbd_frame_future.result()127128129defmain():130ifos.path.exists("capture"):131raiseRuntimeError(132"Frames saving destination folder 'capture' already exists. Please move it."133)134135# Initialize app and create GUI window136app=gui.Application.instance137app.initialize()138o3dvis=o3d.visualization.O3DVisualizer("Open3D: PointCloud video",1024,139768)140o3dvis.show_axes=True141# set view: fov, eye, lookat, up142o3dvis.setup_camera(45,[0.,0.,0.],[0.,0.,-1.],[0.,-1.,0.])143app.add_window(o3dvis)144145have_cam=io3d.RealSenseSensor.list_devices()146have_bag=(len(sys.argv)==2)147148withThreadPoolExecutor(max_workers=4)asexecutor:149# Run IO and compute in threadpool150ifhave_bag:151executor.submit(pcd_video_from_bag,sys.argv[1],executor,o3dvis)152elifhave_cam:153executor.submit(pcd_video_from_camera,executor,o3dvis)154else:155executor.submit(pcd_video_from_bag,156o3d.data.JackJackL515Bag().path,executor,o3dvis)157158app.run()# GUI runs in the main thread.159160161if__name__=="__main__":162main()