Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

box_dimensioner_multicam_demo error fix #9188

Merged
merged 13 commits into from
Jun 13, 2021
Merged

box_dimensioner_multicam_demo error fix #9188

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
7a1cb0c
change stream
Jun 2, 2021
74ec014
streams changed to fit L515
Jun 2, 2021
a7a5fb7
stream_index changed
Jun 3, 2021
039326c
comment removed
Jun 9, 2021
27d9ad7
add L500 pipeline configuration
Jun 9, 2021
bf13a4e
fix comment
Jun 9, 2021
505f582
use product_line
Jun 10, 2021
ebb465b
remove second array
Jun 10, 2021
56c0832
Merge branch 'development' of https://github.com/IntelRealSense/libre…
Jun 10, 2021
6ab3742
CR
Jun 13, 2021
6c992f7
Merge branch 'development' of https://github.com/IntelRealSense/libre…
Jun 13, 2021
b3ba92c
skip setting advanced mode for L500
Jun 13, 2021
8b9068e
Merge branch 'development' of https://github.com/IntelRealSense/libre…
Jun 13, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 15 additions & 3 deletions wrappers/python/examples/box_dimensioner_multicam/box_dimensioner_multicam_demo.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,14 @@
def run_demo():

# Define some constants
L515_resolution_width = 1024 # pixels
L515_resolution_height = 768 # pixels
L515_frame_rate = 30

resolution_width = 1280 # pixels
resolution_height = 720 # pixels
frame_rate = 15 # fps

dispose_frames_for_stablisation = 30 # frames

chessboard_width = 6 # squares
Expand All @@ -38,13 +43,18 @@ def run_demo():

try:
# Enable the streams from all the intel realsense devices
L515_rs_config = rs.config()
L515_rs_config.enable_stream(rs.stream.depth, L515_resolution_width, L515_resolution_height, rs.format.z16, L515_frame_rate)
L515_rs_config.enable_stream(rs.stream.infrared, 0, L515_resolution_width, L515_resolution_height, rs.format.y8, L515_frame_rate)
L515_rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate)

rs_config = rs.config()
rs_config.enable_stream(rs.stream.depth, resolution_width, resolution_height, rs.format.z16, frame_rate)
rs_config.enable_stream(rs.stream.infrared, 1, resolution_width, resolution_height, rs.format.y8, frame_rate)
rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate)

# Use the device manager class to enable the devices and get the frames
device_manager = DeviceManager(rs.context(), rs_config)
device_manager = DeviceManager(rs.context(), rs_config, L515_rs_config)
device_manager.enable_all_devices()

# Allow some frames for the auto-exposure controller to stablise
Expand Down Expand Up @@ -72,7 +82,8 @@ def run_demo():
transformation_result_kabsch = pose_estimator.perform_pose_estimation()
object_point = pose_estimator.get_chessboard_corners_in3d()
calibrated_device_count = 0
for device in device_manager._available_devices:
for device_info in device_manager._available_devices:
device = device_info[0]
if not transformation_result_kabsch[device][0]:
print("Place the chessboard on the plane where the object needs to be detected..")
else:
Expand All @@ -81,7 +92,8 @@ def run_demo():
# Save the transformation object for all devices in an array to use for measurements
transformation_devices={}
chessboard_points_cumulative_3d = np.array([-1,-1,-1]).transpose()
for device in device_manager._available_devices:
for device_info in device_manager._available_devices:
device = device_info[0]
transformation_devices[device] = transformation_result_kabsch[device][1].inverse()
points3D = object_point[device][2][:,object_point[device][3]]
points3D = transformation_devices[device].apply_transformation(points3D)
Expand Down
19 changes: 14 additions & 5 deletions wrappers/python/examples/box_dimensioner_multicam/calibration_kabsch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -150,9 +150,14 @@ def get_chessboard_corners_in3d(self):
Sequence with length N indicating which point in points3D has a valid depth value
"""
corners3D = {}
for (serial, frameset) in self.frames.items():
for (info, frameset) in self.frames.items():
serial = info[0]
product_line = info[1]
depth_frame = post_process_depth_frame(frameset[rs.stream.depth])
infrared_frame = frameset[(rs.stream.infrared, 1)]
if product_line == "L500":
infrared_frame = frameset[(rs.stream.infrared, 0)]
else:
infrared_frame = frameset[(rs.stream.infrared, 1)]
depth_intrinsics = self.intrinsic[serial][rs.stream.depth]
found_corners, points2D = cv_find_chessboard(depth_frame, infrared_frame, self.chessboard_params)
corners3D[serial] = [found_corners, None, None, None]
Expand Down Expand Up @@ -219,10 +224,14 @@ def perform_pose_estimation(self):
def find_chessboard_boundary_for_depth_image(self):
boundary = {}

for (serial, frameset) in self.frames.items():

for (info, frameset) in self.frames.items():
serial = info[0]
product_line = info[1]
depth_frame = post_process_depth_frame(frameset[rs.stream.depth])
infrared_frame = frameset[(rs.stream.infrared, 1)]
if product_line == "L500":
infrared_frame = frameset[(rs.stream.infrared, 0)]
else:
infrared_frame = frameset[(rs.stream.infrared, 1)]
found_corners, points2D = cv_find_chessboard(depth_frame, infrared_frame, self.chessboard_params)
boundary[serial] = [np.floor(np.amin(points2D[0,:])).astype(int), np.floor(np.amax(points2D[0,:])).astype(int), np.floor(np.amin(points2D[1,:])).astype(int), np.floor(np.amax(points2D[1,:])).astype(int)]

Expand Down
14 changes: 8 additions & 6 deletions wrappers/python/examples/box_dimensioner_multicam/measurement_task.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,8 @@ def calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, ro
-----------
frames_devices : dict
The frames from the different devices
keys: str
Serial number of the device
keys: Tuple of (serial, product-line)
Serial number and product line of the device
values: [frame]
frame: rs.frame()
The frameset obtained over the active pipeline from the realsense device
Expand Down Expand Up @@ -47,7 +47,8 @@ def calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, ro
"""
# Use a threshold of 5 centimeters from the chessboard as the area where useful points are found
point_cloud_cumulative = np.array([-1, -1, -1]).transpose()
for (device, frame) in frames_devices.items() :
for (device_info, frame) in frames_devices.items() :
device = device_info[0]
# Filter the depth_frame using the Temporal filter and get the corresponding pointcloud for each frame
filtered_depth_frame = post_process_depth_frame(frame[rs.stream.depth], temporal_smooth_alpha=0.1, temporal_smooth_delta=80)
point_cloud = convert_depth_frame_to_pointcloud( np.asarray( filtered_depth_frame.get_data()), calibration_info_devices[device][1][rs.stream.depth])
Expand Down Expand Up @@ -152,8 +153,8 @@ def visualise_measurements(frames_devices, bounding_box_points_devices, length,
-----------
frames_devices : dict
The frames from the different devices
keys: str
Serial number of the device
keys: Tuple of (serial, product-line)
Serial number and product line of the device
values: [frame]
frame: rs.frame()
The frameset obtained over the active pipeline from the realsense device
Expand All @@ -175,7 +176,8 @@ def visualise_measurements(frames_devices, bounding_box_points_devices, length,
height : double
The height of the bounding box calculated in the world coordinates of the pointcloud
"""
for (device, frame) in frames_devices.items():
for (device_info, frame) in frames_devices.items():
device = device_info[0] #serial number
color_image = np.asarray(frame[rs.stream.color].get_data())
if (length != 0 and width !=0 and height != 0):
bounding_box_points_device_upper = bounding_box_points_devices[device][0:4,:]
Expand Down
77 changes: 52 additions & 25 deletions wrappers/python/examples/box_dimensioner_multicam/realsense_device_manager.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@


class Device:
def __init__(self, pipeline, pipeline_profile):
def __init__(self, pipeline, pipeline_profile, product_line):
self.pipeline = pipeline
self.pipeline_profile = pipeline_profile

self.product_line = product_line

def enumerate_connected_devices(context):
"""
Expand All @@ -36,13 +36,17 @@ def enumerate_connected_devices(context):
Return:
-----------
connect_device : array
Array of enumerated devices which are connected to the PC
Array of (serial, product-line) tuples of devices which are connected to the PC

"""
connect_device = []

for d in context.devices:
if d.get_info(rs.camera_info.name).lower() != 'platform camera':
connect_device.append(d.get_info(rs.camera_info.serial_number))
serial = d.get_info(rs.camera_info.serial_number)
product_line = d.get_info(rs.camera_info.product_line)
device_info = (serial, product_line) # (serial_number, product_line)
connect_device.append( device_info )
return connect_device
maloel marked this conversation as resolved.
Show resolved Hide resolved


Expand Down Expand Up @@ -113,48 +117,63 @@ def post_process_depth_frame(depth_frame, decimation_magnitude=1.0, spatial_magn


class DeviceManager:
def __init__(self, context, pipeline_configuration):
def __init__(self, context, D400_pipeline_configuration, L500_pipeline_configuration = rs.config()):
"""
Class to manage the Intel RealSense devices

Parameters:
-----------
context : rs.context()
The context created for using the realsense library
pipeline_configuration : rs.config()
The realsense library configuration to be used for the application
D400_pipeline_configuration : rs.config()
The realsense library configuration to be used for the application when D400 product is attached.

L500_pipeline_configuration : rs.config()
The realsense library configuration to be used for the application when L500 product is attached.

"""
assert isinstance(context, type(rs.context()))
assert isinstance(pipeline_configuration, type(rs.config()))
assert isinstance(D400_pipeline_configuration, type(rs.config()))
assert isinstance(L500_pipeline_configuration, type(rs.config()))
self._context = context
self._available_devices = enumerate_connected_devices(context)
self._enabled_devices = {}
self._config = pipeline_configuration
self._enabled_devices = {} #serial numbers of te enabled devices
self.D400_config = D400_pipeline_configuration
self.L500_config = L500_pipeline_configuration
self._frame_counter = 0

def enable_device(self, device_serial, enable_ir_emitter):
def enable_device(self, device_info, enable_ir_emitter):
"""
Enable an Intel RealSense Device

Parameters:
-----------
device_serial : string
Serial number of the realsense device
device_info : Tuple of strings (serial_number, product_line)
Serial number and product line of the realsense device
enable_ir_emitter : bool
Enable/Disable the IR-Emitter of the device

"""
pipeline = rs.pipeline()

# Enable the device
self._config.enable_device(device_serial)
pipeline_profile = pipeline.start(self._config)
device_serial = device_info[0]
product_line = device_info[1]

if product_line == "L500":
# Enable L515 device
self.L500_config.enable_device(device_serial)
pipeline_profile = pipeline.start(self.L500_config)
else:
# Enable D400 device
self.D400_config.enable_device(device_serial)
pipeline_profile = pipeline.start(self.D400_config)


# Set the acquisition parameters
sensor = pipeline_profile.get_device().first_depth_sensor()
sensor.set_option(rs.option.emitter_enabled, 1 if enable_ir_emitter else 0)
self._enabled_devices[device_serial] = (Device(pipeline, pipeline_profile))
if sensor.supports(rs.option.emitter_enabled):
sensor.set_option(rs.option.emitter_enabled, 1 if enable_ir_emitter else 0)
self._enabled_devices[device_serial] = (Device(pipeline, pipeline_profile, product_line))

def enable_all_devices(self, enable_ir_emitter=False):
"""
Expand All @@ -163,8 +182,8 @@ def enable_all_devices(self, enable_ir_emitter=False):
"""
print(str(len(self._available_devices)) + " devices have been found")

for serial in self._available_devices:
self.enable_device(serial, enable_ir_emitter)
for device_info in self._available_devices:
self.enable_device(device_info, enable_ir_emitter)

def enable_emitter(self, enable_ir_emitter=True):
"""
Expand All @@ -174,6 +193,8 @@ def enable_emitter(self, enable_ir_emitter=True):
for (device_serial, device) in self._enabled_devices.items():
# Get the active profile and enable the emitter for all the connected devices
sensor = device.pipeline_profile.get_device().first_depth_sensor()
if not sensor.supports(rs.option.emitter_enabled):
continue
sensor.set_option(rs.option.emitter_enabled, 1 if enable_ir_emitter else 0)
if enable_ir_emitter:
sensor.set_option(rs.option.laser_power, 330)
Expand All @@ -188,6 +209,8 @@ def load_settings_json(self, path_to_settings_file):
json_text = file.read().strip()

for (device_serial, device) in self._enabled_devices.items():
if device.product_line == "L500":
continue
# Get the active profile and load the json file which contains settings readable by the realsense
device = device.pipeline_profile.get_device()
advanced_mode = rs.rs400_advanced_mode(device)
Expand All @@ -207,15 +230,16 @@ def poll_frames(self):
streams = device.pipeline_profile.get_streams()
frameset = device.pipeline.poll_for_frames() #frameset will be a pyrealsense2.composite_frame object
if frameset.size() == len(streams):
frames[serial] = {}
dev_info = (serial, device.product_line)
frames[dev_info] = {}
for stream in streams:
if (rs.stream.infrared == stream.stream_type()):
frame = frameset.get_infrared_frame(stream.stream_index())
key_ = (stream.stream_type(), stream.stream_index())
else:
frame = frameset.first_or_default(stream.stream_type())
key_ = stream.stream_type()
frames[serial][key_] = frame
frames[dev_info][key_] = frame

return frames

Expand Down Expand Up @@ -255,7 +279,8 @@ def get_device_intrinsics(self, frames):
Intrinsics of the corresponding device
"""
device_intrinsics = {}
for (serial, frameset) in frames.items():
for (dev_info, frameset) in frames.items():
serial = dev_info[0]
device_intrinsics[serial] = {}
for key, value in frameset.items():
device_intrinsics[serial][key] = value.get_profile().as_video_stream_profile().get_intrinsics()
Expand All @@ -279,14 +304,16 @@ def get_depth_to_color_extrinsics(self, frames):
Extrinsics of the corresponding device
"""
device_extrinsics = {}
for (serial, frameset) in frames.items():
for (dev_info, frameset) in frames.items():
serial = dev_info[0]
device_extrinsics[serial] = frameset[
rs.stream.depth].get_profile().as_video_stream_profile().get_extrinsics_to(
frameset[rs.stream.color].get_profile())
return device_extrinsics

def disable_streams(self):
self._config.disable_all_streams()
self.D400_config.disable_all_streams()
self.L500_config.disable_all_streams()


"""
Expand Down