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 update_pose(self): pose = sl.Pose() if self.zed.get_position(pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD ) == sl.TRACKING_STATE.TRACKING_STATE_OK: t = pose.get_translation() self.translation = [t.get()[0], t.get()[1], t.get()[2]] o = pose.get_orientation() self.orientation = [o.get()[0], o.get()[1], o.get()[2], o.get()[3]]
def pop(self, local_pose, world_pose, image, depth, objects): ''' pop pop data from the FIFO system Parameters: local_pose (sl.Pose): pose of the camera in camera reference frame at objects timestamp world_pose (sl.Pose): pose of the camera in world reference frame at objects timestamp image (sl.Mat): image data at objects timestamp depth (sl.Mat): depth data at objects timestamp objects (sl.Objects): objects in the past ''' objects = sl.Objects() local_pose = sl.Pose() world_pose = sl.Pose() if self.objects_tracked_queue: tracked_merged_obj = self.objects_tracked_queue[0] if (self.init_queue_ts.data_ns == 0): self.init_queue_ts = tracked_merged_obj.timestamp targetTS_ms = tracked_merged_obj.timestamp.get_milliseconds() local_pose = self.find_closest_local_pose_from_ts(targetTS_ms) world_pose = self.find_closest_world_pose_from_ts(targetTS_ms) if WITH_IMAGE_RETENTION: tmp_image = self.find_closest_image_from_ts(targetTS_ms) tmp_image.copy_to(image) tmp_image.free(sl.MEM.CPU) self.image_map_ms[targetTS_ms].free(sl.MEM.CPU) del self.image_map_ms[targetTS_ms] tmp_depth = self.find_closest_depth_from_ts(targetTS_ms) tmp_depth.copy_to(depth) tmp_depth.free(sl.MEM.CPU) self.depth_map_ms[targetTS_ms].free(sl.MEM.CPU) del self.depth_map_ms[targetTS_ms] objects = tracked_merged_obj self.objects_tracked_queue.popleft() return local_pose, world_pose, image, depth, objects
def update_pose_z(self): pose = sl.Pose() if self.zed.get_position(pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD ) == sl.TRACKING_STATE.TRACKING_STATE_OK: t = pose.get_translation() self.translation = [t.get()[0], t.get()[1], t.get()[2]] o = pose.get_orientation() euler = self.quaternion_to_euler(o.get()[0], o.get()[1], o.get()[2], o.get()[3]) new_quat = self.euler_to_quaternion(euler[2], 0, 0) self.orientation = new_quat
def find_closest_world_pose_from_ts(self, timestamp): ''' find_closest_world_pose_from_ts Find the sl.Pose (in world reference frame) that matches the given timestamp Parameters: timestamp (int): timestamp in ms (or at least in the same unit as self.cam_world_pose_map_ms) Return: The matching sl.Pose ''' pose = sl.Pose() if timestamp in self.cam_world_pose_map_ms.keys(): pose = self.cam_world_pose_map_ms[timestamp] return pose
def get_zed_pose(zed, verbose=False): """ Parameters ---------- zed: sl.Camera The ZED camera object verbose: bool If true, will print the translation + orientation to the console Returns ------- list Pose as [time, tx, ty, tz, ox, oy, oz, ow] time is given in seconds. The camera position is [tx, ty, tz] And orientation is the quaternion [ox, oy, oz, ow] """ # Call zed.grab() each time before calling this function zed_pose = sl.Pose() # Get the pose of the camera relative to the world frame state = zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD) # Display 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) time_nsec = zed_pose.timestamp.get_nanoseconds() time_sec = float(time_nsec) / 1e9 if verbose: print( "Translation: tx: {0}, ty: {1}, tz: {2}, timestamp: {3}".format( tx, ty, tz, time_sec)) # Display 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) if verbose: print("Orientation: ox: {0}, oy: {1}, oz: {2}, ow: {3}\n".format( ox, oy, oz, ow)) return [time_sec, tx, ty, tz, ox, oy, oz, ow]
def main(): imu = PX4Data() # Create a Camera object zed = sl.Camera() # Create a InitParameters object and set configuration parameters init_params = sl.InitParameters() init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_VGA # Use HD1080 video mode init_params.camera_fps = 120 # Set fps at 60 init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD init_params.coordinate_units = sl.UNIT.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.TrackingParameters(init_pos=py_transform) err = zed.enable_tracking(tracking_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) # Capture 50 frames and stop i = 0 image = sl.Mat() zed_pose = sl.Pose() zed_imu = sl.IMUData() runtime_parameters = sl.RuntimeParameters() runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD # Use STANDARD sensing mode prevTimeStamp = 0 file = open('data/data.txt', 'w') key = 0 depth = sl.Mat() point_cloud = sl.Mat() pcList = [] while key != 113: # 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 timestamp = zed.get_timestamp(sl.TIME_REFERENCE.TIME_REFERENCE_CURRENT) # Get the timestamp at the time the image was dt = (timestamp - prevTimeStamp) * 1.0 / 10 ** 9 if dt > 0.03: # Get the pose of the left eye of the camera with reference to the world frame zed.get_position(zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD) # Display the translation and timestamp py_translation = sl.Translation() gnd_pos = zed_pose.get_translation(py_translation).get() tx = round(gnd_pos[0], 3) ty = round(gnd_pos[1], 3) tz = round(gnd_pos[2], 3) print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp)) # Display the orientation quaternion py_orientation = sl.Orientation() quat = zed_pose.get_orientation(py_orientation).get() ox = round(quat[0], 3) oy = round(quat[1], 3) oz = round(quat[2], 3) ow = round(quat[3], 3) print("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format(ox, oy, oz, ow)) zed.retrieve_image(image, sl.VIEW.VIEW_LEFT) img = image.get_data() cv2.imwrite('data/images/' + str(timestamp) + '.png', img) zed.retrieve_measure(depth, sl.MEASURE.MEASURE_DEPTH) # Retrieve colored point cloud. Point cloud is aligned on the left image. zed.retrieve_measure(point_cloud, sl.MEASURE.MEASURE_XYZRGBA) print(point_cloud.get_data().shape) pc = np.reshape(point_cloud.get_data(), (1, 376, 672, 4)) pcList.append(pc) cv2.imshow("ZED", img) key = cv2.waitKey(1) prevTimeStamp = timestamp print(dt) print("Image resolution: {0} x {1} || Image timestamp: {2}\n".format(image.get_width(), image.get_height(), timestamp)) file.write('%d ' '%.4f %.4f %.4f ' '%.4f %.4f %.4f %.4f ' '%.4f %.4f %.4f ' '%.4f %.4f %.4f ' '%.4f %.4f %.4f ' '%.4f %.4f %.4f ' '%.4f %.4f %.4f %.4f \n' % (timestamp, tx, ty, tz, ox, oy, oz, ow, imu.acc.x, imu.acc.y, imu.acc.z, imu.gyr.x, imu.gyr.y, imu.gyr.z, imu.gps.x, imu.gps.y, imu.gps.z, imu.vel.x, imu.vel.y, imu.vel.z, imu.quat.x, imu.quat.y, imu.quat.z, imu.quat.w)) i = i + 1 # Close the camera pc = np.concatenate(pcList, axis=0) np.save('pc', pc) zed.close() file.close() imu.close()
def extract_zed_svofile(svo_input_path, output_path, other_frames=None): # Set output path to save frames output_path = Path(output_path) if not os.path.exists(output_path / 'left'): os.makedirs(output_path / 'left') if other_frames is not None: assert other_frames == 'rgb_right' or other_frames == 'depth_left', 'Parameter:: other_frames = [rgb_right | depth_left]' zed, runtime = zutils.configure_zed_camera(img_capture=False, svo_file=svo_input_path) # 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() # Prepare single image containers left_image = sl.Mat() right_image = sl.Mat() depth_image = sl.Mat() camera_pose = sl.Pose() pose_filename = output_path / 'pose.txt' calib_filename = output_path / 'calib.txt' with open(pose_filename, 'w') as file: while True: if zed.grab(runtime) == sl.ERROR_CODE.SUCCESS: svo_position = zed.get_svo_position() #Retrieve SVO images zed.retrieve_image(left_image, sl.VIEW.VIEW_LEFT) if other_frames == 'rgb_right': zed.retrieve_image(right_image, sl.VIEW.VIEW_RIGHT) elif other_frames == 'depth_left': zed.retrieve_measure( depth_image, sl.MEASURE.MEASURE_DEPTH) # depth uint16 # Generate file names filename1 = output_path / 'left' / ( "%s.png" % str(svo_position - 1).zfill(6)) # Save Left images cv2.imwrite(str(filename1), left_image.get_data()) if other_frames is not None: if other_frames == 'rgb_right': if not os.path.exists(output_path / 'right'): os.makedirs(output_path / 'right') # Save right images filename2 = output_path / 'right' / ( "%s.png" % str(svo_position - 1).zfill(6)) cv2.imwrite(str(filename2), right_image.get_data()) else: if not os.path.exists(output_path / 'depth'): os.makedirs(output_path / 'depth') # Save depth images (convert to uint16) filename2 = output_path / 'depth' / ( "%s.png" % str(svo_position - 1).zfill(6)) cv2.imwrite(str(filename2), depth_image.get_data().astype(np.uint16)) # Save pose data into txt file zed.get_position(camera_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD) rotation_matrix = camera_pose.get_rotation_matrix().r translation = camera_pose.get_translation().get() #print(rotation_matrix) #print(translation) RT = np.hstack((rotation_matrix, translation.reshape((3, 1)))) # cam_param['euler_angle'] = camera_pose.get_euler_angles(radian=True) # cam_param['quaternion_orientation'] = camera_pose.get_orientation().get() # cam_param['rotation_matrix'] = camera_pose.get_rotation_matrix().r # cam_param['rodrigues_rotation'] = camera_pose.get_rotation_vector() # cam_param['camera_position'] = camera_pose.get_translation().get() file.write(",".join(map(str, RT.reshape(-1))) + "\n") cam_param = zed.get_camera_information( ).calibration_parameters.left_cam fx, fy = cam_param.fx, cam_param.fy cx, cy = cam_param.cx, cam_param.cy np.savetxt(calib_filename, (fx, fy, cx, cy)) # 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 zed.close() return 0
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) cam = sl.Camera() init = sl.InitParameters() init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 # Use HD720 video mode (default init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP # Use a right-handed Y-up coordinate system init.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters # new for SVO init.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_NONE ## 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.TrackingParameters(transform) cam.enable_tracking(tracking) cam.enable_spatial_mapping(spatial) #from Positional Tracking: # Track the camera position until Keyboard Interupt (ctrl-C) zed_pose = sl.Pose() zed_imu = sl.IMUData() runtime_parameters = sl.RuntimeParameters() path = '/media/nvidia/SD1/translation.csv' position_file = open(path, 'w') #END from positional tracking pymesh = sl.Mesh() print("Processing...") #new for SVO path_output = sys.argv[1] err = cam.enable_recording( path_output, sl.SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_AVCHD) if err != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit(1) print("SVO is Recording, use Ctrl-C to stop.") frames_recorded = 0 ## while True: try: cam.grab(runtime) # new for SVO state = cam.record() if state["status"]: frames_recorded += 1 print("Frame count: " + str(frames_recorded), end="\r") ## cam.request_mesh_async() # Get the pose of the left eye of the camera with reference to the world frame cam.get_position(zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD) cam.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE) # 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) position_file.write("{0},{1},{2},{3}\n".format( tx, ty, tz, zed_pose.timestamp)) except KeyboardInterrupt: cam.extract_whole_mesh(pymesh) cam.disable_tracking() cam.disable_spatial_mapping() # new for .svo cam.disable_recording() ## filter_params = sl.MeshFilterParameters() filter_params.set(sl.MESH_FILTER.MESH_FILTER_HIGH) print("Filtering params : {0}.".format( pymesh.filter(filter_params))) apply_texture = pymesh.apply_texture( sl.MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGBA) print("Applying texture : {0}.".format(apply_texture)) print_mesh_information(pymesh, apply_texture) save_filter(filter_params) save_mesh(pymesh) cam.close() position_file.close() save_position(path) print("\nFINISH") raise
def main(abs_k=None, depth=True, throttle_SP=None, steer_SP=None): # Create a Camera object zed = sl.Camera() # Initilialize Camera Parameters init_params = init_zed(depth) # Open the camera zed_cam = zed.open(init_params) if zed_cam != sl.ERROR_CODE.SUCCESS: print("Error: Zed Camera could not be opened.") exit() print("") print("Zed Camera succesfully opened.") # # Enable positional tracking # if depth: # transform = sl.Transform() # tracking_parameters = sl.TrackingParameters(init_pos=transform) # tracking = zed.enable_tracking(tracking_parameters) # # if tracking != sl.ERROR_CODE.SUCCESS: # exit() # # print("") # print("Tracking succesfully enabled.") runtime_parameters = sl.RuntimeParameters() zed_pose = sl.Pose() # Getting Pose mat = sl.Mat() with open('/media/ctuxavier/ADATASE730H/zed_recording.csv', mode='w') as csv_file: # Open csv file print("Start Recording") print("Press Ctrl+C to stop") fieldnames = ['Tx', 'Ty', 'Tz', 'Pitch', 'Roll', 'Yaw', 'Timestamp'] writer = csv.DictWriter(csv_file, fieldnames=fieldnames) writer.writeheader() with open('/media/ctuxavier/ADATASE730H/img_data.txt', 'w') as data_file: data_file.write("grab_timestamp, write_timestamp\n") threads = [] # Init and start image acquisition thread ImageAcquisition_t = Thread(target=ImageAcquisition, args=(zed, mat, runtime_parameters)) ImageAcquisition_t.daemon = True threads.append(ImageAcquisition_t) ImageAcquisition_t.start() # if depth: # # Init and start data acquisition thread # DataAcquisiton_t = Thread(target=DataAcquisiton, args=(zed, zed_pose, runtime_parameters)) # DataAcquisiton_t.daemon = True # threads.append(DataAcquisiton_t) # DataAcquisiton_t.start() # start car detection thread by Pavel Jahoda - for autonomous car chasing if carChase: car_detection_t = Thread(target=CarDetection, args=(throttle_SP, steer_SP)) car_detection_t.daemon = True threads.append(car_detection_t) car_detection_t.start() # Init and start image segmentation thread # image_segmentation_t = Thread(target=image_segmentation, args=(abs_k, )) # image_segmentation_t.daemon = True # threads.append(image_segmentation) # image_segmentation_t.start() try: for thread in threads: thread.join() except (KeyboardInterrupt, SystemExit): print("Zed data and images threads closed!")
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.RESOLUTION_HD720 # Use HD720 video mode (default fps: 60) # Use a right-handed Y-up coordinate system init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP init_params.coordinate_units = sl.UNIT.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.TrackingParameters(init_pos=py_transform) err = zed.enable_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_imu = sl.IMUData() runtime_parameters = sl.RuntimeParameters() #added! path = '/media/nvidia/SD1/position.csv' position_file = open(path,'w') 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.REFERENCE_FRAME_WORLD) zed.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE) # 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) position_file.write("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp)) # 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) position_file.write("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format(ox, oy, oz, ow)) # Display the Rotation Matrix py_rotationMatrix = zed_pose.get_rotation_matrix() position_file.write("Got Rotation Matrix, but did not print\n") # Display the Rotation Vector py_rotationVector = zed_pose.get_rotation_vector() rx = round(py_rotationVector[0], 3) ry = round(py_rotationVector[1], 3) rz = round(py_rotationVector[2], 3) position_file.write("Rotation Vector: Rx: {0}, Ry: {1}, Rz {2}, Timestamp: {3}\n".format(rx, ry, rz, zed_pose.timestamp)) # Display the Euler Angles py_eulerAngles = zed_pose.get_euler_angles() ex = round(py_eulerAngles[0], 3) ey = round(py_eulerAngles[1], 3) ez = round(py_eulerAngles[2], 3) position_file.write("EulerAngles: EAx: {0}, EAy: {1}, EAz {2}, Timestamp: {3}\n".format(ex, ey, ez, zed_pose.timestamp)) i = i + 1 # Close the camera zed.close() # Close file position_file.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")
from object_detection.utils import ops as utils_ops from object_detection.utils import label_map_util from object_detection.utils import visualization_utils as vis_util # Initial Setup class vars_of_interest(): def __init__(self): self.coord = [] self.rotation = [] self.target_list = [] self.orientation = [] voi = vars_of_interest() zed_pose = sl.Pose() debug = sys.argv[1] zed_robot = basis.essentials(debug) def load_image_into_numpy_array(image): ar = image.get_data() ar = ar[:, :, 0:3] (im_height, im_width, channels) = image.get_data().shape return np.array(ar).reshape((im_height, im_width, 3)).astype(np.uint8) def load_depth_into_numpy_array(depth): ar = depth.get_data() ar = ar[:, :, 0:4] (im_height, im_width, channels) = depth.get_data().shape
def save_parameters(zed, filename): print("Saving parameters of the camera >>> ", filename) def get_camera_parameters(cam_calibration): res_param = {} left = cam_calibration.left_cam right = cam_calibration.right_cam # Save focal length (in pixel) res_param['focal_length'] = {} res_param['focal_length']['left'] = [left.fx, left.fy] res_param['focal_length']['right'] = [right.fx, right.fy] # Save distortion factor res_param['distortion_factor'] = {} res_param['distortion_factor']['left'] = left.disto res_param['distortion_factor']['right'] = right.disto # Optical center (in pixel) res_param['optical_center'] = {} res_param['optical_center']['left'] = [left.cx, left.cy] res_param['optical_center']['right'] = [right.cx, right.cy] # Save field of view (in degrees) [diagonal, horizontal, vertical] res_param['fov'] = {} res_param['fov']['left'] = [left.d_fov, left.h_fov, left.v_fov] res_param['fov']['right'] = [right.d_fov, right.h_fov, right.v_fov] # Save extrinsic parameters between two sensors res_param['Rotation'] = cam_calibration.R res_param['Translation'] = cam_calibration.T return res_param cam_param = {} # Save image information cam_param['resolution'] = zed.get_resolution() cam_param['fps'] = zed.get_current_fps() cam_param['timestamp'] = zed.get_timestamp( sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE) cam_param['confidence_threshold'] = zed.get_confidence_threshold() cam_param['depth_range'] = [ zed.get_depth_min_range_value(), zed.get_depth_max_range_value() ] # Save camera settings # More details refer here: # file:///home/sokim/Desktop/data_collection/zed-python-api/doc/build/html/video.html?highlight=auto%20white%20balance#pyzed.sl.CAMERA_SETTINGS gain = zed.get_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_GAIN) brightness = zed.get_camera_settings( sl.CAMERA_SETTINGS.CAMERA_SETTINGS_BRIGHTNESS) contrast = zed.get_camera_settings( sl.CAMERA_SETTINGS.CAMERA_SETTINGS_CONTRAST) exposure = zed.get_camera_settings( sl.CAMERA_SETTINGS.CAMERA_SETTINGS_EXPOSURE) hue = zed.get_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_HUE) saturation = zed.get_camera_settings( sl.CAMERA_SETTINGS.CAMERA_SETTINGS_SATURATION) whitebalance = zed.get_camera_settings( sl.CAMERA_SETTINGS.CAMERA_SETTINGS_WHITEBALANCE) cam_param['settings'] = [ gain, brightness, contrast, exposure, hue, saturation, whitebalance ] # Save camera intrinsic and extrinsic parameters cam_info_raw = zed.get_camera_information().calibration_parameters_raw cam_info = zed.get_camera_information().calibration_parameters cam_param['unrectified'] = get_camera_parameters(cam_info_raw) cam_param['rectified'] = get_camera_parameters(cam_info) # Save camera position camera_pose = sl.Pose() zed.get_position(camera_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD) cam_param['euler_angle'] = camera_pose.get_euler_angles(radian=True) cam_param['quaternion_orientation'] = camera_pose.get_orientation().get() cam_param['rotation_matrix'] = camera_pose.get_rotation_matrix().r cam_param['rodrigues_rotation'] = camera_pose.get_rotation_vector() cam_param['camera_position'] = camera_pose.get_translation().get() with open(filename + '_param.txt', 'w') as file: file.write(str(cam_param))
def main(): if len(sys.argv) != 3: print( "Please specify collection time (seconds), and path to save files") exit() max_time = sys.argv[1] print(max_time) path = sys.argv[2] print(path) #delay program 60 sec, so that user can get to start location print( "\nYou have 60 seconds to get to start location before program will begin" ) time.sleep(60) print("\nInitializing camera") cam = sl.Camera() init = sl.InitParameters() # Use HD720 video mode (default Use a right-handed Y-up coordinate system) init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP # Set units in meters init.coordinate_units = sl.UNIT.UNIT_METER 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.TrackingParameters(transform) cam.enable_tracking(tracking) cam.enable_spatial_mapping(spatial) #for Positional Tracking: zed_pose = sl.Pose() zed_imu = sl.IMUData() runtime_parameters = sl.RuntimeParameters() #temporary file to save translation data to, until you know the filename (given from the user) #path = '/media/nvidia/SD1/translation.csv' position_file = open((path + ".csv"), 'w+') pymesh = sl.Mesh() print("Camera setup") #get start time start_time = time.time() print("Starting to collect data") while (time.time() - start_time) < float(max_time): cam.grab(runtime) cam.request_mesh_async() # Get the pose of the left eye of the camera with reference to the world frame cam.get_position(zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD) cam.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE) # 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) #position_file.write("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp)) position_file.write("{0},{1},{2},{3}\n".format(tx, ty, tz, zed_pose.timestamp)) print( "Finished collecting data, extracting mesh, saving and shutting down camera" ) cam.extract_whole_mesh(pymesh) cam.disable_tracking() cam.disable_spatial_mapping() filter_params = sl.MeshFilterParameters() filter_params.set(sl.MESH_FILTER.MESH_FILTER_HIGH) print("Filtering params : {0}.".format(pymesh.filter(filter_params))) apply_texture = pymesh.apply_texture( sl.MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGBA) print("Applying texture : {0}.".format(apply_texture)) #print_mesh_information(pymesh, apply_texture) #save_filter(filter_params) #save_mesh(pymesh) cam.close() position_file.close() #save_position(path) save_all_path_arg(filter_params, pymesh, path) print("\nFINISH")
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 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()
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")
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() zed.enable_positional_tracking(tracking_params) runtime = sl.RuntimeParameters() camera_pose = sl.Pose() camera_info = zed.get_camera_information() # Create OpenGL viewer viewer = gl.GLViewer() viewer.init(len(sys.argv), sys.argv, camera_info.camera_model) py_translation = sl.Translation() pose_data = sl.Transform() text_translation = "" text_rotation = "" while viewer.is_available(): if zed.grab(runtime) == sl.ERROR_CODE.SUCCESS: tracking_state = zed.get_position(camera_pose)
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(): #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() #init = sl.InitParameters(svo_input_filename=filepath) init = sl.InitParameters() #new init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 # Use HD720 video mode (default # Use a right-handed Y-up coordinate system init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP init.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters 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.TrackingParameters(transform) cam.enable_tracking(tracking) cam.enable_spatial_mapping(spatial) #from Positional Tracking: # Track the camera position until Keyboard Interupt (ctrl-C) #i = 0 zed_pose = sl.Pose() zed_imu = sl.IMUData() runtime_parameters = sl.RuntimeParameters() path = '/media/nvidia/SD1/translation.csv' position_file = open(path, 'w') #END from positional tracking pymesh = sl.Mesh() print("Processing...") #for i in range(200): while True: try: cam.grab(runtime) cam.request_mesh_async() # Get the pose of the left eye of the camera with reference to the world frame cam.get_position(zed_pose, sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD) cam.get_imu_data(zed_imu, sl.TIME_REFERENCE.TIME_REFERENCE_IMAGE) # 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) #position_file.write("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format(tx, ty, tz, zed_pose.timestamp)) position_file.write("{0},{1},{2},{3}\n".format( tx, ty, tz, zed_pose.timestamp)) except KeyboardInterrupt: cam.extract_whole_mesh(pymesh) cam.disable_tracking() cam.disable_spatial_mapping() filter_params = sl.MeshFilterParameters() filter_params.set(sl.MESH_FILTER.MESH_FILTER_HIGH) print("Filtering params : {0}.".format( pymesh.filter(filter_params))) apply_texture = pymesh.apply_texture( sl.MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGBA) print("Applying texture : {0}.".format(apply_texture)) #print_mesh_information(pymesh, apply_texture) #save_filter(filter_params) #save_mesh(pymesh) cam.close() position_file.close() #save_position(path) save_all(filter_params, pymesh, path) print("\nFINISH") raise
def main(): print("Running Plane Detection sample ... Press 'q' to quit") # Create a camera object zed = sl.Camera() # Set configuration parameters init = sl.InitParameters() init.coordinate_units = sl.UNIT.METER init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP # OpenGL coordinate system # If applicable, use the SVO given as parameter # Otherwise use ZED live stream if len(sys.argv) == 2: filepath = sys.argv[1] print("Reading SVO file: {0}".format(filepath)) init.set_from_svo_file(filepath) # Open the camera status = zed.open(init) if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit(1) # Get camera info and check if IMU data is available camera_infos = zed.get_camera_information() has_imu = camera_infos.sensors_configuration.gyroscope_parameters.is_available # Initialize OpenGL viewer viewer = gl.GLViewer() viewer.init( camera_infos.camera_configuration.calibration_parameters.left_cam, has_imu) image = sl.Mat() # current left image pose = sl.Pose() # positional tracking data plane = sl.Plane() # detected plane mesh = sl.Mesh() # plane mesh find_plane_status = sl.ERROR_CODE.SUCCESS tracking_state = sl.POSITIONAL_TRACKING_STATE.OFF # Timestamp of the last mesh request last_call = time.time() user_action = gl.UserAction() user_action.clear() # Enable positional tracking before starting spatial mapping zed.enable_positional_tracking() runtime_parameters = sl.RuntimeParameters() runtime_parameters.measure3D_reference_frame = sl.REFERENCE_FRAME.WORLD while viewer.is_available(): if zed.grab(runtime_parameters) == 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 tracking_state == sl.POSITIONAL_TRACKING_STATE.OK: # Compute elapse time since the last call of plane detection duration = time.time() - last_call # Ask for a mesh update on mouse click if user_action.hit: image_click = [ user_action.hit_coord[0] * camera_infos. camera_configuration.camera_resolution.width, user_action.hit_coord[1] * camera_infos. camera_configuration.camera_resolution.height ] find_plane_status = zed.find_plane_at_hit( image_click, plane) # Check if 500 ms have elapsed since last mesh request if duration > .5 and user_action.press_space: # Update pose data (used for projection of the mesh over the current image) reset_tracking_floor_frame = sl.Transform() find_plane_status = zed.find_floor_plane( plane, reset_tracking_floor_frame) last_call = time.time() if find_plane_status == sl.ERROR_CODE.SUCCESS: mesh = plane.extract_mesh() viewer.update_mesh(mesh, plane.type) user_action = viewer.update_view(image, pose.pose_data(), tracking_state) viewer.exit() image.free(sl.MEM.CPU) mesh.clear() # Disable modules and close camera zed.disable_positional_tracking() zed.close()
track_view_generator = cv_viewer.TrackingViewer( tracks_resolution, camera_config.camera_fps, init_params.depth_maximum_distance) track_view_generator.set_camera_calibration( camera_config.calibration_parameters) image_track_ocv = np.zeros( (tracks_resolution.height, tracks_resolution.width, 4), np.uint8) # Will store the 2D image and tracklet views global_image = np.full( (display_resolution.height, display_resolution.width + tracks_resolution.width, 4), [245, 239, 239, 255], np.uint8) # Camera pose cam_w_pose = sl.Pose() cam_c_pose = sl.Pose() quit_app = False while (viewer.is_available() and (quit_app == False)): if zed.grab(runtime_params) == sl.ERROR_CODE.SUCCESS: # Retrieve objects returned_state = zed.retrieve_objects(objects, obj_runtime_param) if (returned_state == sl.ERROR_CODE.SUCCESS and objects.is_new): # Retrieve point cloud zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA, sl.MEM.CPU, point_cloud_res) point_cloud.copy_to(point_cloud_render) # Retrieve image