box_dimensioner_multicam_demo.py
Go to the documentation of this file.
1 
14 
15 # Import RealSense, OpenCV and NumPy
16 import pyrealsense2 as rs
17 import cv2
18 import numpy as np
19 
20 # Import helper functions and classes written to wrap the RealSense, OpenCV and Kabsch Calibration usage
21 from collections import defaultdict
22 from realsense_device_manager import DeviceManager
23 from calibration_kabsch import PoseEstimation
24 from helper_functions import get_boundary_corners_2D
25 from measurement_task import calculate_boundingbox_points, calculate_cumulative_pointcloud, visualise_measurements
26 
27 def run_demo():
28 
29  # Define some constants
30  resolution_width = 1280 # pixels
31  resolution_height = 720 # pixels
32  frame_rate = 15 # fps
33 
34  dispose_frames_for_stablisation = 30 # frames
35 
36  chessboard_width = 6 # squares
37  chessboard_height = 9 # squares
38  square_size = 0.0253 # meters
39 
40  try:
41  # Enable the streams from all the intel realsense devices
42  rs_config = rs.config()
43  rs_config.enable_stream(rs.stream.depth, resolution_width, resolution_height, rs.format.z16, frame_rate)
44  rs_config.enable_stream(rs.stream.infrared, 1, resolution_width, resolution_height, rs.format.y8, frame_rate)
45  rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate)
46 
47  # Use the device manager class to enable the devices and get the frames
48  device_manager = DeviceManager(rs.context(), rs_config)
49  device_manager.enable_all_devices()
50 
51  # Allow some frames for the auto-exposure controller to stablise
52  for frame in range(dispose_frames_for_stablisation):
53  frames = device_manager.poll_frames()
54 
55  assert( len(device_manager._available_devices) > 0 )
56  """
57  1: Calibration
58  Calibrate all the available devices to the world co-ordinates.
59  For this purpose, a chessboard printout for use with opencv based calibration process is needed.
60 
61  """
62  # Get the intrinsics of the realsense device
63  intrinsics_devices = device_manager.get_device_intrinsics(frames)
64 
65  # Set the chessboard parameters for calibration
66  chessboard_params = [chessboard_height, chessboard_width, square_size]
67 
68  # Estimate the pose of the chessboard in the world coordinate using the Kabsch Method
69  calibrated_device_count = 0
70  while calibrated_device_count < len(device_manager._available_devices):
71  frames = device_manager.poll_frames()
72  pose_estimator = PoseEstimation(frames, intrinsics_devices, chessboard_params)
73  transformation_result_kabsch = pose_estimator.perform_pose_estimation()
74  object_point = pose_estimator.get_chessboard_corners_in3d()
75  calibrated_device_count = 0
76  for device_info in device_manager._available_devices:
77  device = device_info[0]
78  if not transformation_result_kabsch[device][0]:
79  print("Place the chessboard on the plane where the object needs to be detected..")
80  else:
81  calibrated_device_count += 1
82 
83  # Save the transformation object for all devices in an array to use for measurements
84  transformation_devices={}
85  chessboard_points_cumulative_3d = np.array([-1,-1,-1]).transpose()
86  for device_info in device_manager._available_devices:
87  device = device_info[0]
88  transformation_devices[device] = transformation_result_kabsch[device][1].inverse()
89  points3D = object_point[device][2][:,object_point[device][3]]
90  points3D = transformation_devices[device].apply_transformation(points3D)
91  chessboard_points_cumulative_3d = np.column_stack( (chessboard_points_cumulative_3d,points3D) )
92 
93  # Extract the bounds between which the object's dimensions are needed
94  # It is necessary for this demo that the object's length and breath is smaller than that of the chessboard
95  chessboard_points_cumulative_3d = np.delete(chessboard_points_cumulative_3d, 0, 1)
96  roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d)
97 
98  print("Calibration completed... \nPlace the box in the field of view of the devices...")
99 
100 
101  """
102  2: Measurement and display
103  Measure the dimension of the object using depth maps from multiple RealSense devices
104  The information from Phase 1 will be used here
105 
106  """
107 
108  # Enable the emitter of the devices
109  device_manager.enable_emitter(True)
110 
111  # Load the JSON settings file in order to enable High Accuracy preset for the realsense
112  device_manager.load_settings_json("./HighResHighAccuracyPreset.json")
113 
114  # Get the extrinsics of the device to be used later
115  extrinsics_devices = device_manager.get_depth_to_color_extrinsics(frames)
116 
117  # Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image
118  calibration_info_devices = defaultdict(list)
119  for calibration_info in (transformation_devices, intrinsics_devices, extrinsics_devices):
120  for key, value in calibration_info.items():
121  calibration_info_devices[key].append(value)
122 
123  # Continue acquisition until terminated with Ctrl+C by the user
124  while 1:
125  # Get the frames from all the devices
126  frames_devices = device_manager.poll_frames()
127 
128  # Calculate the pointcloud using the depth frames from all the devices
129  point_cloud = calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, roi_2D)
130 
131  # Get the bounding box for the pointcloud in image coordinates of the color imager
132  bounding_box_points_color_image, length, width, height = calculate_boundingbox_points(point_cloud, calibration_info_devices )
133 
134  # Draw the bounding box points on the color image and visualise the results
135  visualise_measurements(frames_devices, bounding_box_points_color_image, length, width, height)
136 
137  except KeyboardInterrupt:
138  print("The program was interupted by the user. Closing the program...")
139 
140  finally:
141  device_manager.disable_streams()
142  cv2.destroyAllWindows()
143 
144 
145 if __name__ == "__main__":
146  run_demo()
box_dimensioner_multicam_demo.run_demo
def run_demo()
Definition: box_dimensioner_multicam_demo.py:27
measurement_task.visualise_measurements
def visualise_measurements(frames_devices, bounding_box_points_devices, length, width, height)
Definition: measurement_task.py:148
helper_functions.get_boundary_corners_2D
def get_boundary_corners_2D(points)
Definition: helper_functions.py:225
realsense_device_manager.DeviceManager
Definition: realsense_device_manager.py:119
calibration_kabsch.PoseEstimation
Definition: calibration_kabsch.py:121
measurement_task.calculate_cumulative_pointcloud
def calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, roi_2d, depth_threshold=0.01)
Definition: measurement_task.py:14
realdds::print
std::string print(dds_guid const &guid, dds_guid_prefix const &base_prefix, bool readable_name)
Definition: dds-guid.cpp:75
assert
#define assert(condition)
Definition: lz4.c:245
librealsense::inverse
pose inverse(const pose &a)
Definition: src/pose.h:27
measurement_task.calculate_boundingbox_points
def calculate_boundingbox_points(point_cloud, calibration_info_devices, depth_threshold=0.01)
Definition: measurement_task.py:71


librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Fri Aug 2 2024 08:30:01