7 import pyrealsense2
as rs
10 from realsense_device_manager
import post_process_depth_frame
11 from helper_functions
import convert_depth_frame_to_pointcloud, get_clipped_pointcloud
16 Calculate the cumulative pointcloud from the multiple devices 20 The frames from the different devices 22 Serial number of the device 25 The frameset obtained over the active pipeline from the realsense device 27 calibration_info_devices : dict 29 Serial number of the device 30 values: [transformation_devices, intrinsics_devices] 31 transformation_devices: Transformation object 32 The transformation object containing the transformation information between the device and the world coordinate systems 33 intrinsics_devices: rs.intrinscs 34 The intrinsics of the depth_frame of the realsense device 37 The region of interest given in the following order [minX, maxX, minY, maxY] 39 depth_threshold : double 40 The threshold for the depth value (meters) in world-coordinates beyond which the point cloud information will not be used. 41 Following the right-hand coordinate system, if the object is placed on the chessboard plane, the height of the object will increase along the negative Z-axis 45 point_cloud_cumulative : array 46 The cumulative pointcloud from the multiple devices 49 point_cloud_cumulative = np.array([-1, -1, -1]).transpose()
50 for (device, frame)
in frames_devices.items() :
52 filtered_depth_frame =
post_process_depth_frame(frame[rs.stream.depth], temporal_smooth_alpha=0.1, temporal_smooth_delta=80)
54 point_cloud = np.asanyarray(point_cloud)
57 point_cloud = calibration_info_devices[device][0].apply_transformation(point_cloud)
62 point_cloud = point_cloud[:,point_cloud[2,:]<-depth_threshold]
63 point_cloud_cumulative = np.column_stack( ( point_cloud_cumulative, point_cloud ) )
64 point_cloud_cumulative = np.delete(point_cloud_cumulative, 0, 1)
65 return point_cloud_cumulative
72 Calculate the top and bottom bounding box corner points for the point cloud in the image coordinates of the color imager of the realsense device 77 The (3 x N) array containing the pointcloud information 79 calibration_info_devices : dict 81 Serial number of the device 82 values: [transformation_devices, intrinsics_devices, extrinsics_devices] 83 transformation_devices: Transformation object 84 The transformation object containing the transformation information between the device and the world coordinate systems 85 intrinsics_devices: rs.intrinscs 86 The intrinsics of the depth_frame of the realsense device 87 extrinsics_devices: rs.extrinsics 88 The extrinsics between the depth imager 1 and the color imager of the realsense device 90 depth_threshold : double 91 The threshold for the depth value (meters) in world-coordinates beyond which the point cloud information will not be used 92 Following the right-hand coordinate system, if the object is placed on the chessboard plane, the height of the object will increase along the negative Z-axis 96 bounding_box_points_color_image : dict 97 The bounding box corner points in the image coordinate system for the color imager 99 Serial number of the device 102 The (8x2) list of the upper corner points stacked above the lower corner points 105 The length of the bounding box calculated in the world coordinates of the pointcloud 108 The width of the bounding box calculated in the world coordinates of the pointcloud 111 The height of the bounding box calculated in the world coordinates of the pointcloud 115 if point_cloud.shape[1] > 500:
117 coord = np.c_[point_cloud[0,:], point_cloud[1,:]].astype(
'float32')
118 min_area_rectangle = cv2.minAreaRect(coord)
119 bounding_box_world_2d = cv2.boxPoints(min_area_rectangle)
121 height = max(point_cloud[2,:]) -
min(point_cloud[2,:]) + depth_threshold
124 height_array = np.array([[-height], [-height], [-height], [-height], [0], [0], [0], [0]])
125 bounding_box_world_3d = np.column_stack((np.row_stack((bounding_box_world_2d,bounding_box_world_2d)), height_array))
128 bounding_box_points_color_image={}
129 for (device, calibration_info)
in calibration_info_devices.items():
131 bounding_box_device_3d = calibration_info[0].
inverse().apply_transformation(bounding_box_world_3d.transpose())
135 bounding_box_device_3d = bounding_box_device_3d.transpose().tolist()
136 for bounding_box_point
in bounding_box_device_3d:
137 bounding_box_color_image_point = rs.rs2_transform_point_to_point(calibration_info[2], bounding_box_point)
138 color_pixel.append(rs.rs2_project_point_to_pixel(calibration_info[1][rs.stream.color], bounding_box_color_image_point))
140 bounding_box_points_color_image[device] = np.row_stack( color_pixel )
141 return bounding_box_points_color_image, min_area_rectangle[1][0], min_area_rectangle[1][1], height
149 Calculate the cumulative pointcloud from the multiple devices 153 frames_devices : dict 154 The frames from the different devices 156 Serial number of the device 159 The frameset obtained over the active pipeline from the realsense device 161 bounding_box_points_color_image : dict 162 The bounding box corner points in the image coordinate system for the color imager 164 Serial number of the device 167 The (8x2) list of the upper corner points stacked above the lower corner points 170 The length of the bounding box calculated in the world coordinates of the pointcloud 173 The width of the bounding box calculated in the world coordinates of the pointcloud 176 The height of the bounding box calculated in the world coordinates of the pointcloud 178 for (device, frame)
in frames_devices.items():
179 color_image = np.asarray(frame[rs.stream.color].get_data())
180 if (length != 0
and width !=0
and height != 0):
181 bounding_box_points_device_upper = bounding_box_points_devices[device][0:4,:]
182 bounding_box_points_device_lower = bounding_box_points_devices[device][4:8,:]
183 box_info =
"Length, Width, Height (mm): " +
str(int(length*1000)) +
", " +
str(int(width*1000)) +
", " +
str(int(height*1000))
186 bounding_box_points_device_upper = tuple(
map(tuple,bounding_box_points_device_upper.astype(int)))
187 for i
in range(len(bounding_box_points_device_upper)):
188 cv2.line(color_image, bounding_box_points_device_upper[i], bounding_box_points_device_upper[(i+1)%4], (0,255,0), 4)
190 bounding_box_points_device_lower = tuple(
map(tuple,bounding_box_points_device_lower.astype(int)))
191 for i
in range(len(bounding_box_points_device_upper)):
192 cv2.line(color_image, bounding_box_points_device_lower[i], bounding_box_points_device_lower[(i+1)%4], (0,255,0), 1)
194 cv2.line(color_image, bounding_box_points_device_upper[0], bounding_box_points_device_lower[0], (0,255,0), 1)
195 cv2.line(color_image, bounding_box_points_device_upper[1], bounding_box_points_device_lower[1], (0,255,0), 1)
196 cv2.line(color_image, bounding_box_points_device_upper[2], bounding_box_points_device_lower[2], (0,255,0), 1)
197 cv2.line(color_image, bounding_box_points_device_upper[3], bounding_box_points_device_lower[3], (0,255,0), 1)
198 cv2.putText(color_image, box_info, (50,50), cv2.FONT_HERSHEY_PLAIN, 2, (0,255,0) )
201 cv2.imshow(
'Color image from RealSense Device Nr: ' + device, color_image)
def calculate_boundingbox_points(point_cloud, calibration_info_devices, depth_threshold=0.01)
def get_clipped_pointcloud(pointcloud, boundary)
def calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, roi_2d, depth_threshold=0.01)
def convert_depth_frame_to_pointcloud(depth_image, camera_intrinsics)
def visualise_measurements(frames_devices, bounding_box_points_devices, length, width, height)
GeneratorWrapper< T > map(Func &&function, GeneratorWrapper< U > &&generator)
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)