16 import pyrealsense2
as rs
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
30 resolution_width = 1280
31 resolution_height = 720
34 dispose_frames_for_stablisation = 30
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)
49 device_manager.enable_all_devices()
52 for frame
in range(dispose_frames_for_stablisation):
53 frames = device_manager.poll_frames()
55 assert( len(device_manager._available_devices) > 0 )
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.
63 intrinsics_devices = device_manager.get_device_intrinsics(frames)
66 chessboard_params = [chessboard_height, chessboard_width, square_size]
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..")
81 calibrated_device_count += 1
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) )
95 chessboard_points_cumulative_3d = np.delete(chessboard_points_cumulative_3d, 0, 1)
98 print(
"Calibration completed... \nPlace the box in the field of view of the devices...")
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
109 device_manager.enable_emitter(
True)
112 device_manager.load_settings_json(
"./HighResHighAccuracyPreset.json")
115 extrinsics_devices = device_manager.get_depth_to_color_extrinsics(frames)
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)
126 frames_devices = device_manager.poll_frames()
137 except KeyboardInterrupt:
138 print(
"The program was interupted by the user. Closing the program...")
141 device_manager.disable_streams()
142 cv2.destroyAllWindows()
145 if __name__ ==
"__main__":