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 capture_data(zed, height, width, length, no_cajas, no_cajas_base, no_cajas_alto) : print('\n##################################################') print("#Capturando data, no mover la cámara ni el objeto#") print('##################################################') # capture RGB image tmp_img = sl.Mat() zed.retrieve_image(tmp_img, sl.VIEW.LEFT) # capture depth data tmp_depth = sl.Mat() zed.retrieve_measure(tmp_depth, sl.MEASURE.DEPTH) # capture pointcloud tmp_pointcloud = sl.Mat() zed.retrieve_measure(tmp_pointcloud, sl.MEASURE.XYZRGBA) # capture sensors data tmp_sensors = sl.SensorsData() zed.get_sensors_data(tmp_sensors, sl.TIME_REFERENCE.IMAGE) # get timestamp ts = get_timestamp() # create subdir # subdir of current capture subdir = os.path.join('data', ts) os.makedirs(subdir, exist_ok=True) # Saving RGB image save_rgb(subdir, tmp_img) # saving depth image save_depth(subdir, tmp_depth) # saving pointcloud save_pointcloud(subdir, tmp_pointcloud) # saving sensors data save_sensors_data(subdir, tmp_sensors) # get and save dimensions save_dimensions_data(subdir, height, width, length, no_cajas, no_cajas_base, no_cajas_alto) # done print('Listo para capturar siguiente objeto')
def main(): # Create a Camera object zed = sl.Camera() init_params = sl.InitParameters() init_params.depth_mode = sl.DEPTH_MODE.NONE # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: print(repr(err)) zed.close() exit(1) # Get camera information sensors_data info = zed.get_camera_information() cam_model = info.camera_model if cam_model == sl.MODEL.ZED: print( "This tutorial only supports ZED-M and ZED2 camera models, ZED does not have additional sensors" ) exit(1) # Display camera information (model,S/N, fw version) print("Camera Model: " + str(cam_model)) print("Serial Number: " + str(info.serial_number)) print("Camera Firmware: " + str(info.camera_configuration.firmware_version)) print("Sensors Firmware: " + str(info.sensors_configuration.firmware_version)) # Display sensors parameters (imu,barometer,magnetometer) printSensorParameters(info.sensors_configuration.accelerometer_parameters ) # accelerometer configuration printSensorParameters(info.sensors_configuration.gyroscope_parameters ) # gyroscope configuration printSensorParameters(info.sensors_configuration.magnetometer_parameters ) # magnetometer configuration printSensorParameters(info.sensors_configuration.barometer_parameters ) # barometer configuration # Used to store the sensors timestamp to know if the sensors_data is a new one or not ts_handler = TimestampHandler() # Get Sensor Data for 5 seconds i = 0 sensors_data = sl.SensorsData() while i < 100: # retrieve the current sensors sensors_data # Depending on your Camera model or its firmware, differents sensors are presents. # They do not run at the same rate: Therefore, to do not miss samples we iterate as fast as we can and compare timestamp to know when a sensors_data is a new one # NOTE: There is no need to acquire images with grab() function. Sensors sensors_data are running in a separated internal capture thread. if zed.get_sensors_data( sensors_data, sl.TIME_REFERENCE.CURRENT) == sl.ERROR_CODE.SUCCESS: # Check if the data has been updated since the last time # IMU is the sensor with the highest rate if ts_handler.is_new(sensors_data.get_imu_data()): print("Sample " + str(i)) print(" - IMU:") # Filtered orientation quaternion quaternion = sensors_data.get_imu_data().get_pose( ).get_orientation().get() print(" \t Orientation: [ Ox: {0}, Oy: {1}, Oz {2}, Ow: {3} ]". format(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) # linear acceleration linear_acceleration = sensors_data.get_imu_data( ).get_linear_acceleration() print(" \t Acceleration: [ {0} {1} {2} ] [m/sec^2]".format( linear_acceleration[0], linear_acceleration[1], linear_acceleration[2])) # angular velocities angular_velocity = sensors_data.get_imu_data( ).get_angular_velocity() print( " \t Angular Velocities: [ {0} {1} {2} ] [deg/sec]".format( angular_velocity[0], angular_velocity[1], angular_velocity[2])) # Check if Magnetometer data has been updated (not the same frequency than IMU) if ts_handler.is_new(sensors_data.get_magnetometer_data()): magnetic_field_calibrated = sensors_data.get_magnetometer_data( ).get_magnetic_field_calibrated() print( " - Magnetometer\n \t Magnetic Field: [ {0} {1} {2} ] [uT]" .format(magnetic_field_calibrated[0], magnetic_field_calibrated[1], magnetic_field_calibrated[2])) # Check if Barometer data has been updated if ts_handler.is_new(sensors_data.get_barometer_data()): magnetic_field_calibrated = sensors_data.get_barometer_data( ).pressure print(" - Barometer\n \t Atmospheric pressure: {0} [hPa]". format(sensors_data.get_barometer_data().pressure)) i = i + 1 zed.close() return 0
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 ZED camera object zed = sl.Camera() # Set configuration parameters input_type = sl.InputType() init = sl.InitParameters(input_t=input_type) init.camera_resolution = sl.RESOLUTION.HD1080 #init.camera_resolution = sl.RESOLUTION.HD720 init.depth_mode = sl.DEPTH_MODE.PERFORMANCE init.coordinate_units = sl.UNIT.MILLIMETER #init.coordinate_units = sl.UNIT.METER # Open the camera err = zed.open(init) if err != sl.ERROR_CODE.SUCCESS : print(repr(err)) zed.close() exit(1) # Display help in console print_help() # Creating GUI gui = ZED_GUI() # Set runtime parameters after opening the camera runtime = sl.RuntimeParameters() runtime.sensing_mode = sl.SENSING_MODE.STANDARD # Prepare new image size to retrieve half-resolution images image_size = zed.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 = 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) point_cloud = sl.Mat() sensors_data = sl.SensorsData() key = ' ' event = ' ' while key != 113: err = zed.grab(runtime) event, values = gui.window.read(timeout=20) if event == "Exit" or event == sg.WIN_CLOSED: break if event == "Capture": try: height = float(values["-HEIGHT-"]) width = float(values["-WIDTH-"]) length = float(values["-LENGTH-"]) no_cajas = float(values["-NO_CAJAS-"]) no_cajas_base = float(values["-NO_CAJAS_BASE-"]) no_cajas_alto = float(values["-NO_CAJAS_ALTO-"]) sg.popup_timed('Capturando data, no mover la cámara ni el objeto', title='Capturando data', auto_close_duration=5, non_blocking=True) capture_data(zed, height, width, length, no_cajas, no_cajas_base, no_cajas_alto) gui.window["-HEIGHT-"].update('') gui.window["-WIDTH-"].update('') gui.window["-LENGTH-"].update('') gui.window["-NO_CAJAS-"].update('1') gui.window["-NO_CAJAS_BASE-"].update('1') gui.window["-NO_CAJAS_ALTO-"].update('1') sg.popup_timed('Captura guardada correctamente. Listo para continuar', title='Captura exitosa', auto_close_duration=5) except: sg.popup_timed('Debe ingresar valor numérico en los tres campos. Decimales con punto.', title='Valores incorrectos', auto_close_duration=5) pass if err == sl.ERROR_CODE.SUCCESS : # Retrieve the left image, depth image in the half-resolution zed.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size) zed.retrieve_image(depth_image_zed, sl.VIEW.DEPTH, sl.MEM.CPU, image_size) # capture sensors data zed.get_sensors_data(sensors_data, sl.TIME_REFERENCE.IMAGE) # 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 image_ocv = image_zed.get_data() depth_image_ocv = depth_image_zed.get_data() # concatenate both images to show them side by side in the GUI sbs_image = np.concatenate((image_ocv, depth_image_ocv), axis=1) imgbytes = cv2.imencode(".png", sbs_image)[1].tobytes() gui.window["-IMAGE-"].update(data=imgbytes) # show sensors data quaternion, linear_acceleration, angular_velocity, magnetic_field_calibrated, atmospheric_pressure =\ get_data_from_sensors(sensors_data) gui.window["-IMU_ORIENTATION-"].update(quaternion) gui.window["-IMU_ACCELERATION-"].update(linear_acceleration) gui.window["-IMU_ANG_VEL-"].update(angular_velocity) gui.window["-IMU_MAG_FIELD-"].update(magnetic_field_calibrated) gui.window["-IMU_ATM_PRESS-"].update(atmospheric_pressure) key = cv2.waitKey(10) cv2.destroyAllWindows() zed.close() gui.window.close() print("\nFINISH")
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 = "" ui = HSplit( VSplit( Log("Timestamp", color=3, border_color=5), Log("OLA Data", color=3, border_color=5), ), VSplit( HGauge(val=0, title="tracker accuracy", border_color=5), HGauge(val=0, title="mapper accuracy", border_color=5), )) run_dashing_display = True
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 main(): if not sys.argv or len(sys.argv) != 2: print("Only the path of the output SVO file should be passed as argument.") exit(1) path = 'svo_recordings/' try: os.makedirs(path) except FileExistsError: # directory already exists pass init = sl.InitParameters() init.camera_resolution = sl.RESOLUTION.HD720 init.depth_mode = sl.DEPTH_MODE.PERFORMANCE init.coordinate_units = sl.UNIT.MILLIMETER status = cam.open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit(1) path_output = path + sys.argv[1] recording_param = sl.RecordingParameters(path_output, sl.SVO_COMPRESSION_MODE.H264) err = cam.enable_recording(recording_param) if err != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit(1) runtime = sl.RuntimeParameters() runtime.sensing_mode = sl.SENSING_MODE.FILL print("SVO is Recording, use Ctrl-C to stop.") frames_recorded = 0 # Declare your sl.Mat matrices image_size = cam.get_camera_information().camera_resolution image_size.width = image_size.width /2 image_size.height = image_size.height /2 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) # Used to store the sensors timestamp to know if the sensors_data is a new one or not ts_handler = TimestampHandler() sensors_data = sl.SensorsData() rows_list = [] key = ' ' while key != 113: if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS : frames_recorded += 1 # retrieve the current sensors sensors_data # Depending on your Camera model or its firmware, differents sensors are presents. # They do not run at the same rate: Therefore, to do not miss samples we iterate as fast as we can and compare timestamp to know when a sensors_data is a new one # NOTE: There is no need to acquire images with grab() function. Sensors sensors_data are running in a separated internal capture thread. if cam.get_sensors_data(sensors_data, sl.TIME_REFERENCE.CURRENT) == sl.ERROR_CODE.SUCCESS : # Check if the data has been updated since the last time # IMU is the sensor with the highest rate if ts_handler.is_new(sensors_data.get_imu_data()): print("Sample " + str(frames_recorded)) print(" - IMU:") # Filtered orientation quaternion quaternion = sensors_data.get_imu_data().get_pose().get_orientation().get() print(" \t Orientation: [ Ox: {0}, Oy: {1}, Oz {2}, Ow: {3} ]".format(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) # linear acceleration linear_acceleration = sensors_data.get_imu_data().get_linear_acceleration() print(" \t Acceleration: [ {0} {1} {2} ] [m/sec^2]".format(linear_acceleration[0], linear_acceleration[1], linear_acceleration[2])) # angular velocities angular_velocity = sensors_data.get_imu_data().get_angular_velocity() print(" \t Angular Velocities: [ {0} {1} {2} ] [deg/sec]".format(angular_velocity[0], angular_velocity[1], angular_velocity[2])) dict1 = {'frame':frames_recorded, 'time':sensors_data.get_imu_data().timestamp.get_microseconds(), 'Ox':quaternion[0] ,'Oy':quaternion[1], 'Oz':quaternion[2], 'Ow':quaternion[3], 'Ax':linear_acceleration[0], 'Ay':linear_acceleration[1], 'Az':linear_acceleration[2], 'AVx':angular_velocity[0], 'AVy':angular_velocity[1], 'AVz':angular_velocity[2]} rows_list.append(dict1) cam.retrieve_image(image_zed, sl.VIEW.LEFT, sl.MEM.CPU, image_size) cam.retrieve_image(depth_image_zed, sl.VIEW.DEPTH, sl.MEM.CPU, image_size) # 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 image_ocv = image_zed.get_data() depth_image_ocv = depth_image_zed.get_data() cv2.imshow("Image", image_ocv) cv2.imshow("Depth", depth_image_ocv) key = cv2.waitKey(1) print("Frame count: " + str(frames_recorded), end="\r") df = pd.DataFrame(rows_list) df.to_csv(path_output[:-4]+'.csv') cv2.destroyAllWindows() cam.disable_recording() cam.close() print("\nFINISH")
def _poll(self): last_time = self.frame_time self.frame_time = time.time() - self.start_time self.frame_count += 1 # # get the frames # runtime_parameters = sl.RuntimeParameters() if self.enable_rgb: image = sl.Mat() if self.enable_depth: depth = sl.Mat() point_cloud = sl.Mat() # Grab an image, a RuntimeParameters object must be given to grab() if self.zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # A new image is available if grab() returns ERROR_CODE.SUCCESS if self.enable_rgb: # Get the left image self.zed.retrieve_image(image, sl.VIEW.LEFT) # Get an np array from ZED Matrix rgba_np_image = image.get_data() # Drop the alpha channel rgb_np_image = rgba_np_image[:, :, :-1] # Convert rgb to bgr color_image = rgb_np_image[:, :, ::-1] if self.verbose: # Get the image timestamp timestamp = self.zed.get_timestamp(sl.TIME_REFERENCE.IMAGE) print( "Image resolution: {0} x {1} || Image timestamp: {2}\n" .format(image.get_width(), image.get_height(), timestamp.get_milliseconds()), end="\r") if self.enable_depth: # Retrieve depth matrix. Depth is aligned on the left RGB image self.zed.retrieve_measure(depth, sl.MEASURE.DEPTH) depth_image = depth.get_data() # Retrieve colored point cloud self.zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA) self.point_cloud = point_cloud.get_data() if self.verbose and depth_image is not None: y, x = depth_image.shape x = x // 2 y = y // 2 depth_value = depth_image[x, y] print("Distance to Camera at ({0}, {1}): {2} mm".format( x, y, depth_value), end="\r") if self.resize: import cv2 if self.width != WIDTH or self.height != HEIGHT: color_image = cv2.resize( color_image, (self.width, self.height), cv2.INTER_NEAREST) if self.enable_rgb else None depth_image = cv2.resize( depth_image, (self.width, self.height), cv2.INTER_NEAREST) if self.enable_depth else None if self.channels != CHANNELS: color_image = cv2.cvtColor( color_image, cv2.COLOR_BRG2GRAY) if self.enable_rgb else None self.color_image = color_image self.depth_image = depth_image if self.enable_imu: sensors_data = sl.SensorsData() self.zed.get_sensors_data(sensors_data, sl.TIME_REFERENCE.CURRENT) if self.zed_timestamp_handler.is_new( sensors_data.get_imu_data()): self.imu_quaternion = sensors_data.get_imu_data().get_pose( ).get_orientation().get() self.linear_acceleration = sensors_data.get_imu_data( ).get_linear_acceleration() self.angular_velocity = sensors_data.get_imu_data( ).get_angular_velocity() if self.verbose: print("IMU Orientation: {}".format( self.imu_quaternion)) print("IMU Acceleration: {} [m/sec^2]".format( self.linear_acceleration)) print("IMU Angular Velocity: {} [deg/sec]".format( self.angular_velocity)) # Check if Magnetometer data has been updated if self.zed_timestamp_handler.is_new( sensors_data.get_magnetometer_data()): self.magnetic_field = sensors_data.get_magnetometer_data( ).get_magnetic_field_calibrated() if self.verbose: print("Magnetometer Magnetic Field: {} [uT]".format( self.magnetic_field)) # Check if Barometer data has been updated if self.zed_timestamp_handler.is_new( sensors_data.get_barometer_data()): self.barometer_pressure = sensors_data.get_barometer_data( ).pressure if self.verbose: print( "Barometer Atmospheric pressure: {} [hPa]".format( self.barometer_pressure))
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.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP init_params.coordinate_units = sl.UNIT.METER # Open the camera err = zed.open(init_params) if err != sl.ERROR_CODE.SUCCESS: print(repr(err)) zed.close() exit(1) cam_model = zed.get_camera_information().camera_model if cam_model == sl.MODEL.ZED: print("This tutorial only supports ZED-M and ZED2 camera models") exit(1) # Get Sensor Data for 2 seconds (800 samples) i = 0 data = sl.SensorsData() first_ts = sl.Timestamp() prev_imu_ts = sl.Timestamp() prev_baro_ts = sl.Timestamp() prev_mag_ts = sl.Timestamp() while i < 800: # Get Sensor Data not synced with image frames if zed.get_sensors_data( data, sl.TIME_REFERENCE.CURRENT) != sl.ERROR_CODE.SUCCESS: print("Error retrieving Sensor Data") break imu_ts = data.get_imu_data().timestamp if i == 0: first_ts = imu_ts # Check if Sensors Data are updated if prev_imu_ts.data_ns == imu_ts.data_ns: continue prev_imu_ts = imu_ts print("*** Sample #" + str(i)) seconds = data.get_imu_data().timestamp.get_seconds( ) - first_ts.get_seconds() print(" * Relative timestamp: " + str(seconds) + " sec") # Filtered orientation quaternion zed_imu = data.get_imu_data() #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)) if cam_model == sl.MODEL.ZED2: # IMU temperature location = sl.SENSOR_LOCATION.IMU temp = data.get_temperature_data().get(location) if temp != -1: print(" * IMU temperature: " + str(temp) + "C") # Check if Magnetometer Data are updated mag_ts = data.get_magnetometer_data().timestamp if (prev_mag_ts.data_ns != mag_ts.data_ns): prev_mag_ts = mag_ts mx = round(data.get_magnetometer_data(). get_magnetic_field_calibrated()[0]) my = round(data.get_magnetometer_data(). get_magnetic_field_calibrated()[1]) mz = round(data.get_magnetometer_data(). get_magnetic_field_calibrated()[2]) print(" * Magnetic Fields [uT]: x: {0}, y: {1}, z: {2}".format( mx, my, mz)) baro_ts = data.get_barometer_data().timestamp if (prev_baro_ts.data_ns != baro_ts.data_ns): prev_baro_ts = baro_ts # Atmospheric pressure print(" * Atmospheric pressure [hPa]: " + str(data.get_barometer_data().pressure)) # Barometer temperature location = sl.SENSOR_LOCATION.BAROMETER baro_temp = data.get_temperature_data().get(location) if baro_temp != -1: print(" * Barometer temperature: " + str(temp) + "C") # Camera temperatures location_left = sl.SENSOR_LOCATION.ONBOARD_LEFT location_right = sl.SENSOR_LOCATION.ONBOARD_RIGHT left_temp = data.get_temperature_data().get(location_left) right_temp = data.get_temperature_data().get(location_right) print(" * Camera left temperature: " + str(left_temp)) print(" * Camera right temperature: " + str(right_temp)) i = i + 1 zed.close() return 0