def main(): # Create a PyZEDCamera object zed = zcam.PyZEDCamera() # Create a PyInitParameters object and set configuration parameters init_params = zcam.PyInitParameters() init_params.camera_resolution = sl.PyRESOLUTION.PyRESOLUTION_HD720 # Use HD720 video mode (default fps: 60) # Use a right-handed Y-up coordinate system init_params.coordinate_system = sl.PyCOORDINATE_SYSTEM.PyCOORDINATE_SYSTEM_RIGHT_HANDED_Y_UP init_params.coordinate_units = sl.PyUNIT.PyUNIT_METER # Set units in meters # Open the camera err = zed.open(init_params) if err != tp.PyERROR_CODE.PySUCCESS: exit(1) # Enable positional tracking with default parameters. # Positional tracking needs to be enabled before using spatial mapping py_transform = core.PyTransform() tracking_parameters = zcam.PyTrackingParameters(init_pos=py_transform) err = zed.enable_tracking(tracking_parameters) if err != tp.PyERROR_CODE.PySUCCESS: exit(1) # Enable spatial mapping mapping_parameters = zcam.PySpatialMappingParameters() err = zed.enable_spatial_mapping(mapping_parameters) if err != tp.PyERROR_CODE.PySUCCESS: exit(1) # Grab data during 500 frames i = 0 py_mesh = mesh.PyMesh() # Create a PyMesh object while i < 500: # For each new grab, mesh data is updated if zed.grab(zcam.PyRuntimeParameters()) == tp.PyERROR_CODE.PySUCCESS: # 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} / 500 || {1}".format( i, mapping_state)) i = i + 1 print("\n") # Extract, filter and save the mesh in an obj file print("Extracting Mesh...\n") zed.extract_whole_mesh(py_mesh) print("Filtering Mesh...\n") py_mesh.filter(mesh.PyMeshFilterParameters() ) # Filter the mesh (remove unnecessary vertices and faces) print("Saving Mesh...\n") py_mesh.save("mesh.obj") # Disable tracking and mapping and close the camera zed.disable_spatial_mapping() zed.disable_tracking() zed.close()
def main(): init = zcam.PyInitParameters( camera_resolution=sl.PyRESOLUTION.PyRESOLUTION_HD720, depth_mode=sl.PyDEPTH_MODE.PyDEPTH_MODE_PERFORMANCE, coordinate_units=sl.PyUNIT.PyUNIT_METER, coordinate_system=sl.PyCOORDINATE_SYSTEM. PyCOORDINATE_SYSTEM_RIGHT_HANDED_Y_UP, sdk_verbose=True) cam = zcam.PyZEDCamera() status = cam.open(init) if status != tp.PyERROR_CODE.PySUCCESS: print(repr(status)) exit() transform = core.PyTransform() tracking_params = zcam.PyTrackingParameters(transform) cam.enable_tracking(tracking_params) runtime = zcam.PyRuntimeParameters() camera_pose = zcam.PyPose() viewer = tv.PyTrackingViewer() viewer.init() py_translation = core.PyTranslation() start_zed(cam, runtime, camera_pose, viewer, py_translation) viewer.exit() glutMainLoop()
def run(cam, runtime, camera_pose, viewer, py_translation): while True: if cam.grab(runtime) == tp.PyERROR_CODE.PySUCCESS: tracking_state = cam.get_position(camera_pose) text_translation = "" text_rotation = "" if tracking_state == sl.PyTRACKING_STATE.PyTRACKING_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(core.PyTransform()) viewer.update_zed_position(pose_data) viewer.update_text(text_translation, text_rotation, tracking_state) else: tp.c_sleep_ms(1)
def run2D(self, cam, runtime, camera_pose, py_translation): pts = 0 while True: if cam.grab(runtime) == tp.PyERROR_CODE.PySUCCESS: tracking_state = cam.get_position(camera_pose) text_translation = "" text_rotation = "" if tracking_state == sl.PyTRACKING_STATE.PyTRACKING_STATE_OK: rotation = camera_pose.get_rotation_vector() rx = round(rotation[0], 2) ry = round(rotation[1], 2) translation = camera_pose.get_translation(py_translation) tx = round(translation.get()[0], 5) ty = round(translation.get()[1], 5) plt.scatter(tx, ty) plt.draw() # store data point data_point = [time.time(), rx, ry, tx, ty] self.data.append(data_point) text_translation = str((tx, ty)) text_rotation = str((rx, ry)) pose_data = camera_pose.pose_data(core.PyTransform())
def __init__(self, inputQueue, outputQueue, visualize): self.inputQueue = inputQueue self.outputQueue = outputQueue #self.outputQueueImages = outputQueueImages self.visualize = visualize self.xlist = [] self.ylist = [] if (self.visualize): plt.figure(1) init = zcam.PyInitParameters( camera_resolution=sl.PyRESOLUTION.PyRESOLUTION_VGA, depth_mode=sl.PyDEPTH_MODE.PyDEPTH_MODE_PERFORMANCE, coordinate_units=sl.PyUNIT.PyUNIT_METER, coordinate_system=sl.PyCOORDINATE_SYSTEM. PyCOORDINATE_SYSTEM_RIGHT_HANDED_Y_UP, sdk_verbose=False) cam = zcam.PyZEDCamera() self.image = core.PyMat() status = cam.open(init) if status != tp.PyERROR_CODE.PySUCCESS: print(repr(status)) exit() transform = core.PyTransform() tracking_params = zcam.PyTrackingParameters(transform) cam.enable_tracking(tracking_params) runtime = zcam.PyRuntimeParameters() camera_pose = zcam.PyPose() py_translation = core.PyTranslation() print("Starting ZEDPositioning") self.start_zed(cam, runtime, camera_pose, py_translation)
def initZed(fps): # Create a PyZEDCamera object zed = zcam.PyZEDCamera() # Create a PyInitParameters object and set configuration parameters init_params = zcam.PyInitParameters() init_params.camera_resolution = sl.PyRESOLUTION.PyRESOLUTION_HD1080 # Use HD1080 video mode init_params.camera_fps = fps # Set fps at 30 # Use a right-handed Y-up coordinate system init_params.coordinate_system = sl.PyCOORDINATE_SYSTEM.PyCOORDINATE_SYSTEM_RIGHT_HANDED_Y_UP init_params.coordinate_units = sl.PyUNIT.PyUNIT_METER # Set units in meters # Open the camera err = zed.open(init_params) if err != tp.PyERROR_CODE.PySUCCESS: exit(1) # Enable positional tracking with default parameters py_transform = core.PyTransform( ) # First create a PyTransform object for PyTrackingParameters object tracking_parameters = zcam.PyTrackingParameters(init_pos=py_transform) err = zed.enable_tracking(tracking_parameters) if err != tp.PyERROR_CODE.PySUCCESS: exit(1) image = core.PyMat() zed_pose = zcam.PyPose() return zed, image, zed_pose
def main(): # Create a PyZEDCamera object zed = zcam.PyZEDCamera() # Create a PyInitParameters object and set configuration parameters init_params = zcam.PyInitParameters() init_params.camera_resolution = sl.PyRESOLUTION.PyRESOLUTION_HD720 # Use HD720 video mode (default fps: 60) # Use a right-handed Y-up coordinate system init_params.coordinate_system = sl.PyCOORDINATE_SYSTEM.PyCOORDINATE_SYSTEM_RIGHT_HANDED_Y_UP init_params.coordinate_units = sl.PyUNIT.PyUNIT_METER # Set units in meters # Open the camera err = zed.open(init_params) if err != tp.PyERROR_CODE.PySUCCESS: exit(1) # Enable positional tracking with default parameters py_transform = core.PyTransform( ) # First create a PyTransform object for PyTrackingParameters object tracking_parameters = zcam.PyTrackingParameters(init_pos=py_transform) err = zed.enable_tracking(tracking_parameters) if err != tp.PyERROR_CODE.PySUCCESS: exit(1) # Track the camera position during 1000 frames i = 0 zed_pose = zcam.PyPose() runtime_parameters = zcam.PyRuntimeParameters() while i < 1000: if zed.grab(runtime_parameters) == tp.PyERROR_CODE.PySUCCESS: # Get the pose of the left eye of the camera with reference to the world frame zed.get_position(zed_pose, sl.PyREFERENCE_FRAME.PyREFERENCE_FRAME_WORLD) # Display the translation and timestamp py_translation = core.PyTranslation() 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)) # Display the orientation quaternion py_orientation = core.PyOrientation() 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)) i = i + 1 # Close the camera zed.close()
def run(self, cam, runtime, camera_pose, py_translation): while True: starttime = time() if cam.grab(runtime) == tp.PyERROR_CODE.PySUCCESS: cam.retrieve_image(self.image, sl.PyVIEW.PyVIEW_LEFT) # self.CVimage = self.image.get_data() # if (self.outputImageQueue.qsize() > 1): # self.outputImageQueue.get() # Remove last thing from queue # else: # self.outputImageQueue.put(self.CVimage) tracking_state = cam.get_position(camera_pose) text_translation = "" text_rotation = "" if tracking_state == sl.PyTRACKING_STATE.PyTRACKING_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(core.PyTransform()) self.x = float(pose_data[0][3]) self.y = -float(pose_data[2][3]) self.z = float(pose_data[1][3]) self.orientation = float(ry) + 3.14159 / 2 self.xlist.append(self.x) self.ylist.append(self.y) self.position = [self.x, self.y, self.z, self.orientation] if self.outputQueue.qsize() == 0: self.outputQueue.put(self.position) else: self.outputQueue.get() self.outputQueue.put(self.position) endtime = time() if (self.visualize): # print("Time elapsed Zed Positioning: " + str(endtime-starttime)) # print("Approximate fps: " + str(1/(endtime-starttime))) # print("X, Y, Theta: " + str(self.x) + ", " + str(self.y) + ", " + str(self.orientation*180/3.14159)) ax2 = plt.subplot(111) ax2.set_aspect("equal") ax2.scatter(self.x, self.y, c='r', s=3) plt.savefig("Test.png") tempimg = cv2.imread("Test.png") cv2.imshow("ZedPositioning", tempimg) os.remove('Test.png') cv2.waitKey(1) else: tp.c_sleep_ms(1)
def enable_tracking(self, init_vector=(0.0, 0.0, 0.0), load_area=False): params = default_tracking_params() if load_area is not False: params.area_file_path = load_area self.overall_status = self.camera.enable_tracking(params) # enables tracking and updates overall_status at the same time translation = core.PyTranslation() translation.init_vector(init_vector[0], init_vector[1], init_vector[2]) # X, Y, Z transform = core.PyTransform() transform.set_translation(translation) params.set_initial_world_transform(transform) # sets position equal to coordinates in init_vector # TODO TODO TODO self.transformer = rigid_pose_transform((-init_vector[0], -init_vector[1], -init_vector[2])) # saves function that transforms poses such that they ignore the init_vector self.pose = zcam.PyPose() self.tracking_status = 'Enabled'
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 = zcam.PyZEDCamera() init = zcam.PyInitParameters(svo_input_filename=filepath) status = cam.open(init) if status != tp.PyERROR_CODE.PySUCCESS: print(repr(status)) exit() runtime = zcam.PyRuntimeParameters() spatial = zcam.PySpatialMappingParameters() transform = core.PyTransform() tracking = zcam.PyTrackingParameters(transform) cam.enable_tracking(tracking) cam.enable_spatial_mapping(spatial) pymesh = mesh.PyMesh() print("Processing...") for i in range(200): cam.grab(runtime) cam.request_mesh_async() cam.extract_whole_mesh(pymesh) cam.disable_tracking() cam.disable_spatial_mapping() filter_params = mesh.PyMeshFilterParameters() filter_params.set(mesh.PyFILTER.PyFILTER_HIGH) print("Filtering params : {0}.".format(pymesh.filter(filter_params))) apply_texture = pymesh.apply_texture( mesh.PyMESH_TEXTURE_FORMAT.PyMESH_TEXTURE_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 default_tracking_params(): params = zcam.PyTrackingParameters(init_pos=core.PyTransform()) params.enable_spatial_memory = True #params.enable_pose_smoothing = False # Listed in documentation but doesn't work return params
temp = 0 failed = 0 if not cam.is_opened(): print("Opening ZED Camera...") status = cam.open(init) if status != tp.PyERROR_CODE.PySUCCESS: print(repr(status)) streamRunning = False exit(1) #print("Sleeping for 5 seconds...") #time.sleep(5) mat = core.PyMat() py_transform = core.PyTransform() tracking_parameters = zcam.PyTrackingParameters(init_pos=py_transform) err = cam.enable_tracking(tracking_parameters) if err != tp.PyERROR_CODE.PySUCCESS: print("Positional Tracking Failed") zed_pose = zcam.PyPose() zed_imu = zcam.PyIMUData() py_mesh = mesh.PyMesh() # Create a PyMesh object cam.set_camera_settings(sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_EXPOSURE, 60) current_value = cam.get_camera_settings( sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_EXPOSURE) print("Exposure: " + str(current_value)) print_camera_information(cam)
def __init__(self): self.body_io = [] self.path_mem = [] self.path = core.PyTransform() self.set_path(self.path, self.path_mem)