box_dimensioner_multicam_demo.py
Go to the documentation of this file.
1 ###########################################################################################################################
2 ## License: Apache 2.0. See LICENSE file in root directory. ##
3 ###########################################################################################################################
4 ## Simple Box Dimensioner with multiple cameras: Main demo file ##
5 ###########################################################################################################################
6 ## Workflow description: ##
7 ## 1. Place the calibration chessboard object into the field of view of all the realsense cameras. ##
8 ## Update the chessboard parameters in the script in case a different size is chosen. ##
9 ## 2. Start the program. ##
10 ## 3. Allow calibration to occur and place the desired object ON the calibration object when the program asks for it. ##
11 ## Make sure that the object to be measured is not bigger than the calibration object in length and width. ##
12 ## 4. The length, width and height of the bounding box of the object is then displayed in millimeters. ##
13 ###########################################################################################################################
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  dispose_frames_for_stablisation = 30 # frames
34 
35  chessboard_width = 6 # squares
36  chessboard_height = 9 # squares
37  square_size = 0.0253 # meters
38 
39  try:
40  # Enable the streams from all the intel realsense devices
41  rs_config = rs.config()
42  rs_config.enable_stream(rs.stream.depth, resolution_width, resolution_height, rs.format.z16, frame_rate)
43  rs_config.enable_stream(rs.stream.infrared, 1, resolution_width, resolution_height, rs.format.y8, frame_rate)
44  rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate)
45 
46  # Use the device manager class to enable the devices and get the frames
47  device_manager = DeviceManager(rs.context(), rs_config)
48  device_manager.enable_all_devices()
49 
50  # Allow some frames for the auto-exposure controller to stablise
51  for frame in range(dispose_frames_for_stablisation):
52  frames = device_manager.poll_frames()
53 
54  assert( len(device_manager._available_devices) > 0 )
55  """
56  1: Calibration
57  Calibrate all the available devices to the world co-ordinates.
58  For this purpose, a chessboard printout for use with opencv based calibration process is needed.
59 
60  """
61  # Get the intrinsics of the realsense device
62  intrinsics_devices = device_manager.get_device_intrinsics(frames)
63 
64  # Set the chessboard parameters for calibration
65  chessboard_params = [chessboard_height, chessboard_width, square_size]
66 
67  # Estimate the pose of the chessboard in the world coordinate using the Kabsch Method
68  calibrated_device_count = 0
69  while calibrated_device_count < len(device_manager._available_devices):
70  frames = device_manager.poll_frames()
71  pose_estimator = PoseEstimation(frames, intrinsics_devices, chessboard_params)
72  transformation_result_kabsch = pose_estimator.perform_pose_estimation()
73  object_point = pose_estimator.get_chessboard_corners_in3d()
74  calibrated_device_count = 0
75  for device in device_manager._available_devices:
76  if not transformation_result_kabsch[device][0]:
77  print("Place the chessboard on the plane where the object needs to be detected..")
78  else:
79  calibrated_device_count += 1
80 
81  # Save the transformation object for all devices in an array to use for measurements
82  transformation_devices={}
83  chessboard_points_cumulative_3d = np.array([-1,-1,-1]).transpose()
84  for device in device_manager._available_devices:
85  transformation_devices[device] = transformation_result_kabsch[device][1].inverse()
86  points3D = object_point[device][2][:,object_point[device][3]]
87  points3D = transformation_devices[device].apply_transformation(points3D)
88  chessboard_points_cumulative_3d = np.column_stack( (chessboard_points_cumulative_3d,points3D) )
89 
90  # Extract the bounds between which the object's dimensions are needed
91  # It is necessary for this demo that the object's length and breath is smaller than that of the chessboard
92  chessboard_points_cumulative_3d = np.delete(chessboard_points_cumulative_3d, 0, 1)
93  roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d)
94 
95  print("Calibration completed... \nPlace the box in the field of view of the devices...")
96 
97 
98  """
99  2: Measurement and display
100  Measure the dimension of the object using depth maps from multiple RealSense devices
101  The information from Phase 1 will be used here
102 
103  """
104 
105  # Enable the emitter of the devices
106  device_manager.enable_emitter(True)
107 
108  # Load the JSON settings file in order to enable High Accuracy preset for the realsense
109  device_manager.load_settings_json("./HighResHighAccuracyPreset.json")
110 
111  # Get the extrinsics of the device to be used later
112  extrinsics_devices = device_manager.get_depth_to_color_extrinsics(frames)
113 
114  # Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image
115  calibration_info_devices = defaultdict(list)
116  for calibration_info in (transformation_devices, intrinsics_devices, extrinsics_devices):
117  for key, value in calibration_info.items():
118  calibration_info_devices[key].append(value)
119 
120  # Continue acquisition until terminated with Ctrl+C by the user
121  while 1:
122  # Get the frames from all the devices
123  frames_devices = device_manager.poll_frames()
124 
125  # Calculate the pointcloud using the depth frames from all the devices
126  point_cloud = calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, roi_2D)
127 
128  # Get the bounding box for the pointcloud in image coordinates of the color imager
129  bounding_box_points_color_image, length, width, height = calculate_boundingbox_points(point_cloud, calibration_info_devices )
130 
131  # Draw the bounding box points on the color image and visualise the results
132  visualise_measurements(frames_devices, bounding_box_points_color_image, length, width, height)
133 
134  except KeyboardInterrupt:
135  print("The program was interupted by the user. Closing the program...")
136 
137  finally:
138  device_manager.disable_streams()
139  cv2.destroyAllWindows()
140 
141 
142 if __name__ == "__main__":
143  run_demo()
realsense_device_manager.DeviceManager
Definition: realsense_device_manager.py:115
box_dimensioner_multicam_demo.run_demo
def run_demo()
Definition: box_dimensioner_multicam_demo.py:27
calibration_kabsch.PoseEstimation
Definition: calibration_kabsch.py:121
librealsense::algo::depth_to_rgb_calibration::inverse
Definition: k-to-dsm.h:22
measurement_task.calculate_boundingbox_points
def calculate_boundingbox_points(point_cloud, calibration_info_devices, depth_threshold=0.01)
Definition: measurement_task.py:70
print
static std::string print(const transformation &tf)
Definition: rs-pose-apriltag.cpp:64
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
measurement_task.visualise_measurements
def visualise_measurements(frames_devices, bounding_box_points_devices, length, width, height)
Definition: measurement_task.py:147
helper_functions.get_boundary_corners_2D
def get_boundary_corners_2D(points)
Definition: helper_functions.py:225

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