def main(): # Create a Camera object zed = sl.Camera() pcd = PointCloud() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE # Use PERFORMANCE depth mode init_params.coordinate_units = sl.UNIT.UNIT_MILLIMETER # Use milliliter units (for depth measurements) # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Create and set RuntimeParameters after opening the camera runtime_parameters = sl.RuntimeParameters() runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD # Use STANDARD sensing mode image = sl.Mat() depth = sl.Mat() point_cloud = sl.Mat() while 1: # A new image is available if grab() returns SUCCESS if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # Retrieve left image zed.retrieve_image(image, sl.VIEW.VIEW_LEFT) # Retrieve colored point cloud. Point cloud is aligned on the left image. zed.retrieve_measure(point_cloud, sl.MEASURE.MEASURE_XYZRGBA) # Get and print distance value in mm at the center of the image # We measure the distance camera - object using Euclidean distance x = round(image.get_width() / 2) y = round(image.get_height() / 2) err, point_cloud_value = point_cloud.get_value(x, y) numPoints = 1280*720*3 numColors = 1280*720*1 points = np.arange(numPoints).reshape((-1, 3)) colors = np.arange(numColors).reshape((-1, 1)) begin = datetime.datetime.now() my = point_cloud.get_data() aa = my.flatten() aa.resize((numColors,4)) aa = np.array(aa) filPoints = aa[~np.isnan(aa).any(axis=1)] filPoints = filPoints[::15,:] points = filPoints[:,0:3].reshape(-1,3) colors = filPoints[:,3].reshape(-1,1) print(colors.shape) print(points.shape) # filPoints = points[~np.isnan(points).any(axis=1)] pcd.points = Vector3dVector(points) pcd.paint_uniform_color([0,1,0]) for i in range(len(colors)): A, R, G, B = float2decimal(colors[i][0]) print("values of BGR: ", B,G,R) pcd.colors[i] = [B/255, G/255, R/255] end = datetime.datetime.now() k = end - begin draw_geometries([pcd]) distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] + point_cloud_value[1] * point_cloud_value[1] + point_cloud_value[2] * point_cloud_value[2]) if not np.isnan(distance) and not np.isinf(distance): distance = round(distance) print("Distance to Camera at ({0}, {1}): {2} mm\n".format(x, y, distance)) # Increment the loop # i = i + 1 else: print("Can't estimate distance at this position, move the camera\n") sys.stdout.flush() # Close the camera zed.close()
def main(argv): thresh = 0.25 darknet_path="../libdarknet/" config_path = darknet_path + "cfg/yolov3-tiny.cfg" weight_path = "yolov3-tiny.weights" meta_path = "coco.data" svo_path = None zed_id = 0 help_str = 'darknet_zed.py -c <config> -w <weight> -m <meta> -t <threshold> -s <svo_file> -z <zed_id>' try: opts, args = getopt.getopt( argv, "hc:w:m:t:s:z:", ["config=", "weight=", "meta=", "threshold=", "svo_file=", "zed_id="]) except getopt.GetoptError: log.exception(help_str) sys.exit(2) for opt, arg in opts: if opt == '-h': log.info(help_str) sys.exit() elif opt in ("-c", "--config"): config_path = arg elif opt in ("-w", "--weight"): weight_path = arg elif opt in ("-m", "--meta"): meta_path = arg elif opt in ("-t", "--threshold"): thresh = float(arg) elif opt in ("-s", "--svo_file"): svo_path = arg elif opt in ("-z", "--zed_id"): zed_id = int(arg) input_type = sl.InputType() if svo_path is not None: log.info("SVO file : " + svo_path) input_type.set_from_svo_file(svo_path) else: # Launch camera by id input_type.set_from_camera_id(zed_id) init = sl.InitParameters(input_t=input_type) init.coordinate_units = sl.UNIT.METER cam = sl.Camera() if not cam.is_opened(): log.info("Opening ZED Camera...") status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: log.error(repr(status)) exit() runtime = sl.RuntimeParameters() # Use STANDARD sensing mode runtime.sensing_mode = sl.SENSING_MODE.STANDARD mat = sl.Mat() point_cloud_mat = sl.Mat() # Prepare new image size to retrieve half-resolution images image_size = cam.get_camera_information().camera_resolution image_size.width = image_size.width image_size.height = image_size.height depth_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.F32_C1) # Create an RGBA sl.Mat object depth_zed_show = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) # Import the global variables. This lets us instance Darknet once, # then just call performDetect() again without instancing again global metaMain, netMain, altNames # pylint: disable=W0603 assert 0 < thresh < 1, "Threshold should be a float between zero and one (non-inclusive)" if not os.path.exists(config_path): raise ValueError("Invalid config path `" + os.path.abspath(config_path)+"`") if not os.path.exists(weight_path): raise ValueError("Invalid weight path `" + os.path.abspath(weight_path)+"`") if not os.path.exists(meta_path): raise ValueError("Invalid data file path `" + os.path.abspath(meta_path)+"`") if netMain is None: netMain = load_net_custom(config_path.encode( "ascii"), weight_path.encode("ascii"), 0, 1) # batch size = 1 if metaMain is None: metaMain = load_meta(meta_path.encode("ascii")) if altNames is None: # In thon 3, the metafile default access craps out on Windows (but not Linux) # Read the names file and create a list to feed to detect try: with open(meta_path) as meta_fh: meta_contents = meta_fh.read() import re match = re.search("names *= *(.*)$", meta_contents, re.IGNORECASE | re.MULTILINE) if match: result = match.group(1) else: result = None try: if os.path.exists(result): with open(result) as names_fh: names_list = names_fh.read().strip().split("\n") altNames = [x.strip() for x in names_list] except TypeError: pass except Exception: pass color_array = generate_color(meta_path) log.info("Running...") key = '' while key != 113: # for 'q' key start_time = time.time() # start time of the loop err = cam.grab(runtime) if err == sl.ERROR_CODE.SUCCESS: cam.retrieve_image(mat, sl.VIEW.LEFT) image = mat.get_data() cam.retrieve_measure( point_cloud_mat, sl.MEASURE.XYZRGBA) cam.retrieve_measure(depth_zed, sl.MEASURE.DEPTH) depth = point_cloud_mat.get_data() depth_ocv = depth_zed.get_data() # Retrieve the normalized depth image cam.retrieve_image(depth_zed_show, sl.VIEW.DEPTH) # Use get_data() to get the numpy array depth_zed_show_num = depth_zed_show.get_data() # Do the detection detections = detect(netMain, metaMain, image, thresh) grey_color = 153 log.info(chr(27) + "[2J"+"**** " + str(len(detections)) + " Results ****") for detection in detections: label = detection[0] confidence = detection[1] pstring = label+": "+str(np.rint(100 * confidence))+"%" log.info(pstring) bounds = detection[2] y_extent = int(bounds[3]) x_extent = int(bounds[2]) # Coordinates are around the sidecorner x_coord = int(bounds[0] - x_extent/2) y_coord = int(bounds[1] - y_extent/2) #boundingBox = [[x_coord, y_coord], [x_coord, y_coord + y_extent], [x_coord + x_extent, y_coord + y_extent], [x_coord + x_extent, y_coord]] bounds2 = [x_coord,y_coord] thickness = 1 x, y, z = get_object_depth(depth, bounds) print(bounds) print(bounds2) bounds3 = [x_coord + x_extent,y_coord] image_size = cam.get_camera_information().camera_resolution width = image_size.width height = image_size.height #print(width) #print(height) #1ピクセル計算 distance = math.sqrt(x * x + y * y + z * z) pixel_realsize_x = distance / 1280 * 1.92 pixel_realsize_y = distance / 720 * 1.06 distance_x = (x_extent) * pixel_realsize_x distance_y = (y_extent) * pixel_realsize_y #ROI roi = image[y_coord:y_coord+y_extent,x_coord:x_coord+x_extent] roi_depth = depth_ocv[y_coord:y_coord+y_extent,x_coord:x_coord+x_extent] #print(roi_depth.shape) roi_depth_show = depth_zed_show_num[y_coord:y_coord+y_extent,x_coord:x_coord+x_extent] #print(roi_depth_show.shape) roi_depth_show_gray = cv2.cvtColor(roi_depth_show, cv2.COLOR_RGB2GRAY) #depth_image_3d = np.dstack((roi_depth,roi_depth,roi_depth,roi_depth)) #bg_removed = np.where((depth_image_3d > z) | (depth_image_3d <= 0), grey_color, roi) #深度データを与えると2値を返す # 方法2 (OpenCVで実装) ret, th = cv2.threshold(roi_depth_show_gray, 0, 255, cv2.THRESH_OTSU) print("ret{}".format(ret)) depth_image_3d = np.dstack((th,th,th,th)) # th = threshold_otsu(roi_depth_show_gray) contours, hierarchy = cv2.findContours(th, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) for i in range(0, len(contours)): if len(contours[i]) > 0: # remove small objects if cv2.contourArea(contours[i]) < 800: continue cv2.polylines(roi, contours[i], True, (255, 255, 255), 5) image[y_coord:y_coord+y_extent,x_coord:x_coord+x_extent] = depth_image_3d ##表示 distance_string = "v:{1:.2f},h{0:.2f}".format(distance_x,distance_y) cv2.rectangle(image, (x_coord - thickness, y_coord - thickness), (x_coord + x_extent + thickness, y_coord + (18 + thickness*4)), color_array[detection[3]], -1) cv2.putText(image, label + " " + (str(distance_string) + " m"), (x_coord + (thickness * 4), y_coord + (10 + thickness * 4)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) cv2.rectangle(image, (x_coord - thickness, y_coord - thickness), (x_coord + x_extent + thickness, y_coord + y_extent + thickness), color_array[detection[3]], int(thickness*2)) cv2.imshow("ZED", image) key = cv2.waitKey(5) log.info("FPS: {}".format(1.0 / (time.time() - start_time))) else: key = cv2.waitKey(5) cv2.destroyAllWindows() cam.close() log.info("\nFINISH")
def main(): if not sys.argv or len(sys.argv) != 4: sys.stdout.write("Usage: \n\n") sys.stdout.write(" ZED_SVO_Export A B C \n\n") sys.stdout.write( "Please use the following parameters from the command line:\n") sys.stdout.write(" A - SVO file path (input) : \"path/to/file.svo\"\n") sys.stdout.write( " B - AVI file path (output) or image sequence folder(output) :\n") sys.stdout.write( " \"path/to/output/file.avi\" or \"path/to/output/folder\"\n" ) sys.stdout.write(" C - Export mode: 0=Export LEFT+RIGHT AVI.\n") sys.stdout.write(" 1=Export LEFT+DEPTH_VIEW AVI.\n") sys.stdout.write( " 2=Export LEFT+RIGHT image sequence.\n") sys.stdout.write( " 3=Export LEFT+DEPTH_VIEW image sequence.\n") sys.stdout.write( " 4=Export LEFT+DEPTH_16Bit image sequence.\n") sys.stdout.write(" A and B need to end with '/' or '\\'\n\n") sys.stdout.write("Examples: \n") sys.stdout.write( " (AVI LEFT+RIGHT): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/file.avi\" 0\n" ) sys.stdout.write( " (AVI LEFT+DEPTH): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/file.avi\" 1\n" ) sys.stdout.write( " (SEQUENCE LEFT+RIGHT): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/folder\" 2\n" ) sys.stdout.write( " (SEQUENCE LEFT+DEPTH): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/folder\" 3\n" ) sys.stdout.write( " (SEQUENCE LEFT+DEPTH_16Bit): ZED_SVO_Export \"path/to/file.svo\" \"path/to/output/folder\"" " 4\n") exit() # Get input parameters svo_input_path = Path(sys.argv[1]) output_path = Path(sys.argv[2]) output_as_video = True app_type = AppType.LEFT_AND_RIGHT if sys.argv[3] == "1" or sys.argv[3] == "3": app_type = AppType.LEFT_AND_DEPTH if sys.argv[3] == "4": app_type = AppType.LEFT_AND_DEPTH_16 # Check if exporting to AVI or SEQUENCE if sys.argv[3] != "0" and sys.argv[3] != "1": output_as_video = False if not output_as_video and not output_path.is_dir(): sys.stdout.write( "Input directory doesn't exist. Check permissions or create it.\n", output_path, "\n") exit() # Specify SVO path parameter init_params = sl.InitParameters() init_params.set_from_svo_file(str(svo_input_path)) init_params.svo_real_time_mode = False # Don't convert in realtime init_params.coordinate_units = sl.UNIT.MILLIMETER # Use milliliter units (for depth measurements) # Create ZED objects zed = sl.Camera() # Open the SVO file specified as a parameter err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: sys.stdout.write(repr(err)) zed.close() exit() # Get image size image_size = zed.get_camera_information().camera_resolution width = image_size.width height = image_size.height width_sbs = width * 2 # Prepare side by side image container equivalent to CV_8UC4 svo_image_sbs_rgba = np.zeros((height, width_sbs, 4), dtype=np.uint8) # Prepare single image containers left_image = sl.Mat() right_image = sl.Mat() depth_image = sl.Mat() video_writer = None if output_as_video: # Create video writer with MPEG-4 part 2 codec video_writer = cv2.VideoWriter( str(output_path), cv2.VideoWriter_fourcc('M', '4', 'S', '2'), max(zed.get_camera_information().camera_fps, 25), (width_sbs, height)) if not video_writer.isOpened(): sys.stdout.write( "OpenCV video writer cannot be opened. Please check the .avi file path and write " "permissions.\n") zed.close() exit() rt_param = sl.RuntimeParameters() rt_param.sensing_mode = sl.SENSING_MODE.FILL # Start SVO conversion to AVI/SEQUENCE sys.stdout.write("Converting SVO... Use Ctrl-C to interrupt conversion.\n") nb_frames = zed.get_svo_number_of_frames() while True: if zed.grab(rt_param) == sl.ERROR_CODE.SUCCESS: svo_position = zed.get_svo_position() # Retrieve SVO images zed.retrieve_image(left_image, sl.VIEW.LEFT) if app_type == AppType.LEFT_AND_RIGHT: zed.retrieve_image(right_image, sl.VIEW.RIGHT) elif app_type == AppType.LEFT_AND_DEPTH: zed.retrieve_image(right_image, sl.VIEW.DEPTH) elif app_type == AppType.LEFT_AND_DEPTH_16: zed.retrieve_measure(depth_image, sl.MEASURE.DEPTH) if output_as_video: # Copy the left image to the left side of SBS image svo_image_sbs_rgba[0:height, 0:width, :] = left_image.get_data() # Copy the right image to the right side of SBS image svo_image_sbs_rgba[0:, width:, :] = right_image.get_data() # Convert SVO image from RGBA to RGB ocv_image_sbs_rgb = cv2.cvtColor(svo_image_sbs_rgba, cv2.COLOR_RGBA2RGB) # Write the RGB image in the video video_writer.write(ocv_image_sbs_rgb) else: # Generate file names filename1 = output_path / ("left%s.png" % str(svo_position).zfill(6)) filename2 = output_path / ( ("right%s.png" if app_type == AppType.LEFT_AND_RIGHT else "depth%s.png") % str(svo_position).zfill(6)) # Save Left images cv2.imwrite(str(filename1), left_image.get_data()) if app_type != AppType.LEFT_AND_DEPTH_16: # Save right images cv2.imwrite(str(filename2), right_image.get_data()) else: # Save depth images (convert to uint16) cv2.imwrite(str(filename2), depth_image.get_data().astype(np.uint16)) # Display progress progress_bar((svo_position + 1) / nb_frames * 100, 30) # Check if we have reached the end of the video if svo_position >= (nb_frames - 1): # End of SVO sys.stdout.write("\nSVO end has been reached. Exiting now.\n") break if output_as_video: # Close the video writer video_writer.release() zed.close() return 0
def extract_xyz(x_pixel_M1, y_pixel_M1, x_pixel_M2, y_pixel_M2, outputfilename, svofilename): x_world_M1 = [] y_world_M1 = [] z_world_M1 = [] x_world_M2 = [] y_world_M2 = [] z_world_M2 = [] # x_pixel_M1 = [item[8] for item in xy_M1] # y_pixel_M1 = [item[9] for item in xy_M1] # x_pixel_M2 = [item[8] for item in xy_M2] # y_pixel_M2 = [item[9] for item in xy_M2] #filepath = "/home/avnish/zed_marmoset_recording/20200922.svo" #print("SVO file : ", filepath.rsplit('/', 1)[1]) # Create a Camera object # Create a InitParameters object and set configuration parameters #init_params = sl.InitParameters(svo_input_filename=filepath,svo_real_time_mode=False) #InitParameters init_params; // Set initial parameters #init_params.sdk_verbose = True; // Enable verbose mode #init_params.input.setFromSVOFile("/path/to/file.svo"); // Selects the and SVO file to be read input_type = sl.InputType() print("reading " + svofilename) input_type.set_from_svo_file(svofilename) init = sl.InitParameters(input_t=input_type, svo_real_time_mode=False) init.depth_mode = sl.DEPTH_MODE.ULTRA init.coordinate_units = sl.UNIT.MILLIMETER # Use millimeter units (for depth measurements) #init.depth_minimum_distance = 300 #init.depth_maximum_distance = 1500 zed = sl.Camera() # Open the camera err = zed.open(init) if err != sl.ERROR_CODE.SUCCESS: print('error_details: ', repr(err)) exit(1) # Create and set RuntimeParameters after opening the camera runtime_parameters = sl.RuntimeParameters() runtime_parameters.sensing_mode = sl.SENSING_MODE.FILL #runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_FILL # Use STANDARD sensing mode i = 0 pbar = tqdm(total=len(x_pixel_M1)) # image = sl.Mat() point_cloud = sl.Mat() try: while i < len(x_pixel_M1): # A new image is available if grab() returns SUCCESS if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # Retrieve left image # zed.retrieve_image(image, sl.VIEW.VIEW_LEFT) # Retrieve depth map. Depth is aligned on the left image # zed.retrieve_measure(depth, sl.MEASURE.MEASURE_DEPTH) zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA) #print(x_pixel_M1[i], y_pixel_M1[i], x_pixel_M2[i], y_pixel_M2[i]) #print(type(x_pixel_M1[i])) err_M1, point_cloud_value_M1 = point_cloud.get_value( x_pixel_M1[i], y_pixel_M1[i]) err_M2, point_cloud_value_M2 = point_cloud.get_value( x_pixel_M2[i], y_pixel_M2[i]) #print('after get value') # Get and print distance value in mm at the center of the image # We measure the distance camera - object using Euclidean distance # x = round(image.get_width() / 2) # y = round(image.get_height() / 2) #lens2cage_top_mm=693.42 old lens2cage_top_mm = 528.00 #Amelia lens2cage_edge_x_mm = 387.35 lens2cage_edge_y_mm = 387.35 x_world_M1.append( point_cloud_value_M1[0] + lens2cage_edge_x_mm) #millimeters; in cage coordinates y_world_M1.append(-point_cloud_value_M1[1] + lens2cage_edge_y_mm) z_world_M1.append(point_cloud_value_M1[2] - lens2cage_top_mm) x_world_M2.append(point_cloud_value_M2[0] + lens2cage_edge_x_mm) y_world_M2.append(-point_cloud_value_M2[1] + lens2cage_edge_y_mm) z_world_M2.append(point_cloud_value_M2[2] - lens2cage_top_mm) # Increment the loop # print(i) pbar.update(1) i += 1 # if (i % 1000) == 0: # print(i) # sys.stdout.flush() pbar.close() # Close the camera zed.close() except OverflowError as error: print(error) print("frame_number_completed: {}".format(i)) pbar.close() zed.close() finally: rows = zip(x_world_M1, y_world_M1, z_world_M1, x_world_M2, y_world_M2, z_world_M2) print("writing " + outputfilename) with open(outputfilename, 'w', newline='') as f: writer = csv.writer(f) for row in rows: writer.writerow(row) print("Done")
def main(): parser = argparse.ArgumentParser( description="PyTorch Object Detection Webcam Demo") parser.add_argument( "--config-file", default="configs/caffe2/e2e_mask_rcnn_X_101_32x8d_FPN_1x_caffe2.yaml", metavar="FILE", help="path to config file", ) parser.add_argument( "--confidence-threshold", type=float, default=0.6, help="Minimum score for the prediction to be shown", ) parser.add_argument( "--min-image-size", type=int, default=256, help="Smallest size of the image to feed to the model. " "Model was trained with 800, which gives best results", ) parser.add_argument( "--show-mask-heatmaps", dest="show_mask_heatmaps", help="Show a heatmap probability for the top masks-per-dim masks", action="store_true", ) parser.add_argument( "--masks-per-dim", type=int, default=2, help="Number of heatmaps per dimension to show", ) parser.add_argument( "opts", help="Modify model config options using the command-line", default=None, nargs=argparse.REMAINDER, ) parser.add_argument("--svo-filename", help="Optional SVO input filepath", default=None) args = parser.parse_args() # load config from file and command-line arguments cfg.merge_from_file(args.config_file) cfg.merge_from_list(args.opts) cfg.freeze() # prepare object that handles inference plus adds predictions on top of image coco_demo = COCODemo( cfg, confidence_threshold=args.confidence_threshold, show_mask_heatmaps=args.show_mask_heatmaps, masks_per_dim=args.masks_per_dim, min_image_size=args.min_image_size, ) init_cap_params = sl.InitParameters() if args.svo_filename: print("Loading SVO file " + args.svo_filename) init_cap_params.set_from_svo_file(args.svo_filename) init_cap_params.svo_real_time_mode = True init_cap_params.camera_resolution = sl.RESOLUTION.HD720 init_cap_params.depth_mode = sl.DEPTH_MODE.ULTRA init_cap_params.coordinate_units = sl.UNIT.METER init_cap_params.depth_stabilization = True init_cap_params.camera_image_flip = False init_cap_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP cap = sl.Camera() if not cap.is_opened(): print("Opening ZED Camera...") status = cap.open(init_cap_params) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() display = True runtime = sl.RuntimeParameters() left = sl.Mat() ptcloud = sl.Mat() depth_img = sl.Mat() depth = sl.Mat() res = sl.Resolution(1280, 720) py_transform = sl.Transform( ) # First create a Transform object for TrackingParameters object tracking_parameters = sl.PositionalTrackingParameters( init_pos=py_transform) tracking_parameters.set_as_static = True err = cap.enable_positional_tracking(tracking_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) running = True keep_people_only = True if coco_demo.cfg.MODEL.MASK_ON: print("Mask enabled!") if coco_demo.cfg.MODEL.KEYPOINT_ON: print("Keypoints enabled!") while running: start_time = time.time() err_code = cap.grab(runtime) if err_code != sl.ERROR_CODE.SUCCESS: break cap.retrieve_image(left, sl.VIEW.LEFT, resolution=res) cap.retrieve_image(depth_img, sl.VIEW.DEPTH, resolution=res) cap.retrieve_measure(depth, sl.MEASURE.DEPTH, resolution=res) cap.retrieve_measure(ptcloud, sl.MEASURE.XYZ, resolution=res) ptcloud_np = np.array(ptcloud.get_data()) img = cv2.cvtColor(left.get_data(), cv2.COLOR_RGBA2RGB) prediction = coco_demo.select_top_predictions( coco_demo.compute_prediction(img)) # Keep people only if keep_people_only: labels_tmp = prediction.get_field("labels") people_coco_label = 1 keep = torch.nonzero(labels_tmp == people_coco_label).squeeze(1) prediction = prediction[keep] composite = img.copy() humans_3d = None masks_3d = None if coco_demo.show_mask_heatmaps: composite = coco_demo.create_mask_montage(composite, prediction) composite = coco_demo.overlay_boxes(composite, prediction) if coco_demo.cfg.MODEL.MASK_ON: masks_3d = get_masks3d(prediction, depth) composite = coco_demo.overlay_mask(composite, prediction) if coco_demo.cfg.MODEL.KEYPOINT_ON: # Extract 3D skeleton from the ZED depth humans_3d = get_humans3d(prediction, ptcloud_np) composite = coco_demo.overlay_keypoints(composite, prediction) if True: overlay_distances(prediction, get_boxes3d(prediction, ptcloud_np), composite, humans_3d, masks_3d) composite = coco_demo.overlay_class_names(composite, prediction) print(" Time: {:.2f} s".format(time.time() - start_time)) if display: cv2.imshow("COCO detections", composite) cv2.imshow("ZED Depth", depth_img.get_data()) key = cv2.waitKey(10) if key == 27: break # esc to quit
def main(): # Create a Camera object zed = sl.Camera() # Set configuration parameters init_params = sl.InitParameters() init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_MEDIUM init_params.coordinate_units = sl.UNIT.UNIT_METER #init_params.depth_minimum_distance = 0.3 # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: print("Error opening camera") print(err) exit(1) runtime_parameters = sl.RuntimeParameters() runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_FILL rows = 720 cols = 1280 M = np.float32([[1, 0, 300], [0, 1, 0]]) M2 = np.float32([[1, 0, -300], [0, 1, 0]]) #Capture an ideal image depth_image = sl.Mat() zed.retrieve_image(depth_image, sl.VIEW.VIEW_DEPTH) idealDepthImage = cv2.resize(depth_image.get_data(), (0, 0), fx=0.5, fy=0.5) try: while True: depth_image = sl.Mat() depth_map = sl.Mat() rgb_left_image = sl.Mat() rgb_image = sl.Mat() if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: zed.retrieve_image(depth_image, sl.VIEW.VIEW_DEPTH) zed.retrieve_image(rgb_image, sl.VIEW.VIEW_LEFT) zed.retrieve_measure(depth_map, sl.MEASURE.MEASURE_DEPTH) hsv_rgb = cv2.cvtColor(rgb_image.get_data(), cv2.COLOR_BGR2HSV) hsv_rgb = cv2.resize(hsv_rgb, (0, 0), fx=0.5, fy=0.5) gray_rgb = cv2.cvtColor(rgb_image.get_data(), cv2.COLOR_BGR2GRAY) gray_rgb = cv2.resize(gray_rgb, (0, 0), fx=0.5, fy=0.5) hsvmean = cv2.mean(hsv_rgb)[2] resizedDepthImage = cv2.resize(depth_image.get_data(), (0, 0), fx=0.5, fy=0.5) #Compare to ideal image sub = idealDepthImage - resizedDepthImage sub = abs(sub) threshold = 200 sub[sub >= threshold] = 255 sub[sub < threshold] = 0 mean = cv2.mean(resizedDepthImage)[0] min_threshold = 15 max_threshold = 50 edges = cv2.Canny(resizedDepthImage, min_threshold, max_threshold) rgb = cv2.cvtColor(edges, cv2.COLOR_GRAY2RGB) rgb *= np.array((0, 0, 1), np.uint8) depthImageConvertedToColor = cv2.cvtColor( resizedDepthImage, cv2.COLOR_BGRA2BGR) out = np.bitwise_or(depthImageConvertedToColor, rgb) cv2.imshow('Zed2', depth_image.get_data()) cv2.waitKey(1) except Exception as e: print(e) zed.close() exit(1)
def main(): print("Running Spatial Mapping sample ... Press 'q' to quit") # Create a Camera object zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode init_params.coordinate_units = sl.UNIT.METER # Set coordinate units init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP # OpenGL coordinates # If applicable, use the SVO given as parameter # Otherwise use ZED live stream if len(sys.argv) == 2: filepath = sys.argv[1] print("Using SVO file: {0}".format(filepath)) init_params.set_from_svo_file(filepath) # Open the camera status = zed.open(init_params) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() # Get camera parameters camera_parameters = zed.get_camera_information( ).camera_configuration.calibration_parameters.left_cam pymesh = sl.Mesh() # Current incremental mesh image = sl.Mat() # Left image from camera pose = sl.Pose() # Camera pose tracking data viewer = gl.GLViewer() viewer.init(camera_parameters, pymesh) spatial_mapping_parameters = sl.SpatialMappingParameters() tracking_state = sl.POSITIONAL_TRACKING_STATE.OFF mapping_state = sl.SPATIAL_MAPPING_STATE.NOT_ENABLED mapping_activated = False last_call = time.time() # Timestamp of last mesh request # Enable positional tracking err = zed.enable_positional_tracking() if err != sl.ERROR_CODE.SUCCESS: print(repr(err)) exit() # Set runtime parameters runtime = sl.RuntimeParameters() while viewer.is_available(): # Grab an image, a RuntimeParameters object must be given to grab() if zed.grab(runtime) == sl.ERROR_CODE.SUCCESS: # Retrieve left image zed.retrieve_image(image, sl.VIEW.LEFT) # Update pose data (used for projection of the mesh over the current image) tracking_state = zed.get_position(pose) if mapping_activated: mapping_state = zed.get_spatial_mapping_state() # Compute elapsed time since the last call of Camera.request_spatial_map_async() duration = time.time() - last_call # Ask for a mesh update if 500ms elapsed since last request if (duration > .5 and viewer.chunks_updated()): zed.request_spatial_map_async() last_call = time.time() if zed.get_spatial_map_request_status_async( ) == sl.ERROR_CODE.SUCCESS: zed.retrieve_spatial_map_async(pymesh) viewer.update_chunks() change_state = viewer.update_view(image, pose.pose_data(), tracking_state, mapping_state) if change_state: if not mapping_activated: init_pose = sl.Transform() zed.reset_positional_tracking(init_pose) # Configure spatial mapping parameters spatial_mapping_parameters.resolution_meter = sl.SpatialMappingParameters( ).get_resolution_preset(sl.MAPPING_RESOLUTION.MEDIUM) spatial_mapping_parameters.use_chunk_only = True spatial_mapping_parameters.save_texture = False # Set to True to apply texture over the created mesh spatial_mapping_parameters.map_type = sl.SPATIAL_MAP_TYPE.MESH # Enable spatial mapping zed.enable_spatial_mapping() # Clear previous mesh data pymesh.clear() viewer.clear_current_mesh() # Start timer last_call = time.time() mapping_activated = True else: # Extract whole mesh zed.extract_whole_spatial_map(pymesh) filter_params = sl.MeshFilterParameters() filter_params.set(sl.MESH_FILTER.MEDIUM) # Filter the extracted mesh pymesh.filter(filter_params, True) viewer.clear_current_mesh() # If textures have been saved during spatial mapping, apply them to the mesh if (spatial_mapping_parameters.save_texture): print("Save texture set to : {}".format( spatial_mapping_parameters.save_texture)) pymesh.apply_texture(sl.MESH_TEXTURE_FORMAT.RGBA) # Save mesh as an obj file filepath = "mesh_gen.obj" status = pymesh.save(filepath) if status: print("Mesh saved under " + filepath) else: print("Failed to save the mesh under " + filepath) mapping_state = sl.SPATIAL_MAPPING_STATE.NOT_ENABLED mapping_activated = False image.free(memory_type=sl.MEM.CPU) pymesh.clear() # Disable modules and close camera zed.disable_spatial_mapping() zed.disable_positional_tracking() zed.close()
def detector(): # Initialize publisher ROS node pub = rospy.Publisher('predictions', Predictions, queue_size=10) pub1 = rospy.Publisher('peoplecount', Peoplecount, queue_size=10) pub2 = rospy.Publisher('videostream', Image, queue_size=1) rospy.init_node('detector', anonymous=True) # Ceate sort object mot_tracker = Sort() # Define paths for yolo files darknet_path = '/home/ubuntu/catkin_ws/src/jetsontx1_cvmodule/src/pyyolo/darknet' # Only './darknet' is dependent on location of rosrun command datacfg = 'cfg/coco.data' cfgfile = 'cfg/yolov3-tiny.cfg' weightfile = '../yolov3-tiny.weights' #'/media/ubuntu/SDcard/yoloWeights/yolov2-tiny.weights' this also works but it loads way slower filename = darknet_path + '/data/person.jpg' # Image resolution parameters (width, height) = (1280, 720 ) # Use (672,376) for VGA and (1280,720) for HD720 resolution # Initialize visualization fourcc = cv2.VideoWriter_fourcc(*'MJPG') video = cv2.VideoWriter('predictionstest.avi', fourcc, 10, (width, height)) # Initialize pyyolo pyyolo.init(darknet_path, datacfg, cfgfile, weightfile) # Initialize face detector face_cascade = cv2.CascadeClassifier( '/usr/share/OpenCV/haarcascades/haarcascade_frontalface_default.xml') smile_cascade = cv2.CascadeClassifier( '/usr/share/OpenCV/haarcascades/haarcascade_smile.xml') # Create a Camera object zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD1080, HD720 or VGA video mode init_params.camera_fps = 15 # Set fps at 30 # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) zed.set_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE, 50) image = sl.Mat() point_cloud = sl.Mat() colours = 255 * np.random.rand(32, 3) # For drawing different colours on BB runtime_parameters = sl.RuntimeParameters() while not rospy.is_shutdown(): start = time.time() # Grab an image, a RuntimeParameters object must be given to grab() if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # A new image is available if grab() returns SUCCESS zed.retrieve_image(image, sl.VIEW.LEFT) data = image.get_data() gray_picture = cv2.cvtColor( data, cv2.COLOR_BGR2GRAY ) # Make picture gray for face/smile detection # Retrieve colored point cloud. Point cloud is aligned on the left image. zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA) Data = data.transpose(2, 0, 1) start5 = time.time() Data = Data.ravel() / 255.0 end5 = time.time() Data = np.ascontiguousarray(Data, dtype=np.float32) start1 = time.time() outputs = pyyolo.detect( width, height, 4, Data, 0.5, 0.8 ) #pyyolo.detect returns: [{'right':, 'bottom':, 'top':, 'class':, 'prob':, 'left':}] end1 = time.time() print("Section cycle time: ", end1 - start1) dets = np.empty((0, 5), int) count = 0 for output in outputs: if output['class'] == 'person': #track only people count = count + 1 # Count detected people dets = np.append(dets, [[ output['left'], output['top'], output['right'], output['bottom'], output['prob'] ]], axis=0) people = Peoplecount() people.tot_detected_people = count pub1.publish(people) preds = Predictions() trackers = mot_tracker.update( dets ) #update tracking ID. mot_tracker.update() returns: [[x1,y1,x2,y2,id]] for d in trackers: d = d.astype(np.int32) # Create a dictionary to store info for publishing detectinfo = { 'left': d[0], 'top': d[1], 'right': d[2], 'bottom': d[3], 'class': 'person', 'ID': int(d[4]) } euclidean_distance(detectinfo, point_cloud) relative_coordinates(detectinfo, width) start3 = time.time() facesmile_detect(detectinfo, gray_picture, data, face_cascade, smile_cascade) end3 = time.time() #print("facesmile detect cycle time: ", end3 - start3) #print detectinfo['smile'] pred = Prediction() #pred.probabilities.append() pred.classes.append(detectinfo['class']) pred.xmin = detectinfo['left'] pred.ymin = detectinfo['top'] pred.xmax = detectinfo['right'] pred.ymax = detectinfo['bottom'] pred.id = detectinfo['ID'] pred.face = detectinfo['face'] pred.smile = detectinfo['smile'] pred.distance = detectinfo['distance'] pred.angle = detectinfo['angle'] pred.xcoord = detectinfo['x'] pred.ycoord = detectinfo['y'] preds.predictions.append(pred) label = detectinfo['class'] + " " + str(detectinfo['ID']) cv2.rectangle(data, (d[0], d[1]), (d[2], d[3]), (colours[d[4] % 32, 0], colours[d[4] % 32, 1], colours[d[4] % 32, 2]), 2) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(data, label, (d[2], d[1] + 25), font, 1, (0, 0, 255), 1, cv2.LINE_AA) pub.publish(preds) msg_frame = CvBridge().cv2_to_imgmsg(data, "8UC4") pub2.publish(msg_frame) #video.write(data[:,:,:3])#because data.shape is (376,672,4) and it only supports 3 channels. cv2.imshow("image", data) cv2.waitKey(35) end = time.time() print("Total cycle time: ", end - start) # Close the camera zed.close()
def main() : init = sl.InitParameters() init.coordinate_units = sl.UNIT.UNIT_METER cam = sl.Camera() status = cam.open(init) runtime = sl.RuntimeParameters() runtime.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD mat1 = sl.Mat() mat2 = sl.Mat() # if len(sys.argv) == 1 : # print('Please provide ZED serial number') # exit(1) # # Open the ZED camera # cap = cv2.VideoCapture(0) # if cap.isOpened() == 0: # exit(-1) # image_size = Resolution() # image_size.width = 1280 # image_size.height = 720 # # Set the video resolution to HD720 # cap.set(cv2.CAP_PROP_FRAME_WIDTH, image_size.width*2) # cap.set(cv2.CAP_PROP_FRAME_HEIGHT, image_size.height) # serial_number = int(sys.argv[1]) # calibration_file = download_calibration_file(serial_number) # if calibration_file == "": # exit(1) # print("Calibration file found. Loading...") # camera_matrix_left, camera_matrix_right, map_left_x, map_left_y, map_right_x, map_right_y = init_calibration(calibration_file, image_size) while True : # Get a new frame from camera #retval, frame = cap.read() # Extract left and right images from side-by-side #left_right_image = np.split(frame, 2, axis=1) # Display images err = cam.grab(runtime) cam.retrieve_image(mat1, sl.VIEW.VIEW_LEFT) cam.retrieve_image(mat2, sl.VIEW.VIEW_RIGHT) left_rect = mat1.get_data() right_rect = mat2.get_data() #cv2.imshow("left RAW", left_right_image[0]) #left_rect = cv2.remap(left_right_image[0], map_left_x, map_left_y, interpolation=cv2.INTER_LINEAR) #right_rect = cv2.remap(left_right_image[1], map_right_x, map_right_y, interpolation=cv2.INTER_LINEAR) cv2.imshow("left RECT", left_rect) cv2.imshow("right RECT", right_rect) stereo = cv2.StereoSGBM_create(minDisparity = 1, numDisparities = 128, blockSize = 4, uniquenessRatio = 1, speckleRange = 3, speckleWindowSize = 8, disp12MaxDiff = 200, P1 = 600, P2 = 2400 # SADWindowSize = 6, # uniquenessRatio = 10, # speckleWindowSize = 100, # speckleRange = 32, # disp12MaxDiff = 1, # P1 = 8*3*3**2, # P2 = 32*3*3**2, # fullDP = False ) disp = stereo.compute(left_rect, right_rect).astype(np.float32) / 16.0 cv2.imshow("disparity", (disp-1)/(128)) if cv2.waitKey(30) >= 0 : break exit(0)
if __name__ == "__main__": print("grab Depth Sensing image... Press 'Esc' to quit") init = sl.InitParameters(camera_resolution=sl.RESOLUTION.HD2K, depth_mode=sl.DEPTH_MODE.ULTRA, coordinate_units=sl.UNIT.METER, coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP) zed = sl.Camera() status = zed.open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() res = sl.Resolution() res.width = 2208 # 720 res.height = 1242 # 404 point_cloud = sl.Mat(res.width, res.height, sl.MAT_TYPE.F32_C4, sl.MEM.CPU) image = sl.Mat() if zed.grab() == sl.ERROR_CODE.SUCCESS: # zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, res) # # point_cloud.write('C:/Users/user/Desktop/python/b') # # point_cloud_np = point_cloud.get_data() # cutZed(point_cloud_np) zed.retrieve_image(image, sl.VIEW.RIGHT) # Retrieve the left image image.write('C:/Users/user/Desktop/python/d.png') plt.imshow(image.get_data()) zed.close()
ZED_SVO_PATH = os.path.join(DIR, ZED_SVO) framerate = 20 resolution = (1280, 720) fourcc = cv2.VideoWriter_fourcc(*'XVID') colorVideoOut = cv2.VideoWriter('color.avi', fourcc, framerate, resolution) depthVideoOut = cv2.VideoWriter('depth.avi', fourcc, framerate, resolution) init_parameters = sl.InitParameters() init_parameters.set_from_svo_file(ZED_SVO_PATH) zed = sl.Camera() err = zed.open(init_parameters) runtime = sl.RuntimeParameters() svo_image = sl.Mat(resolution[1], resolution[0], sl.MAT_TYPE.U8_C4) depth_map = sl.Mat(resolution[1], resolution[0], sl.MAT_TYPE.U8_C4) i = 0 for i in tqdm(range(zed.get_svo_number_of_frames())): if zed.grab(runtime) == sl.ERROR_CODE.SUCCESS: i+=1 zed.retrieve_image(svo_image, sl.VIEW.LEFT, sl.MEM.CPU) zed.retrieve_image(depth_map, sl.VIEW.DEPTH, sl.MEM.CPU) colorNumpy = svo_image.get_data() colorNumpy = cv2.resize(colorNumpy, resolution, cv2.INTER_AREA) colorNumpy = cv2.cvtColor(colorNumpy, cv2.COLOR_RGBA2RGB) colorVideoOut.write(colorNumpy) depthNumpy = depth_map.get_data() depthNumpy = cv2.resize(depthNumpy, resolution, cv2.INTER_AREA) depthNumpy = cv2.cvtColor(depthNumpy, cv2.COLOR_BGR2RGB)
def main(argv): thresh = 0.25 darknet_path = "../darknet/" # configPath = darknet_path + "cfg/yolov3.cfg" # weightPath = "yolov3.weights" # metaPath = "coco.data" V = 3.2 if (V == 1): configPath = darknet_path + "cfg/yolov3-gripper.cfg" weightPath = "yolov3-gripper_final.weights" metaPath = "gripper.data" elif (V == 3): configPath = darknet_path + "cfg/yolov3-gripper_3.cfg" weightPath = "yolov3-gripper_3_final.weights" metaPath = "gripper_3.data" elif (V == 3.2): configPath = darknet_path + "cfg/yolov3-gripper_3_2_verified.cfg" weightPath = "yolov3-gripper_3_2_verified_final.weights" metaPath = "gripper_3.data" else: configPath = darknet_path + "cfg/yolov3-gripper.cfg" weightPath = "yolov3-gripper_final.weights" metaPath = "gripper.data" #configPath = darknet_path + "cfg/yolov3-gripper.cfg" # weightPath = "yolov3-gripper_final.weights" # weightPath = "yolov3-gripper_3_final.weights" # metaPath = "gripper_3.data" #metaPath = "gripper.data" svoPath = None help_str = 'darknet_zed.py -c <config> -w <weight> -m <meta> -t <threshold> -s <svo_file>' try: opts, args = getopt.getopt( argv, "hc:w:m:t:s:", ["config=", "weight=", "meta=", "threshold=", "svo_file="]) except getopt.GetoptError: print(help_str) sys.exit(2) for opt, arg in opts: if opt == '-h': print(help_str) sys.exit() elif opt in ("-c", "--config"): configPath = arg elif opt in ("-w", "--weight"): weightPath = arg elif opt in ("-m", "--meta"): metaPath = arg elif opt in ("-t", "--threshold"): thresh = float(arg) elif opt in ("-s", "--svo_file"): svoPath = arg init = sl.InitParameters() init.coordinate_units = sl.UNIT.UNIT_METER if svoPath is not None: init.svo_input_filename = svoPath cam = sl.Camera() if not cam.is_opened(): print("Opening ZED Camera...") status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() runtime = sl.RuntimeParameters() # Use STANDARD sensing mode runtime.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD mat = sl.Mat() point_cloud_mat = sl.Mat() # Import the global variables. This lets us instance Darknet once, then just call performDetect() again without instancing again global metaMain, netMain, altNames # pylint: disable=W0603 assert 0 < thresh < 1, "Threshold should be a float between zero and one (non-inclusive)" if not os.path.exists(configPath): raise ValueError("Invalid config path `" + os.path.abspath(configPath) + "`") if not os.path.exists(weightPath): raise ValueError("Invalid weight path `" + os.path.abspath(weightPath) + "`") if not os.path.exists(metaPath): raise ValueError("Invalid data file path `" + os.path.abspath(metaPath) + "`") if netMain is None: netMain = load_net_custom(configPath.encode("ascii"), weightPath.encode("ascii"), 0, 1) # batch size = 1 if metaMain is None: metaMain = load_meta(metaPath.encode("ascii")) if altNames is None: # In thon 3, the metafile default access craps out on Windows (but not Linux) # Read the names file and create a list to feed to detect try: with open(metaPath) as metaFH: metaContents = metaFH.read() import re match = re.search("names *= *(.*)$", metaContents, re.IGNORECASE | re.MULTILINE) if match: result = match.group(1) else: result = None try: if os.path.exists(result): with open(result) as namesFH: namesList = namesFH.read().strip().split("\n") altNames = [x.strip() for x in namesList] except TypeError: pass except Exception: pass color_array = generateColor(metaPath) print("Running...") ##CYW_RemoteControl db = CYW_RemoteControl.global_database() operator = CYW_RemoteControl.operator(db) # operator.send_server_start() #no auto send. use manual publish operator.receive_server_start() ct = CYW_RemoteControl.controller(db, operator) pickup_times = -1 first_loop = True key = '' while key != 113: # for 'q' key err = cam.grab(runtime) if err == sl.ERROR_CODE.SUCCESS: cam.retrieve_image(mat, sl.VIEW.VIEW_LEFT) image = mat.get_data() cam.retrieve_measure(point_cloud_mat, sl.MEASURE.MEASURE_XYZRGBA) depth = point_cloud_mat.get_data() # Do the detection detections = detect(netMain, metaMain, image, thresh) # print(chr(27) + "[2J"+"**** " + # str(len(detections)) + " Results ****") for detection in detections: label = detection[0] # if(label not in show_lables): # break confidence = detection[1] # if(confidence<0.85): # continue pstring = label + ": " + str(np.rint(100 * confidence)) + "%" print(pstring) bounds = detection[2] yExtent = int(bounds[3]) xEntent = int(bounds[2]) # Coordinates are around the center xCoord = int(bounds[0] - bounds[2] / 2) yCoord = int(bounds[1] - bounds[3] / 2) boundingBox = [[xCoord, yCoord], [xCoord, yCoord + yExtent], [xCoord + xEntent, yCoord + yExtent], [xCoord + xEntent, yCoord]] thickness = 1 x, y, z = getObjectDepth(depth, bounds) if (label == "iiwa_gripper"): # db.cam_iiwa_x,db.cam_iiwa_y,db.cam_iiwa_z=x,y+0.05,z+0.09 db.cam_iiwa_x, db.cam_iiwa_y, db.cam_iiwa_z = x + 0.02, y, z - 0.03 db.cam_iiwa_updated = 1 elif (label == "cube"): db.cam_cube_x, db.cam_cube_y, db.cam_cube_z = x, y, z db.cam_cube_updated = 1 distance = math.sqrt(x * x + y * y + z * z) distance = "{:.2f}".format(distance) x = "{:.2f}".format(x) y = "{:.2f}".format(y) z = "{:.2f}".format(z) cv2.rectangle(image, (xCoord - thickness, yCoord - thickness), (xCoord + xEntent + thickness, yCoord + (18 + thickness * 4)), color_array[detection[3]], -1) cv2.putText( image, label + " " + "{:.2f}".format(confidence) + " " + (str(distance) + " m") + (" x=" + str(x)) + (" y=" + str(y)) + (" z=" + str(z)), (xCoord + (thickness * 4), yCoord + (10 + thickness * 4)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) cv2.rectangle(image, (xCoord - thickness, yCoord - thickness), (xCoord + xEntent + thickness, yCoord + yExtent + thickness), color_array[detection[3]], int(thickness * 2)) #control part # print(db.cam_cube_x,db.cam_cube_y,db.cam_cube_z) # print(db.cam_iiwa_x,db.cam_iiwa_y,db.cam_iiwa_z) if (db.cam_cube_updated == 1 and db.cam_iiwa_updated == 1 and db.blocked == 0): cube_z_iiwaBse_add = 0.3 pos_error_cam = [ -(db.cam_iiwa_x - db.cam_cube_x), -(db.cam_iiwa_y - db.cam_cube_y - cube_z_iiwaBse_add), -(db.cam_iiwa_z - db.cam_cube_z) ] pos_error_iiwaBase = db.coordinate_cam_2_iiwaBase( pos_error_cam) error_allow_list = [0.02, 0.02, 0.8] #m if (funcs.check_xyz_error_ok(error_list=pos_error_iiwaBase, allow_list=error_allow_list) ): #at the correct place, ready to pick up main_thread = threading.Thread( target=ct.do_go_to_xy_then_pick, args=(db.r_iiwa_x, db.r_iiwa_y)) main_thread.start() pass print("pos_error_cam", pos_error_cam) print("pos_error_iiwaBase", pos_error_iiwaBase) # db.iiwa_x_sp=db.r_iiwa_x+pos_error_iiwaBase[0]*100 # db.iiwa_y_sp=db.r_iiwa_y+pos_error_iiwaBase[1]*100 # if(pickup_times==0 and first_loop==False): # db.iiwa_x_sp = db.r_iiwa_x + pos_error_iiwaBase[0] * 100 # db.iiwa_y_sp = db.r_iiwa_y + pos_error_iiwaBase[1] * 100 # ct.do_go_to_xy_then_pick(x=db.r_iiwa_x+pos_error_iiwaBase[0]*10,y=db.r_iiwa_y+pos_error_iiwaBase[1]*10) # pickup_times=pickup_times+1 # else: # print("already pick up") # ct.pos_pid_control(pos_error_iiwaBase[0],pos_error_iiwaBase[1],pos_error_iiwaBase[2],P_gain=20,I_gain=0,D_gain=0) ct.pos_pid_control(dx=pos_error_iiwaBase[0], dy=pos_error_iiwaBase[1], dz=0, P_gain=160, I_gain=0, D_gain=0) #for safety, no z control. operator.publish() cv2.imshow("ZED", image) key = cv2.waitKey(5) first_loop = False else: key = cv2.waitKey(5) cv2.destroyAllWindows() cam.close() print("\nFINISH")
def main(): args = cli() # load model model, _ = nets.factory_from_args(args) model = model.to(args.device) processor = decoder.factory_from_args(args, model) # zed init = sl.InitParameters() init.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_ULTRA init.coordinate_units = sl.UNIT.UNIT_METER init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP cam = sl.Camera() status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() runtime_parameters = sl.RuntimeParameters() runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD # Use STANDARD sensing mode img = sl.Mat() depth = sl.Mat() point_cloud = sl.Mat() last_loop = time.time() #capture = cv2.VideoCapture(args.source) visualizer = None while True: err = cam.grab(runtime_parameters) if err == sl.ERROR_CODE.SUCCESS: # Retrieve left image cam.retrieve_image(img, sl.VIEW.VIEW_LEFT) # Retrieve depth map. Depth is aligned on the left image cam.retrieve_measure(depth, sl.MEASURE.MEASURE_DEPTH) # Retrieve colored point cloud. Point cloud is aligned on the left image. cam.retrieve_measure(point_cloud, sl.MEASURE.MEASURE_XYZRGBA) # Get and print distance value in mm at the center of the image # We measure the distance camera - object using Euclidean distance x = round(img.get_width() / 2) y = round(img.get_height() / 2) err, point_cloud_value = point_cloud.get_value(x, y) err, depth_value = depth.get_value(x, y) print("depth ", depth_value) distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] + point_cloud_value[1] * point_cloud_value[1] + point_cloud_value[2] * point_cloud_value[2]) if not np.isnan(distance) and not np.isinf(distance): distance = round(distance) #print("Distance to Camera at ({0}, {1}): {2} mm\n".format(x, y, distance)) else: print( "Can't estimate distance at this position, move the camera\n" ) cv2.imshow("Depth", depth.get_data()) else: print("Err", err) continue image = cv2.resize(img.get_data(), None, fx=args.scale, fy=args.scale) #print('resized image size: {}'.format(image.shape)) image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) if visualizer is None: visualizer = Visualizer(processor, args)(image) visualizer.send(None) start = time.time() image_pil = PIL.Image.fromarray(image) processed_image_cpu, _, __ = transforms.EVAL_TRANSFORM( image_pil, [], None) processed_image = processed_image_cpu.contiguous().to( args.device, non_blocking=True) #print('preprocessing time', time.time() - start) fields = processor.fields(torch.unsqueeze(processed_image, 0))[0] visualizer.send((image, fields)) #print('loop time = {:.3}s, FPS = {:.3}'.format( # time.time() - last_loop, 1.0 / (time.time() - last_loop))) last_loop = time.time() cam.close()
def main(): print("Running...") time_stamp = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d-%H:%M:%S') init = sl.InitParameters() init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD1080 init.camera_linux_id = 0 init.camera_fps = 10 # The framerate is lowered to avoid any USB3 bandwidth issues cam = sl.Camera() if not cam.is_opened(): print("Opening ZED Camera 1...") status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: print('Camera 1: ' + repr(status)) exit() # init.camera_linux_id = 1 # selection of the ZED ID # cam2 = sl.Camera() # if not cam2.is_opened(): # print("Opening ZED Camera 2...") # status = cam2.open(init) # if status != sl.ERROR_CODE.SUCCESS: # print('Camera 2: ' + repr(status)) # exit() runtime = sl.RuntimeParameters() mat = sl.Mat() # mat2 = sl.Mat() print_camera_information(cam) # print_camera_information(cam2) fourcc = cv2.VideoWriter_fourcc(*'MJPG') out_c1 = None out_c2 = None while True: # Grab an image, a RuntimeParameters object must be given to grab() if cam.grab( runtime ) == sl.ERROR_CODE.SUCCESS: # and cam2.grab(runtime) == sl.ERROR_CODE.SUCCESS: # A new image is available if grab() returns SUCCESS cam.retrieve_image(mat, sl.VIEW.VIEW_LEFT) # cam2.retrieve_image(mat2, sl.VIEW.VIEW_LEFT) if out_c1 is None: # or out_c2 is None: print("Saving the recording at: {}".format(PATH + time_stamp + '_cam1.avi')) out_c1 = cv2.VideoWriter( PATH + time_stamp + '_cam1.avi', fourcc, 20.0, (int(mat.get_width()), int(mat.get_height()))) # out_c2 = cv2.VideoWriter(time_stamp + '_cam2.avi', fourcc, 20.0, (int(mat2.get_width()), # int(mat2.get_height()))) c1_frame = mat.get_data() # c2_frame = mat2.get_data() # Save the frames as a video if out_c1 is not None: out_c1.write(c1_frame[:, :, :3]) # out_c2.write(c2_frame) # Display the image frames cv2.imshow('Camera 1 - Left', c1_frame) # cv2.imshow('Camera 2 - Left', c2_frame) if cv2.waitKey(1) & 0xFF == ord('q'): break cam.close() # cam2.close() out_c1.release() # out_c2.release() cv2.destroyAllWindows() print("\nFINISH")
def main(): cam = sl.Camera() init = sl.InitParameters() init.depth_mode = sl.DEPTH_MODE.ULTRA init.coordinate_units = sl.UNIT.METER init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP if len(sys.argv) == 2: filepath = sys.argv[1] print("Reading SVO file: {0}".format(filepath)) init.set_from_svo_file(filepath) status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit(1) runtime = sl.RuntimeParameters() runtime.sensing_mode = sl.SENSING_MODE.STANDARD runtime.measure3D_reference_frame = sl.REFERENCE_FRAME.WORLD spatial = sl.SpatialMappingParameters() transform = sl.Transform() tracking = sl.PositionalTrackingParameters(transform) cam.enable_positional_tracking(tracking) pymesh = sl.Mesh() pyplane = sl.Plane() reset_tracking_floor_frame = sl.Transform() found = 0 print("Processing...") i = 0 calibration_params = cam.get_camera_information().calibration_parameters # Focal length of the left eye in pixels focal_left_x = calibration_params.left_cam.fx focal_left_y = calibration_params.left_cam.fy # First radial distortion coefficient # k1 = calibration_params.left_cam.disto[0] # Translation between left and right eye on z-axis # tz = calibration_params.T.z # Horizontal field of view of the left eye in degrees # h_fov = calibration_params.left_cam.h_fov print("fx", focal_left_x) print("fy", focal_left_y) print("cx", calibration_params.left_cam.cx) print("cy", calibration_params.left_cam.cy) print("distortion", calibration_params.left_cam.disto) cloud = sl.Mat() while i < 1000: if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS : err = cam.find_floor_plane(pyplane, reset_tracking_floor_frame) if err == sl.ERROR_CODE.SUCCESS: # if i > 200 and err == sl.ERROR_CODE.SUCCESS: found = 1 print('Floor found!') pymesh = pyplane.extract_mesh() #get gound plane info print("floor normal",pyplane.get_normal()) #print normal of plane print("floor equation", pyplane.get_plane_equation()) print("plane center", pyplane.get_center()) print("plane pose", pyplane.get_pose()) # camera_pose = sl.Pose() # py_translation = sl.Translation() # tracking_state = cam.get_position(camera_pose) # # if tracking_state == sl.POSITIONAL_TRACKING_STATE.OK: # rotation = camera_pose.get_rotation_vector() # rx = round(rotation[0], 2) # ry = round(rotation[1], 2) # rz = round(rotation[2], 2) # translation = camera_pose.get_translation(py_translation) # tx = round(translation.get()[0], 2) # ty = round(translation.get()[1], 2) # tz = round(translation.get()[2], 2) # text_translation = str((tx, ty, tz)) # text_rotation = str((rx, ry, rz)) # pose_data = camera_pose.pose_data(sl.Transform()) # print("translation",text_translation) # print("rotation",text_rotation) cam.retrieve_measure(cloud, sl.MEASURE.XYZRGBA) break i+=1 if found == 0: print('Floor was not found, please try with another SVO.') cam.close() exit(0) #load image path = "/home/hcl/Documents/ZED/pics/Explorer_HD1080_SN14932_16-24-06.png" im = cv2.imread(path) h, w = im.shape[0], im.shape[1] print("orignal image shape", h,w) im = im[:,:int(w/2),:] #get left image # im = cv2.resize(im,(int(im.shape[1]/2),int(im.shape[0]/2))) #warped img warped = np.zeros((h,w,3)) roll = -80 roll *= np.pi/180 Rx = np.matrix([ [1, 0,0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)] ]) #point cloud # i = 500 # j = 355 if False: point_cloud = sl.Mat() cam.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA) for i in range(100): for j in range(1000): point3D = point_cloud.get_value(i,j) # x = point3D[0] # y = point3D[1] # z = point3D[2] color = point3D[1][3] if ~np.isnan(color) and ~np.isinf(color): xyz = point3D[1][:3].reshape((-1,1)) # xyz_hom = np.vstack((xyz,1)) print('point3D',point3D) warped_index = Rx @ xyz #NEED TO USE H**O COORDINATES? divide by last column? print('warped_index',warped_index) RGB = im[j,i,:] RGB = np.dstack(([255],[255],[255])) print('RGB',RGB) warped[int(np.round(warped_index[0]+10)),int(np.round(warped_index[1])),:] = RGB cam.disable_positional_tracking() # save_mesh(pymesh) cam.close() print("\nFINISH")
def capture_thread_func(): global image_np_global, depth_np_global, exit_signal, new_data zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP init_params.camera_fps = 60 init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE init_params.coordinate_units = sl.UNIT.UNIT_METER init_params.svo_real_time_mode = False # Open the camera err = zed.open(init_params) tracking_parameters = sl.TrackingParameters(sl.Transform()) tracking_parameters.enable_spatial_memory = True err = zed.enable_tracking(tracking_parameters) while err != sl.ERROR_CODE.SUCCESS: err = zed.open(init_params) print(err) sleep(1) image_mat = sl.Mat() depth_mat = sl.Mat() runtime_parameters = sl.RuntimeParameters() while not exit_signal: if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: zed.retrieve_image(image_mat, sl.VIEW.VIEW_LEFT, width=width, height=height) zed.retrieve_measure(depth_mat, sl.MEASURE.MEASURE_XYZRGBA, width=width, height=height) lock.acquire() image_np_global = load_image_into_numpy_array(image_mat) depth_np_global = load_depth_into_numpy_array(depth_mat) new_data = True lock.release() # For spatial tracking tracking_state = zed.get_position( zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD) if tracking_state == sl.TRACKING_STATE.TRACKING_STATE_OK: # Getting spatial position py_translation = sl.Translation() tx = round( zed_pose.get_translation(py_translation).get()[0], 2) ty = round( zed_pose.get_translation(py_translation).get()[1], 2) tz = round( zed_pose.get_translation(py_translation).get()[2], 2) # Getting spatial orientation py_orientation = sl.Orientation() ox = round( zed_pose.get_orientation(py_orientation).get()[0], 2) oy = round( zed_pose.get_orientation(py_orientation).get()[1], 2) oz = round( zed_pose.get_orientation(py_orientation).get()[2], 2) ow = round( zed_pose.get_orientation(py_orientation).get()[3], 2) # Getting spatial orientation rotation = zed_pose.get_rotation_vector() rx = round(rotation[0], 2) ry = round(rotation[1], 2) rz = round(rotation[2], 2) rx *= (180 / math.pi) ry *= (180 / math.pi) rz *= (180 / math.pi) rx = round(rx, 2) ry = round(ry, 2) rz = round(rz, 2) # Storing some position data voi.coord = [tx, ty, tz] voi.rotation = [rx, ry, rz] voi.orientation = [ox, oy, oz, ow] sleep(0.01) zed.close()
zed.enable_positional_tracking() zed.enable_object_detection(obj_param) camera_info = zed.get_camera_information() # Create OpenGL viewer viewer = gl.GLViewer() viewer.init(camera_info.calibration_parameters.left_cam) # Configure object detection runtime parameters obj_runtime_param = sl.ObjectDetectionRuntimeParameters() obj_runtime_param.detection_confidence_threshold = 50 # Create ZED objects filled in the main loop objects = sl.Objects() image = sl.Mat() while viewer.is_available(): # Grab an image, a RuntimeParameters object must be given to grab() if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # Retrieve left image zed.retrieve_image(image, sl.VIEW.LEFT) # Retrieve objects zed.retrieve_objects(objects, obj_runtime_param) # Update GL view viewer.update_view(image, objects) viewer.exit() image.free(memory_type=sl.MEM.CPU) # Disable modules and close camera
def main(argv): thresh = 0.25 darknet_path="../libdarknet/" config_path = darknet_path + "cfg/yolov3-tiny.cfg" weight_path = "yolov3-tiny.weights" meta_path = "coco.data" svo_path = None zed_id = 0 help_str = 'darknet_zed.py -c <config> -w <weight> -m <meta> -t <threshold> -s <svo_file> -z <zed_id>' try: opts, args = getopt.getopt( argv, "hc:w:m:t:s:z:", ["config=", "weight=", "meta=", "threshold=", "svo_file=", "zed_id="]) except getopt.GetoptError: log.exception(help_str) sys.exit(2) for opt, arg in opts: if opt == '-h': log.info(help_str) sys.exit() elif opt in ("-c", "--config"): config_path = arg elif opt in ("-w", "--weight"): weight_path = arg elif opt in ("-m", "--meta"): meta_path = arg elif opt in ("-t", "--threshold"): thresh = float(arg) elif opt in ("-s", "--svo_file"): svo_path = arg elif opt in ("-z", "--zed_id"): zed_id = int(arg) input_type = sl.InputType() if svo_path is not None: log.info("SVO file : " + svo_path) input_type.set_from_svo_file(svo_path) else: # Launch camera by id input_type.set_from_camera_id(zed_id) init = sl.InitParameters(input_t=input_type) init.coordinate_units = sl.UNIT.METER cam = sl.Camera() if not cam.is_opened(): log.info("Opening ZED Camera...") status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: log.error(repr(status)) exit() runtime = sl.RuntimeParameters() # Use STANDARD sensing mode runtime.sensing_mode = sl.SENSING_MODE.STANDARD mat = sl.Mat() point_cloud_mat = sl.Mat() # Import the global variables. This lets us instance Darknet once, # then just call performDetect() again without instancing again global metaMain, netMain, altNames # pylint: disable=W0603 assert 0 < thresh < 1, "Threshold should be a float between zero and one (non-inclusive)" if not os.path.exists(config_path): raise ValueError("Invalid config path `" + os.path.abspath(config_path)+"`") if not os.path.exists(weight_path): raise ValueError("Invalid weight path `" + os.path.abspath(weight_path)+"`") if not os.path.exists(meta_path): raise ValueError("Invalid data file path `" + os.path.abspath(meta_path)+"`") if netMain is None: netMain = load_net_custom(config_path.encode( "ascii"), weight_path.encode("ascii"), 0, 1) # batch size = 1 if metaMain is None: metaMain = load_meta(meta_path.encode("ascii")) if altNames is None: # In thon 3, the metafile default access craps out on Windows (but not Linux) # Read the names file and create a list to feed to detect try: with open(meta_path) as meta_fh: meta_contents = meta_fh.read() import re match = re.search("names *= *(.*)$", meta_contents, re.IGNORECASE | re.MULTILINE) if match: result = match.group(1) else: result = None try: if os.path.exists(result): with open(result) as names_fh: names_list = names_fh.read().strip().split("\n") altNames = [x.strip() for x in names_list] except TypeError: pass except Exception: pass color_array = generate_color(meta_path) log.info("Running...") key = '' while key != 113: # for 'q' key start_time = time.time() # start time of the loop err = cam.grab(runtime) if err == sl.ERROR_CODE.SUCCESS: cam.retrieve_image(mat, sl.VIEW.LEFT) image = mat.get_data() cam.retrieve_measure( point_cloud_mat, sl.MEASURE.XYZRGBA) depth = point_cloud_mat.get_data() # Do the detection detections = detect(netMain, metaMain, image, thresh) log.info(chr(27) + "[2J"+"**** " + str(len(detections)) + " Results ****") for detection in detections: label = detection[0] confidence = detection[1] pstring = label+": "+str(np.rint(100 * confidence))+"%" log.info(pstring) bounds = detection[2] y_extent = int(bounds[3]) x_extent = int(bounds[2]) # Coordinates are around the center x_coord = int(bounds[0] - bounds[2]/2) y_coord = int(bounds[1] - bounds[3]/2) #boundingBox = [[x_coord, y_coord], [x_coord, y_coord + y_extent], [x_coord + x_extent, y_coord + y_extent], [x_coord + x_extent, y_coord]] thickness = 1 x, y, z = get_object_depth(depth, bounds) distance = math.sqrt(x * x + y * y + z * z) distance = "{:.2f}".format(distance) cv2.rectangle(image, (x_coord - thickness, y_coord - thickness), (x_coord + x_extent + thickness, y_coord + (18 + thickness*4)), color_array[detection[3]], -1) cv2.putText(image, label + " " + (str(distance) + " m"), (x_coord + (thickness * 4), y_coord + (10 + thickness * 4)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) cv2.rectangle(image, (x_coord - thickness, y_coord - thickness), (x_coord + x_extent + thickness, y_coord + y_extent + thickness), color_array[detection[3]], int(thickness*2)) cv2.imshow("ZED", image) key = cv2.waitKey(5) log.info("FPS: {}".format(1.0 / (time.time() - start_time))) else: key = cv2.waitKey(5) cv2.destroyAllWindows() cam.close() log.info("\nFINISH")
init_params.camera_resolution = sl.RESOLUTION.VGA init_params.depth_minimum_distance = 300 # Set sensing mode in FILL runtime_parameters = sl.RuntimeParameters() runtime_parameters.sensing_mode = sl.SENSING_MODE.FILL # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: print('Error is ', err) exit(1) camera_height = zed.get_camera_information().camera_resolution.height camera_width = zed.get_camera_information().camera_resolution.width image_zed = sl.Mat(camera_width, camera_height, sl.MAT_TYPE.U8_C4) depth_zed = sl.Mat(camera_width, camera_height, sl.MAT_TYPE.F32_C1) image_depth_zed = sl.Mat(camera_width, camera_height, sl.MAT_TYPE.U8_C4) pub = rospy.Publisher('ball_position', ball_positions) rospy.init_node('vision_processing') rate = rospy.Rate(60) # loops 10 times per second timer_frames = 5 timer_count = timer_frames start_time = time.time() # Video capturing while (not rospy.is_shutdown()) and zed.grab() == sl.ERROR_CODE.SUCCESS:
def main(): recordVideo = bool(False) showImages = bool(False) if len(sys.argv) != 2: print("Please specify path to .svo file.") exit() filepath = sys.argv[1] print("Reading SVO file: {0}".format(filepath)) # Specify SVO path parameter # init_params = sl.InitParameters() # init_params.set_from_svo_file(str(svo_input_path)) # init_params.svo_real_time_mode = False # Don't convert in realtime # init_params.coordinate_units = sl.UNIT.MILLIMETER # Use milliliter units (for depth measurements) input_type = sl.InputType() input_type.set_from_svo_file(filepath) init_params = sl.InitParameters(input_t=input_type, svo_real_time_mode=False) init_params.coordinate_units = sl.UNIT.CENTIMETER # Use milliliter units (for depth measurements) init_params.depth_mode = sl.DEPTH_MODE.ULTRA # Use ULTRA depth mode init_params.depth_minimum_distance = 1 # Set the minimum depth perception distance to 1 m init_params.depth_maximum_distance = 40 # Set the maximum depth perception distance to 40m cam = sl.Camera() status = cam.open(init_params) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() runtime = sl.RuntimeParameters() image = sl.Mat() depth_map = sl.Mat() point_cloud = sl.Mat() ## initialize image window #define the screen resulation screen_res = 1920, 1200 #image size width = cam.get_camera_information().camera_resolution.width height = cam.get_camera_information().camera_resolution.height scale_width = screen_res[0] / width scale_height = screen_res[1] / height scale = min(scale_width, scale_height) #resized window width and height window_width = int(width * scale) window_height = int(height * scale) window_size = (window_width, window_height) #cv2.WINDOW_NORMAL makes the output window resizealbe cv2.namedWindow('result', cv2.WINDOW_NORMAL) #resize the window according to the screen resolution #cv2.resizeWindow("result", window_width, window_height) key = '' print(" Save the current image: s") print(" Quit the video reading: q\n") print(cam.get_svo_position()) cam.set_svo_position(10000) print(cam.get_svo_position()) i = 0 frame_processing_time = time.time() while (key != 113) or (i != 50): # for 'q' key err = cam.grab(runtime) if err == sl.ERROR_CODE.SUCCESS: cam.retrieve_image(image, sl.VIEW.LEFT) # To recover data from sl.Mat to use it with opencv, use the get_data() method # It returns a numpy array that can be used as a matrix with opencv frame = image.get_data() # Get the depth map cam.retrieve_measure(depth_map, sl.MEASURE.DEPTH) cam.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA) #line detection lane_image = np.copy(frame) canny_image = canny(frame) cropped_image = region_of_intrest(canny_image) lines = cv2.HoughLinesP(cropped_image, 2, np.pi / 180, 100, np.array([]), minLineLength=40, maxLineGap=5) averaged_lines = average_slope_intercept(lane_image, lines) Hline_image = display_lines(lane_image, lines) line_image = display_lines(lane_image, averaged_lines) combo_image = cv2.addWeighted(frame, 0.8, line_image, 1, 1) cropped_combo_image = region_of_intrest(combo_image) ## get distance / location # Get and print distance value in mm at the center of the image # We measure the distance camera - object using Euclidean distance x = round(image.get_width() / 2) y = round(image.get_height() / 2) err, point_cloud_value = point_cloud.get_value(x, y) distance = math.sqrt(point_cloud_value[0] * point_cloud_value[0] + point_cloud_value[1] * point_cloud_value[1] + point_cloud_value[2] * point_cloud_value[2]) point_cloud_np = point_cloud.get_data() # point_cloud_np.dot(tr_np) if not np.isnan(distance) and not np.isinf(distance): # print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(x, y, distance), end="\r") # Increment the loop i = i + 1 else: print("Can't estimate distance at this position.") print( "Your camera is probably too close to the scene, please move it backwards.\n" ) sys.stdout.flush() # Get and print distance value in mm at the center of the image # We measure the distance camera - object using Euclidean distance # x = mat.get_width() / 2 # y = mat.get_height() / 2 # point_cloud_value = point_cloud.get_value(x, y) # distance = math.sqrt(point_cloud_value[0]*point_cloud_value[0] + point_cloud_value[1]*point_cloud_value[1] + point_cloud_value[2]*point_cloud_value[2]) # printf("Distance to Camera at (", x, y, "): ", distance, "mm") # x1, y1, x2, y2 = lines[0].reshape(4) # depth_value = depth_map.get_value(x2,y2) # print("x2= ",x2," y2= ",y2," z= ",depth_value) # point3D = point_cloud.get_value(1220,850)#1220 850 # x = point3D[0] # y = point3D[1] # z = point3D[2] # #color = point3D[3] # print("x= ",x," y= ",y," z= ",z) if showImages: # cv2.imshow("cropped_lane_image", cropped_lane_image) # frame_canny = cv2.cvtColor(canny_image, cv2.COLOR_GRAY2BGR) # frame_cropped = cv2.cvtColor(cropped_image, cv2.COLOR_GRAY2BGR) # plt.imshow(cropped_combo_image)#canny_image) # plt.show() # combined_image= combine(lane_image, cropped_combo_image ,canny_image, cropped_image, combo_image , Hline_image )# , line_image #cv2.namedWindow("combined_image", cv2.WINDOW_NORMAL) #com_height, com_width, com_layers = combined_image.shape #imS = cv2.resize(combined_image, window_size) #cv2.imshow("combined_image",imS) #cv2.imshow("result", combo_image) # cv2.imshow("result", combined_image) cv2.imshow("result", cropped_combo_image) # end line detection #cv2.imshow("ZED", mat.get_data()) key = cv2.waitKey(1) saving_image(key, mat) # print(int(cam.get_svo_number_of_frames()//2)) # cam.set_svo_position(10000) else: key = 113 else: key = cv2.waitKey(1) # i= i+1 #measure the time it takes to process one frame print("--- %s seconds ---" % (time.time() - frame_processing_time)) frame_processing_time = time.time() cv2.destroyAllWindows() print_camera_information(cam) cam.close() print("\nFINISH")