def main(): init = sl.InitParameters(camera_resolution=sl.RESOLUTION.HD720, depth_mode=sl.DEPTH_MODE.PERFORMANCE, coordinate_units=sl.UNIT.METER, coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP, sdk_verbose=True) cam = sl.Camera() status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() transform = sl.Transform() tracking_params = sl.PositionalTrackingParameters(transform) cam.enable_positional_tracking(tracking_params) runtime = sl.RuntimeParameters() camera_pose = sl.Pose() viewer = tv.PyTrackingViewer() viewer.init() py_translation = sl.Translation() start_zed(cam, runtime, camera_pose, viewer, py_translation) viewer.exit() glutMainLoop()
def init_params(menu, cam_id=0): global save_dir zed = EasyDict({}) zed.cam = sl.Camera() zed.mat = EasyDict({ 'pose': sl.Pose(), 'translation': sl.Translation(), 'transform': sl.Transform(), 'image': sl.Mat(), # image_map 'depth': sl.Mat(), # depth_map 'point_cloud': sl.Mat(), 'sensors': sl.SensorsData() # sensors_data }) zed.param = EasyDict({ 'init': sl.InitParameters( camera_resolution=mode.resolution[menu.cam.resolution], depth_mode=mode.depth[menu.mode.depth], coordinate_units=mode.unit[menu.unit], coordinate_system=mode.coordinate_system[menu.coordinate_system], depth_minimum_distance=menu.depth_range.min, depth_maximum_distance=menu.depth_range.max, sdk_verbose=verbose), 'runtime': sl.RuntimeParameters(sensing_mode=mode.sensing[menu.mode.sensing]), 'tracking': sl.PositionalTrackingParameters(zed.mat.transform) }) ####### zed.param.init.set_from_camera_id(cam_id) save_dir = save_dir_fmt.format(cam_id) return zed
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 while i < 1000: if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS: err = cam.find_floor_plane(pyplane, reset_tracking_floor_frame) if i > 200 and err == sl.ERROR_CODE.SUCCESS: found = 1 print('Floor found!') pymesh = pyplane.extract_mesh() break i += 1 if found == 0: print('Floor was not found, please try with another SVO.') cam.close() exit(0) cam.disable_positional_tracking() save_mesh(pymesh) cam.close() print("\nFINISH")
def main(): if len(sys.argv) != 2: print("Please specify path to .svo file.") exit() filepath = sys.argv[1] print("Reading SVO file: {0}".format(filepath)) cam = sl.Camera() input_type = sl.InputType() input_type.set_from_svo_file(filepath) init = sl.InitParameters() init.input = input_type status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() runtime = sl.RuntimeParameters() spatial = sl.SpatialMappingParameters() transform = sl.Transform() tracking = sl.PositionalTrackingParameters(transform) cam.enable_positional_tracking(tracking) cam.enable_spatial_mapping(spatial) pymesh = sl.Mesh() print("Processing...") for i in range(200): cam.grab(runtime) cam.request_spatial_map_async() cam.extract_whole_spatial_map(pymesh) cam.disable_positional_tracking() cam.disable_spatial_mapping() filter_params = sl.MeshFilterParameters() filter_params.set(sl.MESH_FILTER.HIGH) print("Filtering params : {0}.".format(pymesh.filter(filter_params))) apply_texture = pymesh.apply_texture(sl.MESH_TEXTURE_FORMAT.RGBA) print("Applying texture : {0}.".format(apply_texture)) print_mesh_information(pymesh, apply_texture) save_filter(filter_params) save_mesh(pymesh) cam.close() print("\nFINISH")
def main(): # 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 (default fps: 60) # Use a right-handed Y-up coordinate system init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP init_params.coordinate_units = sl.UNIT.METER # Set units in meters # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Enable positional tracking with default parameters py_transform = sl.Transform( ) # First create a Transform object for TrackingParameters object tracking_parameters = sl.PositionalTrackingParameters( init_pos=py_transform) err = zed.enable_positional_tracking(tracking_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Track the camera position during 1000 frames i = 0 zed_pose = sl.Pose() zed_sensors = sl.SensorsData() runtime_parameters = sl.RuntimeParameters() while i < 1000: if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # Get the pose of the left eye of the camera with reference to the world frame zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD) zed.get_sensors_data(zed_sensors, sl.TIME_REFERENCE.IMAGE) zed_imu = zed_sensors.get_imu_data() # Display the translation and timestamp py_translation = sl.Translation() tx = round(zed_pose.get_translation(py_translation).get()[0], 3) ty = round(zed_pose.get_translation(py_translation).get()[1], 3) tz = round(zed_pose.get_translation(py_translation).get()[2], 3) print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n". format(tx, ty, tz, zed_pose.timestamp.get_milliseconds())) # Display the orientation quaternion py_orientation = sl.Orientation() ox = round(zed_pose.get_orientation(py_orientation).get()[0], 3) oy = round(zed_pose.get_orientation(py_orientation).get()[1], 3) oz = round(zed_pose.get_orientation(py_orientation).get()[2], 3) ow = round(zed_pose.get_orientation(py_orientation).get()[3], 3) print("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format( ox, oy, oz, ow)) #Display the IMU acceleratoin acceleration = [0, 0, 0] zed_imu.get_linear_acceleration(acceleration) ax = round(acceleration[0], 3) ay = round(acceleration[1], 3) az = round(acceleration[2], 3) print("IMU Acceleration: Ax: {0}, Ay: {1}, Az {2}\n".format( ax, ay, az)) #Display the IMU angular velocity a_velocity = [0, 0, 0] zed_imu.get_angular_velocity(a_velocity) vx = round(a_velocity[0], 3) vy = round(a_velocity[1], 3) vz = round(a_velocity[2], 3) print("IMU Angular Velocity: Vx: {0}, Vy: {1}, Vz {2}\n".format( vx, vy, vz)) # Display the IMU orientation quaternion zed_imu_pose = sl.Transform() ox = round( zed_imu.get_pose(zed_imu_pose).get_orientation().get()[0], 3) oy = round( zed_imu.get_pose(zed_imu_pose).get_orientation().get()[1], 3) oz = round( zed_imu.get_pose(zed_imu_pose).get_orientation().get()[2], 3) ow = round( zed_imu.get_pose(zed_imu_pose).get_orientation().get()[3], 3) print( "IMU Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format( ox, oy, oz, ow)) i = i + 1 # Close the camera zed.close()
def main(): # global stop_signal # signal.signal(signal.SIGINT, signal_handler) # List and open cameras cameras = sl.Camera.get_device_list() index = 0 cams = EasyDict({}) cams.pose_list = [] cams.zed_sensors_list = [] cams.zed_list = [] cams.left_list = [] cams.depth_list = [] cams.pointcloud_list = [] cams.timestamp_list = [] cams.image_size_list = [] cams.image_zed_list = [] cams.depth_image_zed_list = [] cams.name_list = [] cams.name_list = [] cams.py_translation_list = [] cams.py_orientation_list = [] cams.transform_list = [] cams.runtime_list = [] # Set configuration parameters init = sl.InitParameters( camera_resolution=sl.RESOLUTION.HD2K, coordinate_units=sl.UNIT.METER, #coordinate_units=sl.UNIT.MILLIMETER,#■ depth_mode=sl.DEPTH_MODE.PERFORMANCE, coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP) for cam in cameras: init.set_from_serial_number(cam.serial_number) cams.name_list.append("ZED_{}".format(cam.serial_number)) print("Opening {}".format(cams.name_list[index])) # Create a ZED camera object cams.zed_list.append(sl.Camera()) cams.left_list.append(sl.Mat()) cams.depth_list.append(sl.Mat()) cams.pointcloud_list.append(sl.Mat()) cams.pose_list.append(sl.Pose()) cams.zed_sensors_list.append(sl.SensorsData()) cams.timestamp_list.append(0) cams.py_translation_list.append(sl.Translation()) cams.transform_list.append(sl.Transform()) cams.py_orientation_list.append(sl.Orientation()) # Open the camera status = cams.zed_list[index].open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) cams.zed_list[index].close() exit(1) #tracing enable py_transform = cams.transform_list[index] print("PositionalTrackingParameters start") tracking_parameters = sl.PositionalTrackingParameters( init_pos=py_transform) err = cams.zed_list[index].enable_positional_tracking( tracking_parameters) print("PositionalTrackingParameters end") if err != sl.ERROR_CODE.SUCCESS: cams.zed_list[index].close() exit(1) runtime = sl.RuntimeParameters() cams.runtime_list.append(runtime) index = index + 1 #Start camera threads # for index in range(0, len(cams.zed_list)): # if cams.zed_list[index].is_opened(): # thread_list.append(threading.Thread(target=grab_run, args=(cams,index,))) # thread_list[index].start() #https://github.com/stereolabs/zed-examples/blob/master/tutorials/tutorial%204%20-%20positional%20tracking/python/positional_tracking.py # py_translation = sl.Translation() # Display help in console print_help() # Prepare new image size to retrieve half-resolution images for index, cam in enumerate(cameras): fd_cam = f'{basePath}/{cams.name_list[index]}' os.makedirs(fd_cam, exist_ok=True) image_size = cams.zed_list[index].get_camera_information( ).camera_resolution image_size.width = image_size.width / 2 image_size.height = image_size.height / 2 # Declare your sl.Mat matrices #image_zed = cams.left_list[index](image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) #depth_image_zed = cams.depth_list[index](image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) depth_image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) cams.image_size_list.append(image_size) cams.image_zed_list.append(image_zed) cams.depth_image_zed_list.append(depth_image_zed) ######## cam_intr, distortion = get_camera_intrintic_info(cams.zed_list[index]) filename = f'{fd_cam}/camera-intrinsics.csv' np.savetxt(filename, cam_intr) filename = f'{fd_cam}/camera-distortion.csv' np.savetxt(filename, distortion) #******************************************************************* take_by_keyinput(cameras, cams) # take_by_keyinput_camera_view(cameras, cams) #******************************************************************* index = 0 for cam in cameras: cams.zed_list[index].close() index += 1 print("\nFINISH")
def main(): # 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.depth_mode = sl.DEPTH_MODE.PERFORMANCE init_params.coordinate_units = sl.UNIT.METER init_params.sdk_verbose = True # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) obj_param = sl.ObjectDetectionParameters() # Different model can be chosen, optimizing the runtime or the accuracy obj_param.detection_model = sl.DETECTION_MODEL.HUMAN_BODY_FAST obj_param.enable_tracking = True obj_param.image_sync = True obj_param.enable_mask_output = False # Optimize the person joints position, requires more computations obj_param.enable_body_fitting = True camera_infos = zed.get_camera_information() if obj_param.enable_tracking: positional_tracking_param = sl.PositionalTrackingParameters() # positional_tracking_param.set_as_static = True positional_tracking_param.set_floor_as_origin = True zed.enable_positional_tracking(positional_tracking_param) print("Object Detection: Loading Module...") err = zed.enable_object_detection(obj_param) if err != sl.ERROR_CODE.SUCCESS: print(repr(err)) zed.close() exit(1) objects = sl.Objects() obj_runtime_param = sl.ObjectDetectionRuntimeParameters() # For outdoor scene or long range, the confidence should be lowered to avoid missing detections (~20-30) # For indoor scene or closer range, a higher confidence limits the risk of false positives and increase the precision (~50+) obj_runtime_param.detection_confidence_threshold = 40 while zed.grab() == sl.ERROR_CODE.SUCCESS: err = zed.retrieve_objects(objects, obj_runtime_param) if objects.is_new: obj_array = objects.object_list print(str(len(obj_array)) + " Person(s) detected\n") if len(obj_array) > 0: first_object = obj_array[0] print("First Person attributes:") print(" Confidence (" + str(int(first_object.confidence)) + "/100)") if obj_param.enable_tracking: print(" Tracking ID: " + str(int(first_object.id)) + " tracking state: " + repr( first_object.tracking_state) + " / " + repr(first_object.action_state)) position = first_object.position velocity = first_object.velocity dimensions = first_object.dimensions print(" 3D position: [{0},{1},{2}]\n Velocity: [{3},{4},{5}]\n 3D dimentions: [{6},{7},{8}]".format( position[0], position[1], position[2], velocity[0], velocity[1], velocity[2], dimensions[0], dimensions[1], dimensions[2])) if first_object.mask.is_init(): print(" 2D mask available") print(" Keypoint 2D ") keypoint_2d = first_object.keypoint_2d for it in keypoint_2d: print(" " + str(it)) print("\n Keypoint 3D ") keypoint = first_object.keypoint for it in keypoint: print(" " + str(it)) input('\nPress enter to continue: ') # Close the camera zed.close()
def main(): # 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.depth_mode = sl.DEPTH_MODE.PERFORMANCE init_params.coordinate_units = sl.UNIT.METER init_params.sdk_verbose = True # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) obj_param = sl.ObjectDetectionParameters() obj_param.enable_tracking=True obj_param.image_sync=True obj_param.enable_mask_output=True camera_infos = zed.get_camera_information() if obj_param.enable_tracking : positional_tracking_param = sl.PositionalTrackingParameters() #positional_tracking_param.set_as_static = True positional_tracking_param.set_floor_as_origin = True zed.enable_positional_tracking(positional_tracking_param) print("Object Detection: Loading Module...") err = zed.enable_object_detection(obj_param) if err != sl.ERROR_CODE.SUCCESS : print (repr(err)) zed.close() exit(1) objects = sl.Objects() obj_runtime_param = sl.ObjectDetectionRuntimeParameters() obj_runtime_param.detection_confidence_threshold = 40 while zed.grab() == sl.ERROR_CODE.SUCCESS: err = zed.retrieve_objects(objects, obj_runtime_param) if objects.is_new : obj_array = objects.object_list print(str(len(obj_array))+" Object(s) detected\n") if len(obj_array) > 0 : first_object = obj_array[0] print("First object attributes:") print(" Label '"+repr(first_object.label)+"' (conf. "+str(int(first_object.confidence))+"/100)") if obj_param.enable_tracking : print(" Tracking ID: "+str(int(first_object.id))+" tracking state: "+repr(first_object.tracking_state)+" / "+repr(first_object.action_state)) position = first_object.position velocity = first_object.velocity dimensions = first_object.dimensions print(" 3D position: [{0},{1},{2}]\n Velocity: [{3},{4},{5}]\n 3D dimentions: [{6},{7},{8}]".format(position[0],position[1],position[2],velocity[0],velocity[1],velocity[2],dimensions[0],dimensions[1],dimensions[2])) if first_object.mask.is_init(): print(" 2D mask available") print(" Bounding Box 2D ") bounding_box_2d = first_object.bounding_box_2d for it in bounding_box_2d : print(" "+str(it),end='') print("\n Bounding Box 3D ") bounding_box = first_object.bounding_box for it in bounding_box : print(" "+str(it),end='') input('\nPress enter to continue: ') # Close the camera zed.close()
def main(): # 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.depth_mode = sl.DEPTH_MODE.PERFORMANCE init_params.coordinate_units = sl.UNIT.METER init_params.sdk_verbose = True # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) obj_param = sl.ObjectDetectionParameters() obj_param.enable_tracking=True obj_param.image_sync=True obj_param.enable_mask_output=True camera_infos = zed.get_camera_information() if obj_param.enable_tracking : positional_tracking_param = sl.PositionalTrackingParameters() #positional_tracking_param.set_as_static = True positional_tracking_param.set_floor_as_origin = True zed.enable_positional_tracking(positional_tracking_param) print("Object Detection: Loading Module...") err = zed.enable_object_detection(obj_param) if err != sl.ERROR_CODE.SUCCESS : print (repr(err)) zed.close() exit(1) objects = sl.Objects() obj_runtime_param = sl.ObjectDetectionRuntimeParameters() obj_runtime_param.detection_confidence_threshold = 40 while zed.grab() == sl.ERROR_CODE.SUCCESS: err = zed.retrieve_objects(objects, obj_runtime_param) start = timeit.default_timer() if objects.is_new : obj_array = objects.object_list if len(obj_array) > 0 : first_object = obj_array[0] print("First object attributes:") print(" Label '"+repr(first_object.label)+"' (conf. "+str(int(first_object.confidence))+"/100)") position = first_object.position dimensions = first_object.dimensions print(" 3D position: [{0},{1},{2}]\n 3D dimentions: [{3},{4},{5}]".format(position[0],position[1],position[2],dimensions[0],dimensions[1],dimensions[2])) ###################### 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 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) 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") else: pass print("\n Bounding Box 3D ") bounding_box = first_object.bounding_box stop = timeit.default_timer() print("\n FPS:", stop - start) # Close the camera zed.close()
def main(): global stop_signal thread_list = [] signal.signal(signal.SIGINT, signal_handler) # List and open cameras cameras = sl.Camera.get_device_list() index = 0 cams = EasyDict({}) cams.pose_list = [] cams.zed_sensors_list = [] cams.zed_list = [] cams.left_list = [] cams.depth_list = [] cams.pointcloud_list = [] cams.timestamp_list = [] cams.image_size_list = [] cams.image_zed_list = [] cams.depth_image_zed_list = [] cams.name_list = [] cams.name_list = [] cams.py_translation_list = [] cams.py_orientation_list = [] cams.transform_list = [] cams.runtime_list = [] # Set configuration parameters init = sl.InitParameters( camera_resolution=sl.RESOLUTION.HD2K, coordinate_units=sl.UNIT.METER, #coordinate_units=sl.UNIT.MILLIMETER, depth_mode=sl.DEPTH_MODE.PERFORMANCE, coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP) for cam in cameras: init.set_from_serial_number(cam.serial_number) cams.name_list.append("ZED_{}".format(cam.serial_number)) print("Opening {}".format(cams.name_list[index])) # Create a ZED camera object cams.zed_list.append(sl.Camera()) cams.left_list.append(sl.Mat()) cams.depth_list.append(sl.Mat()) cams.pointcloud_list.append(sl.Mat()) cams.pose_list.append(sl.Pose()) cams.zed_sensors_list.append(sl.SensorsData()) cams.timestamp_list.append(0) cams.py_translation_list.append(sl.Translation()) cams.transform_list.append(sl.Transform()) cams.py_orientation_list.append(sl.Orientation()) # Open the camera status = cams.zed_list[index].open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) cams.zed_list[index].close() #tracing enable py_transform = cams.transform_list[index] tracking_parameters = sl.PositionalTrackingParameters( init_pos=py_transform) err = cams.zed_list[index].enable_positional_tracking( tracking_parameters) if err != sl.ERROR_CODE.SUCCESS: cams.zed_list[index].close() exit(1) index = index + 1 #Start camera threads for index in range(0, len(cams.zed_list)): if cams.zed_list[index].is_opened(): thread_list.append( threading.Thread(target=grab_run, args=( cams, index, ))) thread_list[index].start() #https://github.com/stereolabs/zed-examples/blob/master/tutorials/tutorial%204%20-%20positional%20tracking/python/positional_tracking.py # py_translation = sl.Translation() # Display help in console print_help() # Prepare new image size to retrieve half-resolution images for index, cam in enumerate(cameras): image_size = cams.zed_list[index].get_camera_information( ).camera_resolution image_size.width = image_size.width / 2 image_size.height = image_size.height / 2 # Declare your sl.Mat matrices #image_zed = cams.left_list[index](image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) #depth_image_zed = cams.depth_list[index](image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) depth_image_zed = sl.Mat(image_size.width, image_size.height, sl.MAT_TYPE.U8_C4) cams.image_size_list.append(image_size) cams.image_zed_list.append(image_zed) cams.depth_image_zed_list.append(depth_image_zed) ######## cam_intr = get_camera_intrintic_info(cams.zed_list[index]) filename = path + cams.name_list[index] + "-camera-intrinsics.txt" df = pd.DataFrame(cam_intr) df.to_csv(filename, sep=' ', header=None, index=None) #******************************************************************* index = 0 take_cam_id = 0 for cam in cameras: get_cam_data(cams, index, take_cam_id) index += 1 stop_signal = True #******************************************************************* ''' key = ' ' take_cam_id=0 while key != 113 : index=0 image_ocv_cat=None depth_image_ocv_cat=None for cam in cameras: image_ocv, depth_image_ocv=get_cam_color_depth(cams,index) if image_ocv_cat is None: image_ocv_cat=image_ocv depth_image_ocv_cat=depth_image_ocv else: image_ocv_cat=np.hstack([image_ocv_cat,image_ocv]) depth_image_ocv_cat=np.hstack([depth_image_ocv_cat,depth_image_ocv]) index+=1 cv2.imshow("Image", image_ocv_cat) cv2.imshow("Depth", depth_image_ocv_cat) key = cv2.waitKey(10) if key == 114 or key == 82:#R index = 0 for cam in cameras: # process_key_event(cams,key,index) get_cam_data(cams, index,take_cam_id) index+=1 take_cam_id=take_cam_id+1 cv2.destroyAllWindows() ''' index = 0 for cam in cameras: cams.zed_list[index].close() index += 1 print("\nFINISH")
def process_svo_map_and_pose(self, map_file="map.obj", poses_file="poses.txt", n_frames_to_skip=1, n_frames_to_trim=0): """ Process the ZED SVO file and write to file: - A fused point cloud map - The camera pose history This function processes the SVO file with the maximum possible depth sensing quality, to maximize mapping and pose estimation quality. Parameters ---------- map_file: str Name of the map file to output. Should be .obj file format. poses_file: str Name of the poses file ot output. Should be .txt format. n_frames_to_skip: int, default 0 If set to 2 or higher, the pose history output will skip frames. Use this to subsample the SVO output. For example, skip_frames=2 will include every other frame in the pose file. Does not affect the map creation. Returns ------- None """ # Initialize the ZED Camera object init_params = self.default_init_params() init_params.depth_mode = self.depth_quality_map zed = sl.Camera() err = zed.open(init_params) if err == sl.ERROR_CODE.INVALID_SVO_FILE: print("Error processing SVO file: '%s'" % self.svo_path) return # Initialize positional tracking tracking_parameters = sl.PositionalTrackingParameters() zed.enable_positional_tracking(tracking_parameters) # Initialize spatial mapping mapping_parameters = sl.SpatialMappingParameters() mapping_parameters.map_type = sl.SPATIAL_MAP_TYPE.FUSED_POINT_CLOUD # mapping_parameters.resolution_meter = mapping_parameters.get_resolution_preset(sl.MAPPING_RESOLUTION.MEDIUM) mapping_parameters.resolution_meter = mapping_parameters.get_resolution_preset( sl.MAPPING_RESOLUTION.HIGH) # Map at short range (3.5m) to maximize quality # This should reduce errors like points in the sky # mapping_parameters.range_meter = mapping_parameters.get_range_preset(sl.MAPPING_RANGE.SHORT) # mapping_parameters.range_meter = mapping_parameters.get_range_preset(sl.MAPPING_RANGE.LONG) mapping_parameters.range_meter = mapping_parameters.get_range_preset( sl.MAPPING_RANGE.MEDIUM) zed.enable_spatial_mapping(mapping_parameters) pose_history = [] n_frames = zed.get_svo_number_of_frames() assert type(n_frames_to_skip) == int and (1 <= n_frames_to_skip < n_frames), \ "n_frames_to_skip parameter must be an int between 1 and number of frames in the SVO." svo_position = 0 # Keep a separate counter instead of using zed.get_svo_position() - svo position skips sometimes next_frame = 0 # keep track of next frame to process, for subsampling frames last_frame = zed.get_svo_number_of_frames() - n_frames_to_trim # SVO processing loop exit = False while not exit: # With spatial mapping enabled, zed.grab() updates the map in the background err = zed.grab() if err == sl.ERROR_CODE.SUCCESS: if svo_position < last_frame and svo_position >= next_frame: print("\r[Processing frame %d of %d]" % (svo_position, n_frames), end='') pose_history.append(get_zed_pose(zed)) next_frame += n_frames_to_skip svo_position += 1 elif err == sl.ERROR_CODE.END_OF_SVOFILE_REACHED: print("Mapping module: Reached end of SVO file") exit = True # Write outputs self.write_poses(pose_history, self.output_path / poses_file) self.write_map(zed, self.output_path / map_file) zed.close()
def main(): # Create a ZEDCamera 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 (default fps: 60) # Use a right-handed Y-up coordinate system init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP init_params.coordinate_units = sl.UNIT.METER # Set units in meters # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Enable positional tracking with default parameters. # Positional tracking needs to be enabled before using spatial mapping py_transform = sl.Transform() tracking_parameters = sl.PositionalTrackingParameters( init_pos=py_transform) err = zed.enable_positional_tracking(tracking_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Enable spatial mapping mapping_parameters = sl.SpatialMappingParameters( map_type=sl.SPATIAL_MAP_TYPE.FUSED_POINT_CLOUD) err = zed.enable_spatial_mapping(mapping_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Grab data during 3000 frames i = 0 py_fpc = sl.FusedPointCloud() # Create a Mesh object runtime_parameters = sl.RuntimeParameters() while i < 3000: # For each new grab, mesh data is updated if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh mapping_state = zed.get_spatial_mapping_state() print("\rImages captured: {0} / 3000 || {1}".format( i, mapping_state)) i = i + 1 print("\n") # Extract, filter and save the mesh in an obj file print("Extracting Point Cloud...\n") err = zed.extract_whole_spatial_map(py_fpc) print(repr(err)) #print("Filtering Mesh...\n") #py_mesh.filter(sl.MeshFilterParameters()) # Filter the mesh (remove unnecessary vertices and faces) print("Saving Point Cloud...\n") py_fpc.save("fpc.obj") # Disable tracking and mapping and close the camera zed.disable_spatial_mapping() zed.disable_positional_tracking() zed.close()
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 = sl.FLIP_MODE.AUTO 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 zedGrabber(zedFramesQueue): svo_path = "HD1080.svo" zed_id = 0 init_mode = 0 input_type = sl.InputType() if init_mode == 0: input_type.set_from_svo_file(svo_path) else: input_type.set_from_camera_id(zed_id) init = sl.InitParameters(input_t=input_type) init.camera_resolution = sl.RESOLUTION.HD1080 init.camera_fps = 30 init.coordinate_units = sl.UNIT.METER init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP cam = sl.Camera() if not cam.is_opened(): print("Opening ZED Camera...") status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: log.error(repr(status)) exit() # Enable positional tracking with default parameters py_transform = sl.Transform( ) # First create a Transform object for TrackingParameters object tracking_parameters = sl.PositionalTrackingParameters( init_pos=py_transform) err = cam.enable_positional_tracking(tracking_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) zed_pose = sl.Pose() 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, while True: start_time = time.time() # start time of the loop err = cam.grab(runtime) if (err != sl.ERROR_CODE.SUCCESS): break 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() cam.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD) py_translation = sl.Translation() tx = round(zed_pose.get_translation(py_translation).get()[0], 3) ty = round(zed_pose.get_translation(py_translation).get()[1], 3) tz = round(zed_pose.get_translation(py_translation).get()[2], 3) camPosition = (tx, ty, tz) #print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}".format(tx, ty, tz, zed_pose.timestamp.get_milliseconds())) camOrientation = zed_pose.get_rotation_vector() * 180 / math.pi image = rotate(image, -camOrientation[1], center=None, scale=1.0) FPS = 1.0 / (time.time() - start_time) # Do the detection #rawDetections = detect(netMain, metaMain, image, thresh) #detectionsList = detectionsAnalayzer(rawDetections, depth) frame = zedFrame(image, depth, camOrientation, camPosition, FPS) zedFramesQueue.put(frame) #image = cvDrawBoxes(detectionsList, image) #image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) #image = cv2.resize(image,(1280,720),interpolation=cv2.INTER_LINEAR) cam.close()
GPIO.add_event_detect(pin, GPIO.RISING, callback=cb, bouncetime=200) print("GPIO setup finished") ## TODO setup zed camera init = sl.InitParameters( camera_resolution=sl.RESOLUTION.HD720, 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() tracking_params = sl.PositionalTrackingParameters(_enable_pose_smoothing=False, _set_floor_as_origin=False, _enable_imu_fusion=False) tracking_params.area_file_path = "nsh_chair.area" #"smith.area" zed.enable_positional_tracking(tracking_params) runtime = sl.RuntimeParameters() camera_pose = sl.Pose() camera_info = zed.get_camera_information() py_translation = sl.Translation() py_orientation = sl.Orientation() pose_data = sl.Transform() sensors_data = sl.SensorsData() text_translation = ""
# Create a ZED camera object zed = sl.Camera() # Set configuration parameters init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode (default fps: 60) init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP # Use a right-handed Y-up coordinate system init_params.coordinate_units = sl.UNIT.METER # Set units in meters # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: exit(1) tracking_parameters = sl.PositionalTrackingParameters() err = zed.enable_positional_tracking(tracking_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) mapping_parameters = sl.SpatialMappingParameters() err = zed.enable_spatial_mapping(mapping_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Grab data during 3000 frames i = 0 py_mesh = sl.Mesh() # Create a Mesh object runtime_parameters = sl.RuntimeParameters()
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")