def run_demo(): # Define some constants resolution_width = 1280 # pixels resolution_height = 720 # pixels frame_rate = 15 # fps dispose_frames_for_stablisation = 30 # frames # chessboard_width = 6 # squares # chessboard_height = 9 # squares # square_size = 0.0253 # meters try: # Enable the streams from all the intel realsense devices 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.enable_all_devices() # Allow some frames for the auto-exposure controller to stablise for frame in range(dispose_frames_for_stablisation): frames = device_manager.poll_frames() assert( len(device_manager._available_devices) > 0 ) """ 1: Calibration Calibrate all the available devices to the world co-ordinates. For this purpose, a chessboard printout for use with opencv based calibration process is needed. """ # Get the intrinsics of the realsense device intrinsics_devices = device_manager.get_device_intrinsics(frames) # Set the chessboard parameters for calibration # chessboard_params = [chessboard_height, chessboard_width, square_size] # Estimate the pose of the chessboard in the world coordinate using the Kabsch Method # calibrated_device_count = 0 # while calibrated_device_count < len(device_manager._available_devices): # frames = device_manager.poll_frames() # pose_estimator = PoseEstimation(frames, intrinsics_devices, chessboard_params) # 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: # if not transformation_result_kabsch[device][0]: # print("Place the chessboard on the plane where the object needs to be detected..") # else: # calibrated_device_count += 1 # 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: # 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) # chessboard_points_cumulative_3d = np.column_stack( (chessboard_points_cumulative_3d,points3D) ) # # Extract the bounds between which the object's dimensions are needed # # It is necessary for this demo that the object's length and breath is smaller than that of the chessboard # chessboard_points_cumulative_3d = np.delete(chessboard_points_cumulative_3d, 0, 1) # roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d) print("Calibration completed... \nPlace the box in the field of view of the devices...") """ 2: Measurement and display Measure the dimension of the object using depth maps from multiple RealSense devices The information from Phase 1 will be used here """ # Enable the emitter of the devices device_manager.enable_emitter(True) # Load the JSON settings file in order to enable High Accuracy preset for the realsense device_manager.load_settings_json("./HighResHighAccuracyPreset.json") # Get the extrinsics of the device to be used later extrinsics_devices = device_manager.get_depth_to_color_extrinsics(frames) # Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image calibration_info_devices = defaultdict(list) for calibration_info in (transformation_devices, intrinsics_devices, extrinsics_devices): for key, value in calibration_info.items(): calibration_info_devices[key].append(value) # Continue acquisition until terminated with Ctrl+C by the user while 1: # Get the frames from all the devices frames_devices = device_manager.poll_frames() # Calculate the pointcloud using the depth frames from all the devices point_cloud = calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, roi_2D) # Get the bounding box for the pointcloud in image coordinates of the color imager bounding_box_points_color_image, length, width, height = calculate_boundingbox_points(point_cloud, calibration_info_devices ) # Draw the bounding box points on the color image and visualise the results visualise_measurements(frames_devices, bounding_box_points_color_image, length, width, height) except KeyboardInterrupt: print("The program was interupted by the user. Closing the program...") finally: device_manager.disable_streams() cv2.destroyAllWindows()
def run_demo(): # Define some constants resolution_width = 1280 # pixels resolution_height = 720 # pixels frame_rate = 15 # fps dispose_frames_for_stablisation = 30 # frames chessboard_width = 6 # squares chessboard_height = 9 # squares square_size = 0.0253 # meters try: # Enable the streams from all the intel realsense devices 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.enable_all_devices() # Allow some frames for the auto-exposure controller to stablise for frame in range(dispose_frames_for_stablisation): frames = device_manager.poll_frames() assert( len(device_manager._available_devices) > 0 ) """ 1: Calibration Calibrate all the available devices to the world co-ordinates. For this purpose, a chessboard printout for use with opencv based calibration process is needed. """ # Get the intrinsics of the realsense device intrinsics_devices = device_manager.get_device_intrinsics(frames) # Set the chessboard parameters for calibration chessboard_params = [chessboard_height, chessboard_width, square_size] # Estimate the pose of the chessboard in the world coordinate using the Kabsch Method calibrated_device_count = 0 while calibrated_device_count < len(device_manager._available_devices): frames = device_manager.poll_frames() pose_estimator = PoseEstimation(frames, intrinsics_devices, chessboard_params) 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: if not transformation_result_kabsch[device][0]: print("Place the chessboard on the plane where the object needs to be detected..") else: calibrated_device_count += 1 # 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: 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) chessboard_points_cumulative_3d = np.column_stack( (chessboard_points_cumulative_3d,points3D) ) # Extract the bounds between which the object's dimensions are needed # It is necessary for this demo that the object's length and breath is smaller than that of the chessboard chessboard_points_cumulative_3d = np.delete(chessboard_points_cumulative_3d, 0, 1) roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d) print("Calibration completed... \nPlace the box in the field of view of the devices...") """ 2: Measurement and display Measure the dimension of the object using depth maps from multiple RealSense devices The information from Phase 1 will be used here """ # Enable the emitter of the devices device_manager.enable_emitter(True) # Load the JSON settings file in order to enable High Accuracy preset for the realsense device_manager.load_settings_json("./HighResHighAccuracyPreset.json") # Get the extrinsics of the device to be used later extrinsics_devices = device_manager.get_depth_to_color_extrinsics(frames) # Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image calibration_info_devices = defaultdict(list) for calibration_info in (transformation_devices, intrinsics_devices, extrinsics_devices): for key, value in calibration_info.items(): calibration_info_devices[key].append(value) # Continue acquisition until terminated with Ctrl+C by the user while 1: # Get the frames from all the devices frames_devices = device_manager.poll_frames() # Calculate the pointcloud using the depth frames from all the devices point_cloud = calculate_cumulative_pointcloud(frames_devices, calibration_info_devices, roi_2D) # Get the bounding box for the pointcloud in image coordinates of the color imager bounding_box_points_color_image, length, width, height = calculate_boundingbox_points(point_cloud, calibration_info_devices ) # Draw the bounding box points on the color image and visualise the results visualise_measurements(frames_devices, bounding_box_points_color_image, length, width, height) except KeyboardInterrupt: print("The program was interupted by the user. Closing the program...") finally: device_manager.disable_streams() cv2.destroyAllWindows()
def run_demo(): # Define some constants resolution_width = 640 # pixels resolution_height = 480 # pixels frame_rate = 15 # fps dispose_frames_for_stablisation = 30 # frames chessboard_width = 6 # squares chessboard_height = 9 # squares square_size = 0.0253 # meters align_to = rs.stream.color align = rs.align(align_to) try: # Enable the streams from all the intel realsense devices 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.enable_all_devices() # Allow some frames for the auto-exposure controller to stablise for frame in range(dispose_frames_for_stablisation): frames,maps = device_manager.poll_frames(align) assert( len(device_manager._available_devices) > 0 ) """ 1. Calibrate cameras and return transformation matrix (rotation matrix + translation vectors) """ chessboard_params = [chessboard_height, chessboard_width, square_size] cameras = calibrateCameras(align,device_manager,frames,chessboard_params) """ 2. Run OpenPose on each view frame """ # Enable the emitter of the devices device_manager.enable_emitter(True) # Load the JSON settings file in order to enable High Accuracy preset for the realsense device_manager.load_settings_json("./HighResHighAccuracyPreset.json") # Get the extrinsics of the device to be used later extrinsics_devices = device_manager.get_depth_to_color_extrinsics(frames) # Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image # calibration_info_devices = defaultdict(list) # for calibration_info in (transformation_devices, intrinsics_devices, extrinsics_devices): # for key, value in calibration_info.items(): # calibration_info_devices[key].append(value) depth_list = {} color_list = {} frame_id = 0 points = {} # Continue acquisition until terminated with Ctrl+C by the user switch = True while 1: # Get the frames from all the devices if switch: frames_devices, maps = device_manager.poll_frames(align,500) # print(frames_devices) # List collector for display depth_color= [] color = [] devices = [i for i in maps] devices.sort() project_depth = [] for i in devices: # 1. Get depth map and colorize temp = maps[i]['depth'] depth_list.setdefault(i,[]) depth_list[i].append(np.array(temp)) depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(temp, alpha=0.03), cv2.COLORMAP_JET) depth_color.append(depth_colormap) project_depth.append((cameras[i]['matrix'],depth_colormap)) # 2. Run OpenPose detector on image if FLAGS_USE_BBOX: box = bbox[i] else: box = None joints,img = predict_keypoints(maps[i]['color'],box) # 3. Save annotated color image for display color.append(img) color_list.setdefault(i,[]) color_list[i].append(img) # 4. Save keypoints for that camera viewpoint cameras[i][frame_id] = joints # 5. Save images to folder if FLAGS_SAVE_IMGS: cv2.imwrite('./images/depth_{}_{}.png'.format(i,frame_id),temp) cv2.imwrite('./images/color_{}_{}.png'.format(i,frame_id),img) #Triangulate 3d keypoints points[frame_id] = find3dpoints_rt(cameras,0.2,frame_id) if points[frame_id] != 'Invalid Frame': depth_projected = [] for img in project_depth: points2d = project_2d(img[0],points[frame_id]) img_draw = draw_pose(points2d,'openpose',img[1],True,points[frame_id]) depth_projected.append(img_draw) # print(img_draw.shape) depth_color = depth_projected # proj_img = show_img(cameras,devices[0],frame_id,points) frame_id += 1 images = np.vstack((np.hstack(color),np.hstack(depth_color))) # images = proj_img # Show images for debugging cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE) cv2.imshow('RealSense', images) key = cv2.waitKey(1) if key == 32: switch = not(switch) else: continue # Press esc or 'q' to close the image window if key & 0xFF == ord('q') or key == 27: cv2.destroyAllWindows() break except KeyboardInterrupt: print("The program was interupted by the user. Closing the program...") finally: device_manager.disable_streams() cv2.destroyAllWindows() if FLAGS_SAVE_MATRIX: cam_pkl = open('cameras.pkl','wb') pkl.dump(cameras,cam_pkl) points_pkl = open('3dpoints.pkl','wb') pkl.dump(points,points_pkl)
class RealSense: def __init__(self): self.WIN_NAME = 'RealSense' self.pitch, self.yaw = math.radians(-10), math.radians(-15) self.translation = np.array([0, 0, -1], dtype=np.float32) self.distance = 2 self.paused = False self.decimate = 1 self.scale = True self.color = True # Define some constants self.resolution_width = 1280 # pixels self.resolution_height = 720 # pixels self.frame_rate = 15 # fps self.dispose_frames_for_stablisation = 25 # frames self.chessboard_width = 6 # squares self.chessboard_height = 9 # squares self.square_size = 0.0253 # meters # Allow some frames for the auto-exposure controller to stablise self.intrinsics_devices = None self.chessboard_params = None self.rs_config = rs.config() self.rs_config.enable_stream(rs.stream.depth, self.resolution_width, self.resolution_height, rs.format.z16, self.frame_rate) self.rs_config.enable_stream(rs.stream.infrared, 1, self.resolution_width, self.resolution_height, rs.format.y8, self.frame_rate) self.rs_config.enable_stream(rs.stream.color, self.resolution_width, self.resolution_height, rs.format.bgr8, self.frame_rate) # Use the device manager class to enable the devices and get the frames self.device_manager = DeviceManager(rs.context(), self.rs_config) self.device_manager.enable_all_devices() print('0') for self.frame in range(self.dispose_frames_for_stablisation): self.frames = self.device_manager.poll_frames() #print('framm = ',self.frame) # assert( len(self.device_manager._available_devices) > 0 ) print('realsense initialized!!') def calibaration(self): self.intrinsics_devices = self.device_manager.get_device_intrinsics( self.frames) #print(' Set the chessboard parameters for calibration ') self.chessboard_params = [ self.chessboard_height, self.chessboard_width, self.square_size ] # Estimate the pose of the chessboard in the world coordinate using the Kabsch Method calibrated_device_count = 0 while calibrated_device_count < len( self.device_manager._available_devices): self.frames = self.device_manager.poll_frames() pose_estimator = PoseEstimation(self.frames, self.intrinsics_devices, self.chessboard_params) transformation_result_kabsch = pose_estimator.perform_pose_estimation( ) object_point = pose_estimator.get_chessboard_corners_in3d() calibrated_device_count = 0 for device in self.device_manager._available_devices: if not transformation_result_kabsch[device][0]: print( "Place the chessboard on the plane where the object needs to be detected.." ) else: calibrated_device_count += 1 print('calibrated_device_count =', calibrated_device_count) # Save the transformation object for all devices in an array to use for measurements self.transformation_devices = {} chessboard_points_cumulative_3d = np.array([-1, -1, -1]).transpose() for device in self.device_manager._available_devices: self.transformation_devices[device] = transformation_result_kabsch[ device][1].inverse() points3D = object_point[device][2][:, object_point[device][3]] points3D = self.transformation_devices[ device].apply_transformation(points3D) chessboard_points_cumulative_3d = np.column_stack( (chessboard_points_cumulative_3d, points3D)) print( ' Extract the bounds between which the objects dimensions are needed' ) # It is necessary for this demo that the object's length and breath is smaller than that of the chessboard chessboard_points_cumulative_3d = np.delete( chessboard_points_cumulative_3d, 0, 1) self.roi_2D = get_boundary_corners_2D(chessboard_points_cumulative_3d) print( "Calibration completed... \nPlace the box in the field of view of the devices..." ) def setEmitter(self): # print('Enable the emitter of the devices') self.device_manager.enable_emitter(True) # print('-------Enable the emitter of the devices') # Load the JSON settings file in order to enable High Accuracy preset for the realsense #self.device_manager.load_settings_json("HighResHighAccuracyPreset.json") #print(' Get the extrinsics of the device to be used later') extrinsics_devices = self.device_manager.get_depth_to_color_extrinsics( self.frames) print( ' Get the calibration info as a dictionary to help with display of the measurements onto the color image instead of infra red image' ) self.calibration_info_devices = defaultdict(list) for calibration_info in (self.transformation_devices, self.intrinsics_devices, extrinsics_devices): for key, value in calibration_info.items(): self.calibration_info_devices[key].append(value) print("CalibrasetEmittertion completed... ") def processing(self): frames_devices = self.device_manager.poll_frames() print('get frames') # Calculate the pointcloud using the depth frames from all the devices point_cloud = calculate_cumulative_pointcloud( frames_devices, self.calibration_info_devices, self.roi_2D) # Get the bounding box for the pointcloud in image coordinates of the color imager bounding_box_points_color_image, length, width, height = calculate_boundingbox_points( point_cloud, self.calibration_info_devices) print('길이=', length, ' 폭=', width, ' 높이', height) # Draw the bounding box points on the color image and visualise the results # visualise_measurements(frames_devices, bounding_box_points_color_image, length, width, height) def closeSense(self): self.device_manager.disable_streams()