import pyzed.sl as sl import numpy as np import sys import os import time import json import math import json JSON_PATH = '~/sources/configuration.json' def conf_depth(camobject=sl.Camera(), headCoord=(480,540), tailCoord=(1440,540)): # Create a Camera object hc = headCoord tc = tailCoord zed = sl.Camera() # Create a InitParameters object and set configuration parameters init = sl.InitParameters() init.set_from_serial_number init.depth_mode = sl.DEPTH_MODE.PERFORMANCE # Use PERFORMANCE depth mode init.coordinate_units = sl.UNIT.METER # Use meter units (for depth measurements) init.camera_resolution = sl.RESOLUTION.HD1080 # Open the camera err = zed.open(init) 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.STANDARD # Use STANDARD sensing mode # Setting the depth confidence parameters runtime_parameters.confidence_threshold = 100 runtime_parameters.textureness_confidence_threshold = 100 # Capture 150 images and depth, then stop i = 0 image = sl.Mat() depth = sl.Mat() point_cloud = sl.Mat() mirror_ref = sl.Transform() mirror_ref.set_translation(sl.Translation(2.75,4.0,0)) tr_np = mirror_ref.m hx = 0 tx = 0 distance_head_cliff = 0 distance_tail_cliff = 0 while i < 900: # 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.LEFT) # Retrieve depth map. Depth is aligned on the left image zed.retrieve_measure(depth, sl.MEASURE.DEPTH) # Retrieve colored point cloud. Point cloud is aligned on the left image. zed.retrieve_measure(point_cloud, sl.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) if hx == 0 : hx = x tx = x err, point_cloud_value_center = point_cloud.get_value(x, y) distance_center = math.sqrt(point_cloud_value_center[0] * point_cloud_value_center[0] + point_cloud_value_center[1] * point_cloud_value_center[1] + point_cloud_value_center[2] * point_cloud_value_center[2]) err, point_cloud_value_head = point_cloud.get_value(hx,y) distance_head = math.sqrt(point_cloud_value_head[0] * point_cloud_value_head[0] + point_cloud_value_head[1] * point_cloud_value_head[1] + point_cloud_value_head[2] * point_cloud_value_head[2]) if distance_head_cliff != 0: pass elif distance_head > (0.04 + distance_center): if np.isnan(prev_distance_head) == True: pass else: distance_head_cliff = prev_distance_head hx_cliff = hx if np.isnan(distance_head) == False: prev_distance_head = distance_head err, point_cloud_value_tail = point_cloud.get_value(tx,y) distance_tail = math.sqrt(point_cloud_value_tail[0] * point_cloud_value_tail[0] + point_cloud_value_tail[1] * point_cloud_value_tail[1] + point_cloud_value_tail[2] * point_cloud_value_tail[2]) if distance_tail_cliff != 0: pass elif distance_tail > (0.04 + distance_center): if np.isnan(prev_distance_tail) == True: pass else: distance_tail_cliff = prev_distance_tail tx_cliff = tx - 1 if np.isnan(distance_tail) == False: prev_distance_tail = distance_tail point_cloud_np = point_cloud.get_data() point_cloud_np.dot(tr_np) hx -=1 tx +=1 if not np.isnan(distance_center) and not np.isinf(distance_center): print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(hx, y, distance_head)) print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(x, y, distance_center)) print("Distance to Camera at ({}, {}) (image center): {:1.3} m".format(tx, y, distance_tail)) # 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() # Close the camera zed.close() return [distance_center,hx_cliff,distance_head_cliff,tx_cliff,distance_tail_cliff] def main(): cameras = sl.Camera.get_device_list() if len(cameras) == 1: with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "r") as json_file: conf = json.load(json_file) d_list = conf_depth(cameras[0],conf["cam1"]["headCoord"], conf["cam1"]["tailCoord"]) conf["cam1"]["d_center"] = d_list[0] conf["cam1"]["d_headCoord"] = d_list[2] conf["cam1"]["d_tailCoord"] = d_list[4] #as maincam, head is left side of image if conf["mainCam"] == cameras[0][0]: conf["cam1"]["headCoord"] = [d_list[1], 540] conf["cam1"]["tailCoord"] = [d_list[3], 540] else: conf["cam1"]["headCoord"] = [d_list[3], 540] conf["cam1"]["tailCoord"] = [d_list[1], 540] with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "w") as json_file: json.dump(conf,json_file, indent=4) elif len(cameras) == 2: with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "r") as json_file: conf = json.load(json_file) d_list = conf_depth(cameras[1],conf["cam2"]["headCoord"], conf["cam2"]["tailCoord"]) conf["cam2"]["d_center"] = d_list[0] conf["cam2"]["d_headCoord"] = d_list[2] conf["cam2"]["d_tailCoord"] = d_list[4] if conf["mainCam"] == cameras[1][0]: conf["cam2"]["headCoord"] = [d_list[1], 540] conf["cam2"]["tailCoord"] = [d_list[3], 540] else: conf["cam2"]["headCoord"] = [d_list[3], 540] conf["cam2"]["tailCoord"] = [d_list[1], 540] with open (os.path.abspath(os.path.expanduser(JSON_PATH)), "w") as json_file: json.dump(conf,json_file, indent=4) if __name__ == "__main__": main()