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 point_to_image(self, bb_data, circle_color=(0, 255, 0)): """Draws a 3D point on the color image. Converts the given 3D point back to a pixel and draws it on the color image as a small circle. Parameters ---------- bb_data : tuple A sextuple that contains the x, y, z and the respective dimensions of a bounding box. Only the first three elements (x, y, z) are needed. circle_color : tuple, optional by default green (0, 255, 0) """ pixel = rs2.rs2_project_point_to_pixel(self.intrinsics, bb_data[:3]) if self.debug: rospy.loginfo("BB median center xy:" + str(round(pixel[0])) + " " + str(round(pixel[1]))) cv2.circle( self.color_image, ( round(pixel[0]), round(pixel[1]), ), 5, circle_color, 2, )
def torso3DToPixel(self, depth_intrin, position, toRobRotation=np.eye(3), toRobTranslation=np.array([0, 0, 0])): position = np.dot(np.linalg.inv(toRobRotation), (position - toRobTranslation)) return rs.rs2_project_point_to_pixel(depth_intrin, position.tolist())
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 compute_feedback_vector(depth_frame: rs.depth_frame, pos3d: (float, float, float)): depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics x, y = rs.rs2_project_point_to_pixel(depth_intrin, pos3d) plane_depth = depth_frame.as_depth_frame().get_distance(int(x), int(y)) * 1000 if plane_depth > pos3d[2]: return np.array((0, 0, 0)) dist_beyond = pos3d[2] - plane_depth return dist_beyond * compute_normal_vector(depth_frame, x, y)
def depth_distance(self, x, y): depth_intrin = self.depth_intrin #ix, iy = self.ix, self.iy #depth_scale = self.depth_sensor.get_depth_scale() udist = self.depth_frame.get_distance(x, y) #in meters point = rs.rs2_deproject_pixel_to_point(depth_intrin, [x, y], udist) point_array = np.asanyarray(point) point_to_pixel = rs.rs2_project_point_to_pixel(depth_intrin, point) print(point) print(point_to_pixel) return point_array
def generate_frame_flat_surface(width: float, height: float, distance: float, intrinsics: rs2.intrinsics) -> np.ndarray: """ :param width: The width of the flat surface in meters :param height: The height of the flat surface in meters :param distance: The distance of the object from the camera :param intrinsics: The intrinsics of the camera that the object should be 'captured' from :return: The frame that should have been captured if that surface was captured from that camera from that distance """ frame = np.zeros(shape=(intrinsics.height, intrinsics.width)) max_j, max_i = rs2.rs2_project_point_to_pixel(intrinsics, [width / 2, height / 2, distance]) max_i, max_j = int(min(max_i, intrinsics.height)), int(min(max_j, intrinsics.width)) for i in range(intrinsics.height - max_i, max_i): for j in range(intrinsics.width - max_j, max_j): frame[i][j] = distance return frame
def convert_pc_to_frame(pc: np.ndarray, intrinsics: rs2.intrinsics) -> np.ndarray: """ :param pc: A point cloud :param intrinsics: The intrinsics of the camera that the object should be 'captured' from :return: The frame that would give that point cloud if it was captured from that camera """ assert len(pc.shape) == 2 assert pc.shape[1] == 3 frame = np.zeros(shape=(intrinsics.width, intrinsics.height)) for (x, y, z) in pc: j, i = rs2.rs2_project_point_to_pixel(intrinsics, [x, y, z]) # Maybe should use int(i),int(j) because of collisions i, j = round(i), round(j) frame[i][j] = z return frame
def deproject_ros_point_to_frame_point(target_point, frame_set, camera_frame): ''' Converts a 3-D camera-relative point to a 2-D pixel coordinate in frame @param target_point - ROS point that is being converted. @param frame_set - the latest VisionFrameSet object @param camera_frame - the TF frame that represents the camera's frame of reference @return the 3-D point in ROS coordinates: +x forward, +y left, +z up ''' global cameraInfo target_point_cam_rel = transform_to_frame(target_point, camera_frame) if not target_point_cam_rel: return None rs2_point = [ # convert from right-handed z-up to right-handed z-forward -target_point_cam_rel.point.y * 1000, -target_point_cam_rel.point.z * 1000, target_point_cam_rel.point.x * 1000, ] return tuple( rs.rs2_project_point_to_pixel(cameraInfo.get_color_intrinsics(), rs2_point))
def project(self, Pc): ''' Project a 3D camera coordinates point Pc [X Y Z] into the picture frame coordinates pixel (x,y) ''' return tuple([ int(x) for x in rs.rs2_project_point_to_pixel(self.intrinsics, list(Pc)) ])
def markerprocess(self): # Capture the frame first color_image, depth_image, depth_frame, color_frame = self._capture() # Aruco marker part # Detect the markers in the image markerCorners, markerIds, rejectedCandidates = cv2.aruco.detectMarkers( color_image, self.dictionary, parameters=self.parameters) aruco_list = {} # centre= {} result_center = {} # orient_centre= {} if markerIds is not None: # Print corners and ids to the console # result=zip(markerIds, markerCorners) for k in range(len(markerCorners)): temp_1 = markerCorners[k] temp_1 = temp_1[0] temp_2 = markerIds[k] temp_2 = temp_2[0] aruco_list[temp_2] = temp_1 key_list = aruco_list.keys() font = cv2.FONT_HERSHEY_SIMPLEX # print(key_list) for key in key_list: dict_entry = aruco_list[key] centre = dict_entry[0] + dict_entry[1] + dict_entry[ 2] + dict_entry[3] centre[:] = [int(x / 4) for x in centre] # orient_centre = centre + [0.0,5.0] centre = tuple(centre) result_center[key] = centre # orient_centre = tuple((dict_entry[0]+dict_entry[1])/2) cv2.circle(color_image, centre, 1, (0, 0, 255), 8) # Compute distance when matching the conditions # print(result_center) # print(len(result_center)) if len(result_center) < 4: print("No enough marker detected") if len(result_center) >= 4: # To avoid keyerror # start = time.time() try: # Moving object localization marker x_id0 = result_center[0][0] y_id0 = result_center[0][1] p_0 = [x_id0, y_id0] # Target object localization marker # Single ID-5 x_id5 = result_center[5][0] y_id5 = result_center[5][1] p_5 = [x_id5, y_id5] # Dual ID-4 and ID-3 x_id4 = result_center[4][0] y_id4 = result_center[4][1] p_4 = [x_id4, y_id4] x_id3 = result_center[3][0] y_id3 = result_center[3][1] p_3 = [x_id3, y_id3] # Deproject pixel to 3D point point_0 = self.pixel2point(depth_frame, p_0) point_5 = self.pixel2point(depth_frame, p_5) point_4 = self.pixel2point(depth_frame, p_4) point_3 = self.pixel2point(depth_frame, p_3) # Calculate target point point_target = [ point_4[0] + point_3[0] - point_5[0], point_4[1] + point_3[1] - point_5[1], point_4[2] + point_3[2] - point_5[2] ] # Compute distance # dis=distance_3dpoints(point_target,point_0) # Display target and draw a line between them color_intrin = color_frame.profile.as_video_stream_profile( ).intrinsics target_pixel = rs.rs2_project_point_to_pixel( color_intrin, point_target) target_pixel[0] = int(target_pixel[0]) target_pixel[1] = int(target_pixel[1]) cv2.circle(color_image, tuple(target_pixel), 1, (0, 0, 255), 8) cv2.line(color_image, tuple(p_0), tuple(target_pixel), (0, 255, 0), 2) # Euclidean distance dis_obj2target = self.distance_3dpoints( point_0, point_target) dis_obj2target_goal = dis_obj2target * np.sin( np.arccos(0.02 / dis_obj2target)) # print(dis_obj2target_goal) # images = cv2.aruco.drawDetectedMarkers(images, markerCorners, borderColor=(0, 0, 255)) # end = time.time() # print(str(end-start)) except KeyError: print("Keyerror!!!") # Outline all of the markers detected in our image # images = cv2.aruco.drawDetectedMarkers(images, markerCorners, borderColor=(0, 0, 255)) # self.images = color_image self.images = cv2.aruco.drawDetectedMarkers(color_image, markerCorners, borderColor=(0, 0, 255)) return self.images
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
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
# 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) cv2.waitKey(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
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_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[ 0] < 480 and i_top < 300000:
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 point_projection(self, pixel_3d): depth_intrin = self.filtered.profile.as_video_stream_profile( ).intrinsics return rs.rs2_project_point_to_pixel(depth_intrin, pixel_3d)
def start_pipeline(): save_path = "./out" pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30) pipeline.start(config) print('[INFO] Starting pipeline') profile = pipeline.get_active_profile() # profile = pipeline.start(config) depth_profile = rs.video_stream_profile(profile.get_stream( rs.stream.depth)) depth_intrinsics = depth_profile.get_intrinsics() w, h = depth_intrinsics.width, depth_intrinsics.height print("[INFO] Width: ", w, "Height: ", h) fx, fy, = depth_intrinsics.fx, depth_intrinsics.fy print("[INFO] Focal length X: ", fx) print("[INFO] Focal length Y: ", fy) ppx, ppy = depth_intrinsics.ppx, depth_intrinsics.ppy print("[INFO] Principal point X: ", ppx) print("[INFO] Principal point Y: ", ppy) # Identifying depth scaling factor depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() pc = rs.pointcloud() decimate = rs.decimation_filter() spatial = rs.spatial_filter hole = rs.hole_filling_filter temporal = rs.temporal_filter() colorizer = rs.colorizer() align_to = rs.stream.color align = rs.align(align_to) frame_counter = 0 try: print("[INFO] Starting pipeline stream with frame {:d}".format( frame_counter)) while True: frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) aligned_depth_frame = aligned_frames.get_depth_frame() aligned_color_frame = aligned_frames.get_color_frame() depth_colormap = np.asanyarray( colorizer.colorize(aligned_depth_frame).get_data()) color_image = np.asanyarray(aligned_color_frame.get_data()) pts = pc.calculate(aligned_depth_frame) pc.map_to(aligned_color_frame) if not aligned_depth_frame or not aligned_color_frame: print("No depth or color frame, try again...") continue depth_image = np.asanyarray(aligned_depth_frame.get_data()) images = np.hstack( (cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB), depth_colormap)) cv2.imshow(str(frame_counter), images) key = cv2.waitKey(1) if key == ord("d"): landmarks = landmark_detector(color_image) for n, px in landmarks.items(): z = aligned_depth_frame.get_distance(px[0], px[1]) # Test Landmarks px = [width, height] pt = rs.rs2_deproject_pixel_to_point( depth_intrinsics, [px[0], px[1]], z) w, h = rs.rs2_project_point_to_pixel(depth_intrinsics, pt) print("Original Pixel:", px) print("Deprojected Pixel:", [w, h]) return px, w, h except Exception as ex: print(ex) finally: pipeline.stop()
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
point_5 = pixel2point(depth_frame, p_5) point_4 = pixel2point(depth_frame, p_4) point_3 = pixel2point(depth_frame, p_3) # Calculate target point point_target = [ point_4[0] + point_3[0] - point_5[0], point_4[1] + point_3[1] - point_5[1], point_4[2] + point_3[2] - point_5[2] ] # Compute distance # dis=distance_3dpoints(point_target,point_0) # Display target and draw a line between them color_intrin = color_frame.profile.as_video_stream_profile( ).intrinsics target_pixel = rs.rs2_project_point_to_pixel( color_intrin, point_target) target_pixel[0] = int(target_pixel[0]) target_pixel[1] = int(target_pixel[1]) cv2.circle(images, tuple(target_pixel), 1, (0, 0, 255), 8) cv2.line(images, tuple(p_0), tuple(target_pixel), (0, 255, 0), 2) # Euclidean distance dis_obj2target = distance_3dpoints( point_0, point_target) dis_obj2target_goal = dis_obj2target * np.sin( np.arccos(0.02 / dis_obj2target)) print(dis_obj2target_goal) useful_color_image = color_image