realsense_device_manager.py
Go to the documentation of this file.
1 ##################################################################################################
2 ## License: Apache 2.0. See LICENSE file in root directory. ####
3 ##################################################################################################
4 ## Box Dimensioner with multiple cameras: Helper files ####
5 ##################################################################################################
6 
7 
8 import pyrealsense2 as rs
9 import numpy as np
10 
11 """
12  _ _ _ _____ _ _
13  | | | | ___ | | _ __ ___ _ __ | ___|_ _ _ __ ___ | |_ (_) ___ _ __ ___
14  | |_| | / _ \| || '_ \ / _ \| '__| | |_ | | | || '_ \ / __|| __|| | / _ \ | '_ \ / __|
15  | _ || __/| || |_) || __/| | | _| | |_| || | | || (__ | |_ | || (_) || | | |\__ \
16  |_| |_| \___||_|| .__/ \___||_| |_| \__,_||_| |_| \___| \__||_| \___/ |_| |_||___/
17  |_|
18 """
19 
20 
21 class Device:
22  def __init__(self, pipeline, pipeline_profile):
23  self.pipeline = pipeline
24  self.pipeline_profile = pipeline_profile
25 
26 
28  """
29  Enumerate the connected Intel RealSense devices
30 
31  Parameters:
32  -----------
33  context : rs.context()
34  The context created for using the realsense library
35 
36  Return:
37  -----------
38  connect_device : array
39  Array of enumerated devices which are connected to the PC
40 
41  """
42  connect_device = []
43  for d in context.devices:
44  if d.get_info(rs.camera_info.name).lower() != 'platform camera':
45  connect_device.append(d.get_info(rs.camera_info.serial_number))
46  return connect_device
47 
48 
49 def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5,
50  spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20):
51  """
52  Filter the depth frame acquired using the Intel RealSense device
53 
54  Parameters:
55  -----------
56  depth_frame : rs.frame()
57  The depth frame to be post-processed
58  decimation_magnitude : double
59  The magnitude of the decimation filter
60  spatial_magnitude : double
61  The magnitude of the spatial filter
62  spatial_smooth_alpha : double
63  The alpha value for spatial filter based smoothening
64  spatial_smooth_delta : double
65  The delta value for spatial filter based smoothening
66  temporal_smooth_alpha: double
67  The alpha value for temporal filter based smoothening
68  temporal_smooth_delta: double
69  The delta value for temporal filter based smoothening
70 
71  Return:
72  ----------
73  filtered_frame : rs.frame()
74  The post-processed depth frame
75  """
76 
77  # Post processing possible only on the depth_frame
78  assert (depth_frame.is_depth_frame())
79 
80  # Available filters and control options for the filters
81  decimation_filter = rs.decimation_filter()
82  spatial_filter = rs.spatial_filter()
83  temporal_filter = rs.temporal_filter()
84 
85  filter_magnitude = rs.option.filter_magnitude
86  filter_smooth_alpha = rs.option.filter_smooth_alpha
87  filter_smooth_delta = rs.option.filter_smooth_delta
88 
89  # Apply the control parameters for the filter
90  decimation_filter.set_option(filter_magnitude, decimation_magnitude)
91  spatial_filter.set_option(filter_magnitude, spatial_magnitude)
92  spatial_filter.set_option(filter_smooth_alpha, spatial_smooth_alpha)
93  spatial_filter.set_option(filter_smooth_delta, spatial_smooth_delta)
94  temporal_filter.set_option(filter_smooth_alpha, temporal_smooth_alpha)
95  temporal_filter.set_option(filter_smooth_delta, temporal_smooth_delta)
96 
97  # Apply the filters
98  filtered_frame = decimation_filter.process(depth_frame)
99  filtered_frame = spatial_filter.process(filtered_frame)
100  filtered_frame = temporal_filter.process(filtered_frame)
101 
102  return filtered_frame
103 
104 
105 """
106  __ __ _ ____ _ _
107  | \/ | __ _ (_) _ __ / ___| ___ _ __ | |_ ___ _ __ | |_
108  | |\/| | / _` || || '_ \ | | / _ \ | '_ \ | __|/ _ \| '_ \ | __|
109  | | | || (_| || || | | | | |___| (_) || | | || |_| __/| | | || |_
110  |_| |_| \__,_||_||_| |_| \____|\___/ |_| |_| \__|\___||_| |_| \__|
111 
112 """
113 
114 
116  def __init__(self, context, pipeline_configuration):
117  """
118  Class to manage the Intel RealSense devices
119 
120  Parameters:
121  -----------
122  context : rs.context()
123  The context created for using the realsense library
124  pipeline_configuration : rs.config()
125  The realsense library configuration to be used for the application
126 
127  """
128  assert isinstance(context, type(rs.context()))
129  assert isinstance(pipeline_configuration, type(rs.config()))
130  self._context = context
133  self._config = pipeline_configuration
134  self._frame_counter = 0
135 
136  def enable_device(self, device_serial, enable_ir_emitter):
137  """
138  Enable an Intel RealSense Device
139 
140  Parameters:
141  -----------
142  device_serial : string
143  Serial number of the realsense device
144  enable_ir_emitter : bool
145  Enable/Disable the IR-Emitter of the device
146 
147  """
148  pipeline = rs.pipeline()
149 
150  # Enable the device
151  self._config.enable_device(device_serial)
152  pipeline_profile = pipeline.start(self._config)
153 
154  # Set the acquisition parameters
155  sensor = pipeline_profile.get_device().first_depth_sensor()
156  sensor.set_option(rs.option.emitter_enabled, 1 if enable_ir_emitter else 0)
157  self._enabled_devices[device_serial] = (Device(pipeline, pipeline_profile))
158 
159  def enable_all_devices(self, enable_ir_emitter=False):
160  """
161  Enable all the Intel RealSense Devices which are connected to the PC
162 
163  """
164  print(str(len(self._available_devices)) + " devices have been found")
165 
166  for serial in self._available_devices:
167  self.enable_device(serial, enable_ir_emitter)
168 
169  def enable_emitter(self, enable_ir_emitter=True):
170  """
171  Enable/Disable the emitter of the intel realsense device
172 
173  """
174  for (device_serial, device) in self._enabled_devices.items():
175  # Get the active profile and enable the emitter for all the connected devices
176  sensor = device.pipeline_profile.get_device().first_depth_sensor()
177  sensor.set_option(rs.option.emitter_enabled, 1 if enable_ir_emitter else 0)
178  if enable_ir_emitter:
179  sensor.set_option(rs.option.laser_power, 330)
180 
181  def load_settings_json(self, path_to_settings_file):
182  """
183  Load the settings stored in the JSON file
184 
185  """
186 
187  with open(path_to_settings_file, 'r') as file:
188  json_text = file.read().strip()
189 
190  for (device_serial, device) in self._enabled_devices.items():
191  # Get the active profile and load the json file which contains settings readable by the realsense
192  device = device.pipeline_profile.get_device()
193  advanced_mode = rs.rs400_advanced_mode(device)
194  advanced_mode.load_json(json_text)
195 
196  def poll_frames(self):
197  """
198  Poll for frames from the enabled Intel RealSense devices. This will return at least one frame from each device.
199  If temporal post processing is enabled, the depth stream is averaged over a certain amount of frames
200 
201  Parameters:
202  -----------
203  """
204  frames = {}
205  while len(frames) < len(self._enabled_devices.items()) :
206  for (serial, device) in self._enabled_devices.items():
207  streams = device.pipeline_profile.get_streams()
208  frameset = device.pipeline.poll_for_frames() #frameset will be a pyrealsense2.composite_frame object
209  if frameset.size() == len(streams):
210  frames[serial] = {}
211  for stream in streams:
212  if (rs.stream.infrared == stream.stream_type()):
213  frame = frameset.get_infrared_frame(stream.stream_index())
214  key_ = (stream.stream_type(), stream.stream_index())
215  else:
216  frame = frameset.first_or_default(stream.stream_type())
217  key_ = stream.stream_type()
218  frames[serial][key_] = frame
219 
220  return frames
221 
222  def get_depth_shape(self):
223  """
224  Retruns width and height of the depth stream for one arbitrary device
225 
226  Returns:
227  -----------
228  width : int
229  height: int
230  """
231  width = -1
232  height = -1
233  for (serial, device) in self._enabled_devices.items():
234  for stream in device.pipeline_profile.get_streams():
235  if (rs.stream.depth == stream.stream_type()):
236  width = stream.as_video_stream_profile().width()
237  height = stream.as_video_stream_profile().height()
238  return width, height
239 
240  def get_device_intrinsics(self, frames):
241  """
242  Get the intrinsics of the imager using its frame delivered by the realsense device
243 
244  Parameters:
245  -----------
246  frames : rs::frame
247  The frame grabbed from the imager inside the Intel RealSense for which the intrinsic is needed
248 
249  Return:
250  -----------
251  device_intrinsics : dict
252  keys : serial
253  Serial number of the device
254  values: [key]
255  Intrinsics of the corresponding device
256  """
257  device_intrinsics = {}
258  for (serial, frameset) in frames.items():
259  device_intrinsics[serial] = {}
260  for key, value in frameset.items():
261  device_intrinsics[serial][key] = value.get_profile().as_video_stream_profile().get_intrinsics()
262  return device_intrinsics
263 
264  def get_depth_to_color_extrinsics(self, frames):
265  """
266  Get the extrinsics between the depth imager 1 and the color imager using its frame delivered by the realsense device
267 
268  Parameters:
269  -----------
270  frames : rs::frame
271  The frame grabbed from the imager inside the Intel RealSense for which the intrinsic is needed
272 
273  Return:
274  -----------
275  device_intrinsics : dict
276  keys : serial
277  Serial number of the device
278  values: [key]
279  Extrinsics of the corresponding device
280  """
281  device_extrinsics = {}
282  for (serial, frameset) in frames.items():
283  device_extrinsics[serial] = frameset[
284  rs.stream.depth].get_profile().as_video_stream_profile().get_extrinsics_to(
285  frameset[rs.stream.color].get_profile())
286  return device_extrinsics
287 
288  def disable_streams(self):
289  self._config.disable_all_streams()
290 
291 
292 """
293  _____ _ _
294  |_ _|___ ___ | |_ (_) _ __ __ _
295  | | / _ \/ __|| __|| || '_ \ / _` |
296  | || __/\__ \| |_ | || | | || (_| |
297  |_| \___||___/ \__||_||_| |_| \__, |
298  |___/
299 
300 """
301 if __name__ == "__main__":
302  try:
303  c = rs.config()
304  c.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 6)
305  c.enable_stream(rs.stream.infrared, 1, 1280, 720, rs.format.y8, 6)
306  c.enable_stream(rs.stream.infrared, 2, 1280, 720, rs.format.y8, 6)
307  c.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 6)
308  device_manager = DeviceManager(rs.context(), c)
309  device_manager.enable_all_devices()
310  for k in range(150):
311  frames = device_manager.poll_frames()
312  device_manager.enable_emitter(True)
313  device_extrinsics = device_manager.get_depth_to_color_extrinsics(frames)
314  finally:
315  device_manager.disable_streams()
def __init__(self, pipeline, pipeline_profile)
def enable_emitter(self, enable_ir_emitter=True)
def load_settings_json(self, path_to_settings_file)
static std::string print(const transformation &tf)
def enable_all_devices(self, enable_ir_emitter=False)
def __init__(self, context, pipeline_configuration)
def enable_device(self, device_serial, enable_ir_emitter)
def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magnitude=2.0, spatial_smooth_alpha=0.5, spatial_smooth_delta=20, temporal_smooth_alpha=0.4, temporal_smooth_delta=20)


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:39