def get_boundary_in_color(roi_2D, Trans, extrinsics_device, color_internal): roi_color = np.zeros((3, 4)) #roi_2D[x-,x+,y-,y+] roi_color[0, :2] = roi_2D[0] roi_color[0, 2:4] = roi_2D[1] roi_color[1, [0, 3]] = roi_2D[2] roi_color[1, [1, 2]] = roi_2D[3] roi_color[2, :] = 0 transform = Transformation(Trans[4], Trans[5]) roi_color = transform.apply_transformation(roi_color) roi_color = roi_color.transpose().tolist() color_pixel = [] for point in roi_color: cloud_point = rs.rs2_transform_point_to_point(extrinsics_device, point) color_pixel.append( rs.rs2_project_point_to_pixel(color_internal, cloud_point)) color_boundary_point = np.row_stack(color_pixel).T print(np.shape(color_boundary_point)) # transform = Transformation(Trans[2],Trans[3].flatten()) # roi_point = transform.apply_transformation(roi_color) # color_boundary_point=get_point_3D_2D(roi_point,Trans[0]) # print(np.shape(color_boundary_point)) return color_boundary_point
def is_disturbance(plane, contour, depth_frame, depth_intrin, depth_to_color_extrin): distance_sum = 0 i = 0 print('contour', len(contour)) for each in contour: if depth_frame.get_distance(int(each[1]), int(each[0])) != 0: depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, [each[1], each[0]], depth_frame.get_distance(int(each[1]), int(each[0]))) color_point = rs.rs2_transform_point_to_point( depth_to_color_extrin, depth_point) if math.sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]) != 0: distance = abs(color_point[0] * plane[0] + color_point[1] * plane[1] + color_point[2] * plane[2] + plane[3]) / math.sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]) else: distance = 0 continue distance_sum += distance i += 1 if i != 0: average_distance = distance_sum / i print(average_distance) else: print("the points' distance to camera are all 0 ") average_distance = 1 if average_distance < 0.01: return True else: return False
def get_object_points(color_frame, depth_frame): """Short summary. Parameters ---------- color_frame : type Description of parameter `color_frame`. depth_frame : type Description of parameter `depth_frame`. Returns ------- type Description of returned object. """ pc = rs.pointcloud() pc.map_to(color_frame) pointcloud = pc.calculate(depth_frame) v = pointcloud.get_vertices() verts = np.asanyarray(v).view(np.float32).reshape(-1, 3) cloud = pcl.PointCloud(verts) filtered_cloud = do_passthrough_filter(point_cloud=cloud, name_axis='z', min_axis=0.0, max_axis=0.6) downsampled_cloud = do_voxel_grid_filter(point_cloud=filtered_cloud, LEAF_SIZE=0.004) table_cloud, objects_cloud = do_ransac_plane_segmentation( downsampled_cloud, max_distance=0.01) # project_inliers = objects_cloud.make_ProjectInliers() # project_inliers.set_model_type(pcl.SACMODEL_NORMAL_PARALLEL_PLANE) # plane = project_inliers.filter() colorless_cloud = XYZRGB_to_XYZ(objects_cloud) clusters = get_clusters(objects_cloud, tolerance=0.02, min_size=100, max_size=25000) colored_points = get_cloud_clusters(clusters, colorless_cloud) # Create a cloud with each cluster of points having the same color clusters_cloud = pcl.PointCloud() clusters_cloud.from_list(colored_points) color_intrin = color_frame.profile.as_video_stream_profile().intrinsics depth_to_color_extrin = depth_frame.profile.get_extrinsics_to( color_frame.profile) Pixel_Coord = [] for data in clusters_cloud: if np.round(data[2], 2) > 0.1: color_point = rs.rs2_transform_point_to_point( depth_to_color_extrin, list(data)) Pixel_Coord.append( rs.rs2_project_point_to_pixel(color_intrin, color_point)) # pcl.save(downsampled_cloud, "12.ply") # if clusters_cloud.size > 0: # pcl.save(clusters_cloud, "13.ply") return Pixel_Coord
def project_color_pix_to_depth(color_intrin, depth_intrin, color_to_depth_extrin, depth_scale, color_pix): color_point = rs.rs2_deproject_pixel_to_point(color_intrin, color_pix, 1.0 / depth_scale) depth_point = rs.rs2_transform_point_to_point(color_to_depth_extrin, color_point) depth_pix = rs.rs2_project_point_to_pixel(depth_intrin, depth_point) depth_pix = [int(x) for x in depth_pix] return depth_pix
def point_transform(point, depth_frame, rois, depth_intrin, depth_to_color_extrin): left_point = [0, 0] for i in range(5, 100): left_point = [point[0] + i, point[1] - i] if not point_in_mask(left_point, rois) and depth_frame.get_distance( int(left_point[0]), int(left_point[1])) != 0: break if left_point != [0, 0]: depth_point1 = rs.rs2_deproject_pixel_to_point( depth_intrin, left_point, depth_frame.get_distance(int(left_point[0]), int(left_point[1]))) color_point1 = rs.rs2_transform_point_to_point(depth_to_color_extrin, depth_point1) else: color_point1 = [0, 0, 0] print('point_transform fail') return color_point1
def calculate_boundingbox_points(pipeline, pipeline_profile, point_cloud, Trans, extrinsics_device, color_internal): dimensions = [] color_images = [] depth_value0 = point_cloud[2].reshape(720, 1280) if len(depth_value0 > 0.001) > 500: depth_value1 = np.int0( np.where(depth_value0 < 0.005, 0, depth_value0) * 1000) mask = np.where(depth_value1 > 0, 255, depth_value1).astype(np.uint8) contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) contours_0 = [] # 寻找满足的轮 for j in range(len(contours)): cnt = contours[j] area = cv2.contourArea(cnt) length = cv2.arcLength(cnt, True) if area > 100 and length > 200: contours_0.append(contours[j]) #cv2.drawContours(mask,contours_0,-1,[255],3) for i in range(len(contours_0)): index = [] for j in range(len(contours_0[i])): x = contours_0[i][j][0][0] y = contours_0[i][j][0][1] index.append(x + y * 1280) hull = cv2.convexHull(contours_0[i]) angle_0 = cv2.minAreaRect(hull)[2] point_cloud0 = point_cloud[:, index] if point_cloud0.shape[1] > 100: coord = np.c_[point_cloud0[0, :], point_cloud0[1, :]].astype('float32') min_area_rectangle = cv2.minAreaRect(coord) angle_1 = min_area_rectangle[2] bounding_box_world_2d = cv2.boxPoints(min_area_rectangle) # print("debug111") # print(bounding_box_world_2d) height = max(point_cloud0[ 2, :]) # - min(point_cloud[2, :]) + depth_threshold height_array = np.array([[-height], [-height], [-height], [-height], [0], [0], [0], [0]]) bounding_box_world_3d = np.column_stack((np.row_stack( (bounding_box_world_2d, bounding_box_world_2d)), height_array)) trans = Transformation(Trans[4], Trans[5]) bounding_box_device_3d = trans.apply_transformation( bounding_box_world_3d.transpose()) # # print("debug222") # print(bounding_box_device_3d) # trans = Transformation(Trans[2],Trans[3]) # bounding_box_color_image_point=trans.apply_transformation(bounding_box_device_3d) # print("debug333") # print(bounding_box_color_image_point) # # # color_pixel=get_point_3D_2D(bounding_box_color_image_point, Trans[0]).T # color_images.append(color_pixel) # print("debug444") # print(color_pixel) # bounding_box_device_3d = bounding_box_device_3d.transpose( ).tolist() color_pixel = [] for bounding_box_point in bounding_box_device_3d: bounding_box_color_image_point = rs.rs2_transform_point_to_point( extrinsics_device, bounding_box_point) color_pixel.append( rs.rs2_project_point_to_pixel( color_internal, bounding_box_color_image_point)) color_image = np.row_stack(color_pixel) color_images.append(color_image) dimensions.append([ min_area_rectangle[1][0], min_area_rectangle[1][1], height, angle_0 ]) # # cv2.imshow("depth11", mask) # cv2.waitKey(1) return color_images, dimensions else: return color_images, dimensions
def calculate_boundingbox_points(point_cloud, calibration_info_devices, depth_threshold = 0.01): """ 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 Parameters: ----------- point_cloud : ndarray The (3 x N) array containing the pointcloud information calibration_info_devices : dict keys: str Serial number of the device values: [transformation_devices, intrinsics_devices, extrinsics_devices] transformation_devices: Transformation object The transformation object containing the transformation information between the device and the world coordinate systems intrinsics_devices: rs.intrinscs The intrinsics of the depth_frame of the realsense device extrinsics_devices: rs.extrinsics The extrinsics between the depth imager 1 and the color imager of the realsense device depth_threshold : double The threshold for the depth value (meters) in world-coordinates beyond which the point cloud information will not be used 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 Return: ---------- bounding_box_points_color_image : dict The bounding box corner points in the image coordinate system for the color imager keys: str Serial number of the device values: [points] points: list The (8x2) list of the upper corner points stacked above the lower corner points length : double The length of the bounding box calculated in the world coordinates of the pointcloud width : double The width of the bounding box calculated in the world coordinates of the pointcloud height : double The height of the bounding box calculated in the world coordinates of the pointcloud """ # Calculate the dimensions of the filtered and summed up point cloud # Some dirty array manipulations are gonna follow if point_cloud.shape[1] > 500: # Get the bounding box in 2D using the X and Y coordinates coord = np.c_[point_cloud[0,:], point_cloud[1,:]].astype('float32') min_area_rectangle = cv2.minAreaRect(coord) bounding_box_world_2d = cv2.boxPoints(min_area_rectangle) # Caculate the height of the pointcloud height = max(point_cloud[2,:]) - min(point_cloud[2,:]) + depth_threshold # Get the upper and lower bounding box corner points in 3D height_array = np.array([[-height], [-height], [-height], [-height], [0], [0], [0], [0]]) bounding_box_world_3d = np.column_stack((np.row_stack((bounding_box_world_2d,bounding_box_world_2d)), height_array)) # Get the bounding box points in the image coordinates bounding_box_points_color_image={} for (device, calibration_info) in calibration_info_devices.items(): # Transform the bounding box corner points to the device coordinates bounding_box_device_3d = calibration_info[0].inverse().apply_transformation(bounding_box_world_3d.transpose()) # Obtain the image coordinates in the color imager using the bounding box 3D corner points in the device coordinates color_pixel=[] bounding_box_device_3d = bounding_box_device_3d.transpose().tolist() for bounding_box_point in bounding_box_device_3d: bounding_box_color_image_point = rs.rs2_transform_point_to_point(calibration_info[2], bounding_box_point) color_pixel.append(rs.rs2_project_point_to_pixel(calibration_info[1][rs.stream.color], bounding_box_color_image_point)) bounding_box_points_color_image[device] = np.row_stack( color_pixel ) return bounding_box_points_color_image, min_area_rectangle[1][0], min_area_rectangle[1][1], height else : return {},0,0,0
def calculate_boundingbox_points(point_cloud, calibration_info_devices, depth_threshold=0.01): """ 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 Parameters: ----------- point_cloud : ndarray The (3 x N) array containing the pointcloud information calibration_info_devices : dict keys: str Serial number of the device values: [transformation_devices, intrinsics_devices, extrinsics_devices] transformation_devices: Transformation object The transformation object containing the transformation information between the device and the world coordinate systems intrinsics_devices: rs.intrinscs The intrinsics of the depth_frame of the realsense device extrinsics_devices: rs.extrinsics The extrinsics between the depth imager 1 and the color imager of the realsense device depth_threshold : double The threshold for the depth value (meters) in world-coordinates beyond which the point cloud information will not be used 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 Return: ---------- bounding_box_points_color_image : dict The bounding box corner points in the image coordinate system for the color imager keys: str Serial number of the device values: [points] points: list The (8x2) list of the upper corner points stacked above the lower corner points length : double The length of the bounding box calculated in the world coordinates of the pointcloud width : double The width of the bounding box calculated in the world coordinates of the pointcloud height : double The height of the bounding box calculated in the world coordinates of the pointcloud """ # Calculate the dimensions of the filtered and summed up point cloud # Some dirty array manipulations are gonna follow if point_cloud.shape[1] > 500: # Get the bounding box in 2D using the X and Y coordinates coord = np.c_[point_cloud[0, :], point_cloud[1, :]].astype('float32') min_area_rectangle = cv2.minAreaRect(coord) bounding_box_world_2d = cv2.boxPoints(min_area_rectangle) # Caculate the height of the pointcloud height = max(point_cloud[2, :]) - min( point_cloud[2, :]) + depth_threshold # Get the upper and lower bounding box corner points in 3D height_array = np.array([[-height], [-height], [-height], [-height], [0], [0], [0], [0]]) bounding_box_world_3d = np.column_stack((np.row_stack( (bounding_box_world_2d, bounding_box_world_2d)), height_array)) # Get the bounding box points in the image coordinates bounding_box_points_color_image = {} for (device, calibration_info) in calibration_info_devices.items(): # Transform the bounding box corner points to the device coordinates bounding_box_device_3d = calibration_info[0].inverse( ).apply_transformation(bounding_box_world_3d.transpose()) # Obtain the image coordinates in the color imager using the bounding box 3D corner points in the device coordinates color_pixel = [] bounding_box_device_3d = bounding_box_device_3d.transpose().tolist( ) for bounding_box_point in bounding_box_device_3d: bounding_box_color_image_point = rs.rs2_transform_point_to_point( calibration_info[2], bounding_box_point) color_pixel.append( rs.rs2_project_point_to_pixel( calibration_info[1][rs.stream.color], bounding_box_color_image_point)) bounding_box_points_color_image[device] = np.row_stack(color_pixel) return bounding_box_points_color_image, min_area_rectangle[1][ 0], min_area_rectangle[1][1], height else: return {}, 0, 0, 0
color_to_depth_extrin = aligned_color_frame.profile.get_extrinsics_to( aligned_color_depth_frame.profile) # Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print('depth_scale:', depth_scale) #depth_scale = depth_sensor.get_option(rs.option.depth_units) # Map depth to color color_pixel = [396, 270] pixel_distance_in_meters = aligned_depth_frame.get_distance( color_pixel[0], color_pixel[1]) #pixel_distance_in_meters=aligned_color_depth_frame.get_distance(color_pixel[0],color_pixel[1]) print("pixel distance in meters:", pixel_distance_in_meters) color_point = rs.rs2_deproject_pixel_to_point(color_intrin, color_pixel, pixel_distance_in_meters) depth_point = rs.rs2_transform_point_to_point(color_to_depth_extrin, color_point) depth_pixel = rs.rs2_project_point_to_pixel(depth_intrin, depth_point) pipe.stop() print('depth_intrinsic', depth_intrin) print('color_intrinsic', color_intrin) print('depth and color extrinsic', color_to_depth_extrin) print('depth_pixel', depth_pixel) print('depth_point', depth_point) print('color_pixel', color_pixel) print('color_point', color_point) cv2.namedWindow('Align Example', cv2.WINDOW_NORMAL) #cv2.imshow('Align Example', images) cv2.imshow('Align Example', aligned_color_image) cv2.namedWindow('depth example', cv2.WINDOW_NORMAL) cv2.imshow('depth example', aligned_color_depth_image)
def __detect(self, save_img=False): # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color align = rs.align(align_to) frames = self.__pipeline.wait_for_frames() aligned_frames = align.process(frames) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() # time.sleep(8) img_color = np.array(color_frame.get_data()) img_depth = np.array(depth_frame.get_data()) cv2.imwrite(opt.source, img_color) # detection section img_size = ( 320, 192 ) if ONNX_EXPORT else opt.img_size # (320, 192) or (416, 256) or (608, 352) for (height, width) out, source, weights, half, view_img, save_txt = opt.output, opt.source, opt.weights, opt.half, opt.view_img, opt.save_txt webcam = source == '0' or source.startswith( 'rtsp') or source.startswith('http') or source.endswith('.txt') # Initialize device = torch_utils.select_device( device='cpu' if ONNX_EXPORT else opt.device) if os.path.exists(out): shutil.rmtree(out) # delete output folder os.makedirs(out) # make new output folder # Initialize model model = Darknet(opt.cfg, img_size) # Load weights attempt_download(weights) if weights.endswith('.pt'): # pytorch format model.load_state_dict( torch.load(weights, map_location=device)['model']) else: # darknet format load_darknet_weights(model, weights) # Second-stage classifier classify = False if classify: modelc = torch_utils.load_classifier(name='resnet101', n=2) # initialize modelc.load_state_dict( torch.load('weights/resnet101.pt', map_location=device)['model']) # load weights modelc.to(device).eval() # Eval mode model.to(device).eval() # Fuse Conv2d + BatchNorm2d layers # model.fuse() # Export mode if ONNX_EXPORT: model.fuse() img = torch.zeros((1, 3) + img_size) # (1, 3, 320, 192) f = opt.weights.replace(opt.weights.split('.')[-1], 'onnx') # *.onnx filename torch.onnx.export(model, img, f, verbose=False, opset_version=11) # Validate exported model import onnx model = onnx.load(f) # Load the ONNX model onnx.checker.check_model(model) # Check that the IR is well formed print(onnx.helper.printable_graph(model.graph) ) # Print a human readable representation of the graph return # Half precision half = half and device.type != 'cpu' # half precision only supported on CUDA if half: model.half() # Set Dataloader vid_path, vid_writer = None, None if webcam: view_img = True torch.backends.cudnn.benchmark = True # set True to speed up constant image size inference dataset = LoadStreams(source, img_size=img_size) else: save_img = True dataset = LoadImages(source, img_size=img_size) # Get names and colors names = load_classes(opt.names) colors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))] # Run inference t0 = time.time() _ = model(torch.zeros( (1, 3, img_size, img_size), device=device)) if device.type != 'cpu' else None # run once for path, img, im0s, vid_cap in dataset: img = torch.from_numpy(img).to(device) img = img.half() if half else img.float() # uint8 to fp16/32 img /= 255.0 # 0 - 255 to 0.0 - 1.0 if img.ndimension() == 3: img = img.unsqueeze(0) # Inference t1 = torch_utils.time_synchronized() pred = model(img, augment=opt.augment)[0] t2 = torch_utils.time_synchronized() # to float if half: pred = pred.float() # Apply NMS pred = non_max_suppression(pred, opt.conf_thres, opt.iou_thres, multi_label=False, classes=opt.classes, agnostic=opt.agnostic_nms) # Apply Classifier if classify: pred = apply_classifier(pred, modelc, img, im0s) # Process detections for i, det in enumerate(pred): # detections per image if webcam: # batch_size >= 1 p, s, im0 = path[i], '%g: ' % i, im0s[i] else: p, s, im0 = path, '', im0s save_path = str(Path(out) / Path(p).name) s += '%gx%g ' % img.shape[2:] # print string position_result = geometry_msgs.msg.PointStamped() if det is not None and len(det): # Rescale boxes from img_size to im0 size det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round() # Print results for c in det[:, -1].unique(): n = (det[:, -1] == c).sum() # detections per class s += '%g %ss, ' % (n, names[int(c)]) # add to string # stop_num = 0 # print('c = ',len(det)) target_detected = False # Write results for *xyxy, conf, cls in det: # c1, c2 = (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3])) # c0 = ((int(xyxy[0])+int(xyxy[2]))/2, (int(xyxy[1])+int(xyxy[3]))/2) if save_txt: # Write to file with open(save_path + '.txt', 'a') as file: file.write( ('%g ' * 6 + '\n') % (*xyxy, cls, conf)) if save_img or view_img: # Add bbox to image label = '%s %.2f' % (names[int(cls)], conf) plot_one_box(xyxy, im0, label=label, color=colors[int(cls)]) # print(c0) if int(cls) == self.targetclass: x_center = int((int(xyxy[0]) + int(xyxy[2])) / 2) y_center = int((int(xyxy[1]) + int(xyxy[3])) / 2) # Intrinsics and Extrinsics depth_intrin = depth_frame.profile.as_video_stream_profile( ).intrinsics color_intrin = color_frame.profile.as_video_stream_profile( ).intrinsics depth_to_color_extrin = depth_frame.profile.get_extrinsics_to( color_frame.profile) # Depth scale: units of the values inside a depth frame, how to convert the value to units of 1 meter depth_sensor = self.__pipe_profile.get_device( ).first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # Map depth to color depth_pixel = [240, 320] # Random pixel depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, depth_pixel, depth_scale) color_point = rs.rs2_transform_point_to_point( depth_to_color_extrin, depth_point) color_pixel = rs.rs2_project_point_to_pixel( color_intrin, color_point) pc.map_to(color_frame) points = pc.calculate(depth_frame) vtx = np.array(points.get_vertices()) tex = np.array(points.get_texture_coordinates()) pix_num = 1280 * y_center + x_center point_cloud_value = [ np.float(vtx[pix_num][0]), np.float(vtx[pix_num][1]), np.float(vtx[pix_num][2]) ] print('point_cloud_value:', point_cloud_value, names[int(cls)], int(cls)) if np.float(vtx[pix_num][2]) > 0.11: position_result.header.frame_id = 'camera_color_optical_frame' position_result.point.x = np.float( vtx[pix_num][0]) position_result.point.y = np.float( vtx[pix_num][1]) position_result.point.z = np.float( vtx[pix_num][2]) self.__target_position_pub_.publish( position_result) # publish the result target_detected = True rospy.loginfo('The target has been detected!') break # only the target class # os.system('rostopic pub -1 /goal_pose geometry_msgs/PointStamped [0,[0,0],zed_left_camera_optical_frame] [%s,%s,%s]' %(np.float(vtx[pix_num][0]),np.float(vtx[pix_num][1]),np.float(vtx[pix_num][2])) # os.system('rostopic pub -1 /start_plan std_msgs/Bool 1') # else: # os.system('rostopic pub -1 /start_plan std_msgs/Bool 0') # print("Can't estimate point cloud at this position.") # stop_num += 1 # if stop_num >= len(det): # os.system('rostopic pub -1 /start_plan std_msgs/Bool 0') if not target_detected: position_result.header.frame_id = 'empty' self.__target_position_pub_.publish( position_result) # publish failure topic rospy.logwarn('Fail to detect the target!') else: position_result.header.frame_id = 'empty' self.__target_position_pub_.publish( position_result) # publish failure topic rospy.logwarn('Fail to detect any target!') # Print time (inference + NMS) print('%sDone. (%.3fs)' % (s, t2 - t1)) # Stream results if view_img: cv2.imshow(p, im0) if cv2.waitKey(1) == ord('q'): # q to quit raise StopIteration # Save results (image with detections) if save_img: if dataset.mode == 'images': cv2.imwrite(save_path, im0) else: if vid_path != save_path: # new video vid_path = save_path if isinstance(vid_writer, cv2.VideoWriter): vid_writer.release( ) # release previous video writer fps = vid_cap.get(cv2.CAP_PROP_FPS) w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH)) h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) vid_writer = cv2.VideoWriter( save_path, cv2.VideoWriter_fourcc(*opt.fourcc), fps, (w, h)) vid_writer.write(im0) if save_txt or save_img: print('Results saved to %s' % os.getcwd() + os.sep + out) if platform == 'darwin': # MacOS os.system('open ' + out + ' ' + save_path) print('Done. (%.3fs)' % (time.time() - t0)) return
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics color_intrin = color_frame.profile.as_video_stream_profile().intrinsics depth_to_color_extrin = depth_frame.profile.get_extrinsics_to( color_frame.profile) # Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter depth_sensor = pipe_profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # Map depth to color depth_pixel = [240, 320] # Random pixel # depth_pixel = [0,100] depth_point = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, depth_scale) color_point = rs.rs2_transform_point_to_point(depth_to_color_extrin, depth_point) color_pixel = rs.rs2_project_point_to_pixel(color_intrin, color_point) # print ('depth: ',color_point) # print ('depth: ',color_pixel) pc.map_to(color_frame) points = pc.calculate(depth_frame) vtx = np.asanyarray(points.get_vertices()) tex = np.asanyarray(points.get_texture_coordinates()) img_color, top, bottle, right, left = circle_fit.my_fun(img_color) print( "------------------------------------------------------------------------------------------" ) i_top = 640 * int(top[1]) + int(top[0]) print("i_top = ", i_top) if top[1] > 0 and top[1] < 640 and top[0] > 0 and top[
def point_to_another_coord(extrinsics, deprojected_coordinates): new_point = rs.rs2_transform_point_to_point(extrinsics, deprojected_coordinates) return new_point
def find_plane(points, depth_frame, rois, depth_intrin, depth_to_color_extrin): left_point = [0, 0] right_point = [0, 0] third_point = [0, 0] #left_point for i in range(15, 100): left_point = [points['corner2'][0] + i, points['corner2'][1] - i] if not point_in_mask(left_point, rois) and depth_frame.get_distance( int(left_point[0]), int(left_point[1])) != 0: break if left_point != [0, 0]: depth_point1 = rs.rs2_deproject_pixel_to_point( depth_intrin, left_point, depth_frame.get_distance(int(left_point[0]), int(left_point[1]))) color_point1 = rs.rs2_transform_point_to_point(depth_to_color_extrin, depth_point1) else: color_point1 = [0, 0, 0] print('color_point1_transform fail') #right_point for i in range(15, 100): right_point = [points['corner3'][0] - i, points['corner3'][1] - i] if not point_in_mask(right_point, rois) and depth_frame.get_distance( int(right_point[0]), int(right_point[1])) != 0: break if right_point != [0, 0]: depth_point2 = rs.rs2_deproject_pixel_to_point( depth_intrin, right_point, depth_frame.get_distance(int(right_point[0]), int(right_point[1]))) color_point2 = rs.rs2_transform_point_to_point(depth_to_color_extrin, depth_point2) else: color_point2 = [0, 0, 0] print('color_point2_transform fail') #third_point """ for i in range(15,200): third_point=[points['corner1'][0]+i,points['corner1'][1]+i] if not point_in_mask(third_point,rois) and depth_frame.get_distance(int(third_point[0]),int(third_point[1]))!=0: break if third_point!=[0,0]: depth_point3=rs.rs2_deproject_pixel_to_point(depth_intrin, third_point, depth_frame.get_distance(int(third_point[0]),int(third_point[1]))) color_point3=rs.rs2_transform_point_to_point(depth_to_color_extrin, depth_point2) else: color_point3=[0,0,0] print('color_point3_transform fail') """ if left_point != [0, 0] and right_point != [0, 0]: for i in range(50, 200): third_point = [ int((right_point[0] + left_point[0]) / 2), int((right_point[1] + left_point[1]) / 2) - i ] if not point_in_mask( third_point, rois) and depth_frame.get_distance( int(third_point[0]), int(third_point[1])) != 0: break if third_point != [0, 0]: depth_point3 = rs.rs2_deproject_pixel_to_point( depth_intrin, third_point, depth_frame.get_distance(int(third_point[0]), int(third_point[1]))) color_point3 = rs.rs2_transform_point_to_point( depth_to_color_extrin, depth_point3) else: color_point3 = [0, 0, 0] print('color_point3_transform fail') else: print('in depth_image,we can not find left or right point') if left_point != [0, 0] and right_point != [0, 0 ] and third_point != [0, 0]: L1 = [ color_point2[0] - color_point1[0], color_point2[1] - color_point1[1], color_point2[2] - color_point1[2] ] L2 = [ color_point3[0] - color_point1[0], color_point3[1] - color_point1[1], color_point3[2] - color_point1[2] ] A = L1[1] * L2[2] - L1[2] * L2[1] B = L1[2] * L2[0] - L1[0] * L2[2] C = L1[0] * L2[1] - L1[1] * L2[0] D = (A * color_point1[0] + B * color_point1[1] + C * color_point1[2]) * (-1) print("平面的参数", A, B, C, D) return [A, B, C, D] else: return [0, 0, 0, 0]
def extract_based_pixel2(self, color_pixel): self.depth_to_color_extrin = self.depth.profile.get_extrinsics_to( self.color.profile) self.color_to_depth_extrin = self.color.profile.get_extrinsics_to( self.depth.profile) # Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter depth_sensor = self.profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() color_pixel1 = [color_pixel[0], color_pixel[1]] color_pixel2 = [ color_pixel[0] + color_pixel[2], color_pixel[1] + color_pixel[3] ] left_top_color_point = rs.rs2_deproject_pixel_to_point( self.color_intrin, color_pixel1, 1.0) left_top_depth_point = rs.rs2_transform_point_to_point( self.color_to_depth_extrin, left_top_color_point) left_top_depth_pixel = rs.rs2_project_point_to_pixel( self.depth_intrin, left_top_depth_point) right_down_color_point = rs.rs2_deproject_pixel_to_point( self.color_intrin, color_pixel2, 1.0) right_down_depth_point = rs.rs2_transform_point_to_point( self.color_to_depth_extrin, right_down_color_point) right_down_depth_pixel = rs.rs2_project_point_to_pixel( self.depth_intrin, right_down_depth_point) depth_image = np.asanyarray(self.depth.get_data()) height, width = depth_image.shape lx = int(left_top_depth_pixel[0]) ly = int(left_top_depth_pixel[1]) rx = int(right_down_depth_pixel[0]) ry = int(right_down_depth_pixel[1]) index_max = np.argmax(depth_image[lx:rx + 1, ly:ry + 1]) index_min = np.argmin(depth_image[lx:rx + 1, ly:ry + 1]) print('index_max', index_max) print('index_min', index_min) index_max = lx * width + ly + index_max index_min = lx * width + ly + index_min pixel_max = [int(index_max / width), int(index_max % width)] pixel_min = [int(index_min / width), int(index_min % width)] scale1 = depth_image[lx, ly] scale2 = depth_image[rx, ry] scale3 = depth_image[pixel_max[0], pixel_max[1]] scale4 = depth_image[pixel_min[0], pixel_min[1]] print('scale1', scale1) print('scale2', scale2) print('scale3', scale3) print('scale4', scale4) point1 = rs.rs2_deproject_pixel_to_point(self.depth_intrin, left_top_depth_pixel, scale1) point2 = rs.rs2_deproject_pixel_to_point(self.depth_intrin, right_down_depth_pixel, scale2) point3 = rs.rs2_deproject_pixel_to_point(self.depth_intrin, pixel_max, scale3) point4 = rs.rs2_deproject_pixel_to_point(self.depth_intrin, pixel_min, scale4) l1 = min(point1[0], point2[0]) * depth_scale r1 = max(point1[0], point2[0]) * depth_scale l2 = min(point1[1], point2[1]) * depth_scale r2 = max(point1[1], point2[1]) * depth_scale l3 = min(point1[2], point2[2]) * depth_scale r3 = max(point1[2], point2[2]) * depth_scale bounding_cube = [l1, r1, l2, r2, l3, r3] depth_bbox = [ left_top_depth_pixel[0], left_top_depth_pixel[1], right_down_depth_pixel[0], right_down_depth_pixel[1] ] print(depth_bbox) self.show_depth_bounding(depth_bbox) # return [left_top_depth_pixel[0],left_top_depth_pixel[1],right_down_depth_pixel[0],right_down_depth_pixel[1]] return bounding_cube
def extract_based_pixel(self, color_pixel): self.depth_to_color_extrin = self.depth.profile.get_extrinsics_to( self.color.profile) self.color_to_depth_extrin = self.color.profile.get_extrinsics_to( self.depth.profile) # Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter depth_sensor = self.profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # depth = depth_frame.get_distance(j, i) # depth_point = rs.rs2_deproject_pixel_to_point( # depth_intrin, [j, i], depth) # text = "%.5lf, %.5lf, %.5lf\n" % ( # depth_point[0], depth_point[1], depth_point[2]) color_pixel1 = [color_pixel[0], color_pixel[1]] color_pixel2 = [ color_pixel[0] + color_pixel[2], color_pixel[1] + color_pixel[3] ] left_top_color_point = rs.rs2_deproject_pixel_to_point( self.color_intrin, color_pixel1, 1.0) left_top_depth_point = rs.rs2_transform_point_to_point( self.color_to_depth_extrin, left_top_color_point) left_top_depth_pixel = rs.rs2_project_point_to_pixel( self.depth_intrin, left_top_depth_point) right_down_color_point = rs.rs2_deproject_pixel_to_point( self.color_intrin, color_pixel2, 1.0) right_down_depth_point = rs.rs2_transform_point_to_point( self.color_to_depth_extrin, right_down_color_point) right_down_depth_pixel = rs.rs2_project_point_to_pixel( self.depth_intrin, right_down_depth_point) depth_image = np.asanyarray(self.depth.get_data()).T height, width = depth_image.shape lx = int(left_top_depth_pixel[0]) ly = int(left_top_depth_pixel[1]) rx = int(right_down_depth_pixel[0]) ry = int(right_down_depth_pixel[1]) depth_bbox = [ left_top_depth_pixel[0], left_top_depth_pixel[1], right_down_depth_pixel[0], right_down_depth_pixel[1] ] print(color_pixel) print(depth_bbox) self.show_depth_bounding(depth_bbox) scale1 = depth_image[lx, ly] scale2 = depth_image[rx, ry] scale_max = np.max(depth_image[lx:rx, ly:ry]) scale_mean = np.median(depth_image[lx:rx, ly:ry]) #scale_mean = np.mean(depth_image[lx:rx,ly:ry]) #rs.depth_frame.get_distance(x, y) high = max(scale1, scale2) low = min(scale1, scale2) num = (rx - lx) * (ry - ly) points = np.zeros([num, 3]).astype(np.float32) index = 0 for i in range(lx, rx): for j in range(ly, ry): scale = depth_image[i, j] if scale > 0.01 and scale < scale_mean * 1.1: point = rs.rs2_deproject_pixel_to_point( self.depth_intrin, [i, j], scale) points[index, 0] = point[0] points[index, 1] = point[1] points[index, 2] = point[2] index = index + 1 points = points[0:index, :] * depth_scale #print(point.shape) # centroid = np.mean(points, axis=0, keepdims=True) # furthest_distance = np.amax(np.sqrt(np.sum((points - centroid) ** 2, axis=-1)), keepdims=True) # # points = (points - centroid) / furthest_distance # points[:, 1:3] = -points[:, 1:3] return points