diff --git a/LICENSE b/LICENSE index 5ccdd3a..73d8007 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2017 Stereolabs +Copyright (c) 2018 Stereolabs Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/README.md b/README.md index ba035f3..ff3f0f5 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,13 @@ -# Stereolabs ZED - Python Integration (beta) +# Stereolabs ZED - Python API This package lets you use the ZED stereo camera in Python 3. +## Stable API notice + +The ZED Python API is now stable but has some breaking changes from the previous (beta) version. The older beta version can be found in the [legacy branch](https://github.com/stereolabs/zed-python-api/tree/legacy). + +The changes were made to better reflect the C++ API and ease of use. Mainly all classes have a similar name to the C++ SDK (without the "Py" prefix), and all components were migrated to a unified `sl` namespace. + ## Getting started - First, download the latest version of the ZED SDK on [stereolabs.com](https://www.stereolabs.com/developers) @@ -63,18 +69,12 @@ You can use `python setup.py cleanall` to remove every cpp files generated and b Import the packages in your Python terminal or file like this: ``` -import pyzed.camera as zcam -import pyzed.core as mat -import pyzed.defines as sl -import pyzed.types as types -import pyzed.mesh as mesh -import numpy as np +import pyzed.sl as sl ``` + Vectors operations like norm, sum, square, dot, cross, distance but also simple operations can be done with Numpy package. -**Note:** **pyzed.camera* is linked with *pyzed.core* and *pyzed.mesh* packages so you must import *pyzed.camera* before *pyzed.core* and *pyzed.mesh* to avoid import errors. - ### Run the tutorials The [tutorials](tutorials) provide simple projects to show how to use each module of the ZED SDK. For a similar version using the C++ API checkout the [Cpp tutorials](https://github.com/stereolabs/zed-examples/tree/master/tutorials). @@ -85,4 +85,4 @@ Please refer to the [examples](examples) README for more informations. ## Contributing -This is a beta version of the wrapper. Feel free to open an issue if you find a bug, or a pull request for bug fixes, features or other improvements. +Feel free to open an issue if you find a bug, or a pull request for bug fixes, features or other improvements. diff --git a/examples/README.md b/examples/README.md index 34cf0b5..f2028d0 100644 --- a/examples/README.md +++ b/examples/README.md @@ -34,3 +34,18 @@ Mesh sample shows mesh information after filtering and applying texture on frame python examples/mesh_example.py svo_file.svo ``` +### Object + +Object sample shows the objects detected and tracked by the AI module with their bouding boxes and their 3D positions + +``` +python examples/object_example.py +``` + +### Plane + +Plane sample is searching for the floor in a video and extracts it into a mesh if it found it. + +``` +python examples/plane_example.py svo_file.svo +``` diff --git a/examples/live_camera.py b/examples/live_camera.py index a7d41d2..42cfc5a 100644 --- a/examples/live_camera.py +++ b/examples/live_camera.py @@ -24,29 +24,26 @@ """ import cv2 -import pyzed.camera as zcam -import pyzed.types as tp -import pyzed.core as core -import pyzed.defines as sl +import pyzed.sl as sl -camera_settings = sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_BRIGHTNESS +camera_settings = sl.CAMERA_SETTINGS.CAMERA_SETTINGS_BRIGHTNESS str_camera_settings = "BRIGHTNESS" step_camera_settings = 1 def main(): print("Running...") - init = zcam.PyInitParameters() - cam = zcam.PyZEDCamera() + init = sl.InitParameters() + cam = sl.Camera() if not cam.is_opened(): print("Opening ZED Camera...") status = cam.open(init) - if status != tp.PyERROR_CODE.PySUCCESS: + if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() - runtime = zcam.PyRuntimeParameters() - mat = core.PyMat() + runtime = sl.RuntimeParameters() + mat = sl.Mat() print_camera_information(cam) print_help() @@ -54,8 +51,8 @@ def main(): key = '' while key != 113: # for 'q' key err = cam.grab(runtime) - if err == tp.PyERROR_CODE.PySUCCESS: - cam.retrieve_image(mat, sl.PyVIEW.PyVIEW_LEFT) + if err == sl.ERROR_CODE.SUCCESS: + cam.retrieve_image(mat, sl.VIEW.VIEW_LEFT) cv2.imshow("ZED", mat.get_data()) key = cv2.waitKey(5) settings(key, cam, runtime, mat) @@ -97,13 +94,13 @@ def settings(key, cam, runtime, mat): cam.set_camera_settings(camera_settings, current_value - step_camera_settings) print(str_camera_settings + ": " + str(current_value - step_camera_settings)) elif key == 114: # for 'r' key - cam.set_camera_settings(sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_BRIGHTNESS, -1, True) - cam.set_camera_settings(sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_CONTRAST, -1, True) - cam.set_camera_settings(sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_HUE, -1, True) - cam.set_camera_settings(sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_SATURATION, -1, True) - cam.set_camera_settings(sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_GAIN, -1, True) - cam.set_camera_settings(sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_EXPOSURE, -1, True) - cam.set_camera_settings(sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_WHITEBALANCE, -1, True) + cam.set_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_BRIGHTNESS, -1, True) + cam.set_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_CONTRAST, -1, True) + cam.set_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_HUE, -1, True) + cam.set_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_SATURATION, -1, True) + cam.set_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_GAIN, -1, True) + cam.set_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_EXPOSURE, -1, True) + cam.set_camera_settings(sl.CAMERA_SETTINGS.CAMERA_SETTINGS_WHITEBALANCE, -1, True) print("Camera settings: reset") elif key == 122: # for 'z' key record(cam, runtime, mat) @@ -112,51 +109,51 @@ def settings(key, cam, runtime, mat): def switch_camera_settings(): global camera_settings global str_camera_settings - if camera_settings == sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_BRIGHTNESS: - camera_settings = sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_CONTRAST + if camera_settings == sl.CAMERA_SETTINGS.CAMERA_SETTINGS_BRIGHTNESS: + camera_settings = sl.CAMERA_SETTINGS.CAMERA_SETTINGS_CONTRAST str_camera_settings = "Contrast" print("Camera settings: CONTRAST") - elif camera_settings == sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_CONTRAST: - camera_settings = sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_HUE + elif camera_settings == sl.CAMERA_SETTINGS.CAMERA_SETTINGS_CONTRAST: + camera_settings = sl.CAMERA_SETTINGS.CAMERA_SETTINGS_HUE str_camera_settings = "Hue" print("Camera settings: HUE") - elif camera_settings == sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_HUE: - camera_settings = sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_SATURATION + elif camera_settings == sl.CAMERA_SETTINGS.CAMERA_SETTINGS_HUE: + camera_settings = sl.CAMERA_SETTINGS.CAMERA_SETTINGS_SATURATION str_camera_settings = "Saturation" print("Camera settings: SATURATION") - elif camera_settings == sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_SATURATION: - camera_settings = sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_GAIN + elif camera_settings == sl.CAMERA_SETTINGS.CAMERA_SETTINGS_SATURATION: + camera_settings = sl.CAMERA_SETTINGS.CAMERA_SETTINGS_GAIN str_camera_settings = "Gain" print("Camera settings: GAIN") - elif camera_settings == sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_GAIN: - camera_settings = sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_EXPOSURE + elif camera_settings == sl.CAMERA_SETTINGS.CAMERA_SETTINGS_GAIN: + camera_settings = sl.CAMERA_SETTINGS.CAMERA_SETTINGS_EXPOSURE str_camera_settings = "Exposure" print("Camera settings: EXPOSURE") - elif camera_settings == sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_EXPOSURE: - camera_settings = sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_WHITEBALANCE + elif camera_settings == sl.CAMERA_SETTINGS.CAMERA_SETTINGS_EXPOSURE: + camera_settings = sl.CAMERA_SETTINGS.CAMERA_SETTINGS_WHITEBALANCE str_camera_settings = "White Balance" print("Camera settings: WHITEBALANCE") - elif camera_settings == sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_WHITEBALANCE: - camera_settings = sl.PyCAMERA_SETTINGS.PyCAMERA_SETTINGS_BRIGHTNESS + elif camera_settings == sl.CAMERA_SETTINGS.CAMERA_SETTINGS_WHITEBALANCE: + camera_settings = sl.CAMERA_SETTINGS.CAMERA_SETTINGS_BRIGHTNESS str_camera_settings = "Brightness" print("Camera settings: BRIGHTNESS") def record(cam, runtime, mat): - vid = tp.PyERROR_CODE.PyERROR_CODE_FAILURE + vid = sl.ERROR_CODE.ERROR_CODE_FAILURE out = False - while vid != tp.PyERROR_CODE.PySUCCESS and not out: + while vid != sl.ERROR_CODE.SUCCESS and not out: filepath = input("Enter filepath name: ") vid = cam.enable_recording(filepath) print(repr(vid)) - if vid == tp.PyERROR_CODE.PySUCCESS: + if vid == sl.ERROR_CODE.SUCCESS: print("Recording started...") out = True print("Hit spacebar to stop recording: ") key = False while key != 32: # for spacebar err = cam.grab(runtime) - if err == tp.PyERROR_CODE.PySUCCESS: + if err == sl.ERROR_CODE.SUCCESS: cam.retrieve_image(mat) cv2.imshow("ZED", mat.get_data()) key = cv2.waitKey(5) diff --git a/examples/mesh_example.py b/examples/mesh_example.py index 4cfc84c..7bb2ec9 100644 --- a/examples/mesh_example.py +++ b/examples/mesh_example.py @@ -23,10 +23,7 @@ parameters can be saved. """ import sys -import pyzed.camera as zcam -import pyzed.core as core -import pyzed.mesh as mesh -import pyzed.types as tp +import pyzed.sl as sl def main(): @@ -38,22 +35,22 @@ def main(): filepath = sys.argv[1] print("Reading SVO file: {0}".format(filepath)) - cam = zcam.PyZEDCamera() - init = zcam.PyInitParameters(svo_input_filename=filepath) + cam = sl.Camera() + init = sl.InitParameters(svo_input_filename=filepath) status = cam.open(init) - if status != tp.PyERROR_CODE.PySUCCESS: + if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() - runtime = zcam.PyRuntimeParameters() - spatial = zcam.PySpatialMappingParameters() - transform = core.PyTransform() - tracking = zcam.PyTrackingParameters(transform) + runtime = sl.RuntimeParameters() + spatial = sl.SpatialMappingParameters() + transform = sl.Transform() + tracking = sl.TrackingParameters(transform) cam.enable_tracking(tracking) cam.enable_spatial_mapping(spatial) - pymesh = mesh.PyMesh() + pymesh = sl.Mesh() print("Processing...") for i in range(200): cam.grab(runtime) @@ -63,11 +60,11 @@ def main(): cam.disable_tracking() cam.disable_spatial_mapping() - filter_params = mesh.PyMeshFilterParameters() - filter_params.set(mesh.PyFILTER.PyFILTER_HIGH) + 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(mesh.PyMESH_TEXTURE_FORMAT.PyMESH_TEXTURE_RGBA) + 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) @@ -88,7 +85,7 @@ def print_mesh_information(pymesh, apply_texture): print("Triangles : \n{0} \n".format(pymesh.triangles)) break else: - print("Cannot display information of the mesh.") + print("Cannot display information of the sl.") break if res == "n": print("Mesh information will not be displayed.") @@ -101,8 +98,8 @@ def save_filter(filter_params): while True: res = input("Do you want to save the mesh filter parameters? [y/n]: ") if res == "y": - params = tp.PyERROR_CODE.PyERROR_CODE_FAILURE - while params != tp.PyERROR_CODE.PySUCCESS: + params = sl.ERROR_CODE.ERROR_CODE_FAILURE + while params != sl.ERROR_CODE.SUCCESS: filepath = input("Enter filepath name : ") params = filter_params.save(filepath) print("Saving mesh filter parameters: {0}".format(repr(params))) @@ -122,8 +119,8 @@ def save_mesh(pymesh): while True: res = input("Do you want to save the mesh? [y/n]: ") if res == "y": - msh = tp.PyERROR_CODE.PyERROR_CODE_FAILURE - while msh != tp.PyERROR_CODE.PySUCCESS: + msh = sl.ERROR_CODE.ERROR_CODE_FAILURE + while msh != sl.ERROR_CODE.SUCCESS: filepath = input("Enter filepath name: ") msh = pymesh.save(filepath) print("Saving mesh: {0}".format(repr(msh))) diff --git a/examples/multi_camera.py b/examples/multi_camera.py index 76660d3..85a8c19 100644 --- a/examples/multi_camera.py +++ b/examples/multi_camera.py @@ -23,38 +23,35 @@ """ import cv2 -import pyzed.camera as zcam -import pyzed.types as tp -import pyzed.core as core -import pyzed.defines as sl +import pyzed.sl as sl def main(): print("Running...") - init = zcam.PyInitParameters() - init.camera_resolution = sl.PyRESOLUTION.PyRESOLUTION_HD720 + init = sl.InitParameters() + init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 init.camera_linux_id = 0 init.camera_fps = 30 # The framerate is lowered to avoid any USB3 bandwidth issues - cam = zcam.PyZEDCamera() + cam = sl.Camera() if not cam.is_opened(): print("Opening ZED Camera 1...") status = cam.open(init) - if status != tp.PyERROR_CODE.PySUCCESS: + if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() init.camera_linux_id = 1 # selection of the ZED ID - cam2 = zcam.PyZEDCamera() + cam2 = sl.Camera() if not cam2.is_opened(): print("Opening ZED Camera 2...") status = cam2.open(init) - if status != tp.PyERROR_CODE.PySUCCESS: + if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() - runtime = zcam.PyRuntimeParameters() - mat = core.PyMat() - mat2 = core.PyMat() + runtime = sl.RuntimeParameters() + mat = sl.Mat() + mat2 = sl.Mat() print_camera_information(cam) print_camera_information(cam2) @@ -63,13 +60,13 @@ def main(): while key != 113: # for 'q' key # The computation could also be done in a thread, one for each camera err = cam.grab(runtime) - if err == tp.PyERROR_CODE.PySUCCESS: - cam.retrieve_image(mat, sl.PyVIEW.PyVIEW_LEFT) + if err == sl.ERROR_CODE.SUCCESS: + cam.retrieve_image(mat, sl.VIEW.VIEW_LEFT) cv2.imshow("ZED 1", mat.get_data()) err = cam2.grab(runtime) - if err == tp.PyERROR_CODE.PySUCCESS: - cam2.retrieve_image(mat2, sl.PyVIEW.PyVIEW_LEFT) + if err == sl.ERROR_CODE.SUCCESS: + cam2.retrieve_image(mat2, sl.VIEW.VIEW_LEFT) cv2.imshow("ZED 2", mat2.get_data()) key = cv2.waitKey(5) diff --git a/examples/plane_example.py b/examples/plane_example.py index 1ae276a..7cd0821 100644 --- a/examples/plane_example.py +++ b/examples/plane_example.py @@ -19,18 +19,19 @@ ######################################################################## """ - Plane sample that save the floor plane detected as a mesh. + Plane sample that save the floor plane detected as a sl. """ import sys -import pyzed.camera as zcam -import pyzed.core as core -import pyzed.mesh as mesh -import pyzed.types as tp +import pyzed.sl as sl +from time import sleep def main(): - cam = zcam.PyZEDCamera() - init = zcam.PyInitParameters() + cam = sl.Camera() + init = sl.InitParameters() + init.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_ULTRA + init.coordinate_units = sl.UNIT.UNIT_METER + init.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP if len(sys.argv) == 2: filepath = sys.argv[1] @@ -38,32 +39,34 @@ def main(): init.set_from_svo_file(filepath) status = cam.open(init) - if status != tp.PyERROR_CODE.PySUCCESS: + if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit(1) - runtime = zcam.PyRuntimeParameters() - spatial = zcam.PySpatialMappingParameters() - - transform = core.PyTransform() - tracking = zcam.PyTrackingParameters(transform) + runtime = sl.RuntimeParameters() + runtime.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD + runtime.measure3D_reference_frame = sl.REFERENCE_FRAME.REFERENCE_FRAME_WORLD + spatial = sl.SpatialMappingParameters() + transform = sl.Transform() + tracking = sl.TrackingParameters(transform) cam.enable_tracking(tracking) - cam.enable_spatial_mapping(spatial) - pymesh = mesh.PyMesh() - pyplane = mesh.PyPlane() - reset_tracking_floor_frame = core.PyTransform() + pymesh = sl.Mesh() + pyplane = sl.Plane() + reset_tracking_floor_frame = sl.Transform() found = 0 print("Processing...") - for i in range(1000): - cam.grab(runtime) - err = cam.find_floor_plane(pyplane, reset_tracking_floor_frame) - if i > 200 and err == tp.PyERROR_CODE.PySUCCESS: - found = 1 - print('Floor found!') - pymesh = pyplane.extract_mesh() - break + i = 0 + while i < 1000: + if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS : + err = cam.find_floor_plane(pyplane, reset_tracking_floor_frame) + if i > 200 and err == sl.ERROR_CODE.SUCCESS: + found = 1 + print('Floor found!') + pymesh = pyplane.extract_mesh() + break + i+=1 if found == 0: print('Floor was not found, please try with another SVO.') @@ -71,8 +74,6 @@ def main(): exit(0) cam.disable_tracking() - cam.disable_spatial_mapping() - save_mesh(pymesh) cam.close() print("\nFINISH") @@ -82,8 +83,8 @@ def save_mesh(pymesh): while True: res = input("Do you want to save the mesh? [y/n]: ") if res == "y": - msh = tp.PyERROR_CODE.PyERROR_CODE_FAILURE - while msh != tp.PyERROR_CODE.PySUCCESS: + msh = sl.ERROR_CODE.ERROR_CODE_FAILURE + while msh != sl.ERROR_CODE.SUCCESS: filepath = input("Enter filepath name: ") msh = pymesh.save(filepath) print("Saving mesh: {0}".format(repr(msh))) diff --git a/examples/position_example.py b/examples/position_example.py index d9231be..daf3b0a 100644 --- a/examples/position_example.py +++ b/examples/position_example.py @@ -23,37 +23,34 @@ """ from OpenGL.GLUT import * import positional_tracking.tracking_viewer as tv -import pyzed.camera as zcam -import pyzed.types as tp -import pyzed.core as core -import pyzed.defines as sl +import pyzed.sl as sl import threading 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, + init = sl.InitParameters(camera_resolution=sl.RESOLUTION.RESOLUTION_HD720, + depth_mode=sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE, + coordinate_units=sl.UNIT.UNIT_METER, + coordinate_system=sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP, sdk_verbose=True) - cam = zcam.PyZEDCamera() + cam = sl.Camera() status = cam.open(init) - if status != tp.PyERROR_CODE.PySUCCESS: + if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() - transform = core.PyTransform() - tracking_params = zcam.PyTrackingParameters(transform) + transform = sl.Transform() + tracking_params = sl.TrackingParameters(transform) cam.enable_tracking(tracking_params) - runtime = zcam.PyRuntimeParameters() - camera_pose = zcam.PyPose() + runtime = sl.RuntimeParameters() + camera_pose = sl.Pose() viewer = tv.PyTrackingViewer() viewer.init() - py_translation = core.PyTranslation() + py_translation = sl.Translation() start_zed(cam, runtime, camera_pose, viewer, py_translation) @@ -68,11 +65,11 @@ def start_zed(cam, runtime, camera_pose, viewer, py_translation): def run(cam, runtime, camera_pose, viewer, py_translation): while True: - if cam.grab(runtime) == tp.PyERROR_CODE.PySUCCESS: + if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS: tracking_state = cam.get_position(camera_pose) text_translation = "" text_rotation = "" - if tracking_state == sl.PyTRACKING_STATE.PyTRACKING_STATE_OK: + if tracking_state == sl.TRACKING_STATE.TRACKING_STATE_OK: rotation = camera_pose.get_rotation_vector() rx = round(rotation[0], 2) ry = round(rotation[1], 2) @@ -85,12 +82,12 @@ def run(cam, runtime, camera_pose, viewer, py_translation): text_translation = str((tx, ty, tz)) text_rotation = str((rx, ry, rz)) - pose_data = camera_pose.pose_data(core.PyTransform()) + pose_data = camera_pose.pose_data(sl.Transform()) viewer.update_zed_position(pose_data) viewer.update_text(text_translation, text_rotation, tracking_state) else: - tp.c_sleep_ms(1) + sl.c_sleep_ms(1) if __name__ == "__main__": diff --git a/examples/positional_tracking/tracking_viewer.py b/examples/positional_tracking/tracking_viewer.py index 3946c34..2b1a92b 100644 --- a/examples/positional_tracking/tracking_viewer.py +++ b/examples/positional_tracking/tracking_viewer.py @@ -9,7 +9,7 @@ import positional_tracking.utils as ut import positional_tracking.zed_model as zm -import pyzed.defines as sl +import pyzed.sl as sl def safe_glut_bitmap_string(w): @@ -145,7 +145,7 @@ def __init__(self): self.zed_path = [] self.path_locker = threading.Lock() self.zed3d = zm.Zed3D() - self.track_state = sl.PyTRACKING_STATE + self.track_state = sl.TRACKING_STATE self.txt_t = "" self.txt_r = "" self.startx = 0 @@ -413,7 +413,7 @@ def print_text(self): start_w = 20 start_h = h_wnd - 40 - if self.track_state == sl.PyTRACKING_STATE.PyTRACKING_STATE_OK: + if self.track_state == sl.TRACKING_STATE.TRACKING_STATE_OK: tracking_is_ok = 1 else: tracking_is_ok = 0 diff --git a/examples/positional_tracking/zed_model.py b/examples/positional_tracking/zed_model.py index e850c90..ed13d78 100644 --- a/examples/positional_tracking/zed_model.py +++ b/examples/positional_tracking/zed_model.py @@ -1,7 +1,6 @@ from OpenGL.GL import * import numpy as np -import pyzed.camera as cam -import pyzed.core as core +import pyzed.sl as sl import positional_tracking.utils as ut @@ -197,7 +196,7 @@ class Zed3D: def __init__(self): self.body_io = [] self.path_mem = [] - self.path = core.PyTransform() + self.path = sl.Transform() self.set_path(self.path, self.path_mem) diff --git a/examples/read_svo.py b/examples/read_svo.py index 419077d..bb4f4e5 100644 --- a/examples/read_svo.py +++ b/examples/read_svo.py @@ -23,10 +23,7 @@ a JPEG or PNG file. Depth map and Point Cloud can also be saved into files. """ import sys -import pyzed.camera as zcam -import pyzed.types as tp -import pyzed.core as core -import pyzed.defines as sl +import pyzed.sl as sl import cv2 @@ -39,22 +36,22 @@ def main(): filepath = sys.argv[1] print("Reading SVO file: {0}".format(filepath)) - init = zcam.PyInitParameters(svo_input_filename=filepath,svo_real_time_mode=False) - cam = zcam.PyZEDCamera() + init = sl.InitParameters(svo_input_filename=filepath,svo_real_time_mode=False) + cam = sl.Camera() status = cam.open(init) - if status != tp.PyERROR_CODE.PySUCCESS: + if status != sl.ERROR_CODE.SUCCESS: print(repr(status)) exit() - runtime = zcam.PyRuntimeParameters() - mat = core.PyMat() + runtime = sl.RuntimeParameters() + mat = sl.Mat() key = '' print(" Save the current image: s") print(" Quit the video reading: q\n") while key != 113: # for 'q' key err = cam.grab(runtime) - if err == tp.PyERROR_CODE.PySUCCESS: + if err == sl.ERROR_CODE.SUCCESS: cam.retrieve_image(mat) cv2.imshow("ZED", mat.get_data()) key = cv2.waitKey(1) @@ -99,12 +96,12 @@ def print_camera_information(cam): def saving_image(key, mat): if key == 115: - img = tp.PyERROR_CODE.PyERROR_CODE_FAILURE - while img != tp.PyERROR_CODE.PySUCCESS: + img = sl.ERROR_CODE.ERROR_CODE_FAILURE + while img != sl.ERROR_CODE.SUCCESS: filepath = input("Enter filepath name: ") img = mat.write(filepath) print("Saving image : {0}".format(repr(img))) - if img == tp.PyERROR_CODE.PySUCCESS: + if img == sl.ERROR_CODE.SUCCESS: break else: print("Help: you must enter the filepath + filename + PNG extension.") @@ -117,7 +114,7 @@ def saving_depth(cam): save_depth = 0 while not save_depth: filepath = input("Enter filepath name: ") - save_depth = zcam.save_camera_depth_as(cam, sl.PyDEPTH_FORMAT.PyDEPTH_FORMAT_PNG, filepath) + save_depth = sl.save_camera_depth_as(cam, sl.DEPTH_FORMAT.DEPTH_FORMAT_PNG, filepath) if save_depth: print("Depth saved.") break @@ -138,9 +135,9 @@ def saving_point_cloud(cam): save_point_cloud = 0 while not save_point_cloud: filepath = input("Enter filepath name: ") - save_point_cloud = zcam.save_camera_point_cloud_as(cam, - sl.PyPOINT_CLOUD_FORMAT. - PyPOINT_CLOUD_FORMAT_PCD_ASCII, + save_point_cloud = sl.save_camera_point_cloud_as(cam, + sl.POINT_CLOUD_FORMAT. + POINT_CLOUD_FORMAT_PCD_ASCII, filepath, True) if save_point_cloud: print("Point cloud saved.") diff --git a/examples/svo_recording/ZED_SVO_Export.py b/examples/svo_recording/ZED_SVO_Export.py index 376938f..b834929 100755 --- a/examples/svo_recording/ZED_SVO_Export.py +++ b/examples/svo_recording/ZED_SVO_Export.py @@ -1,10 +1,7 @@ #!/usr/bin/env python3 import sys -import pyzed.camera as zcam -import pyzed.core as core -import pyzed.defines as sl -import pyzed.types as tp +import pyzed.sl as sl import numpy as np import cv2 from pathlib import Path @@ -68,17 +65,17 @@ def main(): exit() # Specify SVO path parameter - init_params = zcam.PyInitParameters() + init_params = sl.InitParameters() init_params.svo_input_filename = str(svo_input_path) init_params.svo_real_time_mode = False # Don't convert in realtime - init_params.coordinate_units = sl.PyUNIT.PyUNIT_MILLIMETER # Use milliliter units (for depth measurements) + init_params.coordinate_units = sl.UNIT.UNIT_MILLIMETER # Use milliliter units (for depth measurements) # Create ZED objects - zed = zcam.PyZEDCamera() + zed = sl.Camera() # Open the SVO file specified as a parameter err = zed.open(init_params) - if err != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: sys.stdout.write(repr(err)) zed.close() exit() @@ -93,9 +90,9 @@ def main(): svo_image_sbs_rgba = np.zeros((height, width_sbs, 4), dtype=np.uint8) # Prepare single image containers - left_image = core.PyMat() - right_image = core.PyMat() - depth_image = core.PyMat() + left_image = sl.Mat() + right_image = sl.Mat() + depth_image = sl.Mat() video_writer = None if output_as_video: @@ -111,8 +108,8 @@ def main(): zed.close() exit() - rt_param = zcam.PyRuntimeParameters() - rt_param.sensing_mode = sl.PySENSING_MODE.PySENSING_MODE_FILL + rt_param = sl.RuntimeParameters() + rt_param.sensing_mode = sl.SENSING_MODE.SENSING_MODE_FILL # Start SVO conversion to AVI/SEQUENCE sys.stdout.write("Converting SVO... Use Ctrl-C to interrupt conversion.\n") @@ -120,18 +117,18 @@ def main(): nb_frames = zed.get_svo_number_of_frames() while True: - if zed.grab(rt_param) == tp.PyERROR_CODE.PySUCCESS: + if zed.grab(rt_param) == sl.ERROR_CODE.SUCCESS: svo_position = zed.get_svo_position() # Retrieve SVO images - zed.retrieve_image(left_image, sl.PyVIEW.PyVIEW_LEFT) + zed.retrieve_image(left_image, sl.VIEW.VIEW_LEFT) if app_type == AppType.LEFT_AND_RIGHT: - zed.retrieve_image(right_image, sl.PyVIEW.PyVIEW_RIGHT) + zed.retrieve_image(right_image, sl.VIEW.VIEW_RIGHT) elif app_type == AppType.LEFT_AND_DEPTH: - zed.retrieve_image(right_image, sl.PyVIEW.PyVIEW_DEPTH) + zed.retrieve_image(right_image, sl.VIEW.VIEW_DEPTH) elif app_type == AppType.LEFT_AND_DEPTH_16: - zed.retrieve_measure(depth_image, sl.PyMEASURE.PyMEASURE_DEPTH) + zed.retrieve_measure(depth_image, sl.MEASURE.MEASURE_DEPTH) if output_as_video: # Copy the left image to the left side of SBS image diff --git a/pyzed/camera.pxd b/pyzed/camera.pxd deleted file mode 100644 index c0e514b..0000000 --- a/pyzed/camera.pxd +++ /dev/null @@ -1,289 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# File containing the Cython declarations to use the Camera.hpp functions. - -from libcpp cimport bool -from libcpp.pair cimport pair -from libcpp.vector cimport vector - -cimport pyzed.defines as defines -cimport pyzed.core as core -cimport pyzed.types as types -cimport pyzed.mesh as mesh - -cdef extern from 'cuda.h' : - cdef struct CUctx_st : - pass - ctypedef CUctx_st* CUcontext - -cdef extern from 'sl/Camera.hpp' namespace 'sl': - - ctypedef enum MAPPING_RESOLUTION 'sl::SpatialMappingParameters::MAPPING_RESOLUTION': - MAPPING_RESOLUTION_HIGH 'sl::SpatialMappingParameters::MAPPING_RESOLUTION::MAPPING_RESOLUTION_HIGH' - MAPPING_RESOLUTION_MEDIUM 'sl::SpatialMappingParameters::MAPPING_RESOLUTION::MAPPING_RESOLUTION_MEDIUM' - MAPPING_RESOLUTION_LOW 'sl::SpatialMappingParameters::MAPPING_RESOLUTION::MAPPING_RESOLUTION_LOW' - - - ctypedef enum MAPPING_RANGE 'sl::SpatialMappingParameters::MAPPING_RANGE': - MAPPING_RANGE_NEAR 'sl::SpatialMappingParameters::MAPPING_RANGE::MAPPING_RANGE_NEAR' - MAPPING_RANGE_MEDIUM 'sl::SpatialMappingParameters::MAPPING_RANGE::MAPPING_RANGE_MEDIUM' - MAPPING_RANGE_FAR 'sl::SpatialMappingParameters::MAPPING_RANGE::MAPPING_RANGE_FAR' - - cdef cppclass InputType 'sl::InputType': - InputType() - InputType(InputType &type) - - void setFromCameraID(unsigned int id) - void setFromSerialNumber(unsigned int serial_number) - void setFromSVOFile(types.String svo_input_filename) - - cdef cppclass InitParameters 'sl::InitParameters': - defines.RESOLUTION camera_resolution - int camera_fps - int camera_linux_id - types.String svo_input_filename - bool svo_real_time_mode - defines.UNIT coordinate_units - defines.COORDINATE_SYSTEM coordinate_system - defines.DEPTH_MODE depth_mode - float depth_minimum_distance - int camera_image_flip - bool enable_right_side_measure - bool camera_disable_self_calib - int camera_buffer_count_linux - bool sdk_verbose - int sdk_gpu_id - bool depth_stabilization - - types.String sdk_verbose_log_file - - CUcontext sdk_cuda_ctx - InputType input - types.String optional_settings_path - - InitParameters(defines.RESOLUTION camera_resolution, - int camera_fps, - int camera_linux_id, - types.String svo_input_filename, - bool svo_real_time_mode, - defines.DEPTH_MODE depth_mode, - defines.UNIT coordinate_units, - defines.COORDINATE_SYSTEM coordinate_system, - bool sdk_verbose, - int sdk_gpu_id, - float depth_minimum_distance, - bool camera_disable_self_calib, - bool camera_image_flip, - bool enable_right_side_measure, - int camera_buffer_count_linux, - types.String sdk_verbose_log_file, - bool depth_stabilization, - CUcontext sdk_cuda_ctx, - InputType input, - types.String optional_settings_path) - - bool save(types.String filename) - bool load(types.String filename) - - - cdef cppclass RuntimeParameters 'sl::RuntimeParameters': - defines.SENSING_MODE sensing_mode - bool enable_depth - bool enable_point_cloud - defines.REFERENCE_FRAME measure3D_reference_frame - - RuntimeParameters(defines.SENSING_MODE sensing_mode, - bool enable_depth, - bool enable_point_cloud, - defines.REFERENCE_FRAME measure3D_reference_frame) - - bool save(types.String filename) - bool load(types.String filename) - - - cdef cppclass TrackingParameters 'sl::TrackingParameters': - core.Transform initial_world_transform - bool enable_spatial_memory - bool enable_pose_smoothing - bool set_floor_as_origin - types.String area_file_path - bool enable_imu_fusion - - TrackingParameters(core.Transform init_pos, - bool _enable_memory, - types.String _area_path) - - bool save(types.String filename) - bool load(types.String filename) - - cdef cppclass SpatialMappingParameters 'sl::SpatialMappingParameters': - ctypedef pair[float, float] interval - - SpatialMappingParameters(MAPPING_RESOLUTION resolution, - MAPPING_RANGE range, - int max_memory_usage_, - bool save_texture_, - bool use_chunk_only_, - bool reverse_vertex_order_) - - @staticmethod - float get(MAPPING_RESOLUTION resolution) - - void set(MAPPING_RESOLUTION resolution) - - @staticmethod - float get(MAPPING_RANGE range) - - @staticmethod - float getRecommendedRange(MAPPING_RESOLUTION mapping_resolution, Camera &camera) - - @staticmethod - float getRecommendedRange(float resolution_meters, Camera &camera) - - void set(MAPPING_RANGE range) - - int max_memory_usage - bool save_texture - bool use_chunk_only - bool reverse_vertex_order - - const interval allowed_range - float range_meter - const interval allowed_resolution - float resolution_meter - - bool save(types.String filename) - bool load(types.String filename) - - - cdef cppclass Pose: - Pose() - Pose(const Pose &pose) - Pose(const core.Transform &pose_data, unsigned long long mtimestamp, int mconfidence) - core.Translation getTranslation() - core.Orientation getOrientation() - core.Rotation getRotationMatrix() - types.Vector3[float] getRotationVector() - types.Vector3[float] getEulerAngles(bool radian) - - bool valid - unsigned long long timestamp - - core.Transform pose_data - - int pose_confidence - float pose_covariance[36] - - cdef cppclass IMUData(Pose): - IMUData() - IMUData(const IMUData &pose) - IMUData(const core.Transform &pose_data, unsigned long long mtimestamp, int mconfidence) - - types.Matrix3f orientation_covariance - types.Vector3[float] angular_velocity - types.Vector3[float] linear_acceleration - types.Matrix3f angular_velocity_convariance - types.Matrix3f linear_acceleration_convariance - - - cdef cppclass Camera 'sl::Camera': - Camera() - void close() - types.ERROR_CODE open(InitParameters init_parameters) - bool isOpened() - types.ERROR_CODE grab(RuntimeParameters rt_parameters) - types.ERROR_CODE retrieveImage(core.Mat &mat, defines.VIEW view, core.MEM type, int width, int height) - types.ERROR_CODE retrieveMeasure(core.Mat &mat, defines.MEASURE measure, core.MEM type, int width, int height) - void setConfidenceThreshold(int conf_threshold_value) - int getConfidenceThreshold() - - core.Resolution getResolution() - void setDepthMaxRangeValue(float depth_max_range) - float getDepthMaxRangeValue() - float getDepthMinRangeValue() - void setSVOPosition(int frame_number) - int getSVOPosition() - int getSVONumberOfFrames() - void setCameraSettings(defines.CAMERA_SETTINGS settings, int value, bool use_default) - int getCameraSettings(defines.CAMERA_SETTINGS setting) - float getCameraFPS() - void setCameraFPS(int desired_fps) - float getCurrentFPS() - types.timeStamp getCameraTimestamp() # deprecated - types.timeStamp getCurrentTimestamp() # deprecated - types.timeStamp getTimestamp(defines.TIME_REFERENCE reference_time) - unsigned int getFrameDroppedCount() - core.CameraInformation getCameraInformation(core.Resolution resizer); - defines.SELF_CALIBRATION_STATE getSelfCalibrationState() - void resetSelfCalibration() - - types.ERROR_CODE enableTracking(TrackingParameters tracking_params) - defines.TRACKING_STATE getPosition(Pose &camera_pose, defines.REFERENCE_FRAME reference_frame) - types.ERROR_CODE saveCurrentArea(types.String area_file_path); - defines.AREA_EXPORT_STATE getAreaExportState() - void disableTracking(types.String area_file_path) - types.ERROR_CODE resetTracking(core.Transform &path) - types.ERROR_CODE getIMUData(IMUData &imu_data, defines.TIME_REFERENCE reference_time) - types.ERROR_CODE setIMUPrior(core.Transform &transfom) - - types.ERROR_CODE enableSpatialMapping(SpatialMappingParameters spatial_mapping_parameters) - void pauseSpatialMapping(bool status) - defines.SPATIAL_MAPPING_STATE getSpatialMappingState() - types.ERROR_CODE extractWholeMesh(mesh.Mesh &mesh) - void requestMeshAsync() - types.ERROR_CODE getMeshRequestStatusAsync() - types.ERROR_CODE retrieveMeshAsync(mesh.Mesh &mesh) - void disableSpatialMapping() - - types.ERROR_CODE findPlaneAtHit(types.Vector2[uint] coord, mesh.Plane &plane) - types.ERROR_CODE findFloorPlane(mesh.Plane &plane, core.Transform &resetTrackingFloorFrame, float floor_height_prior, core.Rotation world_orientation_prior, float floor_height_prior_tolerance) - - types.ERROR_CODE enableRecording(types.String video_filename, defines.SVO_COMPRESSION_MODE compression_mode) - defines.RecordingState record() - void disableRecording() - - @staticmethod - types.String getSDKVersion() - - @staticmethod - int isZEDconnected() - - @staticmethod - types.ERROR_CODE sticktoCPUCore(int cpu_core) - - @staticmethod - vector[types.DeviceProperties] getDeviceList() - - bool saveDepthAs(Camera &zed, defines.DEPTH_FORMAT format, types.String name, float factor) - bool savePointCloudAs(Camera &zed, defines.POINT_CLOUD_FORMAT format, types.String name, - bool with_color) - - -cdef extern from "Utils.cpp" namespace "sl": - bool saveMatDepthAs(core.Mat &depth, defines.DEPTH_FORMAT format, types.String name, float factor) - bool saveMatPointCloudAs(core.Mat &cloud, defines.POINT_CLOUD_FORMAT format, types.String name, - bool with_color) - -cdef class PyInputType: - cdef InputType input - -cdef class PyZEDCamera: - cdef Camera camera diff --git a/pyzed/camera.pyx b/pyzed/camera.pyx deleted file mode 100644 index 8bdc819..0000000 --- a/pyzed/camera.pyx +++ /dev/null @@ -1,916 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# Source file of the camera Python module. - -from cython.operator cimport dereference as deref -from cpython cimport bool - -import enum -import numpy as np -cimport numpy as np - -import pyzed.core as core -import pyzed.mesh as mesh -import pyzed.defines as defines -import pyzed.types as types - - -class PyRESOLUTION(enum.Enum): - PyRESOLUTION_HIGH = MAPPING_RESOLUTION_HIGH - PyRESOLUTION_MEDIUM = MAPPING_RESOLUTION_MEDIUM - PyRESOLUTION_LOW = MAPPING_RESOLUTION_LOW - - -class PyRANGE(enum.Enum): - PyRANGE_NEAR = MAPPING_RANGE_NEAR - PyRANGE_MEDIUM = MAPPING_RANGE_MEDIUM - PyRANGE_FAR = MAPPING_RANGE_FAR - -cdef class PyInputType: - def __cinit__(self, input_type=0): - if input_type == 0 : - self.input = InputType() - elif isinstance(input_type, PyInputType) : - input_t = input_type - self.input = InputType(input_t.input) - else : - raise TypeError("Argument is not of right type.") - - def set_from_camera_id(self, id): - self.input.setFromCameraID(id) - - def set_from_serial_number(self, serial_number): - self.input.setFromSerialNumber(serial_number) - - def set_from_svo_file(self, str svo_input_filename): - filename = svo_input_filename.encode() - self.input.setFromSVOFile(types.String( filename)) - - -cdef class PyInitParameters: - cdef InitParameters* init - def __cinit__(self, camera_resolution=defines.PyRESOLUTION.PyRESOLUTION_HD720, camera_fps=0, - camera_linux_id=0, svo_input_filename="", svo_real_time_mode=False, - depth_mode=defines.PyDEPTH_MODE.PyDEPTH_MODE_PERFORMANCE, - coordinate_units=defines.PyUNIT.PyUNIT_MILLIMETER, - coordinate_system=defines.PyCOORDINATE_SYSTEM.PyCOORDINATE_SYSTEM_IMAGE, - sdk_verbose=False, sdk_gpu_id=-1, depth_minimum_distance=-1.0, camera_disable_self_calib=False, - camera_image_flip=False, enable_right_side_measure=False, camera_buffer_count_linux=4, - sdk_verbose_log_file="", depth_stabilization=True, PyInputType input_t=PyInputType(), - optional_settings_path=""): - if (isinstance(camera_resolution, defines.PyRESOLUTION) and isinstance(camera_fps, int) and - isinstance(camera_linux_id, int) and isinstance(svo_input_filename, str) and - isinstance(svo_real_time_mode, bool) and isinstance(depth_mode, defines.PyDEPTH_MODE) and - isinstance(coordinate_units, defines.PyUNIT) and - isinstance(coordinate_system, defines.PyCOORDINATE_SYSTEM) and isinstance(sdk_verbose, bool) and - isinstance(sdk_gpu_id, int) and isinstance(depth_minimum_distance, float) and - isinstance(camera_disable_self_calib, bool) and isinstance(camera_image_flip, bool) and - isinstance(enable_right_side_measure, bool) and isinstance(camera_buffer_count_linux, int) and - isinstance(sdk_verbose_log_file, str) and isinstance(depth_stabilization, bool) and - isinstance(input_t, PyInputType) and isinstance(optional_settings_path, str)): - - filename = svo_input_filename.encode() - filelog = sdk_verbose_log_file.encode() - fileoption = optional_settings_path.encode() - self.init = new InitParameters(camera_resolution.value, camera_fps, camera_linux_id, - types.String( filename), svo_real_time_mode, depth_mode.value, - coordinate_units.value, coordinate_system.value, sdk_verbose, sdk_gpu_id, - depth_minimum_distance, camera_disable_self_calib, camera_image_flip, - enable_right_side_measure, camera_buffer_count_linux, - types.String( filelog), depth_stabilization, - 0, input_t.input, types.String( fileoption)) - else: - raise TypeError("Argument is not of right type.") - - def __dealloc__(self): - del self.init - - def save(self, str filename): - filename_save = filename.encode() - return self.init.save(types.String( filename_save)) - - def load(self, str filename): - filename_load = filename.encode() - return self.init.load(types.String( filename_load)) - - @property - def camera_resolution(self): - return defines.PyRESOLUTION(self.init.camera_resolution) - - @camera_resolution.setter - def camera_resolution(self, value): - if isinstance(value, defines.PyRESOLUTION): - self.init.camera_resolution = value.value - else: - raise TypeError("Argument must be of PyRESOLUTION type.") - - @property - def camera_fps(self): - return self.init.camera_fps - - @camera_fps.setter - def camera_fps(self, int value): - self.init.camera_fps = value - - @property - def camera_linux_id(self): - return self.init.camera_linux_id - - @camera_linux_id.setter - def camera_linux_id(self, int value): - self.init.camera_linux_id = value - - @property - def svo_input_filename(self): - if not self.init.svo_input_filename.empty(): - return self.init.svo_input_filename.get().decode() - else: - return "" - - @svo_input_filename.setter - def svo_input_filename(self, str value): - value_filename = value.encode() - self.init.svo_input_filename.set(value_filename) - - @property - def svo_real_time_mode(self): - return self.init.svo_real_time_mode - - @svo_real_time_mode.setter - def svo_real_time_mode(self, bool value): - self.init.svo_real_time_mode = value - - @property - def depth_mode(self): - return defines.PyDEPTH_MODE(self.init.depth_mode) - - @depth_mode.setter - def depth_mode(self, value): - if isinstance(value, defines.PyDEPTH_MODE): - self.init.depth_mode = value.value - else: - raise TypeError("Argument must be of PyDEPTH_MODE type.") - - @property - def coordinate_units(self): - return defines.PyUNIT(self.init.coordinate_units) - - @coordinate_units.setter - def coordinate_units(self, value): - if isinstance(value, defines.PyUNIT): - self.init.coordinate_units = value.value - else: - raise TypeError("Argument must be of PyUNIT type.") - - @property - def coordinate_system(self): - return defines.PyCOORDINATE_SYSTEM(self.init.coordinate_system) - - @coordinate_system.setter - def coordinate_system(self, value): - if isinstance(value, defines.PyCOORDINATE_SYSTEM): - self.init.coordinate_system = value.value - else: - raise TypeError("Argument must be of PyCOORDINATE_SYSTEM type.") - - @property - def sdk_verbose(self): - return self.init.sdk_verbose - - @sdk_verbose.setter - def sdk_verbose(self, bool value): - self.init.sdk_verbose = value - - @property - def sdk_gpu_id(self): - return self.init.sdk_gpu_id - - @sdk_gpu_id.setter - def sdk_gpu_id(self, int value): - self.init.sdk_gpu_id = value - - @property - def depth_minimum_distance(self): - return self.init.depth_minimum_distance - - @depth_minimum_distance.setter - def depth_minimum_distance(self, float value): - self.init.depth_minimum_distance = value - - @property - def camera_disable_self_calib(self): - return self.init.camera_disable_self_calib - - @camera_disable_self_calib.setter - def camera_disable_self_calib(self, bool value): - self.init.camera_disable_self_calib = value - - @property - def camera_image_flip(self): - return self.init.camera_image_flip - - @camera_image_flip.setter - def camera_image_flip(self, bool value): - self.init.camera_image_flip = value - - @property - def enable_right_side_measure(self): - return self.init.enable_right_side_measure - - @enable_right_side_measure.setter - def enable_right_side_measure(self, bool value): - self.init.enable_right_side_measure = value - - @property - def camera_buffer_count_linux(self): - return self.init.camera_buffer_count_linux - - @camera_buffer_count_linux.setter - def camera_buffer_count_linux(self, int value): - self.init.camera_buffer_count_linux = value - - @property - def sdk_verbose_log_file(self): - if not self.init.sdk_verbose_log_file.empty(): - return self.init.sdk_verbose_log_file.get().decode() - else: - return "" - - @sdk_verbose_log_file.setter - def sdk_verbose_log_file(self, str value): - value_filename = value.encode() - self.init.sdk_verbose_log_file.set(value_filename) - - @property - def depth_stabilization(self): - return self.init.depth_stabilization - - @depth_stabilization.setter - def depth_stabilization(self, bool value): - self.init.depth_stabilization = value - - @property - def input(self): - input_t = PyInputType() - input_t.input = self.init.input - return input_t - - @input.setter - def input(self, PyInputType input_t): - self.init.input = input_t.input - - @property - def optional_settings_path(self): - if not self.init.svo_input_filename.empty(): - return self.init.optional_settings_path.get().decode() - else: - return "" - - @optional_settings_path.setter - def optional_settings_path(self, str value): - value_filename = value.encode() - self.init.optional_settings_path.set(value_filename) - - def set_from_camera_id(self, id): - self.init.input.setFromCameraID(id) - - def set_from_serial_number(self, serial_number): - self.init.input.setFromSerialNumber(serial_number) - - def set_from_svo_file(self, str svo_input_filename): - filename = svo_input_filename.encode() - self.init.input.setFromSVOFile(types.String( filename)) - - -cdef class PyRuntimeParameters: - cdef RuntimeParameters* runtime - def __cinit__(self, sensing_mode=defines.PySENSING_MODE.PySENSING_MODE_STANDARD, enable_depth=True, - enable_point_cloud=True, - measure3D_reference_frame=defines.PyREFERENCE_FRAME.PyREFERENCE_FRAME_CAMERA): - if (isinstance(sensing_mode, defines.PySENSING_MODE) and isinstance(enable_depth, bool) - and isinstance(enable_point_cloud, bool) and - isinstance(measure3D_reference_frame, defines.PyREFERENCE_FRAME)): - - self.runtime = new RuntimeParameters(sensing_mode.value, enable_depth, enable_point_cloud, - measure3D_reference_frame.value) - else: - raise TypeError() - - def __dealloc__(self): - del self.runtime - - def save(self, str filename): - filename_save = filename.encode() - return self.runtime.save(types.String( filename_save)) - - def load(self, str filename): - filename_load = filename.encode() - return self.runtime.load(types.String( filename_load)) - - @property - def sensing_mode(self): - return defines.PySENSING_MODE(self.runtime.sensing_mode) - - @sensing_mode.setter - def sensing_mode(self, value): - if isinstance(value, defines.PySENSING_MODE): - self.runtime.sensing_mode = value.value - else: - raise TypeError("Argument must be of PySENSING_MODE type.") - - @property - def enable_depth(self): - return self.runtime.enable_depth - - @enable_depth.setter - def enable_depth(self, bool value): - self.runtime.enable_depth = value - - @property - def measure3D_reference_frame(self): - return defines.PyREFERENCE_FRAME(self.runtime.measure3D_reference_frame) - - @measure3D_reference_frame.setter - def measure3D_reference_frame(self, value): - if isinstance(value, defines.PyREFERENCE_FRAME): - self.runtime.measure3D_reference_frame = value.value - else: - raise TypeError("Argument must be of PyREFERENCE type.") - - -cdef class PyTrackingParameters: - cdef TrackingParameters* tracking - def __cinit__(self, core.PyTransform init_pos, _enable_memory=True, _area_path=None): - if _area_path is None: - self.tracking = new TrackingParameters(init_pos.transform, _enable_memory, types.String()) - else: - raise TypeError("Argument init_pos must be initialized first with PyTransform().") - - def __dealloc__(self): - del self.tracking - - def save(self, str filename): - filename_save = filename.encode() - return self.tracking.save(types.String( filename_save)) - - def load(self, str filename): - filename_load = filename.encode() - return self.tracking.load(types.String( filename_load)) - - def initial_world_transform(self, core.PyTransform init_pos): - init_pos.transform = self.tracking.initial_world_transform - return init_pos - - def set_initial_world_transform(self, core.PyTransform value): - self.tracking.initial_world_transform = value.transform - - @property - def enable_spatial_memory(self): - return self.tracking.enable_spatial_memory - - @enable_spatial_memory.setter - def enable_spatial_memory(self, bool value): - self.tracking.enable_spatial_memory = value - - @property - def enable_pose_smoothing(self): - return self.tracking.enable_pose_smoothing - - @enable_pose_smoothing.setter - def enable_pose_smoothing(self, bool value): - self.tracking.enable_pose_smoothing = value - - @property - def set_floor_as_origin(self): - return self.tracking.set_floor_as_origin - - @set_floor_as_origin.setter - def set_floor_as_origin(self, bool value): - self.tracking.set_floor_as_origin = value - - @property - def enable_imu_fusion(self): - return self.tracking.enable_imu_fusion - - @enable_imu_fusion.setter - def enable_imu_fusion(self, bool value): - self.tracking.enable_imu_fusion = value - - @property - def area_file_path(self): - if not self.tracking.area_file_path.empty(): - return self.tracking.area_file_path.get().decode() - else: - return "" - - @area_file_path.setter - def area_file_path(self, str value): - value_area = value.encode() - self.tracking.area_file_path.set(value_area) - - -cdef class PySpatialMappingParameters: - cdef SpatialMappingParameters* spatial - def __cinit__(self, resolution=PyRESOLUTION.PyRESOLUTION_HIGH, mapping_range=PyRANGE.PyRANGE_MEDIUM, - max_memory_usage=2048, save_texture=True, use_chunk_only=True, - reverse_vertex_order=False): - if (isinstance(resolution, PyRESOLUTION) and isinstance(mapping_range, PyRANGE) and - isinstance(use_chunk_only, bool) and isinstance(reverse_vertex_order, bool)): - self.spatial = new SpatialMappingParameters(resolution.value, mapping_range.value, max_memory_usage, save_texture, - use_chunk_only, reverse_vertex_order) - else: - raise TypeError() - - def __dealloc__(self): - del self.spatial - - def set_resolution(self, resolution=PyRESOLUTION.PyRESOLUTION_HIGH): - if isinstance(resolution, PyRESOLUTION): - self.spatial.set( resolution.value) - else: - raise TypeError("Argument is not of PyRESOLUTION type.") - - def set_range(self, mapping_range=PyRANGE.PyRANGE_MEDIUM): - if isinstance(mapping_range, PyRANGE): - self.spatial.set( mapping_range.value) - else: - raise TypeError("Argument is not of PyRANGE type.") - - def get_range_preset(self, mapping_range): - if isinstance(mapping_range, PyRANGE): - return self.spatial.get( mapping_range.value) - else: - raise TypeError("Argument is not of PyRANGE type.") - - def get_resolution_preset(self, resolution): - if isinstance(resolution, PyRESOLUTION): - return self.spatial.get( resolution.value) - else: - raise TypeError("Argument is not of PyRESOLUTION type.") - - def get_recommended_range(self, resolution, PyZEDCamera py_cam): - if isinstance(resolution, PyRESOLUTION): - return self.spatial.getRecommendedRange( resolution.value, py_cam.camera) - elif isinstance(resolution, float): - return self.spatial.getRecommendedRange( resolution, py_cam.camera) - else: - raise TypeError("Argument is not of PyRESOLUTION or float type.") - - @property - def max_memory_usage(self): - return self.spatial.max_memory_usage - - @max_memory_usage.setter - def max_memory_usage(self, int value): - self.spatial.max_memory_usage = value - - @property - def save_texture(self): - return self.spatial.save_texture - - @save_texture.setter - def save_texture(self, bool value): - self.spatial.save_texture = value - - @property - def use_chunk_only(self): - return self.spatial.use_chunk_only - - @use_chunk_only.setter - def use_chunk_only(self, bool value): - self.spatial.use_chunk_only = value - - @property - def reverse_vertex_order(self): - return self.spatial.reverse_vertex_order - - @reverse_vertex_order.setter - def reverse_vertex_order(self, bool value): - self.spatial.reverse_vertex_order = value - - @property - def allowed_range(self): - return self.spatial.allowed_range - - @property - def range_meter(self): - return self.spatial.range_meter - - @range_meter.setter - def range_meter(self, float value): - self.spatial.range_meter = value - - @property - def allowed_resolution(self): - return self.spatial.allowed_resolution - - @property - def resolution_meter(self): - return self.spatial.resolution_meter - - @resolution_meter.setter - def resolution_meter(self, float value): - self.spatial.resolution_meter = value - - -cdef class PyPose: - cdef Pose pose - def __cinit__(self): - self.pose = Pose() - - def init_pose(self, PyPose pose): - self.pose = Pose(pose.pose) - - def init_transform(self, core.PyTransform pose_data, mtimestamp=0, mconfidence=0): - self.pose = Pose(pose_data.transform, mtimestamp, mconfidence) - - def get_translation(self, core.PyTranslation py_translation): - py_translation.translation = self.pose.getTranslation() - return py_translation - - def get_orientation(self, core.PyOrientation py_orientation): - py_orientation.orientation = self.pose.getOrientation() - return py_orientation - - def get_rotation_matrix(self, core.PyRotation py_rotation): - py_rotation.rotation = self.pose.getRotationMatrix() - py_rotation.mat = self.pose.getRotationMatrix() - return py_rotation - - def get_rotation_vector(self): - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = self.pose.getRotationVector()[i] - return arr - - def get_euler_angles(self, radian=True): - cdef np.ndarray arr = np.zeros(3) - if isinstance(radian, bool): - for i in range(3): - arr[i] = self.pose.getEulerAngles(radian)[i] - else: - raise TypeError("Argument is not of bool type.") - return arr - - @property - def valid(self): - return self.pose.valid - - @property - def timestamp(self): - return self.pose.timestamp - - def pose_data(self, core.PyTransform pose_data): - pose_data.transform = self.pose.pose_data - pose_data.mat = self.pose.pose_data - return pose_data - - @property - def pose_confidence(self): - return self.pose.pose_confidence - - @property - def pose_covariance(self): - cdef np.ndarray arr = np.zeros(36) - for i in range(36) : - arr[i] = self.pose.pose_covariance[i] - return arr - -cdef class PyIMUData: - cdef IMUData imuData - - def __cinit__(self): - self.imuData = IMUData() - - def init_imuData(self, PyIMUData imuData): - self.imuData = IMUData(imuData.imuData) - - def init_transform(self, core.PyTransform pose_data, mtimestamp=0, mconfidence=0): - self.imuData = IMUData(pose_data.transform, mtimestamp, mconfidence) - - def get_orientation_covariance(self, types.PyMatrix3f orientation_covariance): - orientation_covariance.mat = self.imuData.orientation_covariance - return orientation_covariance - - def get_angular_velocity(self, angular_velocity): - for i in range(3): - angular_velocity[i] = self.imuData.angular_velocity[i] - return angular_velocity - - def get_linear_acceleration(self, linear_acceleration): - for i in range(3): - linear_acceleration[i] = self.imuData.linear_acceleration[i] - return linear_acceleration - - def get_angular_velocity_convariance(self, types.PyMatrix3f angular_velocity_convariance): - angular_velocity_convariance.mat = self.imuData.angular_velocity_convariance - return angular_velocity_convariance - - def get_linear_acceleration_convariance(self, types.PyMatrix3f linear_acceleration_convariance): - linear_acceleration_convariance.mat = self.imuData.linear_acceleration_convariance - return linear_acceleration_convariance - - def get_translation(self, core.PyTranslation py_translation): - py_translation.translation = self.imuData.getTranslation() - return py_translation - - def get_orientation(self, core.PyOrientation py_orientation): - py_orientation.orientation = self.imuData.getOrientation() - return py_orientation - - def get_rotation_matrix(self, core.PyRotation py_rotation): - py_rotation.rotation = self.imuData.getRotationMatrix() - py_rotation.mat = self.imuData.getRotationMatrix() - return py_rotation - - def get_rotation_vector(self): - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = self.imuData.getRotationVector()[i] - return arr - - def get_euler_angles(self, radian=True): - cdef np.ndarray arr = np.zeros(3) - if isinstance(radian, bool): - for i in range(3): - arr[i] = self.imuData.getEulerAngles(radian)[i] - else: - raise TypeError("Argument is not of bool type.") - return arr - - def pose_data(self, core.PyTransform pose_data): - pose_data.transform = self.imuData.pose_data - pose_data.mat = self.imuData.pose_data - return pose_data - - @property - def timestamp(self): - return self.imuData.timestamp - -cdef class PyZEDCamera: - def __cinit__(self): - self.camera = Camera() - - def close(self): - self.camera.close() - - def open(self, PyInitParameters py_init): - if py_init: - return types.PyERROR_CODE(self.camera.open(deref(py_init.init))) - else: - print("InitParameters must be initialized first with PyInitParameters().") - - def is_opened(self): - return self.camera.isOpened() - - def grab(self, PyRuntimeParameters py_runtime): - if py_runtime: - return types.PyERROR_CODE(self.camera.grab(deref(py_runtime.runtime))) - else: - print("RuntimeParameters must be initialized first with PyRuntimeParameters().") - - def retrieve_image(self, core.PyMat py_mat, view=defines.PyVIEW.PyVIEW_LEFT, type=core.PyMEM.PyMEM_CPU, width=0, - height=0): - if (isinstance(view, defines.PyVIEW) and isinstance(type, core.PyMEM) and isinstance(width, int) and - isinstance(height, int)): - return types.PyERROR_CODE(self.camera.retrieveImage(py_mat.mat, view.value, type.value, width, height)) - else: - raise TypeError("Arguments must be of PyVIEW, PyMEM and integer types.") - - def retrieve_measure(self, core.PyMat py_mat, measure=defines.PyMEASURE.PyMEASURE_DEPTH, type=core.PyMEM.PyMEM_CPU, - width=0, height=0): - if (isinstance(measure, defines.PyMEASURE) and isinstance(type, core.PyMEM) and isinstance(width, int) and - isinstance(height, int)): - return types.PyERROR_CODE(self.camera.retrieveMeasure(py_mat.mat, measure.value, type.value, width, height)) - else: - raise TypeError("Arguments must be of PyMEASURE, PyMEM and integer types.") - - def set_confidence_threshold(self, int conf_treshold_value): - self.camera.setConfidenceThreshold(conf_treshold_value) - - def get_confidence_threshold(self): - return self.camera.getConfidenceThreshold() - - def get_resolution(self): - return core.PyResolution(self.camera.getResolution().width, self.camera.getResolution().height) - - def set_depth_max_range_value(self, float depth_max_range): - self.camera.setDepthMaxRangeValue(depth_max_range) - - def get_depth_max_range_value(self): - return self.camera.getDepthMaxRangeValue() - - def get_depth_min_range_value(self): - return self.camera.getDepthMinRangeValue() - - def set_svo_position(self, int frame_number): - self.camera.setSVOPosition(frame_number) - - def get_svo_position(self): - return self.camera.getSVOPosition() - - def get_svo_number_of_frames(self): - return self.camera.getSVONumberOfFrames() - - def set_camera_settings(self, settings, int value, use_default=False): - if isinstance(settings, defines.PyCAMERA_SETTINGS) and isinstance(use_default, bool): - self.camera.setCameraSettings(settings.value, value, use_default) - else: - raise TypeError("Arguments must be of PyCAMERA_SETTINGS and boolean types.") - - def get_camera_settings(self, setting): - if isinstance(setting, defines.PyCAMERA_SETTINGS): - return self.camera.getCameraSettings(setting.value) - else: - raise TypeError("Argument is not of PyCAMERA_SETTINGS type.") - - def get_camera_fps(self): - return self.camera.getCameraFPS() - - def set_camera_fps(self, int desired_fps): - self.camera.setCameraFPS(desired_fps) - - def get_current_fps(self): - return self.camera.getCurrentFPS() - - def get_camera_timestamp(self): - return self.camera.getCameraTimestamp() - - def get_current_timestamp(self): - return self.camera.getCurrentTimestamp() - - def get_timestamp(self, time_reference): - if isinstance(time_reference, defines.PyTIME_REFERENCE): - return self.camera.getTimestamp(time_reference.value) - else: - raise TypeError("Argument is not of PyTIME_REFERENCE type.") - - def get_frame_dropped_count(self): - return self.camera.getFrameDroppedCount() - - def get_camera_information(self, resizer = core.PyResolution(0, 0)): - return core.PyCameraInformation(self, resizer) - - def get_self_calibration_state(self): - return defines.PySELF_CALIBRATION_STATE(self.camera.getSelfCalibrationState()) - - def reset_self_calibration(self): - self.camera.resetSelfCalibration() - - def enable_tracking(self, PyTrackingParameters py_tracking): - if py_tracking: - return types.PyERROR_CODE(self.camera.enableTracking(deref(py_tracking.tracking))) - else: - print("TrackingParameters must be initialized first with PyTrackingParameters().") - - def get_imu_data(self, PyIMUData py_imu_data, time_reference = defines.PyTIME_REFERENCE.PyTIME_REFERENCE_CURRENT): - if isinstance(time_reference, defines.PyTIME_REFERENCE): - return types.PyERROR_CODE(self.camera.getIMUData(py_imu_data.imuData, time_reference.value)) - else: - raise TypeError("Argument is not of PyTIME_REFERENCE type.") - - def set_imu_prior(self, core.PyTransform transfom): - return types.PyERROR_CODE(self.camera.setIMUPrior(transfom.transform)) - - def get_position(self, PyPose py_pose, reference_frame = defines.PyREFERENCE_FRAME.PyREFERENCE_FRAME_WORLD): - if isinstance(reference_frame, defines.PyREFERENCE_FRAME): - return defines.PyTRACKING_STATE(self.camera.getPosition(py_pose.pose, reference_frame.value)) - else: - raise TypeError("Argument is not of PyREFERENCE_FRAME type.") - - def get_area_export_state(self): - return defines.PyAREA_EXPORT_STATE(self.camera.getAreaExportState()) - - def save_current_area(self, str area_file_path): - filename = area_file_path.encode() - return types.PyERROR_CODE(self.camera.saveCurrentArea(types.String( filename))) - - def disable_tracking(self, str area_file_path=""): - filename = area_file_path.encode() - self.camera.disableTracking(types.String( filename)) - - def reset_tracking(self, core.PyTransform path): - return types.PyERROR_CODE(self.camera.resetTracking(path.transform)) - - def enable_spatial_mapping(self, PySpatialMappingParameters py_spatial): - if py_spatial: - return types.PyERROR_CODE(self.camera.enableSpatialMapping(deref(py_spatial.spatial))) - else: - print("SpatialMappingParameters must be initialized first with PySpatialMappingParameters()") - - def pause_spatial_mapping(self, status): - if isinstance(status, bool): - self.camera.pauseSpatialMapping(status) - else: - raise TypeError("Argument is not of boolean type.") - - def get_spatial_mapping_state(self): - return defines.PySPATIAL_MAPPING_STATE(self.camera.getSpatialMappingState()) - - def extract_whole_mesh(self, mesh.PyMesh py_mesh): - return types.PyERROR_CODE(self.camera.extractWholeMesh(deref(py_mesh.mesh))) - - def request_mesh_async(self): - self.camera.requestMeshAsync() - - def get_mesh_request_status_async(self): - return types.PyERROR_CODE(self.camera.getMeshRequestStatusAsync()) - - def retrieve_mesh_async(self, mesh.PyMesh py_mesh): - return types.PyERROR_CODE(self.camera.retrieveMeshAsync(deref(py_mesh.mesh))) - - def find_plane_at_hit(self, coord, mesh.PyPlane py_plane): - cdef types.Vector2[uint] vec = types.Vector2[uint](coord[0], coord[1]) - return types.PyERROR_CODE(self.camera.findPlaneAtHit(vec, py_plane.plane)) - - def find_floor_plane(self, mesh.PyPlane py_plane, core.PyTransform resetTrackingFloorFrame, floor_height_prior = float('nan'), core.PyRotation world_orientation_prior = core.PyRotation(types.PyMatrix3f().zeros()), floor_height_prior_tolerance = float('nan')) : - return types.PyERROR_CODE(self.camera.findFloorPlane(py_plane.plane, resetTrackingFloorFrame.transform, floor_height_prior, world_orientation_prior.rotation, floor_height_prior_tolerance)) - - def disable_spatial_mapping(self): - self.camera.disableSpatialMapping() - - def enable_recording(self, str video_filename, - compression_mode=defines.PySVO_COMPRESSION_MODE.PySVO_COMPRESSION_MODE_LOSSLESS): - if isinstance(compression_mode, defines.PySVO_COMPRESSION_MODE): - filename = video_filename.encode() - return types.PyERROR_CODE(self.camera.enableRecording(types.String( filename), - compression_mode.value)) - else: - raise TypeError("Argument is not of PySVO_COMPRESSION_MODE type.") - - def record(self): - return self.camera.record() - - def disable_recording(self): - self.camera.disableRecording() - - def get_sdk_version(cls): - return cls.camera.getSDKVersion().get().decode() - - def is_zed_connected(cls): - return cls.camera.isZEDconnected() - - def stickto_cpu_core(cls, int cpu_core): - return types.PyERROR_CODE(cls.camera.sticktoCPUCore(cpu_core)) - - def get_device_list(cls): - vect_ = cls.camera.getDeviceList() - vect_python = [] - for i in range(vect_.size()): - prop = types.PyDeviceProperties() - prop.camera_state = vect_[i].camera_state - prop.id = vect_[i].id - prop.path = vect_[i].path.get().decode() - prop.camera_model = vect_[i].camera_model - prop.serial_number = vect_[i].serial_number - vect_python.append(prop) - return vect_python - -def save_camera_depth_as(PyZEDCamera zed, format, str name, factor=1): - if isinstance(format, defines.PyDEPTH_FORMAT) and factor <= 65536: - name_save = name.encode() - return saveDepthAs(zed.camera, format.value, types.String(name_save), factor) - else: - raise TypeError("Arguments must be of PyDEPTH_FORMAT type and factor not over 65536.") - -def save_camera_point_cloud_as(PyZEDCamera zed, format, str name, with_color=False): - if isinstance(format, defines.PyPOINT_CLOUD_FORMAT): - name_save = name.encode() - return savePointCloudAs(zed.camera, format.value, types.String(name_save), - with_color) - else: - raise TypeError("Argument is not of PyPOINT_CLOUD_FORMAT type.") - -def save_mat_depth_as(core.PyMat py_mat, format, str name, factor=1): - if isinstance(format, defines.PyDEPTH_FORMAT) and factor <= 65536: - name_save = name.encode() - return saveMatDepthAs(py_mat.mat, format.value, types.String(name_save), factor) - else: - raise TypeError("Arguments must be of PyDEPTH_FORMAT type and factor not over 65536.") - - -def save_mat_point_cloud_as(core.PyMat py_mat, format, str name, with_color=False): - if isinstance(format, defines.PyPOINT_CLOUD_FORMAT): - name_save = name.encode() - return saveMatPointCloudAs(py_mat.mat, format.value, types.String(name_save), - with_color) - else: - raise TypeError("Argument is not of PyPOINT_CLOUD_FORMAT type.") diff --git a/pyzed/core.pxd b/pyzed/core.pxd deleted file mode 100644 index 1208ac8..0000000 --- a/pyzed/core.pxd +++ /dev/null @@ -1,295 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# File containing the Cython declarations to use the Core.hpp functions. - -from libcpp cimport bool -from libcpp.vector cimport vector -cimport pyzed.types as types - - -cdef extern from "sl/Core.hpp" namespace "sl": - - types.timeStamp getCurrentTimeStamp() - - cdef struct Resolution: - size_t width - size_t height - - - cdef struct CameraParameters: - float fx - float fy - float cx - float cy - double disto[5] - float v_fov - float h_fov - float d_fov - Resolution image_size - - void SetUp(float focal_x, float focal_y, float center_x, float center_y) - - - cdef struct CalibrationParameters: - types.Vector3[float] R - types.Vector3[float] T - CameraParameters left_cam - CameraParameters right_cam - - - cdef struct CameraInformation: - CalibrationParameters calibration_parameters - CalibrationParameters calibration_parameters_raw - Transform camera_imu_transform - unsigned int serial_number - unsigned int firmware_version - types.MODEL camera_model - - cdef enum MEM: - MEM_CPU - MEM_GPU - - MEM operator|(MEM a, MEM b) - - - cdef enum COPY_TYPE: - COPY_TYPE_CPU_CPU - COPY_TYPE_CPU_GPU - COPY_TYPE_GPU_GPU - COPY_TYPE_GPU_CPU - - - cdef enum MAT_TYPE: - MAT_TYPE_32F_C1 - MAT_TYPE_32F_C2 - MAT_TYPE_32F_C3 - MAT_TYPE_32F_C4 - MAT_TYPE_8U_C1 - MAT_TYPE_8U_C2 - MAT_TYPE_8U_C3 - MAT_TYPE_8U_C4 - - - cdef cppclass Mat 'sl::Mat': - types.String name - bool verbose - - Mat() - Mat(size_t width, size_t height, MAT_TYPE mat_type, MEM memory_type) - Mat(size_t width, size_t height, MAT_TYPE mat_type, unsigned char *ptr, size_t step, MEM memory_type) - Mat(size_t width, size_t height, MAT_TYPE mat_type, unsigned char *ptr_cpu, size_t step_cpu, - unsigned char *ptr_gpu, size_t step_gpu) - Mat(Resolution resolution, MAT_TYPE mat_type, MEM memory_type) - Mat(Resolution resolution, MAT_TYPE mat_type, unsigned char *ptr, size_t step, MEM memory_type) - Mat(const Mat &mat) - - void alloc(size_t width, size_t height, MAT_TYPE mat_type, MEM memory_type) - void alloc(Resolution resolution, MAT_TYPE mat_type, MEM memory_type) - void free(MEM memory_type) - Mat &operator=(const Mat &that) - types.ERROR_CODE updateCPUfromGPU() - types.ERROR_CODE updateGPUfromCPU() - types.ERROR_CODE copyTo(Mat &dst, COPY_TYPE cpyType) const - types.ERROR_CODE setFrom(const Mat &src, COPY_TYPE cpyType) const - types.ERROR_CODE read(const char* filePath) - types.ERROR_CODE write(const char* filePath) - size_t getWidth() const - size_t getHeight() const - Resolution getResolution() const - size_t getChannels() const - MAT_TYPE getDataType() const - MEM getMemoryType() const - size_t getStepBytes(MEM memory_type) - size_t getStep(MEM memory_type) - size_t getPixelBytes() - size_t getWidthBytes() - types.String getInfos() - bool isInit() - bool isMemoryOwner() - types.ERROR_CODE clone(const Mat &src) - types.ERROR_CODE move(Mat &dst) - - @staticmethod - void swap(Mat &mat1, Mat &mat2) - - cdef cppclass Rotation(types.Matrix3f): - Rotation() - Rotation(const Rotation &rotation) - Rotation(const types.Matrix3f &mat) - Rotation(const Orientation &orientation) - Rotation(const float angle, const Translation &axis) - void setOrientation(const Orientation &orientation) - Orientation getOrientation() const - types.Vector3[float] getRotationVector() - void setRotationVector(const types.Vector3[float] &vec_rot) - types.Vector3[float] getEulerAngles(bool radian) const - void setEulerAngles(const types.Vector3[float] &euler_angles, bool radian) - - - cdef cppclass Translation(types.Vector3): - Translation() - Translation(const Translation &translation) - Translation(float t1, float t2, float t3) - Translation operator*(const Orientation &mat) const - void normalize() - - @staticmethod - Translation normalize(const Translation &tr) - float &operator()(int x) - - - cdef cppclass Orientation(types.Vector4): - Orientation() - Orientation(const Orientation &orientation) - Orientation(const types.Vector4[float] &input) - Orientation(const Rotation &rotation) - Orientation(const Translation &tr1, const Translation &tr2) - float operator()(int x) - Orientation operator*(const Orientation &orientation) const - void setRotationMatrix(const Rotation &rotation) - Rotation getRotationMatrix() const - void setIdentity() - @staticmethod - Orientation identity() - void setZeros() - @staticmethod - Orientation zeros() - void normalise() - - @staticmethod - Orientation normalise(const Orientation &orient) - - - cdef cppclass Transform(types.Matrix4f): - Transform() - Transform(const Transform &motion) - Transform(const types.Matrix4f &mat) - Transform(const Rotation &rotation, const Translation &translation) - Transform(const Orientation &orientation, const Translation &translation) - void setRotationMatrix(const Rotation &rotation) - Rotation getRotationMatrix() const - void setTranslation(const Translation &translation) - Translation getTranslation() const - void setOrientation(const Orientation &orientation) - Orientation getOrientation() const - types.Vector3[float] getRotationVector() - void setRotationVector(const types.Vector3[float] &vec_rot) - types.Vector3[float] getEulerAngles(bool radian) const - void setEulerAngles(const types.Vector3[float] &euler_angles, bool radian) - - -ctypedef unsigned char uchar1 -ctypedef types.Vector2[unsigned char] uchar2 -ctypedef types.Vector3[unsigned char] uchar3 -ctypedef types.Vector4[unsigned char] uchar4 - -ctypedef float float1 -ctypedef types.Vector2[float] float2 -ctypedef types.Vector3[float] float3 -ctypedef types.Vector4[float] float4 - - -cdef extern from "Utils.cpp" namespace "sl": - - Mat matResolution(Resolution resolution, MAT_TYPE mat_type, uchar1 *ptr_cpu, size_t step_cpu, - uchar1 *ptr_gpu, size_t step_gpu) - - types.ERROR_CODE setToUchar1(Mat &mat, uchar1 value, MEM memory_type) - types.ERROR_CODE setToUchar2(Mat &mat, uchar2 value, MEM memory_type) - types.ERROR_CODE setToUchar3(Mat &mat, uchar3 value, MEM memory_type) - types.ERROR_CODE setToUchar4(Mat &mat, uchar4 value, MEM memory_type) - - types.ERROR_CODE setToFloat1(Mat &mat, float1 value, MEM memory_type) - types.ERROR_CODE setToFloat2(Mat &mat, float2 value, MEM memory_type) - types.ERROR_CODE setToFloat3(Mat &mat, float3 value, MEM memory_type) - types.ERROR_CODE setToFloat4(Mat &mat, float4 value, MEM memory_type) - - types.ERROR_CODE setValueUchar1(Mat &mat, size_t x, size_t y, uchar1 value, MEM memory_type) - types.ERROR_CODE setValueUchar2(Mat &mat, size_t x, size_t y, uchar2 value, MEM memory_type) - types.ERROR_CODE setValueUchar3(Mat &mat, size_t x, size_t y, uchar3 value, MEM memory_type) - types.ERROR_CODE setValueUchar4(Mat &mat, size_t x, size_t y, uchar4 value, MEM memory_type) - - types.ERROR_CODE setValueFloat1(Mat &mat, size_t x, size_t y, float1 value, MEM memory_type) - types.ERROR_CODE setValueFloat2(Mat &mat, size_t x, size_t y, float2 value, MEM memory_type) - types.ERROR_CODE setValueFloat3(Mat &mat, size_t x, size_t y, float3 value, MEM memory_type) - types.ERROR_CODE setValueFloat4(Mat &mat, size_t x, size_t y, float4 value, MEM memory_type) - - types.ERROR_CODE getValueUchar1(Mat &mat, size_t x, size_t y, uchar1 *value, MEM memory_type) - types.ERROR_CODE getValueUchar2(Mat &mat, size_t x, size_t y, types.Vector2[uchar1] *value, MEM memory_type) - types.ERROR_CODE getValueUchar3(Mat &mat, size_t x, size_t y, types.Vector3[uchar1] *value, MEM memory_type) - types.ERROR_CODE getValueUchar4(Mat &mat, size_t x, size_t y, types.Vector4[uchar1] *value, MEM memory_type) - - types.ERROR_CODE getValueFloat1(Mat &mat, size_t x, size_t y, float1 *value, MEM memory_type) - types.ERROR_CODE getValueFloat2(Mat &mat, size_t x, size_t y, types.Vector2[float1] *value, MEM memory_type) - types.ERROR_CODE getValueFloat3(Mat &mat, size_t x, size_t y, types.Vector3[float1] *value, MEM memory_type) - types.ERROR_CODE getValueFloat4(Mat &mat, size_t x, size_t y, types.Vector4[float1] *value, MEM memory_type) - - uchar1 *getPointerUchar1(Mat &mat, MEM memory_type) - uchar2 *getPointerUchar2(Mat &mat, MEM memory_type) - uchar3 *getPointerUchar3(Mat &mat, MEM memory_type) - uchar4 *getPointerUchar4(Mat &mat, MEM memory_type) - - float1 *getPointerFloat1(Mat &mat, MEM memory_type) - float2 *getPointerFloat2(Mat &mat, MEM memory_type) - float3 *getPointerFloat3(Mat &mat, MEM memory_type) - float4 *getPointerFloat4(Mat &mat, MEM memory_type) - - - - -cdef class PyMat: - cdef Mat mat - -cdef class PyRotation(types.PyMatrix3f): - cdef Rotation rotation - - -cdef class PyTranslation: - cdef Translation translation - - -cdef class PyOrientation: - cdef Orientation orientation - - -cdef class PyTransform(types.PyMatrix4f): - cdef Transform transform - - -cdef class PyCameraParameters: - cdef CameraParameters camera_params - - -cdef class PyCalibrationParameters: - cdef CalibrationParameters calibration - cdef PyCameraParameters py_left_cam - cdef PyCameraParameters py_right_cam - cdef types.Vector3[float] R - cdef types.Vector3[float] T - - -cdef class PyCameraInformation: - cdef PyCalibrationParameters py_calib - cdef PyCalibrationParameters py_calib_raw - cdef unsigned int serial_number - cdef unsigned int firmware_version - cdef types.MODEL camera_model - cdef PyTransform py_camera_imu_transform diff --git a/pyzed/core.pyx b/pyzed/core.pyx deleted file mode 100644 index 10dab8e..0000000 --- a/pyzed/core.pyx +++ /dev/null @@ -1,699 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# Source file of the core Python module. - -from cython.operator cimport dereference as deref -from libc.string cimport memcpy -from cpython cimport bool - -import enum -import numpy as np -cimport numpy as np -import pyzed.types as types -cimport pyzed.camera as camera -import pyzed.camera as camera - - -class PyMEM(enum.Enum): - PyMEM_CPU = MEM_CPU - PyMEM_GPU = MEM_GPU - - -class PyCOPY_TYPE(enum.Enum): - PyCOPY_TYPE_CPU_CPU = COPY_TYPE_CPU_CPU - PyCOPY_TYPE_CPU_GPU = COPY_TYPE_CPU_GPU - PyCOPY_TYPE_GPU_GPU = COPY_TYPE_GPU_GPU - PyCOPY_TYPE_GPU_CPU = COPY_TYPE_GPU_CPU - - -class PyMAT_TYPE(enum.Enum): - PyMAT_TYPE_32F_C1 = MAT_TYPE_32F_C1 - PyMAT_TYPE_32F_C2 = MAT_TYPE_32F_C2 - PyMAT_TYPE_32F_C3 = MAT_TYPE_32F_C3 - PyMAT_TYPE_32F_C4 = MAT_TYPE_32F_C4 - PyMAT_TYPE_8U_C1 = MAT_TYPE_8U_C1 - PyMAT_TYPE_8U_C2 = MAT_TYPE_8U_C2 - PyMAT_TYPE_8U_C3 = MAT_TYPE_8U_C3 - PyMAT_TYPE_8U_C4 = MAT_TYPE_8U_C4 - - -def get_current_timestamp(): - return getCurrentTimeStamp() - - -cdef class PyResolution: - cdef size_t width - cdef size_t height - def __cinit__(self, width=0, height=0): - self.width = width - self.height = height - - def py_area(self): - return self.width * self.height - - @property - def width(self): - return self.width - - @property - def height(self): - return self.height - - def __richcmp__(PyResolution left, PyResolution right, int op): - if op == 2: - return left.width==right.width and left.height==right.height - if op == 3: - return left.width!=right.width or left.height!=right.height - else: - raise NotImplementedError() - - -cdef class PyCameraParameters: - @property - def fx(self): - return self.camera_params.fx - - @property - def fy(self): - return self.camera_params.fy - - @property - def cx(self): - return self.camera_params.cx - - @property - def cy(self): - return self.camera_params.cy - - @property - def disto(self): - return self.camera_params.disto - - @property - def v_fov(self): - return self.camera_params.v_fov - - @property - def h_fov(self): - return self.camera_params.h_fov - - @property - def d_fov(self): - return self.camera_params.d_fov - - @property - def image_size(self): - return self.camera_params.image_size - - -cdef class PyCalibrationParameters: - def __cinit__(self): - self.py_left_cam = PyCameraParameters() - self.py_right_cam = PyCameraParameters() - - def set(self): - self.py_left_cam.camera_params = self.calibration.left_cam - self.py_right_cam.camera_params = self.calibration.right_cam - self.R = self.calibration.R - self.T = self.calibration.T - - @property - def R(self): - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = self.calibration.R[i] - return arr - - @property - def T(self): - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = self.calibration.T[i] - return arr - - @property - def left_cam(self): - return self.py_left_cam - - @property - def right_cam(self): - return self.py_right_cam - - -cdef class PyCameraInformation: - def __cinit__(self, camera.PyZEDCamera py_camera, PyResolution resizer): - res = Resolution(resizer.width, resizer.height) - self.py_calib = PyCalibrationParameters() - self.py_calib.calibration = py_camera.camera.getCameraInformation(res).calibration_parameters - self.py_calib_raw = PyCalibrationParameters() - self.py_calib_raw.calibration = py_camera.camera.getCameraInformation(res).calibration_parameters_raw - self.py_calib.set() - self.py_calib_raw.set() - self.py_camera_imu_transform = PyTransform() - self.py_camera_imu_transform.transform = py_camera.camera.getCameraInformation(res).camera_imu_transform - self.serial_number = py_camera.camera.getCameraInformation(res).serial_number - self.firmware_version = py_camera.camera.getCameraInformation(res).firmware_version - self.camera_model = py_camera.camera.getCameraInformation(res).camera_model - - @property - def camera_model(self): - return self.camera_model - - @property - def calibration_parameters(self): - return self.py_calib - - @property - def calibration_parameters_raw(self): - return self.py_calib_raw - - @property - def camera_imu_transform(self): - return self.py_camera_imu_transform - - @property - def serial_number(self): - return self.serial_number - - @property - def firmware_version(self): - return self.firmware_version - - -cdef class PyMat: - def __cinit__(self): - self.mat = Mat() - - def init_mat_type(self, width, height, mat_type, memory_type=PyMEM.PyMEM_CPU): - if isinstance(mat_type, PyMAT_TYPE) and isinstance(memory_type, PyMEM): - self.mat = Mat(width, height, mat_type.value, memory_type.value) - else: - raise TypeError("Argument are not of PyMAT_TYPE or PyMEM type.") - - def init_mat_cpu(self, width, height, mat_type, ptr, step, memory_type=PyMEM.PyMEM_CPU): - if isinstance(mat_type, PyMAT_TYPE) and isinstance(memory_type, PyMEM): - self.mat = Mat(width, height, mat_type.value, ptr.encode(), step, memory_type.value) - else: - raise TypeError("Argument are not of PyMAT_TYPE or PyMEM type.") - - def init_mat_cpu_gpu(self, width, height, mat_type, ptr_cpu, step_cpu, ptr_gpu, step_gpu): - if isinstance(mat_type, PyMAT_TYPE): - self.mat = Mat(width, height, mat_type.value, ptr_cpu.encode(), step_cpu, ptr_gpu.encode(), step_gpu) - else: - raise TypeError("Argument is not of PyMAT_TYPE type.") - - def init_mat_resolution(self, PyResolution resolution, mat_type, memory_type): - if isinstance(mat_type, PyMAT_TYPE) and isinstance(memory_type, PyMEM): - self.mat = Mat(Resolution(resolution.width, resolution.height), mat_type.value, memory_type.value) - else: - raise TypeError("Argument are not of PyMAT_TYPE or PyMEM type.") - - def init_mat_resolution_cpu(self, PyResolution resolution, mat_type, ptr, step, memory_type=PyMEM.PyMEM_CPU): - if isinstance(mat_type, PyMAT_TYPE) and isinstance(memory_type, PyMEM): - self.mat = Mat(Resolution(resolution.width, resolution.height), mat_type.value, ptr.encode(), - step, memory_type.value) - else: - raise TypeError("Argument are not of PyMAT_TYPE or PyMEM type.") - - def init_mat_resolution_cpu_gpu(self, PyResolution resolution, mat_type, ptr_cpu, step_cpu, ptr_gpu, step_gpu): - if isinstance(mat_type, PyMAT_TYPE): - self.mat = matResolution(Resolution(resolution.width, resolution.height), mat_type.value, ptr_cpu.encode(), - step_cpu, ptr_gpu.encode(), step_gpu) - else: - raise TypeError("Argument is not of PyMAT_TYPE type.") - - def init_mat(self, PyMat matrix): - self.mat = Mat(matrix.mat) - - def alloc_size(self, width, height, mat_type, memory_type=PyMEM.PyMEM_CPU): - if isinstance(mat_type, PyMAT_TYPE) and isinstance(memory_type, PyMEM): - self.mat.alloc( width, height, mat_type.value, memory_type.value) - else: - raise TypeError("Arguments must be of PyMat and PyMEM types.") - - def alloc_resolution(self, PyResolution resolution, mat_type, memory_type=PyMEM.PyMEM_CPU): - if isinstance(mat_type, PyMAT_TYPE) and isinstance(memory_type, PyMEM): - self.mat.alloc(resolution.width, resolution.height, mat_type.value, memory_type.value) - else: - raise TypeError("Arguments must be of PyMat and PyMEM types.") - - def free(self, memory_type=None): - if isinstance(memory_type, PyMEM): - self.mat.free(memory_type.value) - elif memory_type is None: - self.mat.free(MEM_CPU or MEM_GPU) - else: - raise TypeError("Argument is not of PyMEM type.") - - def update_cpu_from_gpu(self): - return types.PyERROR_CODE(self.mat.updateCPUfromGPU()) - - def update_gpu_from_cpu(self): - return types.PyERROR_CODE(self.mat.updateGPUfromCPU()) - - def copy_to(self, cpy_type=PyCOPY_TYPE.PyCOPY_TYPE_CPU_CPU): - dst = PyMat() - print(types.PyERROR_CODE(self.mat.copyTo(dst.mat, cpy_type.value))) - return dst - - def set_from(self, cpy_type=PyCOPY_TYPE.PyCOPY_TYPE_CPU_CPU): - dst = PyMat() - print(self.mat.setFrom(dst.mat, cpy_type.value)) - return dst - - def read(self, str filepath): - return types.PyERROR_CODE(self.mat.read(filepath.encode())) - - def write(self, str filepath): - return types.PyERROR_CODE(self.mat.write(filepath.encode())) - - def set_to(self, value, memory_type=PyMEM.PyMEM_CPU): - if self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C1: - return types.PyERROR_CODE(setToUchar1(self.mat, value, memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C2: - return types.PyERROR_CODE(setToUchar2(self.mat, types.Vector2[uchar1](value[0], value[1]), - memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C3: - return types.PyERROR_CODE(setToUchar3(self.mat, types.Vector3[uchar1](value[0], value[1], - value[2]), memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C4: - return types.PyERROR_CODE(setToUchar4(self.mat, types.Vector4[uchar1](value[0], value[1], value[2], - value[3]), memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C1: - return types.PyERROR_CODE(setToFloat1(self.mat, value, memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C2: - return types.PyERROR_CODE(setToFloat2(self.mat, types.Vector2[float1](value[0], value[1]), - memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C3: - return types.PyERROR_CODE(setToFloat3(self.mat, types.Vector3[float1](value[0], value[1], - value[2]), memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C4: - return types.PyERROR_CODE(setToFloat4(self.mat, types.Vector4[float1](value[0], value[1], value[2], - value[3]), memory_type.value)) - - def set_value(self, x, y, value, memory_type=PyMEM.PyMEM_CPU): - if self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C1: - return types.PyERROR_CODE(setValueUchar1(self.mat, x, y, value, memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C2: - return types.PyERROR_CODE(setValueUchar2(self.mat, x, y, types.Vector2[uchar1](value[0], value[1]), - memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C3: - return types.PyERROR_CODE(setValueUchar3(self.mat, x, y, types.Vector3[uchar1](value[0], value[1], - value[2]), memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C4: - return types.PyERROR_CODE(setValueUchar4(self.mat, x, y, types.Vector4[uchar1](value[0], value[1], value[2], - value[3]), memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C1: - return types.PyERROR_CODE(setValueFloat1(self.mat, x, y, value, memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C2: - return types.PyERROR_CODE(setValueFloat2(self.mat, x, y, types.Vector2[float1](value[0], value[1]), - memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C3: - return types.PyERROR_CODE(setValueFloat3(self.mat, x, y, types.Vector3[float1](value[0], value[1], - value[2]), memory_type.value)) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C4: - return types.PyERROR_CODE(setValueFloat4(self.mat, x, y, types.Vector4[float1](value[0], value[1], value[2], - value[3]), memory_type.value)) - - def get_value(self, x, y, memory_type=PyMEM.PyMEM_CPU): - cdef uchar1 value1u - cdef types.Vector2[uchar1] value2u = types.Vector2[uchar1]() - cdef types.Vector3[uchar1] value3u = types.Vector3[uchar1]() - cdef types.Vector4[uchar1] value4u = types.Vector4[uchar1]() - - cdef float1 value1f - cdef types.Vector2[float1] value2f = types.Vector2[float1]() - cdef types.Vector3[float1] value3f = types.Vector3[float1]() - cdef types.Vector4[float1] value4f = types.Vector4[float1]() - - if self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C1: - status = getValueUchar1(self.mat, x, y, &value1u, memory_type.value) - return types.PyERROR_CODE(status), self.get_data()[x, y] - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C2: - status = getValueUchar2(self.mat, x, y, &value2u, memory_type.value) - return types.PyERROR_CODE(status), np.array([value2u.ptr()[0], value2u.ptr()[1]]) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C3: - status = getValueUchar3(self.mat, x, y, &value3u, memory_type.value) - return types.PyERROR_CODE(status), np.array([value3u.ptr()[0], value3u.ptr()[1], value3u.ptr()[2]]) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_8U_C4: - status = getValueUchar4(self.mat, x, y, &value4u, memory_type.value) - return types.PyERROR_CODE(status), np.array([value4u.ptr()[0], value4u.ptr()[1], value4u.ptr()[2], - value4u.ptr()[3]]) - - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C1: - status = getValueFloat1(self.mat, x, y, &value1f, memory_type.value) - return types.PyERROR_CODE(status), self.get_data()[x, y] - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C2: - status = getValueFloat2(self.mat, x, y, &value2f, memory_type.value) - return types.PyERROR_CODE(status), np.array([value2f.ptr()[0], value2f.ptr()[1]]) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C3: - status = getValueFloat3(self.mat, x, y, &value3f, memory_type.value) - return types.PyERROR_CODE(status), np.array([value3f.ptr()[0], value3f.ptr()[1], value3f.ptr()[2]]) - elif self.get_data_type() == PyMAT_TYPE.PyMAT_TYPE_32F_C4: - status = getValueFloat4(self.mat, x, y, &value4f, memory_type.value) - return types.PyERROR_CODE(status), np.array([value4f.ptr()[0], value4f.ptr()[1], value4f.ptr()[2], - value4f.ptr()[3]]) - - def get_width(self): - return self.mat.getWidth() - - def get_height(self): - return self.mat.getHeight() - - def get_resolution(self): - return PyResolution(self.mat.getResolution().width, self.mat.getResolution().height) - - def get_channels(self): - return self.mat.getChannels() - - def get_data_type(self): - return PyMAT_TYPE(self.mat.getDataType()) - - def get_memory_type(self): - return PyMEM(self.mat.getMemoryType()) - - def get_data(self, memory_type=PyMEM.PyMEM_CPU): - shape = None - if self.mat.getChannels() == 1: - shape = (self.mat.getHeight(), self.mat.getWidth()) - else: - shape = (self.mat.getHeight(), self.mat.getWidth(), self.mat.getChannels()) - - cdef size_t size = 0 - dtype = None - if self.mat.getDataType() in (MAT_TYPE_8U_C1, MAT_TYPE_8U_C2, MAT_TYPE_8U_C3, MAT_TYPE_8U_C4): - size = self.mat.getHeight()*self.mat.getWidth()*self.mat.getChannels() - dtype = np.uint8 - elif self.mat.getDataType() in (MAT_TYPE_32F_C1, MAT_TYPE_32F_C2, MAT_TYPE_32F_C3, MAT_TYPE_32F_C4): - size = self.mat.getHeight()*self.mat.getWidth()*self.mat.getChannels()*sizeof(float) - dtype = np.float32 - else: - raise RuntimeError("Unknown Mat data_type value: {0}".format(self.mat.getDataType())) - - cdef np.ndarray arr = np.zeros(shape, dtype=dtype) - if isinstance(memory_type, PyMEM): - if self.mat.getDataType() == MAT_TYPE_8U_C1: - memcpy(arr.data, getPointerUchar1(self.mat, memory_type.value), size) - elif self.mat.getDataType() == MAT_TYPE_8U_C2: - memcpy(arr.data, getPointerUchar2(self.mat, memory_type.value), size) - elif self.mat.getDataType() == MAT_TYPE_8U_C3: - memcpy(arr.data, getPointerUchar3(self.mat, memory_type.value), size) - elif self.mat.getDataType() == MAT_TYPE_8U_C4: - memcpy(arr.data, getPointerUchar4(self.mat, memory_type.value), size) - elif self.mat.getDataType() == MAT_TYPE_32F_C1: - memcpy(arr.data, getPointerFloat1(self.mat, memory_type.value), size) - elif self.mat.getDataType() == MAT_TYPE_32F_C2: - memcpy(arr.data, getPointerFloat2(self.mat, memory_type.value), size) - elif self.mat.getDataType() == MAT_TYPE_32F_C3: - memcpy(arr.data, getPointerFloat3(self.mat, memory_type.value), size) - elif self.mat.getDataType() == MAT_TYPE_32F_C4: - memcpy(arr.data, getPointerFloat4(self.mat, memory_type.value), size) - else: - raise TypeError("Argument is not of PyMEM type.") - - return arr - - def get_step_bytes(self, memory_type=PyMEM.PyMEM_CPU): - if type(memory_type) == PyMEM: - return self.mat.getStepBytes(memory_type.value) - else: - raise TypeError("Argument is not of PyMEM type.") - - def get_step(self, memory_type=PyMEM.PyMEM_CPU): - if type(memory_type) == PyMEM: - return self.mat.getStep(memory_type.value) - else: - raise TypeError("Argument is not of PyMEM type.") - - def get_pixel_bytes(self): - return self.mat.getPixelBytes() - - def get_width_bytes(self): - return self.mat.getWidthBytes() - - def get_infos(self): - return self.mat.getInfos().get().decode() - - def is_init(self): - return self.mat.isInit() - - def is_memory_owner(self): - return self.mat.isMemoryOwner() - - def clone(self, PyMat py_mat): - return types.PyERROR_CODE(self.mat.clone(py_mat.mat)) - - def move(self, PyMat py_mat): - return types.PyERROR_CODE(self.mat.move(py_mat.mat)) - - @staticmethod - def swap(self, PyMat mat1, PyMat mat2): - self.mat.swap(mat1, mat2) - - @property - def name(self): - if not self.mat.name.empty(): - return self.mat.name.get().decode() - else: - return "" - - @property - def verbose(self): - return self.mat.verbose - - def __repr__(self): - return self.get_infos() - - -cdef class PyRotation(types.PyMatrix3f): - def __cinit__(self): - self.rotation = Rotation() - - def init_rotation(self, PyRotation rot): - self.rotation = Rotation(rot.rotation) - self.mat = rot.mat - - def init_matrix(self, types.PyMatrix3f matrix): - self.rotation = Rotation(matrix.mat) - self.mat = matrix.mat - - def init_orientation(self, PyOrientation orient): - self.rotation = Rotation(orient.orientation) - self.mat = types.Matrix3f(self.rotation.r) - - def init_angle_translation(self, float angle, PyTranslation axis): - self.rotation = Rotation(angle, axis.translation) - self.mat = types.Matrix3f(self.rotation.r) - - def set_orientation(self, PyOrientation py_orientation): - self.rotation.setOrientation(py_orientation.orientation) - - def get_orientation(self): - py_orientation = PyOrientation() - py_orientation.orientation = self.rotation.getOrientation() - return py_orientation - - def get_rotation_vector(self): - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = self.rotation.getRotationVector()[i] - return arr - - def set_rotation_vector(self, float input0, float input1, float input2): - self.rotation.setRotationVector(types.Vector3[float](input0, input1, input2)) - - def get_euler_angles(self, radian=True): - cdef np.ndarray arr = np.zeros(3) - if isinstance(radian, bool): - for i in range(3): - arr[i] = self.rotation.getEulerAngles(radian)[i] - else: - raise TypeError("Argument is not of boolean type.") - return arr - - def set_euler_angles(self, float input0, float input1, float input2, bool radian=True): - if isinstance(radian, bool): - self.rotation.setEulerAngles(types.Vector3[float](input0, input1, input2), radian) - else: - raise TypeError("Argument is not of boolean type.") - -cdef class PyTranslation: - def __cinit__(self): - self.translation = Translation() - - def init_translation(self, PyTranslation tr): - self.translation = Translation(tr.translation) - - def init_vector(self, float t1, float t2, float t3): - self.translation = Translation(t1, t2, t3) - - def normalize(self): - self.translation.normalize() - - def normalize_translation(self, PyTranslation tr): - py_translation = PyTranslation() - py_translation.translation = self.translation.normalize(tr.translation) - return py_translation - - def size(self): - return self.translation.size() - - def get(self): - cdef np.ndarray arr = np.zeros(self.size()) - for i in range(self.size()): - arr[i] = self.translation(i) - return arr - - def __mul__(PyTranslation self, PyOrientation other): - tr = PyTranslation() - tr.translation = self.translation * other.orientation - return tr - - -cdef class PyOrientation: - def __cinit__(self): - self.orientation = Orientation() - - def init_orientation(self, PyOrientation orient): - self.orientation = Orientation(orient.orientation) - - def init_vector(self, float v0, float v1, float v2, float v3): - self.orientation = Orientation(types.Vector4[float](v0, v1, v2, v3)) - - def init_rotation(self, PyRotation rotation): - self.orientation = Orientation(rotation.rotation) - - def init_translation(self, PyTranslation tr1, PyTranslation tr2): - self.orientation = Orientation(tr1.translation, tr2.translation) - - def set_rotation_matrix(self, PyRotation py_rotation): - self.orientation.setRotationMatrix(py_rotation.rotation) - - def get_rotation_matrix(self): - py_rotation = PyRotation() - py_rotation.mat = self.orientation.getRotationMatrix() - return py_rotation - - def set_identity(self): - self.orientation.setIdentity() - return self - - def identity(self): - self.orientation.identity() - return self - - def set_zeros(self): - self.orientation.setZeros() - - def zeros(self): - self.orientation.zeros() - return self - - def normalize(self): - self.orientation.normalise() - - @staticmethod - def normalize_orientation(PyOrientation orient): - orient.orientation.normalise() - return orient - - def size(self): - return self.orientation.size() - - def get(self): - cdef np.ndarray arr = np.zeros(self.size()) - for i in range(self.size()): - arr[i] = self.orientation(i) - return arr - - def __mul__(PyOrientation self, PyOrientation other): - orient = PyOrientation() - orient.orientation = self.orientation * other.orientation - return orient - - -cdef class PyTransform(types.PyMatrix4f): - def __cinit__(self): - self.transform = Transform() - - def init_transform(self, PyTransform motion): - self.transform = Transform(motion.transform) - self.mat = motion.mat - - def init_matrix(self, types.PyMatrix4f matrix): - self.transform = Transform(matrix.mat) - self.mat = matrix.mat - - def init_rotation_translation(self, PyRotation rot, PyTranslation tr): - self.transform = Transform(rot.rotation, tr.translation) - self.mat = types.Matrix4f(self.transform.m) - - def init_orientation_translation(self, PyOrientation orient, PyTranslation tr): - self.transform = Transform(orient.orientation, tr.translation) - self.mat = types.Matrix4f(self.transform.m) - - def set_rotation_matrix(self, PyRotation py_rotation): - self.transform.setRotationMatrix(py_rotation.mat) - - def get_rotation_matrix(self): - py_rotation = PyRotation() - py_rotation.mat = self.transform.getRotationMatrix() - return py_rotation - - def set_translation(self, PyTranslation py_translation): - self.transform.setTranslation(py_translation.translation) - - def get_translation(self): - py_translation = PyTranslation() - py_translation.translation = self.transform.getTranslation() - return py_translation - - def set_orientation(self, PyOrientation py_orientation): - self.transform.setOrientation(py_orientation.orientation) - - def get_orientation(self): - py_orientation = PyOrientation() - py_orientation.orientation = self.transform.getOrientation() - return py_orientation - - def get_rotation_vector(self): - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = self.transform.getRotationVector()[i] - return arr - - def set_rotation_vector(self, float input0, float input1, float input2): - self.transform.setRotationVector(types.Vector3[float](input0, input1, input2)) - - def get_euler_angles(self, radian=True): - cdef np.ndarray arr = np.zeros(3) - if isinstance(radian, bool): - for i in range(3): - arr[i] = self.transform.getEulerAngles(radian)[i] - else: - raise TypeError("Argument is not of boolean type.") - return arr - - def set_euler_angles(self, float input0, float input1, float input2, radian=True): - if isinstance(radian, bool): - self.transform.setEulerAngles(types.Vector3[float](input0, input1, input2), radian) - else: - raise TypeError("Argument is not of boolean type.") diff --git a/pyzed/defines.pxd b/pyzed/defines.pxd deleted file mode 100644 index add276a..0000000 --- a/pyzed/defines.pxd +++ /dev/null @@ -1,261 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# File containing the Cython declarations to use the defines.hpp functions. - -from libcpp.string cimport string -from libc.string cimport const_char -from libcpp.vector cimport vector -from libcpp.pair cimport pair -from libcpp cimport bool - -cdef extern from "Utils.cpp" namespace "sl": - string to_str(String sl_str) - -cdef extern from "sl/types.hpp" namespace "sl": - - cdef cppclass String 'sl::String': - String() - String(const char *data) - void set(const char *data) - const char *get() const - bool empty() const - string std_str() const - - ctypedef enum UNIT: - UNIT_MILLIMETER - UNIT_CENTIMETER - UNIT_METER - UNIT_INCH - UNIT_FOOT - UNIT_LAST - - String toString(UNIT o) - - ctypedef enum COORDINATE_SYSTEM: - COORDINATE_SYSTEM_IMAGE - COORDINATE_SYSTEM_LEFT_HANDED_Y_UP - COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP - COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP - COORDINATE_SYSTEM_LEFT_HANDED_Z_UP - COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD - COORDINATE_SYSTEM_LAST - - String toString(COORDINATE_SYSTEM o) - -cdef extern from "sl/defines.hpp" namespace "sl": - - ctypedef enum RESOLUTION: - RESOLUTION_HD2K - RESOLUTION_HD1080 - RESOLUTION_HD720 - RESOLUTION_VGA - RESOLUTION_LAST - - String toString(RESOLUTION o) - - ctypedef enum CAMERA_SETTINGS: - CAMERA_SETTINGS_BRIGHTNESS - CAMERA_SETTINGS_CONTRAST - CAMERA_SETTINGS_HUE - CAMERA_SETTINGS_SATURATION - CAMERA_SETTINGS_GAIN - CAMERA_SETTINGS_EXPOSURE - CAMERA_SETTINGS_WHITEBALANCE - CAMERA_SETTINGS_AUTO_WHITEBALANCE - CAMERA_SETTINGS_LAST - - String toString(CAMERA_SETTINGS o) - - ctypedef enum SELF_CALIBRATION_STATE: - SELF_CALIBRATION_STATE_NOT_STARTED - SELF_CALIBRATION_STATE_RUNNING - SELF_CALIBRATION_STATE_FAILED - SELF_CALIBRATION_STATE_SUCCESS - SELF_CALIBRATION_STATE_LAST - - String toString(SELF_CALIBRATION_STATE o) - - ctypedef enum DEPTH_MODE: - DEPTH_MODE_NONE - DEPTH_MODE_PERFORMANCE - DEPTH_MODE_MEDIUM - DEPTH_MODE_QUALITY - DEPTH_MODE_ULTRA - DEPTH_MODE_LAST - - String toString(DEPTH_MODE o) - - ctypedef enum SENSING_MODE: - SENSING_MODE_STANDARD - SENSING_MODE_FILL - SENSING_MODE_LAST - - String toString(SENSING_MODE o) - - ctypedef enum MEASURE: - MEASURE_DISPARITY - MEASURE_DEPTH - MEASURE_CONFIDENCE - MEASURE_XYZ - MEASURE_XYZRGBA - MEASURE_XYZBGRA - MEASURE_XYZARGB - MEASURE_XYZABGR - MEASURE_NORMALS - MEASURE_DISPARITY_RIGHT - MEASURE_DEPTH_RIGHT - MEASURE_XYZ_RIGHT - MEASURE_XYZRGBA_RIGHT - MEASURE_XYZBGRA_RIGHT - MEASURE_XYZARGB_RIGHT - MEASURE_XYZABGR_RIGHT - MEASURE_NORMALS_RIGHT - MEASURE_LAST - - String toString(MEASURE o) - - ctypedef enum VIEW: - VIEW_LEFT - VIEW_RIGHT - VIEW_LEFT_GRAY - VIEW_RIGHT_GRAY - VIEW_LEFT_UNRECTIFIED - VIEW_RIGHT_UNRECTIFIED - VIEW_LEFT_UNRECTIFIED_GRAY - VIEW_RIGHT_UNRECTIFIED_GRAY - VIEW_SIDE_BY_SIDE - VIEW_DEPTH - VIEW_CONFIDENCE - VIEW_NORMALS - VIEW_DEPTH_RIGHT - VIEW_NORMALS_RIGHT - VIEW_LAST - - String toString(VIEW o) - - ctypedef enum TIME_REFERENCE: - TIME_REFERENCE_IMAGE - TIME_REFERENCE_CURRENT - TIME_REFERENCE_LAST - - String toString(TIME_REFERENCE o) - - ctypedef enum DEPTH_FORMAT: - DEPTH_FORMAT_PNG - DEPTH_FORMAT_PFM - DEPTH_FORMAT_PGM - DEPTH_FORMAT_LAST - - String toString(DEPTH_FORMAT o) - - ctypedef enum POINT_CLOUD_FORMAT: - POINT_CLOUD_FORMAT_XYZ_ASCII - POINT_CLOUD_FORMAT_PCD_ASCII - POINT_CLOUD_FORMAT_PLY_ASCII - POINT_CLOUD_FORMAT_VTK_ASCII - POINT_CLOUD_FORMAT_LAST - - String toString(POINT_CLOUD_FORMAT o) - - ctypedef enum TRACKING_STATE: - TRACKING_STATE_SEARCHING - TRACKING_STATE_OK - TRACKING_STATE_OFF - TRACKING_STATE_FPS_TOO_LOW - TRACKING_STATE_LAST - - String toString(TRACKING_STATE o) - - ctypedef enum AREA_EXPORT_STATE: - AREA_EXPORT_STATE_SUCCESS - AREA_EXPORT_STATE_RUNNING - AREA_EXPORT_STATE_NOT_STARTED - AREA_EXPORT_STATE_FILE_EMPTY - AREA_EXPORT_STATE_FILE_ERROR - AREA_EXPORT_STATE_SPATIAL_MEMORY_DISABLED - AREA_EXPORT_STATE_LAST - - String toString(AREA_EXPORT_STATE o) - - ctypedef enum REFERENCE_FRAME: - REFERENCE_FRAME_WORLD - REFERENCE_FRAME_CAMERA - REFERENCE_FRAME_LAST - - String toString(REFERENCE_FRAME o) - - ctypedef enum SPATIAL_MAPPING_STATE: - SPATIAL_MAPPING_STATE_INITIALIZING - SPATIAL_MAPPING_STATE_OK - SPATIAL_MAPPING_STATE_NOT_ENOUGH_MEMORY - SPATIAL_MAPPING_STATE_NOT_ENABLED - SPATIAL_MAPPING_STATE_FPS_TOO_LOW - SPATIAL_MAPPING_STATE_LAST - - String toString(SPATIAL_MAPPING_STATE o) - - ctypedef enum SVO_COMPRESSION_MODE: - SVO_COMPRESSION_MODE_RAW - SVO_COMPRESSION_MODE_LOSSLESS - SVO_COMPRESSION_MODE_LOSSY - SVO_COMPRESSION_MODE_AVCHD - SVO_COMPRESSION_MODE_HEVC - SVO_COMPRESSION_MODE_LAST - - String toString(SVO_COMPRESSION_MODE o) - - cdef struct RecordingState: - bool status - double current_compression_time - double current_compression_ratio - double average_compression_time - double average_compression_ratio - - - @staticmethod - cdef vector[pair[int, int]] cameraResolution - - @staticmethod - cdef const_char* resolution2str(RESOLUTION res) - - @staticmethod - cdef const_char* statusCode2str(SELF_CALIBRATION_STATE state) - - @staticmethod - cdef DEPTH_MODE str2mode(const_char* mode) - - @staticmethod - cdef const_char* depthMode2str(DEPTH_MODE mode) - - @staticmethod - cdef const_char* sensingMode2str(SENSING_MODE mode) - - @staticmethod - cdef const_char* unit2str(UNIT unit) - - @staticmethod - cdef UNIT str2unit(const_char* unit) - - @staticmethod - cdef const_char* trackingState2str(TRACKING_STATE state) - - @staticmethod - cdef const_char* spatialMappingState2str(SPATIAL_MAPPING_STATE state) diff --git a/pyzed/defines.pyx b/pyzed/defines.pyx deleted file mode 100644 index c41340f..0000000 --- a/pyzed/defines.pyx +++ /dev/null @@ -1,286 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# Source file of the defines Python module. - -import enum - - -class PyRESOLUTION(enum.Enum): - PyRESOLUTION_HD2K = RESOLUTION_HD2K - PyRESOLUTION_HD1080 = RESOLUTION_HD1080 - PyRESOLUTION_HD720 = RESOLUTION_HD720 - PyRESOLUTION_VGA = RESOLUTION_VGA - PyRESOLUTION_LAST = RESOLUTION_LAST - - def __str__(self): - return ( resolution2str(self.value)).decode() - - def __repr__(self): - return ( resolution2str(self.value)).decode() - - -class PyCAMERA_SETTINGS(enum.Enum): - PyCAMERA_SETTINGS_BRIGHTNESS = CAMERA_SETTINGS_BRIGHTNESS - PyCAMERA_SETTINGS_CONTRAST = CAMERA_SETTINGS_CONTRAST - PyCAMERA_SETTINGS_HUE = CAMERA_SETTINGS_HUE - PyCAMERA_SETTINGS_SATURATION = CAMERA_SETTINGS_SATURATION - PyCAMERA_SETTINGS_GAIN = CAMERA_SETTINGS_GAIN - PyCAMERA_SETTINGS_EXPOSURE = CAMERA_SETTINGS_EXPOSURE - PyCAMERA_SETTINGS_WHITEBALANCE = CAMERA_SETTINGS_WHITEBALANCE - PyCAMERA_SETTINGS_AUTO_WHITEBALANCE = CAMERA_SETTINGS_AUTO_WHITEBALANCE - PyCAMERA_SETTINGS_LAST = CAMERA_SETTINGS_LAST - - -class PySELF_CALIBRATION_STATE(enum.Enum): - PySELF_CALIBRATION_STATE_NOT_STARTED = SELF_CALIBRATION_STATE_NOT_STARTED - PySELF_CALIBRATION_STATE_RUNNING = SELF_CALIBRATION_STATE_RUNNING - PySELF_CALIBRATION_STATE_FAILED = SELF_CALIBRATION_STATE_FAILED - PySELF_CALIBRATION_STATE_SUCCESS = SELF_CALIBRATION_STATE_SUCCESS - PySELF_CALIBRATION_STATE_LAST = SELF_CALIBRATION_STATE_LAST - - def __str__(self): - return ( statusCode2str(self.value)).decode() - - def __repr__(self): - return ( statusCode2str(self.value)).decode() - - -class PyDEPTH_MODE(enum.Enum): - PyDEPTH_MODE_NONE = DEPTH_MODE_NONE - PyDEPTH_MODE_PERFORMANCE = DEPTH_MODE_PERFORMANCE - PyDEPTH_MODE_MEDIUM = DEPTH_MODE_MEDIUM - PyDEPTH_MODE_QUALITY = DEPTH_MODE_QUALITY - PyDEPTH_MODE_ULTRA = DEPTH_MODE_ULTRA - PyDEPTH_MODE_LAST = DEPTH_MODE_LAST - - def __str__(self): - return ( depthMode2str(self.value)).decode() - - def __repr__(self): - return ( depthMode2str(self.value)).decode() - - -class PySENSING_MODE(enum.Enum): - PySENSING_MODE_STANDARD = SENSING_MODE_STANDARD - PySENSING_MODE_FILL = SENSING_MODE_FILL - PySENSING_MODE_LAST = SENSING_MODE_LAST - - def __str__(self): - return ( sensingMode2str(self.value)).decode() - - def __repr__(self): - return ( sensingMode2str(self.value)).decode() - - -class PyUNIT(enum.Enum): - PyUNIT_MILLIMETER = UNIT_MILLIMETER - PyUNIT_CENTIMETER = UNIT_CENTIMETER - PyUNIT_METER = UNIT_METER - PyUNIT_INCH = UNIT_INCH - PyUNIT_FOOT = UNIT_FOOT - PyUNIT_LAST = UNIT_LAST - - def __str__(self): - return ( unit2str(self.value)).decode() - - def __repr__(self): - return ( unit2str(self.value)).decode() - - -class PyCOORDINATE_SYSTEM(enum.Enum): - PyCOORDINATE_SYSTEM_IMAGE = COORDINATE_SYSTEM_IMAGE - PyCOORDINATE_SYSTEM_LEFT_HANDED_Y_UP = COORDINATE_SYSTEM_LEFT_HANDED_Y_UP - PyCOORDINATE_SYSTEM_RIGHT_HANDED_Y_UP = COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP - PyCOORDINATE_SYSTEM_RIGHT_HANDED_Z_UP = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP - PyCOORDINATE_SYSTEM_LEFT_HANDED_Z_UP = COORDINATE_SYSTEM_LEFT_HANDED_Z_UP - PyCOORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD - PyCOORDINATE_SYSTEM_LAST = COORDINATE_SYSTEM_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyMEASURE(enum.Enum): - PyMEASURE_DISPARITY = MEASURE_DISPARITY - PyMEASURE_DEPTH = MEASURE_DEPTH - PyMEASURE_CONFIDENCE = MEASURE_CONFIDENCE - PyMEASURE_XYZ = MEASURE_XYZ - PyMEASURE_XYZRGBA = MEASURE_XYZRGBA - PyMEASURE_XYZBGRA = MEASURE_XYZBGRA - PyMEASURE_XYZARGB = MEASURE_XYZARGB - PyMEASURE_XYZABGR = MEASURE_XYZABGR - PyMEASURE_NORMALS = MEASURE_NORMALS - PyMEASURE_DISPARITY_RIGHT = MEASURE_DISPARITY_RIGHT - PyMEASURE_DEPTH_RIGHT = MEASURE_DEPTH_RIGHT - PyMEASURE_XYZ_RIGHT = MEASURE_XYZ_RIGHT - PyMEASURE_XYZRGBA_RIGHT = MEASURE_XYZRGBA_RIGHT - PyMEASURE_XYZBGRA_RIGHT = MEASURE_XYZBGRA_RIGHT - PyMEASURE_XYZARGB_RIGHT = MEASURE_XYZARGB_RIGHT - PyMEASURE_XYZABGR_RIGHT = MEASURE_XYZABGR_RIGHT - PyMEASURE_NORMALS_RIGHT = MEASURE_NORMALS_RIGHT - PyMEASURE_LAST = MEASURE_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyVIEW(enum.Enum): - PyVIEW_LEFT = VIEW_LEFT - PyVIEW_RIGHT = VIEW_RIGHT - PyVIEW_LEFT_GRAY = VIEW_LEFT_GRAY - PyVIEW_RIGHT_GRAY = VIEW_RIGHT_GRAY - PyVIEW_LEFT_UNRECTIFIED = VIEW_LEFT_UNRECTIFIED - PyVIEW_RIGHT_UNRECTIFIED = VIEW_RIGHT_UNRECTIFIED - PyVIEW_LEFT_UNRECTIFIED_GRAY = VIEW_LEFT_UNRECTIFIED_GRAY - PyVIEW_RIGHT_UNRECTIFIED_GRAY = VIEW_RIGHT_UNRECTIFIED_GRAY - PyVIEW_SIDE_BY_SIDE = VIEW_SIDE_BY_SIDE - PyVIEW_DEPTH = VIEW_DEPTH - PyVIEW_CONFIDENCE = VIEW_CONFIDENCE - PyVIEW_NORMALS = VIEW_NORMALS - PyVIEW_DEPTH_RIGHT = VIEW_DEPTH_RIGHT - PyVIEW_NORMALS_RIGHT = VIEW_NORMALS_RIGHT - PyVIEW_LAST = VIEW_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyDEPTH_FORMAT(enum.Enum): - PyDEPTH_FORMAT_PNG = DEPTH_FORMAT_PNG - PyDEPTH_FORMAT_PFM = DEPTH_FORMAT_PFM - PyDEPTH_FORMAT_PGM = DEPTH_FORMAT_PGM - PyDEPTH_FORMAT_LAST = DEPTH_FORMAT_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyPOINT_CLOUD_FORMAT(enum.Enum): - PyPOINT_CLOUD_FORMAT_XYZ_ASCII = POINT_CLOUD_FORMAT_XYZ_ASCII - PyPOINT_CLOUD_FORMAT_PCD_ASCII = POINT_CLOUD_FORMAT_PCD_ASCII - PyPOINT_CLOUD_FORMAT_PLY_ASCII = POINT_CLOUD_FORMAT_PLY_ASCII - PyPOINT_CLOUD_FORMAT_VTK_ASCII = POINT_CLOUD_FORMAT_VTK_ASCII - PyPOINT_CLOUD_FORMAT_LAST = POINT_CLOUD_FORMAT_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyTRACKING_STATE(enum.Enum): - PyTRACKING_STATE_SEARCHING = TRACKING_STATE_SEARCHING - PyTRACKING_STATE_OK = TRACKING_STATE_OK - PyTRACKING_STATE_OFF = TRACKING_STATE_OFF - PyTRACKING_STATE_FPS_TOO_LOW = TRACKING_STATE_FPS_TOO_LOW - PyTRACKING_STATE_LAST = TRACKING_STATE_LAST - - def __str__(self): - return ( trackingState2str(self.value)).decode() - - def __repr__(self): - return ( trackingState2str(self.value)).decode() - - -class PyAREA_EXPORT_STATE(enum.Enum): - PyAREA_EXPORT_STATE_SUCCESS = AREA_EXPORT_STATE_SUCCESS - PyAREA_EXPORT_STATE_RUNNING = AREA_EXPORT_STATE_RUNNING - PyAREA_EXPORT_STATE_NOT_STARTED = AREA_EXPORT_STATE_NOT_STARTED - PyAREA_EXPORT_STATE_FILE_EMPTY = AREA_EXPORT_STATE_FILE_EMPTY - PyAREA_EXPORT_STATE_FILE_ERROR = AREA_EXPORT_STATE_FILE_ERROR - PyAREA_EXPORT_STATE_SPATIAL_MEMORY_DISABLED = AREA_EXPORT_STATE_SPATIAL_MEMORY_DISABLED - PyAREA_EXPORT_STATE_LAST = AREA_EXPORT_STATE_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyREFERENCE_FRAME(enum.Enum): - PyREFERENCE_FRAME_WORLD = REFERENCE_FRAME_WORLD - PyREFERENCE_FRAME_CAMERA = REFERENCE_FRAME_CAMERA - PyREFERENCE_FRAME_LAST = REFERENCE_FRAME_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyTIME_REFERENCE(enum.Enum): - PyTIME_REFERENCE_IMAGE = TIME_REFERENCE_IMAGE - PyTIME_REFERENCE_CURRENT = TIME_REFERENCE_CURRENT - PyTIME_REFERENCE_LAST = TIME_REFERENCE_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PySPATIAL_MAPPING_STATE(enum.Enum): - PySPATIAL_MAPPING_STATE_INITIALIZING = SPATIAL_MAPPING_STATE_INITIALIZING - PySPATIAL_MAPPING_STATE_OK = SPATIAL_MAPPING_STATE_OK - PySPATIAL_MAPPING_STATE_NOT_ENOUGH_MEMORY = SPATIAL_MAPPING_STATE_NOT_ENOUGH_MEMORY - PySPATIAL_MAPPING_STATE_NOT_ENABLED = SPATIAL_MAPPING_STATE_NOT_ENABLED - PySPATIAL_MAPPING_STATE_FPS_TOO_LOW = SPATIAL_MAPPING_STATE_FPS_TOO_LOW - PySPATIAL_MAPPING_STATE_LAST = SPATIAL_MAPPING_STATE_LAST - - def __str__(self): - return ( spatialMappingState2str(self.value)).decode() - - def __repr__(self): - return ( spatialMappingState2str(self.value)).decode() - - -class PySVO_COMPRESSION_MODE(enum.Enum): - PySVO_COMPRESSION_MODE_RAW = SVO_COMPRESSION_MODE_RAW - PySVO_COMPRESSION_MODE_LOSSLESS = SVO_COMPRESSION_MODE_LOSSLESS - PySVO_COMPRESSION_MODE_LOSSY = SVO_COMPRESSION_MODE_LOSSY - PySVO_COMPRESSION_MODE_AVCHD = SVO_COMPRESSION_MODE_AVCHD - PySVO_COMPRESSION_MODE_HEVC = SVO_COMPRESSION_MODE_HEVC - PySVO_COMPRESSION_MODE_LAST = SVO_COMPRESSION_MODE_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -def video_modes(): - return cameraResolution - - -def str_to_mode(str mode): - return PyDEPTH_MODE(str2mode(mode.encode())) - - -def str_to_unit(str unit): - return PyUNIT(str2unit(unit.encode())) diff --git a/pyzed/mesh.pxd b/pyzed/mesh.pxd deleted file mode 100644 index 61d6991..0000000 --- a/pyzed/mesh.pxd +++ /dev/null @@ -1,129 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# File containing the Cython declarations to use the Mesh.hpp functions. - -from libcpp cimport bool -from libcpp.vector cimport vector -from libcpp.string cimport string - -cimport pyzed.types as types -cimport pyzed.core as core - - -ctypedef unsigned int uint - -cdef extern from "sl/Mesh.hpp" namespace "sl": - - ctypedef enum MESH_FILE_FORMAT: - MESH_FILE_PLY - MESH_FILE_PLY_BIN - MESH_FILE_OBJ - MESH_FILE_LAST - - - ctypedef enum MESH_TEXTURE_FORMAT: - MESH_TEXTURE_RGB - MESH_TEXTURE_RGBA - MESH_TEXTURE_LAST - - - ctypedef enum MESH_FILTER 'sl::MeshFilterParameters::MESH_FILTER': - MESH_FILTER_LOW 'sl::MeshFilterParameters::MESH_FILTER::MESH_FILTER_LOW' - MESH_FILTER_MEDIUM 'sl::MeshFilterParameters::MESH_FILTER::MESH_FILTER_MEDIUM' - MESH_FILTER_HIGH 'sl::MeshFilterParameters::MESH_FILTER::MESH_FILTER_HIGH' - - ctypedef enum PLANE_TYPE: - PLANE_TYPE_HORIZONTAL - PLANE_TYPE_VERTICAL - PLANE_TYPE_UNKNOWN - PLANE_TYPE_LAST - - cdef cppclass MeshFilterParameters 'sl::MeshFilterParameters': - MeshFilterParameters(MESH_FILTER filtering_) - void set(MESH_FILTER filtering_) - bool save(types.String filename) - bool load(types.String filename) - - - cdef cppclass Texture 'sl::Texture': - Texture() - types.String name - core.Mat data - unsigned int indice_gl - void clear() - - cdef cppclass Chunk 'sl::Chunk': - Chunk() - vector[types.Vector3[float]] vertices - vector[types.Vector3[uint]] triangles - vector[types.Vector3[float]] normals - vector[types.Vector2[float]] uv - unsigned long long timestamp - types.Vector3[float] barycenter - bool has_been_updated - void clear() - - cdef cppclass Mesh 'sl::Mesh': - ctypedef vector[size_t] chunkList - Mesh() - vector[Chunk] chunks - Chunk &operator[](int index) - vector[types.Vector3[float]] vertices - vector[types.Vector3[uint]] triangles - vector[types.Vector3[float]] normals - vector[types.Vector2[float]] uv - Texture texture - size_t getNumberOfTriangles() - void mergeChunks(int faces_per_chunk) - types.Vector3[float] getGravityEstimate() - chunkList getVisibleList(core.Transform camera_pose) - chunkList getSurroundingList(core.Transform camera_pose, float radius) - void updateMeshFromChunkList(chunkList IDs) - bool filter(MeshFilterParameters params, bool updateMesh) - bool applyTexture(MESH_TEXTURE_FORMAT texture_format) - bool save(types.String filename, MESH_FILE_FORMAT type, chunkList IDs) - bool load(const types.String filename, bool updateMesh) - void clear() - - cdef cppclass Plane 'sl::Plane': - Plane() - PLANE_TYPE type - types.Vector3[float] getNormal() - types.Vector3[float] getCenter() - core.Transform getPose() - types.Vector2[float] getExtents() - types.Vector4[float] getPlaneEquation() - vector[types.Vector3[float]] getBounds() - Mesh extractMesh() - float getClosestDistance(types.Vector3[float] point) - - -cdef class PyTexture: - cdef Texture texture - -cdef class PyChunk: - cdef Chunk chunk - -cdef class PyMesh: - cdef Mesh* mesh - -cdef class PyPlane: - cdef Plane plane diff --git a/pyzed/mesh.pyx b/pyzed/mesh.pyx deleted file mode 100644 index d4ad7f5..0000000 --- a/pyzed/mesh.pyx +++ /dev/null @@ -1,318 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# Source file of the mesh Python module. - -from cython.operator cimport dereference as deref -from cpython cimport bool - -import enum -import numpy as np -cimport numpy as np - -cimport pyzed.types as types - - -class PyMESH_FILE_FORMAT(enum.Enum): - PyMESH_FILE_PLY = MESH_FILE_PLY - PyMESH_FILE_PLY_BIN = MESH_FILE_PLY_BIN - PyMESH_FILE_OBJ = MESH_FILE_OBJ - PyMESH_FILE_LAST = MESH_FILE_LAST - -class PyMESH_TEXTURE_FORMAT(enum.Enum): - PyMESH_TEXTURE_RGB = MESH_TEXTURE_RGB - PyMESH_TEXTURE_RGBA = MESH_TEXTURE_RGBA - PyMESH_TEXTURE_LAST = MESH_TEXTURE_LAST - -class PyFILTER(enum.Enum): - PyFILTER_LOW = MESH_FILTER_LOW - PyFILTER_MEDIUM = MESH_FILTER_MEDIUM - PyFILTER_HIGH = MESH_FILTER_HIGH - -class PyPLANE_TYPE(enum.Enum): - PyPLANE_TYPE_HORIZONTAL = PLANE_TYPE_HORIZONTAL - PyPLANE_TYPE_VERTICAL = PLANE_TYPE_VERTICAL - PyPLANE_TYPE_UNKNOWN = PLANE_TYPE_UNKNOWN - PyPLANE_TYPE_LAST = PLANE_TYPE_LAST - -cdef class PyMeshFilterParameters: - cdef MeshFilterParameters* meshFilter - def __cinit__(self): - self.meshFilter = new MeshFilterParameters(MESH_FILTER_LOW) - - def __dealloc__(self): - del self.meshFilter - - def set(self, filter=PyFILTER.PyFILTER_LOW): - if isinstance(filter, PyFILTER): - self.meshFilter.set(filter.value) - else: - raise TypeError("Argument is not of PyFILTER type.") - - def save(self, str filename): - filename_save = filename.encode() - return self.meshFilter.save(types.String( filename_save)) - - def load(self, str filename): - filename_load = filename.encode() - return self.meshFilter.load(types.String( filename_load)) - - -cdef class PyTexture: - def __cinit__(self): - self.texture = Texture() - - @property - def name(self): - if not self.texture.name.empty(): - return self.texture.name.get().decode() - else: - return "" - - def get_data(self, core.PyMat py_mat): - py_mat.mat = self.texture.data - return py_mat - - @property - def indice_gl(self): - return self.texture.indice_gl - - def clear(self): - self.texture.clear() - - -cdef class PyChunk: - def __cinit__(self): - self.chunk = Chunk() - - @property - def vertices(self): - cdef np.ndarray arr = np.zeros((self.chunk.vertices.size(), 3)) - for i in range(self.chunk.vertices.size()): - for j in range(3): - arr[i,j] = self.chunk.vertices[i].ptr()[j] - return arr - - @property - def triangles(self): - cdef np.ndarray arr = np.zeros((self.chunk.triangles.size(), 3)) - for i in range(self.chunk.triangles.size()): - for j in range(3): - arr[i,j] = self.chunk.triangles[i].ptr()[j]+1 - return arr - - @property - def normals(self): - cdef np.ndarray arr = np.zeros((self.chunk.normals.size(), 3)) - for i in range(self.chunk.normals.size()): - for j in range(3): - arr[i,j] = self.chunk.normals[i].ptr()[j] - return arr - - @property - def uv(self): - cdef np.ndarray arr = np.zeros((self.chunk.uv.size(), 2)) - for i in range(self.chunk.uv.size()): - for j in range(2): - arr[i,j] = self.chunk.uv[i].ptr()[j] - return arr - - @property - def timestamp(self): - return self.chunk.timestamp - - @property - def barycenter(self): - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = self.chunk.barycenter[i] - return arr - - @property - def has_been_updated(self): - return self.chunk.has_been_updated - - def clear(self): - self.chunk.clear() - -cdef class PyMesh: - def __cinit__(self): - self.mesh = new Mesh() - - def __dealloc__(self): - del self.mesh - - @property - def chunks(self): - list = [] - for i in range(self.mesh.chunks.size()): - py_chunk = PyChunk() - py_chunk.chunk = self.mesh.chunks[i] - list.append(py_chunk) - return list - - def __getitem__(self, x): - return self.chunks[x] - - def filter(self, PyMeshFilterParameters params, update_mesh=True): - if isinstance(update_mesh, bool): - return self.mesh.filter(deref(params.meshFilter), update_mesh) - else: - raise TypeError("Argument is not of boolean type.") - - def apply_texture(self, texture_format=PyMESH_TEXTURE_FORMAT.PyMESH_TEXTURE_RGB): - if isinstance(texture_format, PyMESH_TEXTURE_FORMAT): - return self.mesh.applyTexture(texture_format.value) - else: - raise TypeError("Argument is not of PyMESH_TEXTURE_FORMAT type.") - - def save(self, str filename, typeMesh=PyMESH_FILE_FORMAT.PyMESH_FILE_OBJ, id=[]): - if isinstance(typeMesh, PyMESH_FILE_FORMAT): - return self.mesh.save(types.String(filename.encode()), typeMesh.value, id) - else: - raise TypeError("Argument is not of PyMESH_FILE_FORMAT type.") - - def load(self, str filename, update_mesh=True): - if isinstance(update_mesh, bool): - return self.mesh.load(types.String(filename.encode()), update_mesh) - else: - raise TypeError("Argument is not of boolean type.") - - def clear(self): - self.mesh.clear() - - @property - def vertices(self): - cdef np.ndarray arr = np.zeros((self.mesh.vertices.size(), 3)) - for i in range(self.mesh.vertices.size()): - for j in range(3): - arr[i,j] = self.mesh.vertices[i].ptr()[j] - return arr - - @property - def triangles(self): - cdef np.ndarray arr = np.zeros((self.mesh.triangles.size(), 3)) - for i in range(self.mesh.triangles.size()): - for j in range(3): - arr[i,j] = self.mesh.triangles[i].ptr()[j]+1 - return arr - - @property - def normals(self): - cdef np.ndarray arr = np.zeros((self.mesh.normals.size(), 3)) - for i in range(self.mesh.normals.size()): - for j in range(3): - arr[i,j] = self.mesh.normals[i].ptr()[j] - return arr - - @property - def uv(self): - cdef np.ndarray arr = np.zeros((self.mesh.uv.size(), 2)) - for i in range(self.mesh.uv.size()): - for j in range(2): - arr[i,j] = self.mesh.uv[i].ptr()[j] - return arr - - @property - def texture(self): - py_texture = PyTexture() - py_texture.texture = self.mesh.texture - return py_texture - - def get_number_of_triangles(self): - return self.mesh.getNumberOfTriangles() - - def merge_chunks(self, faces_per_chunk): - self.mesh.mergeChunks(faces_per_chunk) - - def get_gravity_estimate(self): - gravity = self.mesh.getGravityEstimate() - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = gravity[i] - return arr - - def get_visible_list(self, core.PyTransform camera_pose): - return self.mesh.getVisibleList(camera_pose.transform) - - def get_surrounding_list(self, core.PyTransform camera_pose, float radius): - return self.mesh.getSurroundingList(camera_pose.transform, radius) - - def update_mesh_from_chunklist(self, id=[]): - self.mesh.updateMeshFromChunkList(id) - -cdef class PyPlane: - def __cinit__(self): - self.plane = Plane() - - @property - def type(self): - return self.plane.type - - def get_normal(self): - normal = self.plane.getNormal() - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = normal[i] - return arr - - def get_center(self): - center = self.plane.getCenter() - cdef np.ndarray arr = np.zeros(3) - for i in range(3): - arr[i] = center[i] - return arr - - def get_pose(self, core.PyTransform py_pose): - py_pose.transform = self.plane.getPose() - return py_pose - - def get_extents(self): - extents = self.plane.getExtents() - cdef np.ndarray arr = np.zeros(2) - for i in range(2): - arr[i] = extents[i] - return arr - - def get_plane_equation(self): - plane_eq = self.plane.getPlaneEquation() - cdef np.ndarray arr = np.zeros(4) - for i in range(4): - arr[i] = plane_eq[i] - return arr - - def get_bounds(self): - cdef np.ndarray arr = np.zeros((self.plane.getBounds().size(), 3)) - for i in range(self.plane.getBounds().size()): - for j in range(3): - arr[i,j] = self.plane.getBounds()[i].ptr()[j] - return arr - - def extract_mesh(self): - ext_mesh = self.plane.extractMesh() - pymesh = PyMesh() - pymesh.mesh[0] = ext_mesh - return pymesh - - def get_closest_distance(self, point=[0,0,0]): - cdef types.Vector3[float] vec = types.Vector3[float](point[0], point[1], point[2]) - return self.plane.getClosestDistance(vec) - - diff --git a/pyzed/sl.pyx b/pyzed/sl.pyx new file mode 100644 index 0000000..c25baf9 --- /dev/null +++ b/pyzed/sl.pyx @@ -0,0 +1,2606 @@ +######################################################################## +# +# Copyright (c) 2017, STEREOLABS. +# +# All rights reserved. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +######################################################################## + +# Source file of the types Python module. + +from libcpp.vector cimport vector +from libc.string cimport const_char +from libcpp.string cimport string +from libcpp.pair cimport pair +from sl_c cimport to_str, ERROR_CODE as c_ERROR_CODE, toString, sleep_ms, MODEL as c_MODEL, model2str, CAMERA_STATE as c_CAMERA_STATE, String, DeviceProperties as c_DeviceProperties, Vector2, Vector3, Vector4, Matrix3f as c_Matrix3f, Matrix4f as c_Matrix4f, UNIT as c_UNIT, COORDINATE_SYSTEM as c_COORDINATE_SYSTEM, RESOLUTION as c_RESOLUTION, CAMERA_SETTINGS as c_CAMERA_SETTINGS, SELF_CALIBRATION_STATE as c_SELF_CALIBRATION_STATE, DEPTH_MODE as c_DEPTH_MODE, SENSING_MODE as c_SENSING_MODE, MEASURE as c_MEASURE, VIEW as c_VIEW, TIME_REFERENCE as c_TIME_REFERENCE, DEPTH_FORMAT as c_DEPTH_FORMAT, POINT_CLOUD_FORMAT as c_POINT_CLOUD_FORMAT, TRACKING_STATE as c_TRACKING_STATE, AREA_EXPORT_STATE as c_AREA_EXPORT_STATE, REFERENCE_FRAME as c_REFERENCE_FRAME, SPATIAL_MAPPING_STATE as c_SPATIAL_MAPPING_STATE, SVO_COMPRESSION_MODE as c_SVO_COMPRESSION_MODE, RecordingState, cameraResolution, resolution2str, statusCode2str, str2mode, depthMode2str, sensingMode2str, unit2str, str2unit, trackingState2str, spatialMappingState2str, getCurrentTimeStamp, Resolution as c_Resolution, CameraParameters as c_CameraParameters, CalibrationParameters as c_CalibrationParameters, CameraInformation as c_CameraInformation, MEM as c_MEM, COPY_TYPE as c_COPY_TYPE, MAT_TYPE as c_MAT_TYPE, Mat as c_Mat, Rotation as c_Rotation, Translation as c_Translation, Orientation as c_Orientation, Transform as c_Transform, uchar1, uchar2, uchar3, uchar4, float1, float2, float3, float4, matResolution, setToUchar1, setToUchar2, setToUchar3, setToUchar4, setToFloat1, setToFloat2, setToFloat3, setToFloat4, setValueUchar1, setValueUchar2, setValueUchar3, setValueUchar4, setValueFloat1, setValueFloat2, setValueFloat3, setValueFloat4, getValueUchar1, getValueUchar2, getValueUchar3, getValueUchar4, getValueFloat1, getValueFloat2, getValueFloat3, getValueFloat4, getPointerUchar1, getPointerUchar2, getPointerUchar3, getPointerUchar4, getPointerFloat1, getPointerFloat2, getPointerFloat3, getPointerFloat4, uint, MESH_FILE_FORMAT as c_MESH_FILE_FORMAT, MESH_TEXTURE_FORMAT as c_MESH_TEXTURE_FORMAT, MESH_FILTER as c_MESH_FILTER, PLANE_TYPE as c_PLANE_TYPE, MeshFilterParameters as c_MeshFilterParameters, Texture as c_Texture, Chunk as c_Chunk, Mesh as c_Mesh, Plane as c_Plane, CUctx_st, CUcontext, MAPPING_RESOLUTION as c_MAPPING_RESOLUTION, MAPPING_RANGE as c_MAPPING_RANGE, InputType as c_InputType, InitParameters as c_InitParameters, RuntimeParameters as c_RuntimeParameters, TrackingParameters as c_TrackingParameters, SpatialMappingParameters as c_SpatialMappingParameters, Pose as c_Pose, IMUData as c_IMUData, Camera as c_Camera, saveDepthAs, savePointCloudAs, saveMatDepthAs, saveMatPointCloudAs + +from cython.operator cimport dereference as deref +from libc.string cimport memcpy +from cpython cimport bool + +import enum + +import numpy as np +cimport numpy as np +from math import sqrt + +class ERROR_CODE(enum.Enum): + SUCCESS = c_ERROR_CODE.SUCCESS + ERROR_CODE_FAILURE = c_ERROR_CODE.ERROR_CODE_FAILURE + ERROR_CODE_NO_GPU_COMPATIBLE = c_ERROR_CODE.ERROR_CODE_NO_GPU_COMPATIBLE + ERROR_CODE_NOT_ENOUGH_GPUMEM = c_ERROR_CODE.ERROR_CODE_NOT_ENOUGH_GPUMEM + ERROR_CODE_CAMERA_NOT_DETECTED = c_ERROR_CODE.ERROR_CODE_CAMERA_NOT_DETECTED + ERROR_CODE_SENSOR_NOT_DETECTED = c_ERROR_CODE.ERROR_CODE_SENSOR_NOT_DETECTED + ERROR_CODE_INVALID_RESOLUTION = c_ERROR_CODE.ERROR_CODE_INVALID_RESOLUTION + ERROR_CODE_LOW_USB_BANDWIDTH = c_ERROR_CODE.ERROR_CODE_LOW_USB_BANDWIDTH + ERROR_CODE_CALIBRATION_FILE_NOT_AVAILABLE = c_ERROR_CODE.ERROR_CODE_CALIBRATION_FILE_NOT_AVAILABLE + ERROR_CODE_INVALID_SVO_FILE = c_ERROR_CODE.ERROR_CODE_INVALID_SVO_FILE + ERROR_CODE_SVO_RECORDING_ERROR = c_ERROR_CODE.ERROR_CODE_SVO_RECORDING_ERROR + ERROR_CODE_SVO_UNSUPPORTED_COMPRESSION = c_ERROR_CODE.ERROR_CODE_SVO_UNSUPPORTED_COMPRESSION + ERROR_CODE_INVALID_COORDINATE_SYSTEM = c_ERROR_CODE.ERROR_CODE_INVALID_COORDINATE_SYSTEM + ERROR_CODE_INVALID_FIRMWARE = c_ERROR_CODE.ERROR_CODE_INVALID_FIRMWARE + ERROR_CODE_INVALID_FUNCTION_PARAMETERS = c_ERROR_CODE.ERROR_CODE_INVALID_FUNCTION_PARAMETERS + ERROR_CODE_NOT_A_NEW_FRAME = c_ERROR_CODE.ERROR_CODE_NOT_A_NEW_FRAME + ERROR_CODE_CUDA_ERROR = c_ERROR_CODE.ERROR_CODE_CUDA_ERROR + ERROR_CODE_CAMERA_NOT_INITIALIZED = c_ERROR_CODE.ERROR_CODE_CAMERA_NOT_INITIALIZED + ERROR_CODE_NVIDIA_DRIVER_OUT_OF_DATE = c_ERROR_CODE.ERROR_CODE_NVIDIA_DRIVER_OUT_OF_DATE + ERROR_CODE_INVALID_FUNCTION_CALL = c_ERROR_CODE.ERROR_CODE_INVALID_FUNCTION_CALL + ERROR_CODE_CORRUPTED_SDK_INSTALLATION = c_ERROR_CODE.ERROR_CODE_CORRUPTED_SDK_INSTALLATION + ERROR_CODE_INCOMPATIBLE_SDK_VERSION = c_ERROR_CODE.ERROR_CODE_INCOMPATIBLE_SDK_VERSION + ERROR_CODE_INVALID_AREA_FILE = c_ERROR_CODE.ERROR_CODE_INVALID_AREA_FILE + ERROR_CODE_INCOMPATIBLE_AREA_FILE = c_ERROR_CODE.ERROR_CODE_INCOMPATIBLE_AREA_FILE + ERROR_CODE_CAMERA_FAILED_TO_SETUP = c_ERROR_CODE.ERROR_CODE_CAMERA_FAILED_TO_SETUP + ERROR_CODE_CAMERA_DETECTION_ISSUE = c_ERROR_CODE.ERROR_CODE_CAMERA_DETECTION_ISSUE + ERROR_CODE_CAMERA_ALREADY_IN_USE = c_ERROR_CODE.ERROR_CODE_CAMERA_ALREADY_IN_USE + ERROR_CODE_NO_GPU_DETECTED = c_ERROR_CODE.ERROR_CODE_NO_GPU_DETECTED + ERROR_CODE_PLANE_NOT_FOUND = c_ERROR_CODE.ERROR_CODE_PLANE_NOT_FOUND + ERROR_CODE_LAST = c_ERROR_CODE.ERROR_CODE_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class MODEL(enum.Enum): + MODEL_ZED = c_MODEL.MODEL_ZED + MODEL_ZED_M = c_MODEL.MODEL_ZED_M + MODEL_LAST = c_MODEL.MODEL_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class CAMERA_STATE(enum.Enum): + CAMERA_STATE_AVAILABLE = c_CAMERA_STATE.CAMERA_STATE_AVAILABLE + CAMERA_STATE_NOT_AVAILABLE = c_CAMERA_STATE.CAMERA_STATE_NOT_AVAILABLE + CAMERA_STATE_LAST = c_CAMERA_STATE.CAMERA_STATE_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +def c_sleep_ms(int time): + sleep_ms(time) + +cdef class DeviceProperties: + cdef c_DeviceProperties c_device_properties + + def __cinit__(self): + self.c_device_properties = c_DeviceProperties() + + @property + def camera_state(self): + return self.c_device_properties.camera_state + @camera_state.setter + def camera_state(self, camera_state): + if isinstance(camera_state, CAMERA_STATE): + self.c_device_properties.camera_state = ( camera_state.value) + elif isinstance(camera_state, int): + self.c_device_properties.camera_state = ( camera_state) + else: + raise TypeError("Argument is not of CAMERA_STATE type.") + + @property + def id(self): + return self.c_device_properties.id + @id.setter + def id(self, id): + self.c_device_properties.id = id + + @property + def path(self): + if not self.c_device_properties.path.empty(): + return self.c_device_properties.path.get().decode() + else: + return "" + @path.setter + def path(self, str path): + path_ = path.encode() + self.c_device_properties.path = (String( path_)) + + @property + def camera_model(self): + return self.c_device_properties.camera_model + @camera_model.setter + def camera_model(self, camera_model): + if isinstance(camera_model, MODEL): + self.c_device_properties.camera_model = ( camera_model.value) + elif isinstance(camera_model, int): + self.c_device_properties.camera_model = ( camera_model) + else: + raise TypeError("Argument is not of MODEL type.") + + @property + def serial_number(self): + return self.c_device_properties.serial_number + @serial_number.setter + def serial_number(self, serial_number): + self.c_device_properties.serial_number = serial_number + + def __str__(self): + return to_str(toString(self.c_device_properties)).decode() + + def __repr__(self): + return to_str(toString(self.c_device_properties)).decode() + + +cdef class Matrix3f: + cdef c_Matrix3f mat + def __cinit__(self): + self.mat = c_Matrix3f() + + def init_matrix(self, Matrix3f matrix): + self.mat = c_Matrix3f(matrix.mat) + + def inverse(self): + self.mat.inverse() + + def inverse_mat(self, Matrix3f rotation): + rotation.mat.inverse(rotation.mat) + return rotation + + def transpose(self): + self.mat.transpose() + + def transpose(self, Matrix3f rotation): + rotation.mat.transpose(rotation.mat) + return rotation + + def set_identity(self): + self.mat.setIdentity() + return self + + def identity(self): + self.mat.identity() + return self + + def set_zeros(self): + self.mat.setZeros() + + def zeros(self): + self.mat.zeros() + return self + + def get_infos(self): + return to_str(self.mat.getInfos()).decode() + + @property + def matrix_name(self): + if not self.mat.matrix_name.empty(): + return self.mat.matrix_name.get().decode() + else: + return "" + + @property + def nbElem(self): + return self.mat.nbElem + + @property + def r(self): + nbElem = self.nbElem + sqrt_nbElem = int(sqrt(nbElem)) + cdef np.ndarray arr = np.zeros(nbElem) + for i in range(nbElem): + arr[i] = self.mat.r[i] + return arr.reshape(sqrt_nbElem, sqrt_nbElem) + + @r.setter + def r(self, value): + if isinstance(value, list): + if len(value) <= self.nbElem: + for i in range(len(value)): + self.mat.r[i] = value[i] + else: + raise IndexError("Value list must be of length 9 max.") + elif isinstance(value, np.ndarray): + if value.size <= self.nbElem: + for i in range(value.size): + self.mat.r[i] = value[i] + else: + raise IndexError("Numpy array must be of size 9.") + else: + raise TypeError("Argument must be numpy array or list type.") + + def __mul__(self, other): + matrix = Matrix3f() + if isinstance(other, Matrix3f): + matrix.r = (self.r * other.r).reshape(self.nbElem) + elif isinstance(other, float) or isinstance(other, int) or isinstance(other, long): + matrix.r = (other * self.r).reshape(self.nbElem) + else: + raise TypeError("Argument must be Matrix3f or scalar type.") + return matrix + + def __richcmp__(Matrix3f left, Matrix3f right, int op): + if op == 2: + return left.mat == right.mat + if op == 3: + return left.mat != right.mat + else: + raise NotImplementedError() + + def __getitem__(self, x): + return self.r[x] + + def __setitem__(self, key, value): + if key == (0,0): + self.mat.r00 = value + elif key == (0,1): + self.mat.r01 = value + elif key == (0,2): + self.mat.r02 = value + elif key == (1,0): + self.mat.r10 = value + elif key == (1,1): + self.mat.r11 = value + elif key == (1,2): + self.mat.r12 = value + elif key == (2,0): + self.mat.r20 = value + elif key == (2,1): + self.mat.r21 = value + elif key == (2,2): + self.mat.r22 = value + + def __repr__(self): + return self.get_infos() + + +cdef class Matrix4f: + cdef c_Matrix4f mat + def __cinit__(self): + self.mat = c_Matrix4f() + + def init_matrix(self, Matrix4f matrix): + self.mat = c_Matrix4f(matrix.mat) + + def inverse(self): + return ERROR_CODE(self.mat.inverse()) + + def inverse_mat(self, Matrix4f rotation): + rotation.mat.inverse(rotation.mat) + return rotation + + def transpose(self): + self.mat.transpose() + + def transpose(self, Matrix4f rotation): + rotation.mat.transpose(rotation.mat) + return rotation + + def set_identity(self): + self.mat.setIdentity() + return self + + def identity(self): + self.mat.identity() + return self + + def set_zeros(self): + self.mat.setZeros() + + def zeros(self): + self.mat.zeros() + return self + + def get_infos(self): + return to_str(self.mat.getInfos()).decode() + + def set_sub_matrix3f(self, Matrix3f input, row=0, column=0): + if row != 0 and row != 1 or column != 0 and column != 1: + raise TypeError("Arguments row and column must be 0 or 1.") + else: + return ERROR_CODE(self.mat.setSubMatrix3f(input.mat, row, column)) + + def set_sub_vector3f(self, float input0, float input1, float input2, column=3): + return ERROR_CODE(self.mat.setSubVector3f(Vector3[float](input0, input1, input2), column)) + + def set_sub_vector4f(self, float input0, float input1, float input2, float input3, column=3): + return ERROR_CODE(self.mat.setSubVector4f(Vector4[float](input0, input1, input2, input3), column)) + + @property + def nbElem(self): + return self.mat.nbElem + + @property + def matrix_name(self): + if not self.mat.matrix_name.empty(): + return self.mat.matrix_name.get().decode() + else: + return "" + + @property + def m(self): + nbElem = self.nbElem + sqrt_nbElem = int(sqrt(nbElem)) + cdef np.ndarray arr = np.zeros(nbElem) + for i in range(nbElem): + arr[i] = self.mat.m[i] + return arr.reshape(sqrt_nbElem, sqrt_nbElem) + + @m.setter + def m(self, value): + if isinstance(value, list): + if len(value) <= self.nbElem: + for i in range(len(value)): + self.mat.m[i] = value[i] + else: + raise IndexError("Value list must be of length 16 max.") + elif isinstance(value, np.ndarray): + if value.size <= self.nbElem: + for i in range(value.size): + self.mat.m[i] = value[i] + else: + raise IndexError("Numpy array must be of size 16.") + else: + raise TypeError("Argument must be numpy array or list type.") + + def __mul__(self, other): + matrix = Matrix4f() + if isinstance(other, Matrix4f): + matrix.m = (self.m * other.m).reshape(self.nbElem) + elif isinstance(other, float) or isinstance(other, int) or isinstance(other, long): + matrix.m = (other * self.m).reshape(self.nbElem) + else: + raise TypeError("Argument must be Matrix4f or scalar type.") + return matrix + + def __richcmp__(Matrix4f left, Matrix4f right, int op): + if op == 2: + return left.mat == right.mat + if op == 3: + return left.mat != right.mat + else: + raise NotImplementedError() + + def __getitem__(self, x): + return self.m[x] + + def __setitem__(self, key, value): + if key == (0,0): + self.mat.r00 = value + elif key == (0,1): + self.mat.r01 = value + elif key == (0,2): + self.mat.r02 = value + elif key == (0,3): + self.mat.tx = value + elif key == (1,0): + self.mat.r10 = value + elif key == (1,1): + self.mat.r11 = value + elif key == (1,2): + self.mat.r12 = value + elif key == (1,3): + self.mat.ty = value + elif key == (2,0): + self.mat.r20 = value + elif key == (2,1): + self.mat.r21 = value + elif key == (2,2): + self.mat.r22 = value + elif key == (2,3): + self.mat.tz = value + elif key == (3,0): + self.mat.m30 = value + elif key == (3,1): + self.mat.m31 = value + elif key == (3,2): + self.mat.m32 = value + elif key == (3,3): + self.mat.m33 = value + + def __repr__(self): + return self.get_infos() + + +class RESOLUTION(enum.Enum): + RESOLUTION_HD2K = c_RESOLUTION.RESOLUTION_HD2K + RESOLUTION_HD1080 = c_RESOLUTION.RESOLUTION_HD1080 + RESOLUTION_HD720 = c_RESOLUTION.RESOLUTION_HD720 + RESOLUTION_VGA = c_RESOLUTION.RESOLUTION_VGA + RESOLUTION_LAST = c_RESOLUTION.RESOLUTION_LAST + + def __str__(self): + return ( resolution2str(self.value)).decode() + + def __repr__(self): + return ( resolution2str(self.value)).decode() + + +class CAMERA_SETTINGS(enum.Enum): + CAMERA_SETTINGS_BRIGHTNESS = c_CAMERA_SETTINGS.CAMERA_SETTINGS_BRIGHTNESS + CAMERA_SETTINGS_CONTRAST = c_CAMERA_SETTINGS.CAMERA_SETTINGS_CONTRAST + CAMERA_SETTINGS_HUE = c_CAMERA_SETTINGS.CAMERA_SETTINGS_HUE + CAMERA_SETTINGS_SATURATION = c_CAMERA_SETTINGS.CAMERA_SETTINGS_SATURATION + CAMERA_SETTINGS_GAIN = c_CAMERA_SETTINGS.CAMERA_SETTINGS_GAIN + CAMERA_SETTINGS_EXPOSURE = c_CAMERA_SETTINGS.CAMERA_SETTINGS_EXPOSURE + CAMERA_SETTINGS_WHITEBALANCE = c_CAMERA_SETTINGS.CAMERA_SETTINGS_WHITEBALANCE + CAMERA_SETTINGS_AUTO_WHITEBALANCE = c_CAMERA_SETTINGS.CAMERA_SETTINGS_AUTO_WHITEBALANCE + CAMERA_SETTINGS_LAST = c_CAMERA_SETTINGS.CAMERA_SETTINGS_LAST + + +class SELF_CALIBRATION_STATE(enum.Enum): + SELF_CALIBRATION_STATE_NOT_STARTED = c_SELF_CALIBRATION_STATE.SELF_CALIBRATION_STATE_NOT_STARTED + SELF_CALIBRATION_STATE_RUNNING = c_SELF_CALIBRATION_STATE.SELF_CALIBRATION_STATE_RUNNING + SELF_CALIBRATION_STATE_FAILED = c_SELF_CALIBRATION_STATE.SELF_CALIBRATION_STATE_FAILED + SELF_CALIBRATION_STATE_SUCCESS = c_SELF_CALIBRATION_STATE.SELF_CALIBRATION_STATE_SUCCESS + SELF_CALIBRATION_STATE_LAST = c_SELF_CALIBRATION_STATE.SELF_CALIBRATION_STATE_LAST + + def __str__(self): + return ( statusCode2str(self.value)).decode() + + def __repr__(self): + return ( statusCode2str(self.value)).decode() + + +class DEPTH_MODE(enum.Enum): + DEPTH_MODE_NONE = c_DEPTH_MODE.DEPTH_MODE_NONE + DEPTH_MODE_PERFORMANCE = c_DEPTH_MODE.DEPTH_MODE_PERFORMANCE + DEPTH_MODE_MEDIUM = c_DEPTH_MODE.DEPTH_MODE_MEDIUM + DEPTH_MODE_QUALITY = c_DEPTH_MODE.DEPTH_MODE_QUALITY + DEPTH_MODE_ULTRA = c_DEPTH_MODE.DEPTH_MODE_ULTRA + DEPTH_MODE_LAST = c_DEPTH_MODE.DEPTH_MODE_LAST + + def __str__(self): + return ( depthMode2str(self.value)).decode() + + def __repr__(self): + return ( depthMode2str(self.value)).decode() + + +class SENSING_MODE(enum.Enum): + SENSING_MODE_STANDARD = c_SENSING_MODE.SENSING_MODE_STANDARD + SENSING_MODE_FILL = c_SENSING_MODE.SENSING_MODE_FILL + SENSING_MODE_LAST = c_SENSING_MODE.SENSING_MODE_LAST + + def __str__(self): + return ( sensingMode2str(self.value)).decode() + + def __repr__(self): + return ( sensingMode2str(self.value)).decode() + + +class UNIT(enum.Enum): + UNIT_MILLIMETER = c_UNIT.UNIT_MILLIMETER + UNIT_CENTIMETER = c_UNIT.UNIT_CENTIMETER + UNIT_METER = c_UNIT.UNIT_METER + UNIT_INCH = c_UNIT.UNIT_INCH + UNIT_FOOT = c_UNIT.UNIT_FOOT + UNIT_LAST = c_UNIT.UNIT_LAST + + def __str__(self): + return ( unit2str(self.value)).decode() + + def __repr__(self): + return ( unit2str(self.value)).decode() + + +class COORDINATE_SYSTEM(enum.Enum): + COORDINATE_SYSTEM_IMAGE = c_COORDINATE_SYSTEM.COORDINATE_SYSTEM_IMAGE + COORDINATE_SYSTEM_LEFT_HANDED_Y_UP = c_COORDINATE_SYSTEM.COORDINATE_SYSTEM_LEFT_HANDED_Y_UP + COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP = c_COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP + COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP = c_COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP + COORDINATE_SYSTEM_LEFT_HANDED_Z_UP = c_COORDINATE_SYSTEM.COORDINATE_SYSTEM_LEFT_HANDED_Z_UP + COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD = c_COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD + COORDINATE_SYSTEM_LAST = c_COORDINATE_SYSTEM.COORDINATE_SYSTEM_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class MEASURE(enum.Enum): + MEASURE_DISPARITY = c_MEASURE.MEASURE_DISPARITY + MEASURE_DEPTH = c_MEASURE.MEASURE_DEPTH + MEASURE_CONFIDENCE = c_MEASURE.MEASURE_CONFIDENCE + MEASURE_XYZ = c_MEASURE.MEASURE_XYZ + MEASURE_XYZRGBA = c_MEASURE.MEASURE_XYZRGBA + MEASURE_XYZBGRA = c_MEASURE.MEASURE_XYZBGRA + MEASURE_XYZARGB = c_MEASURE.MEASURE_XYZARGB + MEASURE_XYZABGR = c_MEASURE.MEASURE_XYZABGR + MEASURE_NORMALS = c_MEASURE.MEASURE_NORMALS + MEASURE_DISPARITY_RIGHT = c_MEASURE.MEASURE_DISPARITY_RIGHT + MEASURE_DEPTH_RIGHT = c_MEASURE.MEASURE_DEPTH_RIGHT + MEASURE_XYZ_RIGHT = c_MEASURE.MEASURE_XYZ_RIGHT + MEASURE_XYZRGBA_RIGHT = c_MEASURE.MEASURE_XYZRGBA_RIGHT + MEASURE_XYZBGRA_RIGHT = c_MEASURE.MEASURE_XYZBGRA_RIGHT + MEASURE_XYZARGB_RIGHT = c_MEASURE.MEASURE_XYZARGB_RIGHT + MEASURE_XYZABGR_RIGHT = c_MEASURE.MEASURE_XYZABGR_RIGHT + MEASURE_NORMALS_RIGHT = c_MEASURE.MEASURE_NORMALS_RIGHT + MEASURE_LAST = c_MEASURE.MEASURE_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class VIEW(enum.Enum): + VIEW_LEFT = c_VIEW.VIEW_LEFT + VIEW_RIGHT = c_VIEW.VIEW_RIGHT + VIEW_LEFT_GRAY = c_VIEW.VIEW_LEFT_GRAY + VIEW_RIGHT_GRAY = c_VIEW.VIEW_RIGHT_GRAY + VIEW_LEFT_UNRECTIFIED = c_VIEW.VIEW_LEFT_UNRECTIFIED + VIEW_RIGHT_UNRECTIFIED = c_VIEW.VIEW_RIGHT_UNRECTIFIED + VIEW_LEFT_UNRECTIFIED_GRAY = c_VIEW.VIEW_LEFT_UNRECTIFIED_GRAY + VIEW_RIGHT_UNRECTIFIED_GRAY = c_VIEW.VIEW_RIGHT_UNRECTIFIED_GRAY + VIEW_SIDE_BY_SIDE = c_VIEW.VIEW_SIDE_BY_SIDE + VIEW_DEPTH = c_VIEW.VIEW_DEPTH + VIEW_CONFIDENCE = c_VIEW.VIEW_CONFIDENCE + VIEW_NORMALS = c_VIEW.VIEW_NORMALS + VIEW_DEPTH_RIGHT = c_VIEW.VIEW_DEPTH_RIGHT + VIEW_NORMALS_RIGHT = c_VIEW.VIEW_NORMALS_RIGHT + VIEW_LAST = c_VIEW.VIEW_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class DEPTH_FORMAT(enum.Enum): + DEPTH_FORMAT_PNG = c_DEPTH_FORMAT.DEPTH_FORMAT_PNG + DEPTH_FORMAT_PFM = c_DEPTH_FORMAT.DEPTH_FORMAT_PFM + DEPTH_FORMAT_PGM = c_DEPTH_FORMAT.DEPTH_FORMAT_PGM + DEPTH_FORMAT_LAST = c_DEPTH_FORMAT.DEPTH_FORMAT_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class POINT_CLOUD_FORMAT(enum.Enum): + POINT_CLOUD_FORMAT_XYZ_ASCII = c_POINT_CLOUD_FORMAT.POINT_CLOUD_FORMAT_XYZ_ASCII + POINT_CLOUD_FORMAT_PCD_ASCII = c_POINT_CLOUD_FORMAT.POINT_CLOUD_FORMAT_PCD_ASCII + POINT_CLOUD_FORMAT_PLY_ASCII = c_POINT_CLOUD_FORMAT.POINT_CLOUD_FORMAT_PLY_ASCII + POINT_CLOUD_FORMAT_VTK_ASCII = c_POINT_CLOUD_FORMAT.POINT_CLOUD_FORMAT_VTK_ASCII + POINT_CLOUD_FORMAT_LAST = c_POINT_CLOUD_FORMAT.POINT_CLOUD_FORMAT_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class TRACKING_STATE(enum.Enum): + TRACKING_STATE_SEARCHING = c_TRACKING_STATE.TRACKING_STATE_SEARCHING + TRACKING_STATE_OK = c_TRACKING_STATE.TRACKING_STATE_OK + TRACKING_STATE_OFF = c_TRACKING_STATE.TRACKING_STATE_OFF + TRACKING_STATE_FPS_TOO_LOW = c_TRACKING_STATE.TRACKING_STATE_FPS_TOO_LOW + TRACKING_STATE_LAST = c_TRACKING_STATE.TRACKING_STATE_LAST + + def __str__(self): + return ( trackingState2str(self.value)).decode() + + def __repr__(self): + return ( trackingState2str(self.value)).decode() + + +class AREA_EXPORT_STATE(enum.Enum): + AREA_EXPORT_STATE_SUCCESS = c_AREA_EXPORT_STATE.AREA_EXPORT_STATE_SUCCESS + AREA_EXPORT_STATE_RUNNING = c_AREA_EXPORT_STATE.AREA_EXPORT_STATE_RUNNING + AREA_EXPORT_STATE_NOT_STARTED = c_AREA_EXPORT_STATE.AREA_EXPORT_STATE_NOT_STARTED + AREA_EXPORT_STATE_FILE_EMPTY = c_AREA_EXPORT_STATE.AREA_EXPORT_STATE_FILE_EMPTY + AREA_EXPORT_STATE_FILE_ERROR = c_AREA_EXPORT_STATE.AREA_EXPORT_STATE_FILE_ERROR + AREA_EXPORT_STATE_SPATIAL_MEMORY_DISABLED = c_AREA_EXPORT_STATE.AREA_EXPORT_STATE_SPATIAL_MEMORY_DISABLED + AREA_EXPORT_STATE_LAST = c_AREA_EXPORT_STATE.AREA_EXPORT_STATE_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class REFERENCE_FRAME(enum.Enum): + REFERENCE_FRAME_WORLD = c_REFERENCE_FRAME.REFERENCE_FRAME_WORLD + REFERENCE_FRAME_CAMERA = c_REFERENCE_FRAME.REFERENCE_FRAME_CAMERA + REFERENCE_FRAME_LAST = c_REFERENCE_FRAME.REFERENCE_FRAME_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class TIME_REFERENCE(enum.Enum): + TIME_REFERENCE_IMAGE = c_TIME_REFERENCE.TIME_REFERENCE_IMAGE + TIME_REFERENCE_CURRENT = c_TIME_REFERENCE.TIME_REFERENCE_CURRENT + TIME_REFERENCE_LAST = c_TIME_REFERENCE.TIME_REFERENCE_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +class SPATIAL_MAPPING_STATE(enum.Enum): + SPATIAL_MAPPING_STATE_INITIALIZING = c_SPATIAL_MAPPING_STATE.SPATIAL_MAPPING_STATE_INITIALIZING + SPATIAL_MAPPING_STATE_OK = c_SPATIAL_MAPPING_STATE.SPATIAL_MAPPING_STATE_OK + SPATIAL_MAPPING_STATE_NOT_ENOUGH_MEMORY = c_SPATIAL_MAPPING_STATE.SPATIAL_MAPPING_STATE_NOT_ENOUGH_MEMORY + SPATIAL_MAPPING_STATE_NOT_ENABLED = c_SPATIAL_MAPPING_STATE.SPATIAL_MAPPING_STATE_NOT_ENABLED + SPATIAL_MAPPING_STATE_FPS_TOO_LOW = c_SPATIAL_MAPPING_STATE.SPATIAL_MAPPING_STATE_FPS_TOO_LOW + SPATIAL_MAPPING_STATE_LAST = c_SPATIAL_MAPPING_STATE.SPATIAL_MAPPING_STATE_LAST + + def __str__(self): + return ( spatialMappingState2str(self.value)).decode() + + def __repr__(self): + return ( spatialMappingState2str(self.value)).decode() + + +class SVO_COMPRESSION_MODE(enum.Enum): + SVO_COMPRESSION_MODE_RAW = c_SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_RAW + SVO_COMPRESSION_MODE_LOSSLESS = c_SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_LOSSLESS + SVO_COMPRESSION_MODE_LOSSY = c_SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_LOSSY + SVO_COMPRESSION_MODE_AVCHD = c_SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_AVCHD + SVO_COMPRESSION_MODE_HEVC = c_SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_HEVC + SVO_COMPRESSION_MODE_LAST = c_SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_LAST + + def __str__(self): + return to_str(toString(self.value)).decode() + + def __repr__(self): + return to_str(toString(self.value)).decode() + +def video_modes(): + return cameraResolution + + +def str_to_mode(str mode): + return DEPTH_MODE(str2mode(mode.encode())) + + +def str_to_unit(str unit): + return UNIT(str2unit(unit.encode())) + +class MEM(enum.Enum): + MEM_CPU = c_MEM.MEM_CPU + MEM_GPU = c_MEM.MEM_GPU + + +class COPY_TYPE(enum.Enum): + COPY_TYPE_CPU_CPU = c_COPY_TYPE.COPY_TYPE_CPU_CPU + COPY_TYPE_CPU_GPU = c_COPY_TYPE.COPY_TYPE_CPU_GPU + COPY_TYPE_GPU_GPU = c_COPY_TYPE.COPY_TYPE_GPU_GPU + COPY_TYPE_GPU_CPU = c_COPY_TYPE.COPY_TYPE_GPU_CPU + + +class MAT_TYPE(enum.Enum): + MAT_TYPE_32F_C1 = c_MAT_TYPE.MAT_TYPE_32F_C1 + MAT_TYPE_32F_C2 = c_MAT_TYPE.MAT_TYPE_32F_C2 + MAT_TYPE_32F_C3 = c_MAT_TYPE.MAT_TYPE_32F_C3 + MAT_TYPE_32F_C4 = c_MAT_TYPE.MAT_TYPE_32F_C4 + MAT_TYPE_8U_C1 = c_MAT_TYPE.MAT_TYPE_8U_C1 + MAT_TYPE_8U_C2 = c_MAT_TYPE.MAT_TYPE_8U_C2 + MAT_TYPE_8U_C3 = c_MAT_TYPE.MAT_TYPE_8U_C3 + MAT_TYPE_8U_C4 = c_MAT_TYPE.MAT_TYPE_8U_C4 + + +def get_current_timestamp(): + return getCurrentTimeStamp() + + +cdef class Resolution: + cdef size_t width + cdef size_t height + def __cinit__(self, width=0, height=0): + self.width = width + self.height = height + + def py_area(self): + return self.width * self.height + + @property + def width(self): + return self.width + + @property + def height(self): + return self.height + + def __richcmp__(Resolution left, Resolution right, int op): + if op == 2: + return left.width==right.width and left.height==right.height + if op == 3: + return left.width!=right.width or left.height!=right.height + else: + raise NotImplementedError() + + +cdef class CameraParameters: + cdef c_CameraParameters camera_params + @property + def fx(self): + return self.camera_params.fx + + @property + def fy(self): + return self.camera_params.fy + + @property + def cx(self): + return self.camera_params.cx + + @property + def cy(self): + return self.camera_params.cy + + @property + def disto(self): + return self.camera_params.disto + + @property + def v_fov(self): + return self.camera_params.v_fov + + @property + def h_fov(self): + return self.camera_params.h_fov + + @property + def d_fov(self): + return self.camera_params.d_fov + + @property + def image_size(self): + return self.camera_params.image_size + + +cdef class CalibrationParameters: + cdef c_CalibrationParameters calibration + cdef CameraParameters py_left_cam + cdef CameraParameters py_right_cam + cdef Vector3[float] R + cdef Vector3[float] T + + def __cinit__(self): + self.py_left_cam = CameraParameters() + self.py_right_cam = CameraParameters() + + def set(self): + self.py_left_cam.camera_params = self.calibration.left_cam + self.py_right_cam.camera_params = self.calibration.right_cam + self.R = self.calibration.R + self.T = self.calibration.T + + @property + def R(self): + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = self.calibration.R[i] + return arr + + @property + def T(self): + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = self.calibration.T[i] + return arr + + @property + def left_cam(self): + return self.py_left_cam + + @property + def right_cam(self): + return self.py_right_cam + + +cdef class CameraInformation: + cdef CalibrationParameters py_calib + cdef CalibrationParameters py_calib_raw + cdef unsigned int serial_number + cdef unsigned int firmware_version + cdef c_MODEL camera_model + cdef Transform py_camera_imu_transform + + def __cinit__(self, Camera py_camera, Resolution resizer): + res = c_Resolution(resizer.width, resizer.height) + self.py_calib = CalibrationParameters() + self.py_calib.calibration = py_camera.camera.getCameraInformation(res).calibration_parameters + self.py_calib_raw = CalibrationParameters() + self.py_calib_raw.calibration = py_camera.camera.getCameraInformation(res).calibration_parameters_raw + self.py_calib.set() + self.py_calib_raw.set() + self.py_camera_imu_transform = Transform() + self.py_camera_imu_transform.transform = py_camera.camera.getCameraInformation(res).camera_imu_transform + self.serial_number = py_camera.camera.getCameraInformation(res).serial_number + self.firmware_version = py_camera.camera.getCameraInformation(res).firmware_version + self.camera_model = py_camera.camera.getCameraInformation(res).camera_model + + @property + def camera_model(self): + return self.camera_model + + @property + def calibration_parameters(self): + return self.py_calib + + @property + def calibration_parameters_raw(self): + return self.py_calib_raw + + @property + def camera_imu_transform(self): + return self.py_camera_imu_transform + + @property + def serial_number(self): + return self.serial_number + + @property + def firmware_version(self): + return self.firmware_version + + +cdef class Mat: + cdef c_Mat mat + def __cinit__(self): + self.mat = c_Mat() + + def init_mat_type(self, width, height, mat_type, memory_type=MEM.MEM_CPU): + if isinstance(mat_type, MAT_TYPE) and isinstance(memory_type, MEM): + self.mat = c_Mat(width, height, mat_type.value, memory_type.value) + else: + raise TypeError("Argument are not of MAT_TYPE or MEM type.") + + def init_mat_cpu(self, width, height, mat_type, ptr, step, memory_type=MEM.MEM_CPU): + if isinstance(mat_type, MAT_TYPE) and isinstance(memory_type, MEM): + self.mat = c_Mat(width, height, mat_type.value, ptr.encode(), step, memory_type.value) + else: + raise TypeError("Argument are not of MAT_TYPE or MEM type.") + + def init_mat_cpu_gpu(self, width, height, mat_type, ptr_cpu, step_cpu, ptr_gpu, step_gpu): + if isinstance(mat_type, MAT_TYPE): + self.mat = c_Mat(width, height, mat_type.value, ptr_cpu.encode(), step_cpu, ptr_gpu.encode(), step_gpu) + else: + raise TypeError("Argument is not of MAT_TYPE type.") + + def init_mat_resolution(self, Resolution resolution, mat_type, memory_type): + if isinstance(mat_type, MAT_TYPE) and isinstance(memory_type, MEM): + self.mat = c_Mat(c_Resolution(resolution.width, resolution.height), mat_type.value, memory_type.value) + else: + raise TypeError("Argument are not of MAT_TYPE or MEM type.") + + def init_mat_resolution_cpu(self, Resolution resolution, mat_type, ptr, step, memory_type=MEM.MEM_CPU): + if isinstance(mat_type, MAT_TYPE) and isinstance(memory_type, MEM): + self.mat = c_Mat(c_Resolution(resolution.width, resolution.height), mat_type.value, ptr.encode(), + step, memory_type.value) + else: + raise TypeError("Argument are not of MAT_TYPE or MEM type.") + + def init_mat_resolution_cpu_gpu(self, Resolution resolution, mat_type, ptr_cpu, step_cpu, ptr_gpu, step_gpu): + if isinstance(mat_type, MAT_TYPE): + self.mat = matResolution(c_Resolution(resolution.width, resolution.height), mat_type.value, ptr_cpu.encode(), + step_cpu, ptr_gpu.encode(), step_gpu) + else: + raise TypeError("Argument is not of MAT_TYPE type.") + + def init_mat(self, Mat matrix): + self.mat = c_Mat(matrix.mat) + + def alloc_size(self, width, height, mat_type, memory_type=MEM.MEM_CPU): + if isinstance(mat_type, MAT_TYPE) and isinstance(memory_type, MEM): + self.mat.alloc( width, height, mat_type.value, memory_type.value) + else: + raise TypeError("Arguments must be of Mat and MEM types.") + + def alloc_resolution(self, Resolution resolution, mat_type, memory_type=MEM.MEM_CPU): + if isinstance(mat_type, MAT_TYPE) and isinstance(memory_type, MEM): + self.mat.alloc(resolution.width, resolution.height, mat_type.value, memory_type.value) + else: + raise TypeError("Arguments must be of Mat and MEM types.") + + def free(self, memory_type=None): + if isinstance(memory_type, MEM): + self.mat.free(memory_type.value) + elif memory_type is None: + self.mat.free(MEM.MEM_CPU or MEM.MEM_GPU) + else: + raise TypeError("Argument is not of MEM type.") + + def update_cpu_from_gpu(self): + return ERROR_CODE(self.mat.updateCPUfromGPU()) + + def update_gpu_from_cpu(self): + return ERROR_CODE(self.mat.updateGPUfromCPU()) + + def copy_to(self, cpy_type=COPY_TYPE.COPY_TYPE_CPU_CPU): + dst = Mat() + print(ERROR_CODE(self.mat.copyTo(dst.mat, cpy_type.value))) + return dst + + def set_from(self, cpy_type=COPY_TYPE.COPY_TYPE_CPU_CPU): + dst = Mat() + print(self.mat.setFrom(dst.mat, cpy_type.value)) + return dst + + def read(self, str filepath): + return ERROR_CODE(self.mat.read(filepath.encode())) + + def write(self, str filepath): + return ERROR_CODE(self.mat.write(filepath.encode())) + + def set_to(self, value, memory_type=MEM.MEM_CPU): + if self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C1: + return ERROR_CODE(setToUchar1(self.mat, value, memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C2: + return ERROR_CODE(setToUchar2(self.mat, Vector2[uchar1](value[0], value[1]), + memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C3: + return ERROR_CODE(setToUchar3(self.mat, Vector3[uchar1](value[0], value[1], + value[2]), memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C4: + return ERROR_CODE(setToUchar4(self.mat, Vector4[uchar1](value[0], value[1], value[2], + value[3]), memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C1: + return ERROR_CODE(setToFloat1(self.mat, value, memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C2: + return ERROR_CODE(setToFloat2(self.mat, Vector2[float1](value[0], value[1]), + memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C3: + return ERROR_CODE(setToFloat3(self.mat, Vector3[float1](value[0], value[1], + value[2]), memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C4: + return ERROR_CODE(setToFloat4(self.mat, Vector4[float1](value[0], value[1], value[2], + value[3]), memory_type.value)) + + def set_value(self, x, y, value, memory_type=MEM.MEM_CPU): + if self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C1: + return ERROR_CODE(setValueUchar1(self.mat, x, y, value, memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C2: + return ERROR_CODE(setValueUchar2(self.mat, x, y, Vector2[uchar1](value[0], value[1]), + memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C3: + return ERROR_CODE(setValueUchar3(self.mat, x, y, Vector3[uchar1](value[0], value[1], + value[2]), memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C4: + return ERROR_CODE(setValueUchar4(self.mat, x, y, Vector4[uchar1](value[0], value[1], value[2], + value[3]), memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C1: + return ERROR_CODE(setValueFloat1(self.mat, x, y, value, memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C2: + return ERROR_CODE(setValueFloat2(self.mat, x, y, Vector2[float1](value[0], value[1]), + memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C3: + return ERROR_CODE(setValueFloat3(self.mat, x, y, Vector3[float1](value[0], value[1], + value[2]), memory_type.value)) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C4: + return ERROR_CODE(setValueFloat4(self.mat, x, y, Vector4[float1](value[0], value[1], value[2], + value[3]), memory_type.value)) + + def get_value(self, x, y, memory_type=MEM.MEM_CPU): + cdef uchar1 value1u + cdef Vector2[uchar1] value2u = Vector2[uchar1]() + cdef Vector3[uchar1] value3u = Vector3[uchar1]() + cdef Vector4[uchar1] value4u = Vector4[uchar1]() + + cdef float1 value1f + cdef Vector2[float1] value2f = Vector2[float1]() + cdef Vector3[float1] value3f = Vector3[float1]() + cdef Vector4[float1] value4f = Vector4[float1]() + + if self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C1: + status = getValueUchar1(self.mat, x, y, &value1u, memory_type.value) + return ERROR_CODE(status), self.get_data()[x, y] + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C2: + status = getValueUchar2(self.mat, x, y, &value2u, memory_type.value) + return ERROR_CODE(status), np.array([value2u.ptr()[0], value2u.ptr()[1]]) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C3: + status = getValueUchar3(self.mat, x, y, &value3u, memory_type.value) + return ERROR_CODE(status), np.array([value3u.ptr()[0], value3u.ptr()[1], value3u.ptr()[2]]) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_8U_C4: + status = getValueUchar4(self.mat, x, y, &value4u, memory_type.value) + return ERROR_CODE(status), np.array([value4u.ptr()[0], value4u.ptr()[1], value4u.ptr()[2], + value4u.ptr()[3]]) + + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C1: + status = getValueFloat1(self.mat, x, y, &value1f, memory_type.value) + return ERROR_CODE(status), self.get_data()[x, y] + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C2: + status = getValueFloat2(self.mat, x, y, &value2f, memory_type.value) + return ERROR_CODE(status), np.array([value2f.ptr()[0], value2f.ptr()[1]]) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C3: + status = getValueFloat3(self.mat, x, y, &value3f, memory_type.value) + return ERROR_CODE(status), np.array([value3f.ptr()[0], value3f.ptr()[1], value3f.ptr()[2]]) + elif self.get_data_type() == MAT_TYPE.MAT_TYPE_32F_C4: + status = getValueFloat4(self.mat, x, y, &value4f, memory_type.value) + return ERROR_CODE(status), np.array([value4f.ptr()[0], value4f.ptr()[1], value4f.ptr()[2], + value4f.ptr()[3]]) + + def get_width(self): + return self.mat.getWidth() + + def get_height(self): + return self.mat.getHeight() + + def get_resolution(self): + return Resolution(self.mat.getResolution().width, self.mat.getResolution().height) + + def get_channels(self): + return self.mat.getChannels() + + def get_data_type(self): + return MAT_TYPE(self.mat.getDataType()) + + def get_memory_type(self): + return MEM(self.mat.getMemoryType()) + + def get_data(self, memory_type=MEM.MEM_CPU): + shape = None + if self.mat.getChannels() == 1: + shape = (self.mat.getHeight(), self.mat.getWidth()) + else: + shape = (self.mat.getHeight(), self.mat.getWidth(), self.mat.getChannels()) + + cdef size_t size = 0 + dtype = None + if self.mat.getDataType() in (c_MAT_TYPE.MAT_TYPE_8U_C1, c_MAT_TYPE.MAT_TYPE_8U_C2, c_MAT_TYPE.MAT_TYPE_8U_C3, c_MAT_TYPE.MAT_TYPE_8U_C4): + size = self.mat.getHeight()*self.mat.getWidth()*self.mat.getChannels() + dtype = np.uint8 + elif self.mat.getDataType() in (c_MAT_TYPE.MAT_TYPE_32F_C1, c_MAT_TYPE.MAT_TYPE_32F_C2, c_MAT_TYPE.MAT_TYPE_32F_C3, c_MAT_TYPE.MAT_TYPE_32F_C4): + size = self.mat.getHeight()*self.mat.getWidth()*self.mat.getChannels()*sizeof(float) + dtype = np.float32 + else: + raise RuntimeError("Unknown Mat data_type value: {0}".format(self.mat.getDataType())) + + cdef np.ndarray arr = np.zeros(shape, dtype=dtype) + if isinstance(memory_type, MEM): + if self.mat.getDataType() == c_MAT_TYPE.MAT_TYPE_8U_C1: + memcpy(arr.data, getPointerUchar1(self.mat, memory_type.value), size) + elif self.mat.getDataType() == c_MAT_TYPE.MAT_TYPE_8U_C2: + memcpy(arr.data, getPointerUchar2(self.mat, memory_type.value), size) + elif self.mat.getDataType() == c_MAT_TYPE.MAT_TYPE_8U_C3: + memcpy(arr.data, getPointerUchar3(self.mat, memory_type.value), size) + elif self.mat.getDataType() == c_MAT_TYPE.MAT_TYPE_8U_C4: + memcpy(arr.data, getPointerUchar4(self.mat, memory_type.value), size) + elif self.mat.getDataType() == c_MAT_TYPE.MAT_TYPE_32F_C1: + memcpy(arr.data, getPointerFloat1(self.mat, memory_type.value), size) + elif self.mat.getDataType() == c_MAT_TYPE.MAT_TYPE_32F_C2: + memcpy(arr.data, getPointerFloat2(self.mat, memory_type.value), size) + elif self.mat.getDataType() == c_MAT_TYPE.MAT_TYPE_32F_C3: + memcpy(arr.data, getPointerFloat3(self.mat, memory_type.value), size) + elif self.mat.getDataType() == c_MAT_TYPE.MAT_TYPE_32F_C4: + memcpy(arr.data, getPointerFloat4(self.mat, memory_type.value), size) + else: + raise TypeError("Argument is not of MEM type.") + + return arr + + def get_step_bytes(self, memory_type=MEM.MEM_CPU): + if type(memory_type) == MEM: + return self.mat.getStepBytes(memory_type.value) + else: + raise TypeError("Argument is not of MEM type.") + + def get_step(self, memory_type=MEM.MEM_CPU): + if type(memory_type) == MEM: + return self.mat.getStep(memory_type.value) + else: + raise TypeError("Argument is not of MEM type.") + + def get_pixel_bytes(self): + return self.mat.getPixelBytes() + + def get_width_bytes(self): + return self.mat.getWidthBytes() + + def get_infos(self): + return self.mat.getInfos().get().decode() + + def is_init(self): + return self.mat.isInit() + + def is_memory_owner(self): + return self.mat.isMemoryOwner() + + def clone(self, Mat py_mat): + return ERROR_CODE(self.mat.clone(py_mat.mat)) + + def move(self, Mat py_mat): + return ERROR_CODE(self.mat.move(py_mat.mat)) + + @staticmethod + def swap(self, Mat mat1, Mat mat2): + self.mat.swap(mat1, mat2) + + @property + def name(self): + if not self.mat.name.empty(): + return self.mat.name.get().decode() + else: + return "" + + @property + def verbose(self): + return self.mat.verbose + + def __repr__(self): + return self.get_infos() + + +cdef class Rotation(Matrix3f): + cdef c_Rotation rotation + def __cinit__(self): + self.rotation = c_Rotation() + + def init_rotation(self, Rotation rot): + self.rotation = c_Rotation(rot.rotation) + self.mat = rot.mat + + def init_matrix(self, Matrix3f matrix): + self.rotation = c_Rotation(matrix.mat) + self.mat = matrix.mat + + def init_orientation(self, Orientation orient): + self.rotation = c_Rotation(orient.orientation) + self.mat = c_Matrix3f(self.rotation.r) + + def init_angle_translation(self, float angle, Translation axis): + self.rotation = c_Rotation(angle, axis.translation) + self.mat = c_Matrix3f(self.rotation.r) + + def set_orientation(self, Orientation py_orientation): + self.rotation.setOrientation(py_orientation.orientation) + + def get_orientation(self): + py_orientation = Orientation() + py_orientation.orientation = self.rotation.getOrientation() + return py_orientation + + def get_rotation_vector(self): + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = self.rotation.getRotationVector()[i] + return arr + + def set_rotation_vector(self, float input0, float input1, float input2): + self.rotation.setRotationVector(Vector3[float](input0, input1, input2)) + + def get_euler_angles(self, radian=True): + cdef np.ndarray arr = np.zeros(3) + if isinstance(radian, bool): + for i in range(3): + arr[i] = self.rotation.getEulerAngles(radian)[i] + else: + raise TypeError("Argument is not of boolean type.") + return arr + + def set_euler_angles(self, float input0, float input1, float input2, bool radian=True): + if isinstance(radian, bool): + self.rotation.setEulerAngles(Vector3[float](input0, input1, input2), radian) + else: + raise TypeError("Argument is not of boolean type.") + +cdef class Translation: + cdef c_Translation translation + def __cinit__(self): + self.translation = c_Translation() + + def init_translation(self, Translation tr): + self.translation = c_Translation(tr.translation) + + def init_vector(self, float t1, float t2, float t3): + self.translation = c_Translation(t1, t2, t3) + + def normalize(self): + self.translation.normalize() + + def normalize_translation(self, Translation tr): + py_translation = Translation() + py_translation.translation = self.translation.normalize(tr.translation) + return py_translation + + def size(self): + return self.translation.size() + + def get(self): + cdef np.ndarray arr = np.zeros(self.size()) + for i in range(self.size()): + arr[i] = self.translation(i) + return arr + + def __mul__(Translation self, Orientation other): + tr = Translation() + tr.translation = self.translation * other.orientation + return tr + + +cdef class Orientation: + cdef c_Orientation orientation + def __cinit__(self): + self.orientation = c_Orientation() + + def init_orientation(self, Orientation orient): + self.orientation = c_Orientation(orient.orientation) + + def init_vector(self, float v0, float v1, float v2, float v3): + self.orientation = c_Orientation(Vector4[float](v0, v1, v2, v3)) + + def init_rotation(self, Rotation rotation): + self.orientation = c_Orientation(rotation.rotation) + + def init_translation(self, Translation tr1, Translation tr2): + self.orientation = c_Orientation(tr1.translation, tr2.translation) + + def set_rotation_matrix(self, Rotation py_rotation): + self.orientation.setRotationMatrix(py_rotation.rotation) + + def get_rotation_matrix(self): + py_rotation = Rotation() + py_rotation.mat = self.orientation.getRotationMatrix() + return py_rotation + + def set_identity(self): + self.orientation.setIdentity() + return self + + def identity(self): + self.orientation.identity() + return self + + def set_zeros(self): + self.orientation.setZeros() + + def zeros(self): + self.orientation.zeros() + return self + + def normalize(self): + self.orientation.normalise() + + @staticmethod + def normalize_orientation(Orientation orient): + orient.orientation.normalise() + return orient + + def size(self): + return self.orientation.size() + + def get(self): + cdef np.ndarray arr = np.zeros(self.size()) + for i in range(self.size()): + arr[i] = self.orientation(i) + return arr + + def __mul__(Orientation self, Orientation other): + orient = Orientation() + orient.orientation = self.orientation * other.orientation + return orient + + +cdef class Transform(Matrix4f): + cdef c_Transform transform + def __cinit__(self): + self.transform = c_Transform() + + def init_transform(self, Transform motion): + self.transform = c_Transform(motion.transform) + self.mat = motion.mat + + def init_matrix(self, Matrix4f matrix): + self.transform = c_Transform(matrix.mat) + self.mat = matrix.mat + + def init_rotation_translation(self, Rotation rot, Translation tr): + self.transform = c_Transform(rot.rotation, tr.translation) + self.mat = c_Matrix4f(self.transform.m) + + def init_orientation_translation(self, Orientation orient, Translation tr): + self.transform = c_Transform(orient.orientation, tr.translation) + self.mat = c_Matrix4f(self.transform.m) + + def set_rotation_matrix(self, Rotation py_rotation): + self.transform.setRotationMatrix(py_rotation.mat) + + def get_rotation_matrix(self): + py_rotation = Rotation() + py_rotation.mat = self.transform.getRotationMatrix() + return py_rotation + + def set_translation(self, Translation py_translation): + self.transform.setTranslation(py_translation.translation) + + def get_translation(self): + py_translation = Translation() + py_translation.translation = self.transform.getTranslation() + return py_translation + + def set_orientation(self, Orientation py_orientation): + self.transform.setOrientation(py_orientation.orientation) + + def get_orientation(self): + py_orientation = Orientation() + py_orientation.orientation = self.transform.getOrientation() + return py_orientation + + def get_rotation_vector(self): + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = self.transform.getRotationVector()[i] + return arr + + def set_rotation_vector(self, float input0, float input1, float input2): + self.transform.setRotationVector(Vector3[float](input0, input1, input2)) + + def get_euler_angles(self, radian=True): + cdef np.ndarray arr = np.zeros(3) + if isinstance(radian, bool): + for i in range(3): + arr[i] = self.transform.getEulerAngles(radian)[i] + else: + raise TypeError("Argument is not of boolean type.") + return arr + + def set_euler_angles(self, float input0, float input1, float input2, radian=True): + if isinstance(radian, bool): + self.transform.setEulerAngles(Vector3[float](input0, input1, input2), radian) + else: + raise TypeError("Argument is not of boolean type.") + + + +class MESH_FILE_FORMAT(enum.Enum): + MESH_FILE_PLY = c_MESH_FILE_FORMAT.MESH_FILE_PLY + MESH_FILE_PLY_BIN = c_MESH_FILE_FORMAT.MESH_FILE_PLY_BIN + MESH_FILE_OBJ = c_MESH_FILE_FORMAT.MESH_FILE_OBJ + MESH_FILE_LAST = c_MESH_FILE_FORMAT.MESH_FILE_LAST + +class MESH_TEXTURE_FORMAT(enum.Enum): + MESH_TEXTURE_RGB = c_MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGB + MESH_TEXTURE_RGBA = c_MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGBA + MESH_TEXTURE_LAST = c_MESH_TEXTURE_FORMAT.MESH_TEXTURE_LAST + +class MESH_FILTER(enum.Enum): + MESH_FILTER_LOW = c_MESH_FILTER.MESH_FILTER_LOW + MESH_FILTER_MEDIUM = c_MESH_FILTER.MESH_FILTER_MEDIUM + MESH_FILTER_HIGH = c_MESH_FILTER.MESH_FILTER_HIGH + +class PLANE_TYPE(enum.Enum): + PLANE_TYPE_HORIZONTAL = c_PLANE_TYPE.PLANE_TYPE_HORIZONTAL + PLANE_TYPE_VERTICAL = c_PLANE_TYPE.PLANE_TYPE_VERTICAL + PLANE_TYPE_UNKNOWN = c_PLANE_TYPE.PLANE_TYPE_UNKNOWN + PLANE_TYPE_LAST = c_PLANE_TYPE.PLANE_TYPE_LAST + +cdef class MeshFilterParameters: + cdef c_MeshFilterParameters* meshFilter + def __cinit__(self): + self.meshFilter = new c_MeshFilterParameters(c_MESH_FILTER.MESH_FILTER_LOW) + + def __dealloc__(self): + del self.meshFilter + + def set(self, filter=MESH_FILTER.MESH_FILTER_LOW): + if isinstance(filter, MESH_FILTER): + self.meshFilter.set(filter.value) + else: + raise TypeError("Argument is not of MESH_FILTER type.") + + def save(self, str filename): + filename_save = filename.encode() + return self.meshFilter.save(String( filename_save)) + + def load(self, str filename): + filename_load = filename.encode() + return self.meshFilter.load(String( filename_load)) + + +cdef class Texture: + cdef c_Texture texture + def __cinit__(self): + self.texture = c_Texture() + + @property + def name(self): + if not self.texture.name.empty(): + return self.texture.name.get().decode() + else: + return "" + + def get_data(self, Mat py_mat): + py_mat.mat = self.texture.data + return py_mat + + @property + def indice_gl(self): + return self.texture.indice_gl + + def clear(self): + self.texture.clear() + + +cdef class Chunk: + cdef c_Chunk chunk + def __cinit__(self): + self.chunk = c_Chunk() + + @property + def vertices(self): + cdef np.ndarray arr = np.zeros((self.chunk.vertices.size(), 3)) + for i in range(self.chunk.vertices.size()): + for j in range(3): + arr[i,j] = self.chunk.vertices[i].ptr()[j] + return arr + + @property + def triangles(self): + cdef np.ndarray arr = np.zeros((self.chunk.triangles.size(), 3)) + for i in range(self.chunk.triangles.size()): + for j in range(3): + arr[i,j] = self.chunk.triangles[i].ptr()[j]+1 + return arr + + @property + def normals(self): + cdef np.ndarray arr = np.zeros((self.chunk.normals.size(), 3)) + for i in range(self.chunk.normals.size()): + for j in range(3): + arr[i,j] = self.chunk.normals[i].ptr()[j] + return arr + + @property + def uv(self): + cdef np.ndarray arr = np.zeros((self.chunk.uv.size(), 2)) + for i in range(self.chunk.uv.size()): + for j in range(2): + arr[i,j] = self.chunk.uv[i].ptr()[j] + return arr + + @property + def timestamp(self): + return self.chunk.timestamp + + @property + def barycenter(self): + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = self.chunk.barycenter[i] + return arr + + @property + def has_been_updated(self): + return self.chunk.has_been_updated + + def clear(self): + self.chunk.clear() + +cdef class Mesh: + cdef c_Mesh* mesh + def __cinit__(self): + self.mesh = new c_Mesh() + + def __dealloc__(self): + del self.mesh + + @property + def chunks(self): + list = [] + for i in range(self.mesh.chunks.size()): + py_chunk = Chunk() + py_chunk.chunk = self.mesh.chunks[i] + list.append(py_chunk) + return list + + def __getitem__(self, x): + return self.chunks[x] + + def filter(self, MeshFilterParameters params, update_mesh=True): + if isinstance(update_mesh, bool): + return self.mesh.filter(deref(params.meshFilter), update_mesh) + else: + raise TypeError("Argument is not of boolean type.") + + def apply_texture(self, texture_format=MESH_TEXTURE_FORMAT.MESH_TEXTURE_RGB): + if isinstance(texture_format, MESH_TEXTURE_FORMAT): + return self.mesh.applyTexture(texture_format.value) + else: + raise TypeError("Argument is not of MESH_TEXTURE_FORMAT type.") + + def save(self, str filename, typeMesh=MESH_FILE_FORMAT.MESH_FILE_OBJ, id=[]): + if isinstance(typeMesh, MESH_FILE_FORMAT): + return self.mesh.save(String(filename.encode()), typeMesh.value, id) + else: + raise TypeError("Argument is not of MESH_FILE_FORMAT type.") + + def load(self, str filename, update_mesh=True): + if isinstance(update_mesh, bool): + return self.mesh.load(String(filename.encode()), update_mesh) + else: + raise TypeError("Argument is not of boolean type.") + + def clear(self): + self.mesh.clear() + + @property + def vertices(self): + cdef np.ndarray arr = np.zeros((self.mesh.vertices.size(), 3)) + for i in range(self.mesh.vertices.size()): + for j in range(3): + arr[i,j] = self.mesh.vertices[i].ptr()[j] + return arr + + @property + def triangles(self): + cdef np.ndarray arr = np.zeros((self.mesh.triangles.size(), 3)) + for i in range(self.mesh.triangles.size()): + for j in range(3): + arr[i,j] = self.mesh.triangles[i].ptr()[j]+1 + return arr + + @property + def normals(self): + cdef np.ndarray arr = np.zeros((self.mesh.normals.size(), 3)) + for i in range(self.mesh.normals.size()): + for j in range(3): + arr[i,j] = self.mesh.normals[i].ptr()[j] + return arr + + @property + def uv(self): + cdef np.ndarray arr = np.zeros((self.mesh.uv.size(), 2)) + for i in range(self.mesh.uv.size()): + for j in range(2): + arr[i,j] = self.mesh.uv[i].ptr()[j] + return arr + + @property + def texture(self): + py_texture = Texture() + py_texture.texture = self.mesh.texture + return py_texture + + def get_number_of_triangles(self): + return self.mesh.getNumberOfTriangles() + + def merge_chunks(self, faces_per_chunk): + self.mesh.mergeChunks(faces_per_chunk) + + def get_gravity_estimate(self): + gravity = self.mesh.getGravityEstimate() + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = gravity[i] + return arr + + def get_visible_list(self, Transform camera_pose): + return self.mesh.getVisibleList(camera_pose.transform) + + def get_surrounding_list(self, Transform camera_pose, float radius): + return self.mesh.getSurroundingList(camera_pose.transform, radius) + + def update_mesh_from_chunklist(self, id=[]): + self.mesh.updateMeshFromChunkList(id) + +cdef class Plane: + cdef c_Plane plane + def __cinit__(self): + self.plane = c_Plane() + + @property + def type(self): + return self.plane.type + + def get_normal(self): + normal = self.plane.getNormal() + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = normal[i] + return arr + + def get_center(self): + center = self.plane.getCenter() + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = center[i] + return arr + + def get_pose(self, Transform py_pose): + py_pose.transform = self.plane.getPose() + return py_pose + + def get_extents(self): + extents = self.plane.getExtents() + cdef np.ndarray arr = np.zeros(2) + for i in range(2): + arr[i] = extents[i] + return arr + + def get_plane_equation(self): + plane_eq = self.plane.getPlaneEquation() + cdef np.ndarray arr = np.zeros(4) + for i in range(4): + arr[i] = plane_eq[i] + return arr + + def get_bounds(self): + cdef np.ndarray arr = np.zeros((self.plane.getBounds().size(), 3)) + for i in range(self.plane.getBounds().size()): + for j in range(3): + arr[i,j] = self.plane.getBounds()[i].ptr()[j] + return arr + + def extract_mesh(self): + ext_mesh = self.plane.extractMesh() + pymesh = Mesh() + pymesh.mesh[0] = ext_mesh + return pymesh + + def get_closest_distance(self, point=[0,0,0]): + cdef Vector3[float] vec = Vector3[float](point[0], point[1], point[2]) + return self.plane.getClosestDistance(vec) + + + + +class MAPPING_RESOLUTION(enum.Enum): + MAPPING_RESOLUTION_HIGH = c_MAPPING_RESOLUTION.MAPPING_RESOLUTION_HIGH + MAPPING_RESOLUTION_MEDIUM = c_MAPPING_RESOLUTION.MAPPING_RESOLUTION_MEDIUM + MAPPING_RESOLUTION_LOW = c_MAPPING_RESOLUTION.MAPPING_RESOLUTION_LOW + + +class MAPPING_RANGE(enum.Enum): + MAPPING_RANGE_NEAR = c_MAPPING_RANGE.MAPPING_RANGE_NEAR + MAPPING_RANGE_MEDIUM = c_MAPPING_RANGE.MAPPING_RANGE_MEDIUM + MAPPING_RANGE_FAR = c_MAPPING_RANGE.MAPPING_RANGE_FAR + +cdef class InputType: + cdef c_InputType input + def __cinit__(self, input_type=0): + if input_type == 0 : + self.input = c_InputType() + elif isinstance(input_type, InputType) : + input_t = input_type + self.input = c_InputType(input_t.input) + else : + raise TypeError("Argument is not of right type.") + + def set_from_camera_id(self, id): + self.input.setFromCameraID(id) + + def set_from_serial_number(self, serial_number): + self.input.setFromSerialNumber(serial_number) + + def set_from_svo_file(self, str svo_input_filename): + filename = svo_input_filename.encode() + self.input.setFromSVOFile(String( filename)) + + +cdef class InitParameters: + cdef c_InitParameters* init + def __cinit__(self, camera_resolution=RESOLUTION.RESOLUTION_HD720, camera_fps=0, + camera_linux_id=0, svo_input_filename="", svo_real_time_mode=False, + depth_mode=DEPTH_MODE.DEPTH_MODE_PERFORMANCE, + coordinate_units=UNIT.UNIT_MILLIMETER, + coordinate_system=COORDINATE_SYSTEM.COORDINATE_SYSTEM_IMAGE, + sdk_verbose=False, sdk_gpu_id=-1, depth_minimum_distance=-1.0, camera_disable_self_calib=False, + camera_image_flip=False, enable_right_side_measure=False, camera_buffer_count_linux=4, + sdk_verbose_log_file="", depth_stabilization=True, InputType input_t=InputType(), + optional_settings_path=""): + if (isinstance(camera_resolution, RESOLUTION) and isinstance(camera_fps, int) and + isinstance(camera_linux_id, int) and isinstance(svo_input_filename, str) and + isinstance(svo_real_time_mode, bool) and isinstance(depth_mode, DEPTH_MODE) and + isinstance(coordinate_units, UNIT) and + isinstance(coordinate_system, COORDINATE_SYSTEM) and isinstance(sdk_verbose, bool) and + isinstance(sdk_gpu_id, int) and isinstance(depth_minimum_distance, float) and + isinstance(camera_disable_self_calib, bool) and isinstance(camera_image_flip, bool) and + isinstance(enable_right_side_measure, bool) and isinstance(camera_buffer_count_linux, int) and + isinstance(sdk_verbose_log_file, str) and isinstance(depth_stabilization, bool) and + isinstance(input_t, InputType) and isinstance(optional_settings_path, str)) : + + filename = svo_input_filename.encode() + filelog = sdk_verbose_log_file.encode() + fileoption = optional_settings_path.encode() + self.init = new c_InitParameters(camera_resolution.value, camera_fps, camera_linux_id, + String( filename), svo_real_time_mode, depth_mode.value, + coordinate_units.value, coordinate_system.value, sdk_verbose, sdk_gpu_id, + depth_minimum_distance, camera_disable_self_calib, camera_image_flip, + enable_right_side_measure, camera_buffer_count_linux, + String( filelog), depth_stabilization, + 0, input_t.input, String( fileoption)) + else: + raise TypeError("Argument is not of right type.") + + def __dealloc__(self): + del self.init + + def save(self, str filename): + filename_save = filename.encode() + return self.init.save(String( filename_save)) + + def load(self, str filename): + filename_load = filename.encode() + return self.init.load(String( filename_load)) + + @property + def camera_resolution(self): + return RESOLUTION(self.init.camera_resolution) + + @camera_resolution.setter + def camera_resolution(self, value): + if isinstance(value, RESOLUTION): + self.init.camera_resolution = value.value + else: + raise TypeError("Argument must be of RESOLUTION type.") + + @property + def camera_fps(self): + return self.init.camera_fps + + @camera_fps.setter + def camera_fps(self, int value): + self.init.camera_fps = value + + @property + def camera_linux_id(self): + return self.init.camera_linux_id + + @camera_linux_id.setter + def camera_linux_id(self, int value): + self.init.camera_linux_id = value + + @property + def svo_input_filename(self): + if not self.init.svo_input_filename.empty(): + return self.init.svo_input_filename.get().decode() + else: + return "" + + @svo_input_filename.setter + def svo_input_filename(self, str value): + value_filename = value.encode() + self.init.svo_input_filename.set(value_filename) + + @property + def svo_real_time_mode(self): + return self.init.svo_real_time_mode + + @svo_real_time_mode.setter + def svo_real_time_mode(self, bool value): + self.init.svo_real_time_mode = value + + @property + def depth_mode(self): + return DEPTH_MODE(self.init.depth_mode) + + @depth_mode.setter + def depth_mode(self, value): + if isinstance(value, DEPTH_MODE): + self.init.depth_mode = value.value + else: + raise TypeError("Argument must be of DEPTH_MODE type.") + + @property + def coordinate_units(self): + return UNIT(self.init.coordinate_units) + + @coordinate_units.setter + def coordinate_units(self, value): + if isinstance(value, UNIT): + self.init.coordinate_units = value.value + else: + raise TypeError("Argument must be of UNIT type.") + + @property + def coordinate_system(self): + return COORDINATE_SYSTEM(self.init.coordinate_system) + + @coordinate_system.setter + def coordinate_system(self, value): + if isinstance(value, COORDINATE_SYSTEM): + self.init.coordinate_system = value.value + else: + raise TypeError("Argument must be of COORDINATE_SYSTEM type.") + + @property + def sdk_verbose(self): + return self.init.sdk_verbose + + @sdk_verbose.setter + def sdk_verbose(self, bool value): + self.init.sdk_verbose = value + + @property + def sdk_gpu_id(self): + return self.init.sdk_gpu_id + + @sdk_gpu_id.setter + def sdk_gpu_id(self, int value): + self.init.sdk_gpu_id = value + + @property + def depth_minimum_distance(self): + return self.init.depth_minimum_distance + + @depth_minimum_distance.setter + def depth_minimum_distance(self, float value): + self.init.depth_minimum_distance = value + + @property + def camera_disable_self_calib(self): + return self.init.camera_disable_self_calib + + @camera_disable_self_calib.setter + def camera_disable_self_calib(self, bool value): + self.init.camera_disable_self_calib = value + + @property + def camera_image_flip(self): + return self.init.camera_image_flip + + @camera_image_flip.setter + def camera_image_flip(self, bool value): + self.init.camera_image_flip = value + + @property + def enable_right_side_measure(self): + return self.init.enable_right_side_measure + + @enable_right_side_measure.setter + def enable_right_side_measure(self, bool value): + self.init.enable_right_side_measure = value + + @property + def camera_buffer_count_linux(self): + return self.init.camera_buffer_count_linux + + @camera_buffer_count_linux.setter + def camera_buffer_count_linux(self, int value): + self.init.camera_buffer_count_linux = value + + @property + def sdk_verbose_log_file(self): + if not self.init.sdk_verbose_log_file.empty(): + return self.init.sdk_verbose_log_file.get().decode() + else: + return "" + + @sdk_verbose_log_file.setter + def sdk_verbose_log_file(self, str value): + value_filename = value.encode() + self.init.sdk_verbose_log_file.set(value_filename) + + @property + def depth_stabilization(self): + return self.init.depth_stabilization + + @depth_stabilization.setter + def depth_stabilization(self, bool value): + self.init.depth_stabilization = value + + @property + def input(self): + input_t = InputType() + input_t.input = self.init.input + return input_t + + @input.setter + def input(self, InputType input_t): + self.init.input = input_t.input + + @property + def optional_settings_path(self): + if not self.init.optional_settings_path.empty(): + return self.init.optional_settings_path.get().decode() + else: + return "" + + @optional_settings_path.setter + def optional_settings_path(self, str value): + value_filename = value.encode() + self.init.optional_settings_path.set(value_filename) + + def set_from_camera_id(self, id): + self.init.input.setFromCameraID(id) + + def set_from_serial_number(self, serial_number): + self.init.input.setFromSerialNumber(serial_number) + + def set_from_svo_file(self, str svo_input_filename): + filename = svo_input_filename.encode() + self.init.input.setFromSVOFile(String( filename)) + + +cdef class RuntimeParameters: + cdef c_RuntimeParameters* runtime + def __cinit__(self, sensing_mode=SENSING_MODE.SENSING_MODE_STANDARD, enable_depth=True, + enable_point_cloud=True, + measure3D_reference_frame=REFERENCE_FRAME.REFERENCE_FRAME_CAMERA): + if (isinstance(sensing_mode, SENSING_MODE) and isinstance(enable_depth, bool) + and isinstance(enable_point_cloud, bool) and + isinstance(measure3D_reference_frame, REFERENCE_FRAME)): + + self.runtime = new c_RuntimeParameters(sensing_mode.value, enable_depth, enable_point_cloud, + measure3D_reference_frame.value) + else: + raise TypeError() + + def __dealloc__(self): + del self.runtime + + def save(self, str filename): + filename_save = filename.encode() + return self.runtime.save(String( filename_save)) + + def load(self, str filename): + filename_load = filename.encode() + return self.runtime.load(String( filename_load)) + + @property + def sensing_mode(self): + return SENSING_MODE(self.runtime.sensing_mode) + + @sensing_mode.setter + def sensing_mode(self, value): + if isinstance(value, SENSING_MODE): + self.runtime.sensing_mode = value.value + else: + raise TypeError("Argument must be of SENSING_MODE type.") + + @property + def enable_depth(self): + return self.runtime.enable_depth + + @enable_depth.setter + def enable_depth(self, bool value): + self.runtime.enable_depth = value + + @property + def measure3D_reference_frame(self): + return REFERENCE_FRAME(self.runtime.measure3D_reference_frame) + + @measure3D_reference_frame.setter + def measure3D_reference_frame(self, value): + if isinstance(value, REFERENCE_FRAME): + self.runtime.measure3D_reference_frame = value.value + else: + raise TypeError("Argument must be of REFERENCE type.") + + +cdef class TrackingParameters: + cdef c_TrackingParameters* tracking + def __cinit__(self, Transform init_pos, _enable_memory=True, _area_path=None): + if _area_path is None: + self.tracking = new c_TrackingParameters(init_pos.transform, _enable_memory, String()) + else: + raise TypeError("Argument init_pos must be initialized first with Transform().") + + def __dealloc__(self): + del self.tracking + + def save(self, str filename): + filename_save = filename.encode() + return self.tracking.save(String( filename_save)) + + def load(self, str filename): + filename_load = filename.encode() + return self.tracking.load(String( filename_load)) + + def initial_world_transform(self, Transform init_pos): + init_pos.transform = self.tracking.initial_world_transform + return init_pos + + def set_initial_world_transform(self, Transform value): + self.tracking.initial_world_transform = value.transform + + @property + def enable_spatial_memory(self): + return self.tracking.enable_spatial_memory + + @enable_spatial_memory.setter + def enable_spatial_memory(self, bool value): + self.tracking.enable_spatial_memory = value + + @property + def enable_pose_smoothing(self): + return self.tracking.enable_pose_smoothing + + @enable_pose_smoothing.setter + def enable_pose_smoothing(self, bool value): + self.tracking.enable_pose_smoothing = value + + @property + def set_floor_as_origin(self): + return self.tracking.set_floor_as_origin + + @set_floor_as_origin.setter + def set_floor_as_origin(self, bool value): + self.tracking.set_floor_as_origin = value + + @property + def enable_imu_fusion(self): + return self.tracking.enable_imu_fusion + + @enable_imu_fusion.setter + def enable_imu_fusion(self, bool value): + self.tracking.enable_imu_fusion = value + + @property + def area_file_path(self): + if not self.tracking.area_file_path.empty(): + return self.tracking.area_file_path.get().decode() + else: + return "" + + @area_file_path.setter + def area_file_path(self, str value): + value_area = value.encode() + self.tracking.area_file_path.set(value_area) + + +cdef class SpatialMappingParameters: + cdef c_SpatialMappingParameters* spatial + def __cinit__(self, resolution=MAPPING_RESOLUTION.MAPPING_RESOLUTION_HIGH, mapping_range=MAPPING_RANGE.MAPPING_RANGE_MEDIUM, + max_memory_usage=2048, save_texture=True, use_chunk_only=True, + reverse_vertex_order=False): + if (isinstance(resolution, MAPPING_RESOLUTION) and isinstance(mapping_range, MAPPING_RANGE) and + isinstance(use_chunk_only, bool) and isinstance(reverse_vertex_order, bool)): + self.spatial = new c_SpatialMappingParameters(resolution.value, mapping_range.value, max_memory_usage, save_texture, + use_chunk_only, reverse_vertex_order) + else: + raise TypeError() + + def __dealloc__(self): + del self.spatial + + def set_resolution(self, resolution=MAPPING_RESOLUTION.MAPPING_RESOLUTION_HIGH): + if isinstance(resolution, MAPPING_RESOLUTION): + self.spatial.set( resolution.value) + else: + raise TypeError("Argument is not of RESOLUTION type.") + + def set_range(self, mapping_range=MAPPING_RANGE.MAPPING_RANGE_MEDIUM): + if isinstance(mapping_range, MAPPING_RANGE): + self.spatial.set( mapping_range.value) + else: + raise TypeError("Argument is not of MAPPING_RANGE type.") + + def get_range_preset(self, mapping_range): + if isinstance(mapping_range, MAPPING_RANGE): + return self.spatial.get( mapping_range.value) + else: + raise TypeError("Argument is not of MAPPING_RANGE type.") + + def get_resolution_preset(self, resolution): + if isinstance(resolution, MAPPING_RESOLUTION): + return self.spatial.get( resolution.value) + else: + raise TypeError("Argument is not of MAPPING_RESOLUTION type.") + + def get_recommended_range(self, resolution, Camera py_cam): + if isinstance(resolution, MAPPING_RESOLUTION): + return self.spatial.getRecommendedRange( resolution.value, py_cam.camera) + elif isinstance(resolution, float): + return self.spatial.getRecommendedRange( resolution, py_cam.camera) + else: + raise TypeError("Argument is not of MAPPING_RESOLUTION or float type.") + + @property + def max_memory_usage(self): + return self.spatial.max_memory_usage + + @max_memory_usage.setter + def max_memory_usage(self, int value): + self.spatial.max_memory_usage = value + + @property + def save_texture(self): + return self.spatial.save_texture + + @save_texture.setter + def save_texture(self, bool value): + self.spatial.save_texture = value + + @property + def use_chunk_only(self): + return self.spatial.use_chunk_only + + @use_chunk_only.setter + def use_chunk_only(self, bool value): + self.spatial.use_chunk_only = value + + @property + def reverse_vertex_order(self): + return self.spatial.reverse_vertex_order + + @reverse_vertex_order.setter + def reverse_vertex_order(self, bool value): + self.spatial.reverse_vertex_order = value + + @property + def allowed_range(self): + return self.spatial.allowed_range + + @property + def range_meter(self): + return self.spatial.range_meter + + @range_meter.setter + def range_meter(self, float value): + self.spatial.range_meter = value + + @property + def allowed_resolution(self): + return self.spatial.allowed_resolution + + @property + def resolution_meter(self): + return self.spatial.resolution_meter + + @resolution_meter.setter + def resolution_meter(self, float value): + self.spatial.resolution_meter = value + +cdef class Pose: + cdef c_Pose pose + def __cinit__(self): + self.pose = c_Pose() + + def init_pose(self, Pose pose): + self.pose = c_Pose(pose.pose) + + def init_transform(self, Transform pose_data, mtimestamp=0, mconfidence=0): + self.pose = c_Pose(pose_data.transform, mtimestamp, mconfidence) + + def get_translation(self, Translation py_translation): + py_translation.translation = self.pose.getTranslation() + return py_translation + + def get_orientation(self, Orientation py_orientation): + py_orientation.orientation = self.pose.getOrientation() + return py_orientation + + def get_rotation_matrix(self, Rotation py_rotation): + py_rotation.rotation = self.pose.getRotationMatrix() + py_rotation.mat = self.pose.getRotationMatrix() + return py_rotation + + def get_rotation_vector(self): + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = self.pose.getRotationVector()[i] + return arr + + def get_euler_angles(self, radian=True): + cdef np.ndarray arr = np.zeros(3) + if isinstance(radian, bool): + for i in range(3): + arr[i] = self.pose.getEulerAngles(radian)[i] + else: + raise TypeError("Argument is not of bool type.") + return arr + + @property + def valid(self): + return self.pose.valid + + @valid.setter + def valid(self, bool valid_): + self.pose.valid = valid_ + + @property + def timestamp(self): + return self.pose.timestamp + + @timestamp.setter + def timestamp(self, unsigned long long timestamp): + self.pose.timestamp = timestamp + + def pose_data(self, Transform pose_data): + pose_data.transform = self.pose.pose_data + pose_data.mat = self.pose.pose_data + return pose_data + + @property + def pose_confidence(self): + return self.pose.pose_confidence + + @pose_confidence.setter + def pose_confidence(self, int pose_confidence_): + self.pose.pose_confidence = pose_confidence_ + + @property + def pose_covariance(self): + cdef np.ndarray arr = np.zeros(36) + for i in range(36) : + arr[i] = self.pose.pose_covariance[i] + return arr + + @pose_covariance.setter + def pose_covariance(self, np.ndarray pose_covariance_): + for i in range(36) : + self.pose.pose_covariance[i] = pose_covariance_[i] + +cdef class IMUData: + cdef c_IMUData imuData + + def __cinit__(self): + self.imuData = c_IMUData() + + def init_imuData(self, IMUData imuData): + self.imuData = c_IMUData(imuData.imuData) + + def init_transform(self, Transform pose_data, mtimestamp=0, mconfidence=0): + self.imuData = c_IMUData(pose_data.transform, mtimestamp, mconfidence) + + def get_orientation_covariance(self, Matrix3f orientation_covariance): + orientation_covariance.mat = self.imuData.orientation_covariance + return orientation_covariance + + def get_angular_velocity(self, angular_velocity): + for i in range(3): + angular_velocity[i] = self.imuData.angular_velocity[i] + return angular_velocity + + def get_linear_acceleration(self, linear_acceleration): + for i in range(3): + linear_acceleration[i] = self.imuData.linear_acceleration[i] + return linear_acceleration + + def get_angular_velocity_convariance(self, Matrix3f angular_velocity_convariance): + angular_velocity_convariance.mat = self.imuData.angular_velocity_convariance + return angular_velocity_convariance + + def get_linear_acceleration_convariance(self, Matrix3f linear_acceleration_convariance): + linear_acceleration_convariance.mat = self.imuData.linear_acceleration_convariance + return linear_acceleration_convariance + + def get_translation(self, Translation py_translation): + py_translation.translation = self.imuData.getTranslation() + return py_translation + + def get_orientation(self, Orientation py_orientation): + py_orientation.orientation = self.imuData.getOrientation() + return py_orientation + + def get_rotation_matrix(self, Rotation py_rotation): + py_rotation.rotation = self.imuData.getRotationMatrix() + py_rotation.mat = self.imuData.getRotationMatrix() + return py_rotation + + def get_rotation_vector(self): + cdef np.ndarray arr = np.zeros(3) + for i in range(3): + arr[i] = self.imuData.getRotationVector()[i] + return arr + + def get_euler_angles(self, radian=True): + cdef np.ndarray arr = np.zeros(3) + if isinstance(radian, bool): + for i in range(3): + arr[i] = self.imuData.getEulerAngles(radian)[i] + else: + raise TypeError("Argument is not of bool type.") + return arr + + def pose_data(self, Transform pose_data): + pose_data.transform = self.imuData.pose_data + pose_data.mat = self.imuData.pose_data + return pose_data + + @property + def valid(self): + return self.imuData.valid + + @valid.setter + def valid(self, bool valid_): + self.imuData.valid = valid_ + + @property + def timestamp(self): + return self.imuData.timestamp + + @timestamp.setter + def timestamp(self, unsigned long long timestamp): + self.imuData.timestamp = timestamp + + @property + def pose_confidence(self): + return self.imuData.pose_confidence + + @pose_confidence.setter + def pose_confidence(self, int pose_confidence_): + self.imuData.pose_confidence = pose_confidence_ + + @property + def pose_covariance(self): + cdef np.ndarray arr = np.zeros(36) + for i in range(36) : + arr[i] = self.imuData.pose_covariance[i] + return arr + + @pose_covariance.setter + def pose_covariance(self, np.ndarray pose_covariance_): + for i in range(36) : + self.imuData.pose_covariance[i] = pose_covariance_[i] + + +cdef class Camera: + cdef c_Camera camera + def __cinit__(self): + self.camera = c_Camera() + + def close(self): + self.camera.close() + + def open(self, InitParameters py_init): + if py_init: + return ERROR_CODE(self.camera.open(deref(py_init.init))) + else: + print("InitParameters must be initialized first with InitParameters().") + + def is_opened(self): + return self.camera.isOpened() + + def grab(self, RuntimeParameters py_runtime): + if py_runtime: + return ERROR_CODE(self.camera.grab(deref(py_runtime.runtime))) + else: + print("RuntimeParameters must be initialized first with RuntimeParameters().") + + def retrieve_image(self, Mat py_mat, view=VIEW.VIEW_LEFT, type=MEM.MEM_CPU, width=0, + height=0): + if (isinstance(view, VIEW) and isinstance(type, MEM) and isinstance(width, int) and + isinstance(height, int)): + return ERROR_CODE(self.camera.retrieveImage(py_mat.mat, view.value, type.value, width, height)) + else: + raise TypeError("Arguments must be of VIEW, MEM and integer types.") + + def retrieve_measure(self, Mat py_mat, measure=MEASURE.MEASURE_DEPTH, type=MEM.MEM_CPU, + width=0, height=0): + if (isinstance(measure, MEASURE) and isinstance(type, MEM) and isinstance(width, int) and + isinstance(height, int)): + return ERROR_CODE(self.camera.retrieveMeasure(py_mat.mat, measure.value, type.value, width, height)) + else: + raise TypeError("Arguments must be of MEASURE, MEM and integer types.") + + def set_confidence_threshold(self, int conf_treshold_value): + self.camera.setConfidenceThreshold(conf_treshold_value) + + def get_confidence_threshold(self): + return self.camera.getConfidenceThreshold() + + def get_resolution(self): + return Resolution(self.camera.getResolution().width, self.camera.getResolution().height) + + def set_depth_max_range_value(self, float depth_max_range): + self.camera.setDepthMaxRangeValue(depth_max_range) + + def get_depth_max_range_value(self): + return self.camera.getDepthMaxRangeValue() + + def get_depth_min_range_value(self): + return self.camera.getDepthMinRangeValue() + + def set_svo_position(self, int frame_number): + self.camera.setSVOPosition(frame_number) + + def get_svo_position(self): + return self.camera.getSVOPosition() + + def get_svo_number_of_frames(self): + return self.camera.getSVONumberOfFrames() + + def set_camera_settings(self, settings, int value, use_default=False): + if isinstance(settings, CAMERA_SETTINGS) and isinstance(use_default, bool): + self.camera.setCameraSettings(settings.value, value, use_default) + else: + raise TypeError("Arguments must be of CAMERA_SETTINGS and boolean types.") + + def get_camera_settings(self, setting): + if isinstance(setting, CAMERA_SETTINGS): + return self.camera.getCameraSettings(setting.value) + else: + raise TypeError("Argument is not of CAMERA_SETTINGS type.") + + def get_camera_fps(self): + return self.camera.getCameraFPS() + + def set_camera_fps(self, int desired_fps): + self.camera.setCameraFPS(desired_fps) + + def get_current_fps(self): + return self.camera.getCurrentFPS() + + def get_camera_timestamp(self): + return self.camera.getCameraTimestamp() + + def get_current_timestamp(self): + return self.camera.getCurrentTimestamp() + + def get_timestamp(self, time_reference): + if isinstance(time_reference, TIME_REFERENCE): + return self.camera.getTimestamp(time_reference.value) + else: + raise TypeError("Argument is not of TIME_REFERENCE type.") + + def get_frame_dropped_count(self): + return self.camera.getFrameDroppedCount() + + def get_camera_information(self, resizer = Resolution(0, 0)): + return CameraInformation(self, resizer) + + def get_self_calibration_state(self): + return SELF_CALIBRATION_STATE(self.camera.getSelfCalibrationState()) + + def reset_self_calibration(self): + self.camera.resetSelfCalibration() + + def enable_tracking(self, TrackingParameters py_tracking): + if py_tracking: + return ERROR_CODE(self.camera.enableTracking(deref(py_tracking.tracking))) + else: + print("TrackingParameters must be initialized first with TrackingParameters().") + + def get_imu_data(self, IMUData py_imu_data, time_reference = TIME_REFERENCE.TIME_REFERENCE_CURRENT): + if isinstance(time_reference, TIME_REFERENCE): + return ERROR_CODE(self.camera.getIMUData(py_imu_data.imuData, time_reference.value)) + else: + raise TypeError("Argument is not of TIME_REFERENCE type.") + + def set_imu_prior(self, Transform transfom): + return ERROR_CODE(self.camera.setIMUPrior(transfom.transform)) + + def get_position(self, Pose py_pose, reference_frame = REFERENCE_FRAME.REFERENCE_FRAME_WORLD): + if isinstance(reference_frame, REFERENCE_FRAME): + return TRACKING_STATE(self.camera.getPosition(py_pose.pose, reference_frame.value)) + else: + raise TypeError("Argument is not of REFERENCE_FRAME type.") + + def get_area_export_state(self): + return AREA_EXPORT_STATE(self.camera.getAreaExportState()) + + def save_current_area(self, str area_file_path): + filename = area_file_path.encode() + return ERROR_CODE(self.camera.saveCurrentArea(String( filename))) + + def disable_tracking(self, str area_file_path=""): + filename = area_file_path.encode() + self.camera.disableTracking(String( filename)) + + def reset_tracking(self, Transform path): + return ERROR_CODE(self.camera.resetTracking(path.transform)) + + def enable_spatial_mapping(self, SpatialMappingParameters py_spatial): + if py_spatial: + return ERROR_CODE(self.camera.enableSpatialMapping(deref(py_spatial.spatial))) + else: + print("SpatialMappingParameters must be initialized first with SpatialMappingParameters()") + + def pause_spatial_mapping(self, status): + if isinstance(status, bool): + self.camera.pauseSpatialMapping(status) + else: + raise TypeError("Argument is not of boolean type.") + + def get_spatial_mapping_state(self): + return SPATIAL_MAPPING_STATE(self.camera.getSpatialMappingState()) + + def extract_whole_mesh(self, Mesh py_mesh): + return ERROR_CODE(self.camera.extractWholeMesh(deref(py_mesh.mesh))) + + def request_mesh_async(self): + self.camera.requestMeshAsync() + + def get_mesh_request_status_async(self): + return ERROR_CODE(self.camera.getMeshRequestStatusAsync()) + + def retrieve_mesh_async(self, Mesh py_mesh): + return ERROR_CODE(self.camera.retrieveMeshAsync(deref(py_mesh.mesh))) + + def find_plane_at_hit(self, coord, Plane py_plane): + cdef Vector2[uint] vec = Vector2[uint](coord[0], coord[1]) + return ERROR_CODE(self.camera.findPlaneAtHit(vec, py_plane.plane)) + + def find_floor_plane(self, Plane py_plane, Transform resetTrackingFloorFrame, floor_height_prior = float('nan'), Rotation world_orientation_prior = Rotation(Matrix3f().zeros()), floor_height_prior_tolerance = float('nan')) : + return ERROR_CODE(self.camera.findFloorPlane(py_plane.plane, resetTrackingFloorFrame.transform, floor_height_prior, world_orientation_prior.rotation, floor_height_prior_tolerance)) + + def disable_spatial_mapping(self): + self.camera.disableSpatialMapping() + + def enable_recording(self, str video_filename, + compression_mode=SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_LOSSLESS): + if isinstance(compression_mode, SVO_COMPRESSION_MODE): + filename = video_filename.encode() + return ERROR_CODE(self.camera.enableRecording(String( filename), + compression_mode.value)) + else: + raise TypeError("Argument is not of SVO_COMPRESSION_MODE type.") + + def record(self): + return self.camera.record() + + def disable_recording(self): + self.camera.disableRecording() + + def get_sdk_version(cls): + return cls.camera.getSDKVersion().get().decode() + + def is_zed_connected(cls): + return cls.camera.isZEDconnected() + + def stickto_cpu_core(cls, int cpu_core): + return ERROR_CODE(cls.camera.sticktoCPUCore(cpu_core)) + + def get_device_list(cls): + vect_ = cls.camera.getDeviceList() + vect_python = [] + for i in range(vect_.size()): + prop = DeviceProperties() + prop.camera_state = vect_[i].camera_state + prop.id = vect_[i].id + prop.path = vect_[i].path.get().decode() + prop.camera_model = vect_[i].camera_model + prop.serial_number = vect_[i].serial_number + vect_python.append(prop) + return vect_python + +def save_camera_depth_as(Camera zed, format, str name, factor=1): + if isinstance(format, DEPTH_FORMAT) and factor <= 65536: + name_save = name.encode() + return saveDepthAs(zed.camera, format.value, String(name_save), factor) + else: + raise TypeError("Arguments must be of DEPTH_FORMAT type and factor not over 65536.") + +def save_camera_point_cloud_as(Camera zed, format, str name, with_color=False): + if isinstance(format, POINT_CLOUD_FORMAT): + name_save = name.encode() + return savePointCloudAs(zed.camera, format.value, String(name_save), + with_color) + else: + raise TypeError("Argument is not of POINT_CLOUD_FORMAT type.") + +def save_mat_depth_as(Mat py_mat, format, str name, factor=1): + if isinstance(format, DEPTH_FORMAT) and factor <= 65536: + name_save = name.encode() + return saveMatDepthAs(py_mat.mat, format.value, String(name_save), factor) + else: + raise TypeError("Arguments must be of DEPTH_FORMAT type and factor not over 65536.") + + +def save_mat_point_cloud_as(Mat py_mat, format, str name, with_color=False): + if isinstance(format, POINT_CLOUD_FORMAT): + name_save = name.encode() + return saveMatPointCloudAs(py_mat.mat, format.value, String(name_save), + with_color) + else: + raise TypeError("Argument is not of POINT_CLOUD_FORMAT type.") diff --git a/pyzed/sl_c.pxd b/pyzed/sl_c.pxd new file mode 100644 index 0000000..8b8af2d --- /dev/null +++ b/pyzed/sl_c.pxd @@ -0,0 +1,1015 @@ +######################################################################## +# +# Copyright (c) 2017, STEREOLABS. +# +# All rights reserved. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +######################################################################## + +# File containing the Cython declarations to use the sl functions. + +from libcpp.string cimport string +from libcpp cimport bool +from libcpp.pair cimport pair +from libcpp.vector cimport vector +from libc.string cimport const_char + +cdef extern from "Utils.cpp" namespace "sl": + string to_str(String sl_str) + +cdef extern from "sl/types.hpp" namespace "sl": + + ctypedef unsigned long long timeStamp + + ctypedef enum ERROR_CODE: + SUCCESS, + ERROR_CODE_FAILURE, + ERROR_CODE_NO_GPU_COMPATIBLE, + ERROR_CODE_NOT_ENOUGH_GPUMEM, + ERROR_CODE_CAMERA_NOT_DETECTED, + ERROR_CODE_SENSOR_NOT_DETECTED, + ERROR_CODE_INVALID_RESOLUTION, + ERROR_CODE_LOW_USB_BANDWIDTH, + ERROR_CODE_CALIBRATION_FILE_NOT_AVAILABLE, + ERROR_CODE_INVALID_CALIBRATION_FILE, + ERROR_CODE_INVALID_SVO_FILE, + ERROR_CODE_SVO_RECORDING_ERROR, + ERROR_CODE_SVO_UNSUPPORTED_COMPRESSION, + ERROR_CODE_INVALID_COORDINATE_SYSTEM, + ERROR_CODE_INVALID_FIRMWARE, + ERROR_CODE_INVALID_FUNCTION_PARAMETERS, + ERROR_CODE_NOT_A_NEW_FRAME, + ERROR_CODE_CUDA_ERROR, + ERROR_CODE_CAMERA_NOT_INITIALIZED, + ERROR_CODE_NVIDIA_DRIVER_OUT_OF_DATE, + ERROR_CODE_INVALID_FUNCTION_CALL, + ERROR_CODE_CORRUPTED_SDK_INSTALLATION, + ERROR_CODE_INCOMPATIBLE_SDK_VERSION, + ERROR_CODE_INVALID_AREA_FILE, + ERROR_CODE_INCOMPATIBLE_AREA_FILE, + ERROR_CODE_CAMERA_FAILED_TO_SETUP, + ERROR_CODE_CAMERA_DETECTION_ISSUE, + ERROR_CODE_CAMERA_ALREADY_IN_USE, + ERROR_CODE_NO_GPU_DETECTED, + ERROR_CODE_PLANE_NOT_FOUND, + ERROR_CODE_LAST + + String toString(ERROR_CODE o) + + void sleep_ms(int time) + + ctypedef enum MODEL: + MODEL_ZED, + MODEL_ZED_M, + MODEL_LAST + + String model2str(MODEL model) + String toString(MODEL o) + + ctypedef enum CAMERA_STATE: + CAMERA_STATE_AVAILABLE, + CAMERA_STATE_NOT_AVAILABLE, + CAMERA_STATE_LAST + + String toString(CAMERA_STATE o) + + cdef cppclass String 'sl::String': + String() + String(const char *data) + void set(const char *data) + const char *get() const + bool empty() const + string std_str() const + + cdef cppclass DeviceProperties: + DeviceProperties() + CAMERA_STATE camera_state + int id + String path + MODEL camera_model + unsigned int serial_number + + String toString(DeviceProperties o) + + cdef cppclass Vector2[T]: + int size() + Vector2() + Vector2(const T v0, const T v1) + T *ptr() + T &operator[](int i) + + + cdef cppclass Vector3[T]: + int size() + Vector3() + Vector3(const T v0, const T v1, const T v2) + T *ptr() + T &operator[](int i) + + + cdef cppclass Vector4[T]: + int size() + Vector4() + Vector4(const T v0, const T v1, const T v2, const T v3) + T *ptr() + T &operator[](int i) + + + cdef cppclass Matrix3f 'sl::Matrix3f': + int nbElem + float r00 + float r01 + float r02 + float r10 + float r11 + float r12 + float r20 + float r21 + float r22 + float r[] + Matrix3f() + Matrix3f(float data[]) + Matrix3f(const Matrix3f &mat) + Matrix3f operator*(const Matrix3f &mat) const + Matrix3f operator*(const double &scalar) const + bool operator==(const Matrix3f &mat) const + bool operator!=(const Matrix3f &mat) const + void inverse() + + @staticmethod + Matrix3f inverse(const Matrix3f &rotation) + + void transpose() + + @staticmethod + Matrix3f transpose(const Matrix3f &rotation) + + void setIdentity() + + @staticmethod + Matrix3f identity() + + void setZeros() + + @staticmethod + Matrix3f zeros() + + String getInfos() + String matrix_name + + + cdef cppclass Matrix4f 'sl::Matrix4f': + int nbElem + float r00 + float r01 + float r02 + float tx + float r10 + float r11 + float r12 + float ty + float r20 + float r21 + float r22 + float tz + float m30 + float m31 + float m32 + float m33 + + float m[] + Matrix4f() + Matrix4f(float data[]) + Matrix4f(const Matrix4f &mat) + Matrix4f operator*(const Matrix4f &mat) const + Matrix4f operator*(const double &scalar) const + bool operator==(const Matrix4f &mat) const + bool operator!=(const Matrix4f &mat) const + ERROR_CODE inverse() + + @staticmethod + Matrix4f inverse(const Matrix4f &mat) + + void transpose() + + @staticmethod + Matrix4f transpose(const Matrix4f &mat) + + void setIdentity() + + @staticmethod + Matrix4f identity() + + void setZeros() + + @staticmethod + Matrix4f zeros() + + ERROR_CODE setSubMatrix3f(Matrix3f input, int row, int column) + ERROR_CODE setSubVector3f(Vector3[float] input, int column) + ERROR_CODE setSubVector4f(Vector4[float] input, int column) + + + String getInfos() + String matrix_name + + ctypedef enum UNIT: + UNIT_MILLIMETER + UNIT_CENTIMETER + UNIT_METER + UNIT_INCH + UNIT_FOOT + UNIT_LAST + + String toString(UNIT o) + + ctypedef enum COORDINATE_SYSTEM: + COORDINATE_SYSTEM_IMAGE + COORDINATE_SYSTEM_LEFT_HANDED_Y_UP + COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP + COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP + COORDINATE_SYSTEM_LEFT_HANDED_Z_UP + COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD + COORDINATE_SYSTEM_LAST + + String toString(COORDINATE_SYSTEM o) + + +cdef extern from "sl/defines.hpp" namespace "sl": + + ctypedef enum RESOLUTION: + RESOLUTION_HD2K + RESOLUTION_HD1080 + RESOLUTION_HD720 + RESOLUTION_VGA + RESOLUTION_LAST + + String toString(RESOLUTION o) + + ctypedef enum CAMERA_SETTINGS: + CAMERA_SETTINGS_BRIGHTNESS + CAMERA_SETTINGS_CONTRAST + CAMERA_SETTINGS_HUE + CAMERA_SETTINGS_SATURATION + CAMERA_SETTINGS_GAIN + CAMERA_SETTINGS_EXPOSURE + CAMERA_SETTINGS_WHITEBALANCE + CAMERA_SETTINGS_AUTO_WHITEBALANCE + CAMERA_SETTINGS_LAST + + String toString(CAMERA_SETTINGS o) + + ctypedef enum SELF_CALIBRATION_STATE: + SELF_CALIBRATION_STATE_NOT_STARTED + SELF_CALIBRATION_STATE_RUNNING + SELF_CALIBRATION_STATE_FAILED + SELF_CALIBRATION_STATE_SUCCESS + SELF_CALIBRATION_STATE_LAST + + String toString(SELF_CALIBRATION_STATE o) + + ctypedef enum DEPTH_MODE: + DEPTH_MODE_NONE + DEPTH_MODE_PERFORMANCE + DEPTH_MODE_MEDIUM + DEPTH_MODE_QUALITY + DEPTH_MODE_ULTRA + DEPTH_MODE_LAST + + String toString(DEPTH_MODE o) + + ctypedef enum SENSING_MODE: + SENSING_MODE_STANDARD + SENSING_MODE_FILL + SENSING_MODE_LAST + + String toString(SENSING_MODE o) + + ctypedef enum MEASURE: + MEASURE_DISPARITY + MEASURE_DEPTH + MEASURE_CONFIDENCE + MEASURE_XYZ + MEASURE_XYZRGBA + MEASURE_XYZBGRA + MEASURE_XYZARGB + MEASURE_XYZABGR + MEASURE_NORMALS + MEASURE_DISPARITY_RIGHT + MEASURE_DEPTH_RIGHT + MEASURE_XYZ_RIGHT + MEASURE_XYZRGBA_RIGHT + MEASURE_XYZBGRA_RIGHT + MEASURE_XYZARGB_RIGHT + MEASURE_XYZABGR_RIGHT + MEASURE_NORMALS_RIGHT + MEASURE_LAST + + String toString(MEASURE o) + + ctypedef enum VIEW: + VIEW_LEFT + VIEW_RIGHT + VIEW_LEFT_GRAY + VIEW_RIGHT_GRAY + VIEW_LEFT_UNRECTIFIED + VIEW_RIGHT_UNRECTIFIED + VIEW_LEFT_UNRECTIFIED_GRAY + VIEW_RIGHT_UNRECTIFIED_GRAY + VIEW_SIDE_BY_SIDE + VIEW_DEPTH + VIEW_CONFIDENCE + VIEW_NORMALS + VIEW_DEPTH_RIGHT + VIEW_NORMALS_RIGHT + VIEW_LAST + + String toString(VIEW o) + + ctypedef enum TIME_REFERENCE: + TIME_REFERENCE_IMAGE + TIME_REFERENCE_CURRENT + TIME_REFERENCE_LAST + + String toString(TIME_REFERENCE o) + + ctypedef enum DEPTH_FORMAT: + DEPTH_FORMAT_PNG + DEPTH_FORMAT_PFM + DEPTH_FORMAT_PGM + DEPTH_FORMAT_LAST + + String toString(DEPTH_FORMAT o) + + ctypedef enum POINT_CLOUD_FORMAT: + POINT_CLOUD_FORMAT_XYZ_ASCII + POINT_CLOUD_FORMAT_PCD_ASCII + POINT_CLOUD_FORMAT_PLY_ASCII + POINT_CLOUD_FORMAT_VTK_ASCII + POINT_CLOUD_FORMAT_LAST + + String toString(POINT_CLOUD_FORMAT o) + + ctypedef enum TRACKING_STATE: + TRACKING_STATE_SEARCHING + TRACKING_STATE_OK + TRACKING_STATE_OFF + TRACKING_STATE_FPS_TOO_LOW + TRACKING_STATE_LAST + + String toString(TRACKING_STATE o) + + ctypedef enum AREA_EXPORT_STATE: + AREA_EXPORT_STATE_SUCCESS + AREA_EXPORT_STATE_RUNNING + AREA_EXPORT_STATE_NOT_STARTED + AREA_EXPORT_STATE_FILE_EMPTY + AREA_EXPORT_STATE_FILE_ERROR + AREA_EXPORT_STATE_SPATIAL_MEMORY_DISABLED + AREA_EXPORT_STATE_LAST + + String toString(AREA_EXPORT_STATE o) + + ctypedef enum REFERENCE_FRAME: + REFERENCE_FRAME_WORLD + REFERENCE_FRAME_CAMERA + REFERENCE_FRAME_LAST + + String toString(REFERENCE_FRAME o) + + ctypedef enum SPATIAL_MAPPING_STATE: + SPATIAL_MAPPING_STATE_INITIALIZING + SPATIAL_MAPPING_STATE_OK + SPATIAL_MAPPING_STATE_NOT_ENOUGH_MEMORY + SPATIAL_MAPPING_STATE_NOT_ENABLED + SPATIAL_MAPPING_STATE_FPS_TOO_LOW + SPATIAL_MAPPING_STATE_LAST + + String toString(SPATIAL_MAPPING_STATE o) + + ctypedef enum SVO_COMPRESSION_MODE: + SVO_COMPRESSION_MODE_RAW + SVO_COMPRESSION_MODE_LOSSLESS + SVO_COMPRESSION_MODE_LOSSY + SVO_COMPRESSION_MODE_AVCHD + SVO_COMPRESSION_MODE_HEVC + SVO_COMPRESSION_MODE_LAST + + String toString(SVO_COMPRESSION_MODE o) + + cdef struct RecordingState: + bool status + double current_compression_time + double current_compression_ratio + double average_compression_time + double average_compression_ratio + + + @staticmethod + cdef vector[pair[int, int]] cameraResolution + + @staticmethod + cdef const_char* resolution2str(RESOLUTION res) + + @staticmethod + cdef const_char* statusCode2str(SELF_CALIBRATION_STATE state) + @staticmethod + cdef DEPTH_MODE str2mode(const_char* mode) + + @staticmethod + cdef const_char* depthMode2str(DEPTH_MODE mode) + + @staticmethod + cdef const_char* sensingMode2str(SENSING_MODE mode) + + @staticmethod + cdef const_char* unit2str(UNIT unit) + + @staticmethod + cdef UNIT str2unit(const_char* unit) + + @staticmethod + cdef const_char* trackingState2str(TRACKING_STATE state) + + @staticmethod + cdef const_char* spatialMappingState2str(SPATIAL_MAPPING_STATE state) + + +cdef extern from "sl/Core.hpp" namespace "sl": + + timeStamp getCurrentTimeStamp() + + cdef struct Resolution: + size_t width + size_t height + + + cdef struct CameraParameters: + float fx + float fy + float cx + float cy + double disto[5] + float v_fov + float h_fov + float d_fov + Resolution image_size + + void SetUp(float focal_x, float focal_y, float center_x, float center_y) + + + cdef struct CalibrationParameters: + Vector3[float] R + Vector3[float] T + CameraParameters left_cam + CameraParameters right_cam + + + cdef struct CameraInformation: + CalibrationParameters calibration_parameters + CalibrationParameters calibration_parameters_raw + Transform camera_imu_transform + unsigned int serial_number + unsigned int firmware_version + MODEL camera_model + + cdef enum MEM: + MEM_CPU + MEM_GPU + + MEM operator|(MEM a, MEM b) + + + cdef enum COPY_TYPE: + COPY_TYPE_CPU_CPU + COPY_TYPE_CPU_GPU + COPY_TYPE_GPU_GPU + COPY_TYPE_GPU_CPU + + + cdef enum MAT_TYPE: + MAT_TYPE_32F_C1 + MAT_TYPE_32F_C2 + MAT_TYPE_32F_C3 + MAT_TYPE_32F_C4 + MAT_TYPE_8U_C1 + MAT_TYPE_8U_C2 + MAT_TYPE_8U_C3 + MAT_TYPE_8U_C4 + + + cdef cppclass Mat 'sl::Mat': + String name + bool verbose + + Mat() + Mat(size_t width, size_t height, MAT_TYPE mat_type, MEM memory_type) + Mat(size_t width, size_t height, MAT_TYPE mat_type, unsigned char *ptr, size_t step, MEM memory_type) + Mat(size_t width, size_t height, MAT_TYPE mat_type, unsigned char *ptr_cpu, size_t step_cpu, + unsigned char *ptr_gpu, size_t step_gpu) + Mat(Resolution resolution, MAT_TYPE mat_type, MEM memory_type) + Mat(Resolution resolution, MAT_TYPE mat_type, unsigned char *ptr, size_t step, MEM memory_type) + Mat(const Mat &mat) + + void alloc(size_t width, size_t height, MAT_TYPE mat_type, MEM memory_type) + void alloc(Resolution resolution, MAT_TYPE mat_type, MEM memory_type) + void free(MEM memory_type) + Mat &operator=(const Mat &that) + ERROR_CODE updateCPUfromGPU() + ERROR_CODE updateGPUfromCPU() + ERROR_CODE copyTo(Mat &dst, COPY_TYPE cpyType) const + ERROR_CODE setFrom(const Mat &src, COPY_TYPE cpyType) const + ERROR_CODE read(const char* filePath) + ERROR_CODE write(const char* filePath) + size_t getWidth() const + size_t getHeight() const + Resolution getResolution() const + size_t getChannels() const + MAT_TYPE getDataType() const + MEM getMemoryType() const + size_t getStepBytes(MEM memory_type) + size_t getStep(MEM memory_type) + size_t getPixelBytes() + size_t getWidthBytes() + String getInfos() + bool isInit() + bool isMemoryOwner() + ERROR_CODE clone(const Mat &src) + ERROR_CODE move(Mat &dst) + + @staticmethod + void swap(Mat &mat1, Mat &mat2) + + cdef cppclass Rotation(Matrix3f): + Rotation() + Rotation(const Rotation &rotation) + Rotation(const Matrix3f &mat) + Rotation(const Orientation &orientation) + Rotation(const float angle, const Translation &axis) + void setOrientation(const Orientation &orientation) + Orientation getOrientation() const + Vector3[float] getRotationVector() + void setRotationVector(const Vector3[float] &vec_rot) + Vector3[float] getEulerAngles(bool radian) const + void setEulerAngles(const Vector3[float] &euler_angles, bool radian) + + + cdef cppclass Translation(Vector3): + Translation() + Translation(const Translation &translation) + Translation(float t1, float t2, float t3) + Translation operator*(const Orientation &mat) const + void normalize() + + @staticmethod + Translation normalize(const Translation &tr) + float &operator()(int x) + + + cdef cppclass Orientation(Vector4): + Orientation() + Orientation(const Orientation &orientation) + Orientation(const Vector4[float] &input) + Orientation(const Rotation &rotation) + Orientation(const Translation &tr1, const Translation &tr2) + float operator()(int x) + Orientation operator*(const Orientation &orientation) const + void setRotationMatrix(const Rotation &rotation) + Rotation getRotationMatrix() const + void setIdentity() + @staticmethod + Orientation identity() + void setZeros() + @staticmethod + Orientation zeros() + void normalise() + + @staticmethod + Orientation normalise(const Orientation &orient) + + + cdef cppclass Transform(Matrix4f): + Transform() + Transform(const Transform &motion) + Transform(const Matrix4f &mat) + Transform(const Rotation &rotation, const Translation &translation) + Transform(const Orientation &orientation, const Translation &translation) + void setRotationMatrix(const Rotation &rotation) + Rotation getRotationMatrix() const + void setTranslation(const Translation &translation) + Translation getTranslation() const + void setOrientation(const Orientation &orientation) + Orientation getOrientation() const + Vector3[float] getRotationVector() + void setRotationVector(const Vector3[float] &vec_rot) + Vector3[float] getEulerAngles(bool radian) const + void setEulerAngles(const Vector3[float] &euler_angles, bool radian) + +ctypedef unsigned char uchar1 +ctypedef Vector2[unsigned char] uchar2 +ctypedef Vector3[unsigned char] uchar3 +ctypedef Vector4[unsigned char] uchar4 + +ctypedef float float1 +ctypedef Vector2[float] float2 +ctypedef Vector3[float] float3 +ctypedef Vector4[float] float4 + +cdef extern from "Utils.cpp" namespace "sl": + + Mat matResolution(Resolution resolution, MAT_TYPE mat_type, uchar1 *ptr_cpu, size_t step_cpu, + uchar1 *ptr_gpu, size_t step_gpu) + + ERROR_CODE setToUchar1(Mat &mat, uchar1 value, MEM memory_type) + ERROR_CODE setToUchar2(Mat &mat, uchar2 value, MEM memory_type) + ERROR_CODE setToUchar3(Mat &mat, uchar3 value, MEM memory_type) + ERROR_CODE setToUchar4(Mat &mat, uchar4 value, MEM memory_type) + + ERROR_CODE setToFloat1(Mat &mat, float1 value, MEM memory_type) + ERROR_CODE setToFloat2(Mat &mat, float2 value, MEM memory_type) + ERROR_CODE setToFloat3(Mat &mat, float3 value, MEM memory_type) + ERROR_CODE setToFloat4(Mat &mat, float4 value, MEM memory_type) + + ERROR_CODE setValueUchar1(Mat &mat, size_t x, size_t y, uchar1 value, MEM memory_type) + ERROR_CODE setValueUchar2(Mat &mat, size_t x, size_t y, uchar2 value, MEM memory_type) + ERROR_CODE setValueUchar3(Mat &mat, size_t x, size_t y, uchar3 value, MEM memory_type) + ERROR_CODE setValueUchar4(Mat &mat, size_t x, size_t y, uchar4 value, MEM memory_type) + + ERROR_CODE setValueFloat1(Mat &mat, size_t x, size_t y, float1 value, MEM memory_type) + ERROR_CODE setValueFloat2(Mat &mat, size_t x, size_t y, float2 value, MEM memory_type) + ERROR_CODE setValueFloat3(Mat &mat, size_t x, size_t y, float3 value, MEM memory_type) + ERROR_CODE setValueFloat4(Mat &mat, size_t x, size_t y, float4 value, MEM memory_type) + + ERROR_CODE getValueUchar1(Mat &mat, size_t x, size_t y, uchar1 *value, MEM memory_type) + ERROR_CODE getValueUchar2(Mat &mat, size_t x, size_t y, Vector2[uchar1] *value, MEM memory_type) + ERROR_CODE getValueUchar3(Mat &mat, size_t x, size_t y, Vector3[uchar1] *value, MEM memory_type) + ERROR_CODE getValueUchar4(Mat &mat, size_t x, size_t y, Vector4[uchar1] *value, MEM memory_type) + + ERROR_CODE getValueFloat1(Mat &mat, size_t x, size_t y, float1 *value, MEM memory_type) + ERROR_CODE getValueFloat2(Mat &mat, size_t x, size_t y, Vector2[float1] *value, MEM memory_type) + ERROR_CODE getValueFloat3(Mat &mat, size_t x, size_t y, Vector3[float1] *value, MEM memory_type) + ERROR_CODE getValueFloat4(Mat &mat, size_t x, size_t y, Vector4[float1] *value, MEM memory_type) + + uchar1 *getPointerUchar1(Mat &mat, MEM memory_type) + uchar2 *getPointerUchar2(Mat &mat, MEM memory_type) + uchar3 *getPointerUchar3(Mat &mat, MEM memory_type) + uchar4 *getPointerUchar4(Mat &mat, MEM memory_type) + + float1 *getPointerFloat1(Mat &mat, MEM memory_type) + float2 *getPointerFloat2(Mat &mat, MEM memory_type) + float3 *getPointerFloat3(Mat &mat, MEM memory_type) + float4 *getPointerFloat4(Mat &mat, MEM memory_type) + + +ctypedef unsigned int uint + +cdef extern from "sl/Mesh.hpp" namespace "sl": + + ctypedef enum MESH_FILE_FORMAT: + MESH_FILE_PLY + MESH_FILE_PLY_BIN + MESH_FILE_OBJ + MESH_FILE_LAST + + + ctypedef enum MESH_TEXTURE_FORMAT: + MESH_TEXTURE_RGB + MESH_TEXTURE_RGBA + MESH_TEXTURE_LAST + + + ctypedef enum MESH_FILTER 'sl::MeshFilterParameters::MESH_FILTER': + MESH_FILTER_LOW 'sl::MeshFilterParameters::MESH_FILTER::MESH_FILTER_LOW' + MESH_FILTER_MEDIUM 'sl::MeshFilterParameters::MESH_FILTER::MESH_FILTER_MEDIUM' + MESH_FILTER_HIGH 'sl::MeshFilterParameters::MESH_FILTER::MESH_FILTER_HIGH' + + ctypedef enum PLANE_TYPE: + PLANE_TYPE_HORIZONTAL + PLANE_TYPE_VERTICAL + PLANE_TYPE_UNKNOWN + PLANE_TYPE_LAST + + cdef cppclass MeshFilterParameters 'sl::MeshFilterParameters': + MeshFilterParameters(MESH_FILTER filtering_) + void set(MESH_FILTER filtering_) + bool save(String filename) + bool load(String filename) + + cdef cppclass Texture 'sl::Texture': + Texture() + String name + Mat data + unsigned int indice_gl + void clear() + + cdef cppclass Chunk 'sl::Chunk': + Chunk() + vector[Vector3[float]] vertices + vector[Vector3[uint]] triangles + vector[Vector3[float]] normals + vector[Vector2[float]] uv + unsigned long long timestamp + Vector3[float] barycenter + bool has_been_updated + void clear() + + cdef cppclass Mesh 'sl::Mesh': + ctypedef vector[size_t] chunkList + Mesh() + vector[Chunk] chunks + Chunk &operator[](int index) + vector[Vector3[float]] vertices + vector[Vector3[uint]] triangles + vector[Vector3[float]] normals + vector[Vector2[float]] uv + Texture texture + size_t getNumberOfTriangles() + void mergeChunks(int faces_per_chunk) + Vector3[float] getGravityEstimate() + chunkList getVisibleList(Transform camera_pose) + chunkList getSurroundingList(Transform camera_pose, float radius) + void updateMeshFromChunkList(chunkList IDs) + bool filter(MeshFilterParameters params, bool updateMesh) + bool applyTexture(MESH_TEXTURE_FORMAT texture_format) + bool save(String filename, MESH_FILE_FORMAT type, chunkList IDs) + bool load(const String filename, bool updateMesh) + void clear() + + cdef cppclass Plane 'sl::Plane': + Plane() + PLANE_TYPE type + Vector3[float] getNormal() + Vector3[float] getCenter() + Transform getPose() + Vector2[float] getExtents() + Vector4[float] getPlaneEquation() + vector[Vector3[float]] getBounds() + Mesh extractMesh() + float getClosestDistance(Vector3[float] point) + + + +cdef extern from 'cuda.h' : + cdef struct CUctx_st : + pass + ctypedef CUctx_st* CUcontext + +cdef extern from 'sl/Camera.hpp' namespace 'sl': + + ctypedef enum MAPPING_RESOLUTION 'sl::SpatialMappingParameters::MAPPING_RESOLUTION': + MAPPING_RESOLUTION_HIGH 'sl::SpatialMappingParameters::MAPPING_RESOLUTION::MAPPING_RESOLUTION_HIGH' + MAPPING_RESOLUTION_MEDIUM 'sl::SpatialMappingParameters::MAPPING_RESOLUTION::MAPPING_RESOLUTION_MEDIUM' + MAPPING_RESOLUTION_LOW 'sl::SpatialMappingParameters::MAPPING_RESOLUTION::MAPPING_RESOLUTION_LOW' + + + ctypedef enum MAPPING_RANGE 'sl::SpatialMappingParameters::MAPPING_RANGE': + MAPPING_RANGE_NEAR 'sl::SpatialMappingParameters::MAPPING_RANGE::MAPPING_RANGE_NEAR' + MAPPING_RANGE_MEDIUM 'sl::SpatialMappingParameters::MAPPING_RANGE::MAPPING_RANGE_MEDIUM' + MAPPING_RANGE_FAR 'sl::SpatialMappingParameters::MAPPING_RANGE::MAPPING_RANGE_FAR' + + cdef cppclass InputType 'sl::InputType': + InputType() + InputType(InputType &type) + + void setFromCameraID(unsigned int id) + void setFromSerialNumber(unsigned int serial_number) + void setFromSVOFile(String svo_input_filename) + + cdef cppclass InitParameters 'sl::InitParameters': + RESOLUTION camera_resolution + int camera_fps + int camera_linux_id + String svo_input_filename + bool svo_real_time_mode + UNIT coordinate_units + COORDINATE_SYSTEM coordinate_system + DEPTH_MODE depth_mode + float depth_minimum_distance + int camera_image_flip + bool enable_right_side_measure + bool camera_disable_self_calib + int camera_buffer_count_linux + bool sdk_verbose + int sdk_gpu_id + bool depth_stabilization + + String sdk_verbose_log_file + + CUcontext sdk_cuda_ctx + InputType input + String optional_settings_path + + InitParameters(RESOLUTION camera_resolution, + int camera_fps, + int camera_linux_id, + String svo_input_filename, + bool svo_real_time_mode, + DEPTH_MODE depth_mode, + UNIT coordinate_units, + COORDINATE_SYSTEM coordinate_system, + bool sdk_verbose, + int sdk_gpu_id, + float depth_minimum_distance, + bool camera_disable_self_calib, + bool camera_image_flip, + bool enable_right_side_measure, + int camera_buffer_count_linux, + String sdk_verbose_log_file, + bool depth_stabilization, + CUcontext sdk_cuda_ctx, + InputType input, + String optional_settings_path) + + bool save(String filename) + bool load(String filename) + + + cdef cppclass RuntimeParameters 'sl::RuntimeParameters': + SENSING_MODE sensing_mode + bool enable_depth + bool enable_point_cloud + REFERENCE_FRAME measure3D_reference_frame + + RuntimeParameters(SENSING_MODE sensing_mode, + bool enable_depth, + bool enable_point_cloud, + REFERENCE_FRAME measure3D_reference_frame) + + bool save(String filename) + bool load(String filename) + + + cdef cppclass TrackingParameters 'sl::TrackingParameters': + Transform initial_world_transform + bool enable_spatial_memory + bool enable_pose_smoothing + bool set_floor_as_origin + String area_file_path + bool enable_imu_fusion + + TrackingParameters(Transform init_pos, + bool _enable_memory, + String _area_path) + + bool save(String filename) + bool load(String filename) + + cdef cppclass SpatialMappingParameters 'sl::SpatialMappingParameters': + ctypedef pair[float, float] interval + + SpatialMappingParameters(MAPPING_RESOLUTION resolution, + MAPPING_RANGE range, + int max_memory_usage_, + bool save_texture_, + bool use_chunk_only_, + bool reverse_vertex_order_) + + @staticmethod + float get(MAPPING_RESOLUTION resolution) + + void set(MAPPING_RESOLUTION resolution) + + @staticmethod + float get(MAPPING_RANGE range) + + @staticmethod + float getRecommendedRange(MAPPING_RESOLUTION mapping_resolution, Camera &camera) + + @staticmethod + float getRecommendedRange(float resolution_meters, Camera &camera) + + void set(MAPPING_RANGE range) + + int max_memory_usage + bool save_texture + bool use_chunk_only + bool reverse_vertex_order + + const interval allowed_range + float range_meter + const interval allowed_resolution + float resolution_meter + + bool save(String filename) + bool load(String filename) + + cdef cppclass Pose: + Pose() + Pose(const Pose &pose) + Pose(const Transform &pose_data, unsigned long long mtimestamp, int mconfidence) + Translation getTranslation() + Orientation getOrientation() + Rotation getRotationMatrix() + Vector3[float] getRotationVector() + Vector3[float] getEulerAngles(bool radian) + + bool valid + unsigned long long timestamp + + Transform pose_data + + int pose_confidence + float pose_covariance[36] + + cdef cppclass IMUData(Pose): + IMUData() + IMUData(const IMUData &pose) + IMUData(const Transform &pose_data, unsigned long long mtimestamp, int mconfidence) + + Matrix3f orientation_covariance + Vector3[float] angular_velocity + Vector3[float] linear_acceleration + Matrix3f angular_velocity_convariance + Matrix3f linear_acceleration_convariance + + + cdef cppclass Camera 'sl::Camera': + Camera() + void close() + ERROR_CODE open(InitParameters init_parameters) + bool isOpened() + ERROR_CODE grab(RuntimeParameters rt_parameters) + ERROR_CODE retrieveImage(Mat &mat, VIEW view, MEM type, int width, int height) + ERROR_CODE retrieveMeasure(Mat &mat, MEASURE measure, MEM type, int width, int height) + void setConfidenceThreshold(int conf_threshold_value) + int getConfidenceThreshold() + + Resolution getResolution() + void setDepthMaxRangeValue(float depth_max_range) + float getDepthMaxRangeValue() + float getDepthMinRangeValue() + void setSVOPosition(int frame_number) + int getSVOPosition() + int getSVONumberOfFrames() + void setCameraSettings(CAMERA_SETTINGS settings, int value, bool use_default) + int getCameraSettings(CAMERA_SETTINGS setting) + float getCameraFPS() + void setCameraFPS(int desired_fps) + float getCurrentFPS() + timeStamp getCameraTimestamp() # deprecated + timeStamp getCurrentTimestamp() # deprecated + timeStamp getTimestamp(TIME_REFERENCE reference_time) + unsigned int getFrameDroppedCount() + CameraInformation getCameraInformation(Resolution resizer); + SELF_CALIBRATION_STATE getSelfCalibrationState() + void resetSelfCalibration() + + ERROR_CODE enableTracking(TrackingParameters tracking_params) + TRACKING_STATE getPosition(Pose &camera_pose, REFERENCE_FRAME reference_frame) + ERROR_CODE saveCurrentArea(String area_file_path); + AREA_EXPORT_STATE getAreaExportState() + void disableTracking(String area_file_path) + ERROR_CODE resetTracking(Transform &path) + ERROR_CODE getIMUData(IMUData &imu_data, TIME_REFERENCE reference_time) + ERROR_CODE setIMUPrior(Transform &transfom) + + ERROR_CODE enableSpatialMapping(SpatialMappingParameters spatial_mapping_parameters) + void pauseSpatialMapping(bool status) + SPATIAL_MAPPING_STATE getSpatialMappingState() + ERROR_CODE extractWholeMesh(Mesh &mesh) + void requestMeshAsync() + ERROR_CODE getMeshRequestStatusAsync() + ERROR_CODE retrieveMeshAsync(Mesh &mesh) + void disableSpatialMapping() + + ERROR_CODE findPlaneAtHit(Vector2[uint] coord, Plane &plane) + ERROR_CODE findFloorPlane(Plane &plane, Transform &resetTrackingFloorFrame, float floor_height_prior, Rotation world_orientation_prior, float floor_height_prior_tolerance) + + ERROR_CODE enableRecording(String video_filename, SVO_COMPRESSION_MODE compression_mode) + RecordingState record() + void disableRecording() + + @staticmethod + String getSDKVersion() + + @staticmethod + int isZEDconnected() + + @staticmethod + ERROR_CODE sticktoCPUCore(int cpu_core) + + @staticmethod + vector[DeviceProperties] getDeviceList() + + bool saveDepthAs(Camera &zed, DEPTH_FORMAT format, String name, float factor) + bool savePointCloudAs(Camera &zed, POINT_CLOUD_FORMAT format, String name, + bool with_color) + + +cdef extern from "Utils.cpp" namespace "sl": + bool saveMatDepthAs(Mat &depth, DEPTH_FORMAT format, String name, float factor) + bool saveMatPointCloudAs(Mat &cloud, POINT_CLOUD_FORMAT format, String name, + bool with_color) + diff --git a/pyzed/types.pxd b/pyzed/types.pxd deleted file mode 100644 index f850e68..0000000 --- a/pyzed/types.pxd +++ /dev/null @@ -1,231 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# File containing the Cython declarations to use the types.hpp functions. - -from libcpp.string cimport string -from libcpp cimport bool - -cdef extern from "Utils.cpp" namespace "sl": - string to_str(String sl_str) - -cdef extern from "sl/types.hpp" namespace "sl": - - ctypedef unsigned long long timeStamp - - ctypedef enum ERROR_CODE: - SUCCESS, - ERROR_CODE_FAILURE, - ERROR_CODE_NO_GPU_COMPATIBLE, - ERROR_CODE_NOT_ENOUGH_GPUMEM, - ERROR_CODE_CAMERA_NOT_DETECTED, - ERROR_CODE_SENSOR_NOT_DETECTED, - ERROR_CODE_INVALID_RESOLUTION, - ERROR_CODE_LOW_USB_BANDWIDTH, - ERROR_CODE_CALIBRATION_FILE_NOT_AVAILABLE, - ERROR_CODE_INVALID_CALIBRATION_FILE, - ERROR_CODE_INVALID_SVO_FILE, - ERROR_CODE_SVO_RECORDING_ERROR, - ERROR_CODE_SVO_UNSUPPORTED_COMPRESSION, - ERROR_CODE_INVALID_COORDINATE_SYSTEM, - ERROR_CODE_INVALID_FIRMWARE, - ERROR_CODE_INVALID_FUNCTION_PARAMETERS, - ERROR_CODE_NOT_A_NEW_FRAME, - ERROR_CODE_CUDA_ERROR, - ERROR_CODE_CAMERA_NOT_INITIALIZED, - ERROR_CODE_NVIDIA_DRIVER_OUT_OF_DATE, - ERROR_CODE_INVALID_FUNCTION_CALL, - ERROR_CODE_CORRUPTED_SDK_INSTALLATION, - ERROR_CODE_INCOMPATIBLE_SDK_VERSION, - ERROR_CODE_INVALID_AREA_FILE, - ERROR_CODE_INCOMPATIBLE_AREA_FILE, - ERROR_CODE_CAMERA_FAILED_TO_SETUP, - ERROR_CODE_CAMERA_DETECTION_ISSUE, - ERROR_CODE_CAMERA_ALREADY_IN_USE, - ERROR_CODE_NO_GPU_DETECTED, - ERROR_CODE_PLANE_NOT_FOUND, - ERROR_CODE_LAST - - String toString(ERROR_CODE o) - - void sleep_ms(int time) - - ctypedef enum MODEL: - MODEL_ZED, - MODEL_ZED_M, - MODEL_LAST - - String model2str(MODEL model) - String toString(MODEL o) - - ctypedef enum CAMERA_STATE: - CAMERA_STATE_AVAILABLE, - CAMERA_STATE_NOT_AVAILABLE, - CAMERA_STATE_LAST - - String toString(CAMERA_STATE o) - - cdef cppclass String 'sl::String': - String() - String(const char *data) - void set(const char *data) - const char *get() const - bool empty() const - string std_str() const - - cdef cppclass DeviceProperties: - DeviceProperties() - CAMERA_STATE camera_state - int id - String path - MODEL camera_model - unsigned int serial_number - - String toString(DeviceProperties o) - - cdef cppclass Vector2[T]: - int size() - Vector2() - Vector2(const T v0, const T v1) - T *ptr() - T &operator[](int i) - - - cdef cppclass Vector3[T]: - int size() - Vector3() - Vector3(const T v0, const T v1, const T v2) - T *ptr() - T &operator[](int i) - - - cdef cppclass Vector4[T]: - int size() - Vector4() - Vector4(const T v0, const T v1, const T v2, const T v3) - T *ptr() - T &operator[](int i) - - - cdef cppclass Matrix3f 'sl::Matrix3f': - int nbElem - float r00 - float r01 - float r02 - float r10 - float r11 - float r12 - float r20 - float r21 - float r22 - float r[] - Matrix3f() - Matrix3f(float data[]) - Matrix3f(const Matrix3f &mat) - Matrix3f operator*(const Matrix3f &mat) const - Matrix3f operator*(const double &scalar) const - bool operator==(const Matrix3f &mat) const - bool operator!=(const Matrix3f &mat) const - void inverse() - - @staticmethod - Matrix3f inverse(const Matrix3f &rotation) - - void transpose() - - @staticmethod - Matrix3f transpose(const Matrix3f &rotation) - - void setIdentity() - - @staticmethod - Matrix3f identity() - - void setZeros() - - @staticmethod - Matrix3f zeros() - - String getInfos() - String matrix_name - - - cdef cppclass Matrix4f 'sl::Matrix4f': - int nbElem - float r00 - float r01 - float r02 - float tx - float r10 - float r11 - float r12 - float ty - float r20 - float r21 - float r22 - float tz - float m30 - float m31 - float m32 - float m33 - - float m[] - Matrix4f() - Matrix4f(float data[]) - Matrix4f(const Matrix4f &mat) - Matrix4f operator*(const Matrix4f &mat) const - Matrix4f operator*(const double &scalar) const - bool operator==(const Matrix4f &mat) const - bool operator!=(const Matrix4f &mat) const - ERROR_CODE inverse() - - @staticmethod - Matrix4f inverse(const Matrix4f &mat) - - void transpose() - - @staticmethod - Matrix4f transpose(const Matrix4f &mat) - - void setIdentity() - - @staticmethod - Matrix4f identity() - - void setZeros() - - @staticmethod - Matrix4f zeros() - - ERROR_CODE setSubMatrix3f(Matrix3f input, int row, int column) - ERROR_CODE setSubVector3f(Vector3[float] input, int column) - ERROR_CODE setSubVector4f(Vector4[float] input, int column) - - - String getInfos() - String matrix_name - - -cdef class PyMatrix3f: - cdef Matrix3f mat - - -cdef class PyMatrix4f: - cdef Matrix4f mat diff --git a/pyzed/types.pyx b/pyzed/types.pyx deleted file mode 100644 index 4de2ffc..0000000 --- a/pyzed/types.pyx +++ /dev/null @@ -1,420 +0,0 @@ -######################################################################## -# -# Copyright (c) 2017, STEREOLABS. -# -# All rights reserved. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -######################################################################## - -# Source file of the types Python module. - -import enum - -import numpy as np -cimport numpy as np -from math import sqrt - - -class PyERROR_CODE(enum.Enum): - PySUCCESS = SUCCESS - PyERROR_CODE_FAILURE = ERROR_CODE_FAILURE - PyERROR_CODE_NO_GPU_COMPATIBLE = ERROR_CODE_NO_GPU_COMPATIBLE - PyERROR_CODE_NOT_ENOUGH_GPUMEM = ERROR_CODE_NOT_ENOUGH_GPUMEM - PyERROR_CODE_CAMERA_NOT_DETECTED = ERROR_CODE_CAMERA_NOT_DETECTED - PyERROR_CODE_SENSOR_NOT_DETECTED = ERROR_CODE_SENSOR_NOT_DETECTED - PyERROR_CODE_INVALID_RESOLUTION = ERROR_CODE_INVALID_RESOLUTION - PyERROR_CODE_LOW_USB_BANDWIDTH = ERROR_CODE_LOW_USB_BANDWIDTH - PyERROR_CODE_CALIBRATION_FILE_NOT_AVAILABLE = ERROR_CODE_CALIBRATION_FILE_NOT_AVAILABLE - PyERROR_CODE_INVALID_SVO_FILE = ERROR_CODE_INVALID_SVO_FILE - PyERROR_CODE_SVO_RECORDING_ERROR = ERROR_CODE_SVO_RECORDING_ERROR - PyERROR_CODE_SVO_UNSUPPORTED_COMPRESSION = ERROR_CODE_SVO_UNSUPPORTED_COMPRESSION - PyERROR_CODE_INVALID_COORDINATE_SYSTEM = ERROR_CODE_INVALID_COORDINATE_SYSTEM - PyERROR_CODE_INVALID_FIRMWARE = ERROR_CODE_INVALID_FIRMWARE - PyERROR_CODE_INVALID_FUNCTION_PARAMETERS = ERROR_CODE_INVALID_FUNCTION_PARAMETERS - PyERROR_CODE_NOT_A_NEW_FRAME = ERROR_CODE_NOT_A_NEW_FRAME - PyERROR_CODE_CUDA_ERROR = ERROR_CODE_CUDA_ERROR - PyERROR_CODE_CAMERA_NOT_INITIALIZED = ERROR_CODE_CAMERA_NOT_INITIALIZED - PyERROR_CODE_NVIDIA_DRIVER_OUT_OF_DATE = ERROR_CODE_NVIDIA_DRIVER_OUT_OF_DATE - PyERROR_CODE_INVALID_FUNCTION_CALL = ERROR_CODE_INVALID_FUNCTION_CALL - PyERROR_CODE_CORRUPTED_SDK_INSTALLATION = ERROR_CODE_CORRUPTED_SDK_INSTALLATION - PyERROR_CODE_INCOMPATIBLE_SDK_VERSION = ERROR_CODE_INCOMPATIBLE_SDK_VERSION - PyERROR_CODE_INVALID_AREA_FILE = ERROR_CODE_INVALID_AREA_FILE - PyERROR_CODE_INCOMPATIBLE_AREA_FILE = ERROR_CODE_INCOMPATIBLE_AREA_FILE - PyERROR_CODE_CAMERA_FAILED_TO_SETUP = ERROR_CODE_CAMERA_FAILED_TO_SETUP - PyERROR_CODE_CAMERA_DETECTION_ISSUE = ERROR_CODE_CAMERA_DETECTION_ISSUE - PyERROR_CODE_CAMERA_ALREADY_IN_USE = ERROR_CODE_CAMERA_ALREADY_IN_USE - PyERROR_CODE_NO_GPU_DETECTED = ERROR_CODE_NO_GPU_DETECTED - PyERROR_CODE_PLANE_NOT_FOUND = ERROR_CODE_PLANE_NOT_FOUND - PyERROR_CODE_LAST = ERROR_CODE_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyMODEL(enum.Enum): - PyMODEL_ZED = MODEL_ZED - PyMODEL_ZED_M = MODEL_ZED_M - PyMODEL_LAST = MODEL_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -class PyCAMERA_STATE(enum.Enum): - PyCAMERA_STATE_AVAILABLE = CAMERA_STATE_AVAILABLE - PyCAMERA_STATE_NOT_AVAILABLE = CAMERA_STATE_NOT_AVAILABLE - PyCAMERA_STATE_LAST = CAMERA_STATE_LAST - - def __str__(self): - return to_str(toString(self.value)).decode() - - def __repr__(self): - return to_str(toString(self.value)).decode() - -def c_sleep_ms(int time): - sleep_ms(time) - -cdef class PyDeviceProperties: - cdef DeviceProperties c_device_properties - - def __cinit__(self): - self.c_device_properties = DeviceProperties() - - @property - def camera_state(self): - return self.c_device_properties.camera_state - @camera_state.setter - def camera_state(self, camera_state): - if isinstance(camera_state, PyCAMERA_STATE): - self.c_device_properties.camera_state = ( camera_state.value) - elif isinstance(camera_state, int): - self.c_device_properties.camera_state = ( camera_state) - else: - raise TypeError("Argument is not of PyCAMERA_STATE type.") - - @property - def id(self): - return self.c_device_properties.id - @id.setter - def id(self, id): - self.c_device_properties.id = id - - @property - def path(self): - if not self.c_device_properties.path.empty(): - return self.c_device_properties.path.get().decode() - else: - return "" - @path.setter - def path(self, str path): - path_ = path.encode() - self.c_device_properties.path = (String( path_)) - - @property - def camera_model(self): - return self.c_device_properties.camera_model - @camera_model.setter - def camera_model(self, camera_model): - if isinstance(camera_model, PyMODEL): - self.c_device_properties.camera_model = ( camera_model.value) - elif isinstance(camera_model, int): - self.c_device_properties.camera_model = ( camera_model) - else: - raise TypeError("Argument is not of PyMODEL type.") - - @property - def serial_number(self): - return self.c_device_properties.serial_number - @serial_number.setter - def serial_number(self, serial_number): - self.c_device_properties.serial_number = serial_number - - def __str__(self): - return to_str(toString(self.c_device_properties)).decode() - - def __repr__(self): - return to_str(toString(self.c_device_properties)).decode() - - -cdef class PyMatrix3f: - def __cinit__(self): - self.mat = Matrix3f() - - def init_matrix(self, PyMatrix3f matrix): - self.mat = Matrix3f(matrix.mat) - - def inverse(self): - self.mat.inverse() - - def inverse_mat(self, PyMatrix3f rotation): - rotation.mat.inverse(rotation.mat) - return rotation - - def transpose(self): - self.mat.transpose() - - def transpose(self, PyMatrix3f rotation): - rotation.mat.transpose(rotation.mat) - return rotation - - def set_identity(self): - self.mat.setIdentity() - return self - - def identity(self): - self.mat.identity() - return self - - def set_zeros(self): - self.mat.setZeros() - - def zeros(self): - self.mat.zeros() - return self - - def get_infos(self): - return to_str(self.mat.getInfos()).decode() - - @property - def matrix_name(self): - if not self.mat.matrix_name.empty(): - return self.mat.matrix_name.get().decode() - else: - return "" - - @property - def nbElem(self): - return self.mat.nbElem - - @property - def r(self): - nbElem = self.nbElem - sqrt_nbElem = int(sqrt(nbElem)) - cdef np.ndarray arr = np.zeros(nbElem) - for i in range(nbElem): - arr[i] = self.mat.r[i] - return arr.reshape(sqrt_nbElem, sqrt_nbElem) - - @r.setter - def r(self, value): - if isinstance(value, list): - if len(value) <= self.nbElem: - for i in range(len(value)): - self.mat.r[i] = value[i] - else: - raise IndexError("Value list must be of length 9 max.") - elif isinstance(value, np.ndarray): - if value.size <= self.nbElem: - for i in range(value.size): - self.mat.r[i] = value[i] - else: - raise IndexError("Numpy array must be of size 9.") - else: - raise TypeError("Argument must be numpy array or list type.") - - def __mul__(self, other): - matrix = PyMatrix3f() - if isinstance(other, PyMatrix3f): - matrix.r = (self.r * other.r).reshape(self.nbElem) - elif isinstance(other, float) or isinstance(other, int) or isinstance(other, long): - matrix.r = (other * self.r).reshape(self.nbElem) - else: - raise TypeError("Argument must be PyMatrix3f or scalar type.") - return matrix - - def __richcmp__(PyMatrix3f left, PyMatrix3f right, int op): - if op == 2: - return left.mat == right.mat - if op == 3: - return left.mat != right.mat - else: - raise NotImplementedError() - - def __getitem__(self, x): - return self.r[x] - - def __setitem__(self, key, value): - if key == (0,0): - self.mat.r00 = value - elif key == (0,1): - self.mat.r01 = value - elif key == (0,2): - self.mat.r02 = value - elif key == (1,0): - self.mat.r10 = value - elif key == (1,1): - self.mat.r11 = value - elif key == (1,2): - self.mat.r12 = value - elif key == (2,0): - self.mat.r20 = value - elif key == (2,1): - self.mat.r21 = value - elif key == (2,2): - self.mat.r22 = value - - def __repr__(self): - return self.get_infos() - - -cdef class PyMatrix4f: - def __cinit__(self): - self.mat = Matrix4f() - - def init_matrix(self, PyMatrix4f matrix): - self.mat = Matrix4f(matrix.mat) - - def inverse(self): - return PyERROR_CODE(self.mat.inverse()) - - def inverse_mat(self, PyMatrix4f rotation): - rotation.mat.inverse(rotation.mat) - return rotation - - def transpose(self): - self.mat.transpose() - - def transpose(self, PyMatrix4f rotation): - rotation.mat.transpose(rotation.mat) - return rotation - - def set_identity(self): - self.mat.setIdentity() - return self - - def identity(self): - self.mat.identity() - return self - - def set_zeros(self): - self.mat.setZeros() - - def zeros(self): - self.mat.zeros() - return self - - def get_infos(self): - return to_str(self.mat.getInfos()).decode() - - def set_sub_matrix3f(self, PyMatrix3f input, row=0, column=0): - if row != 0 and row != 1 or column != 0 and column != 1: - raise TypeError("Arguments row and column must be 0 or 1.") - else: - return PyERROR_CODE(self.mat.setSubMatrix3f(input.mat, row, column)) - - def set_sub_vector3f(self, float input0, float input1, float input2, column=3): - return PyERROR_CODE(self.mat.setSubVector3f(Vector3[float](input0, input1, input2), column)) - - def set_sub_vector4f(self, float input0, float input1, float input2, float input3, column=3): - return PyERROR_CODE(self.mat.setSubVector4f(Vector4[float](input0, input1, input2, input3), column)) - - @property - def nbElem(self): - return self.mat.nbElem - - @property - def matrix_name(self): - if not self.mat.matrix_name.empty(): - return self.mat.matrix_name.get().decode() - else: - return "" - - @property - def m(self): - nbElem = self.nbElem - sqrt_nbElem = int(sqrt(nbElem)) - cdef np.ndarray arr = np.zeros(nbElem) - for i in range(nbElem): - arr[i] = self.mat.m[i] - return arr.reshape(sqrt_nbElem, sqrt_nbElem) - - @m.setter - def m(self, value): - if isinstance(value, list): - if len(value) <= self.nbElem: - for i in range(len(value)): - self.mat.m[i] = value[i] - else: - raise IndexError("Value list must be of length 16 max.") - elif isinstance(value, np.ndarray): - if value.size <= self.nbElem: - for i in range(value.size): - self.mat.m[i] = value[i] - else: - raise IndexError("Numpy array must be of size 16.") - else: - raise TypeError("Argument must be numpy array or list type.") - - def __mul__(self, other): - matrix = PyMatrix4f() - if isinstance(other, PyMatrix4f): - matrix.m = (self.m * other.m).reshape(self.nbElem) - elif isinstance(other, float) or isinstance(other, int) or isinstance(other, long): - matrix.m = (other * self.m).reshape(self.nbElem) - else: - raise TypeError("Argument must be PyMatrix4f or scalar type.") - return matrix - - def __richcmp__(PyMatrix4f left, PyMatrix4f right, int op): - if op == 2: - return left.mat == right.mat - if op == 3: - return left.mat != right.mat - else: - raise NotImplementedError() - - def __getitem__(self, x): - return self.m[x] - - def __setitem__(self, key, value): - if key == (0,0): - self.mat.r00 = value - elif key == (0,1): - self.mat.r01 = value - elif key == (0,2): - self.mat.r02 = value - elif key == (0,3): - self.mat.tx = value - elif key == (1,0): - self.mat.r10 = value - elif key == (1,1): - self.mat.r11 = value - elif key == (1,2): - self.mat.r12 = value - elif key == (1,3): - self.mat.ty = value - elif key == (2,0): - self.mat.r20 = value - elif key == (2,1): - self.mat.r21 = value - elif key == (2,2): - self.mat.r22 = value - elif key == (2,3): - self.mat.tz = value - elif key == (3,0): - self.mat.m30 = value - elif key == (3,1): - self.mat.m31 = value - elif key == (3,2): - self.mat.m32 = value - elif key == (3,3): - self.mat.m33 = value - - def __repr__(self): - return self.get_infos() diff --git a/setup.py b/setup.py index 7d5cb51..e1d9449 100644 --- a/setup.py +++ b/setup.py @@ -1,7 +1,7 @@ # !/usr/bin/env python ######################################################################## # -# Copyright (c) 2017, STEREOLABS. +# Copyright (c) 2018, STEREOLABS. # # All rights reserved. # @@ -37,7 +37,7 @@ cflags = "" ZED_SDK_MAJOR = "2" -ZED_SDK_MINOR = "3" +ZED_SDK_MINOR = "7" cuda_path = "/usr/local/cuda" @@ -176,13 +176,7 @@ def create_extension(name, sources): py_packages = ["pyzed"] -package_data = {"pyzed": ["*.pxd"]} - -GPUmodulesTable = [("pyzed.defines", ["pyzed/defines.pyx"]), - ("pyzed.types", ["pyzed/types.pyx"]), - ("pyzed.core", ["pyzed/core.pyx"]), - ("pyzed.mesh", ["pyzed/mesh.pyx"]), - ("pyzed.camera", ["pyzed/camera.pyx"]) +GPUmodulesTable = [("pyzed.sl", ["pyzed/sl.pyx"]) ] for mod in GPUmodulesTable: @@ -194,9 +188,8 @@ def create_extension(name, sources): extensions.extend(extList) setup(name="pyzed", - version="2.3", + version="2.7", author_email="developers@stereolabs.com", description="Use the ZED SDK with Python", packages=py_packages, - ext_modules=extensions, - package_data=package_data) + ext_modules=extensions) diff --git a/tutorials/depth_sensing.py b/tutorials/depth_sensing.py index 6bfa45b..e2e258d 100644 --- a/tutorials/depth_sensing.py +++ b/tutorials/depth_sensing.py @@ -18,47 +18,44 @@ # ######################################################################## -import pyzed.camera as zcam -import pyzed.defines as sl -import pyzed.types as tp -import pyzed.core as core +import pyzed.sl as sl import math import numpy as np import sys def main(): - # Create a PyZEDCamera object - zed = zcam.PyZEDCamera() + # Create a Camera object + zed = sl.Camera() - # Create a PyInitParameters object and set configuration parameters - init_params = zcam.PyInitParameters() - init_params.depth_mode = sl.PyDEPTH_MODE.PyDEPTH_MODE_PERFORMANCE # Use PERFORMANCE depth mode - init_params.coordinate_units = sl.PyUNIT.PyUNIT_MILLIMETER # Use milliliter units (for depth measurements) + # Create a InitParameters object and set configuration parameters + init_params = sl.InitParameters() + init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE # Use PERFORMANCE depth mode + init_params.coordinate_units = sl.UNIT.UNIT_MILLIMETER # Use milliliter units (for depth measurements) # Open the camera err = zed.open(init_params) - if err != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: exit(1) - # Create and set PyRuntimeParameters after opening the camera - runtime_parameters = zcam.PyRuntimeParameters() - runtime_parameters.sensing_mode = sl.PySENSING_MODE.PySENSING_MODE_STANDARD # Use STANDARD sensing mode + # Create and set RuntimeParameters after opening the camera + runtime_parameters = sl.RuntimeParameters() + runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_STANDARD # Use STANDARD sensing mode # Capture 50 images and depth, then stop i = 0 - image = core.PyMat() - depth = core.PyMat() - point_cloud = core.PyMat() + image = sl.Mat() + depth = sl.Mat() + point_cloud = sl.Mat() while i < 50: - # A new image is available if grab() returns PySUCCESS - if zed.grab(runtime_parameters) == tp.PyERROR_CODE.PySUCCESS: + # A new image is available if grab() returns SUCCESS + if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # Retrieve left image - zed.retrieve_image(image, sl.PyVIEW.PyVIEW_LEFT) + zed.retrieve_image(image, sl.VIEW.VIEW_LEFT) # Retrieve depth map. Depth is aligned on the left image - zed.retrieve_measure(depth, sl.PyMEASURE.PyMEASURE_DEPTH) + 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.PyMEASURE.PyMEASURE_XYZRGBA) + zed.retrieve_measure(point_cloud, sl.MEASURE.MEASURE_XYZRGBA) # Get and print distance value in mm at the center of the image # We measure the distance camera - object using Euclidean distance diff --git a/tutorials/hello_zed.py b/tutorials/hello_zed.py index 7fee864..639175a 100644 --- a/tutorials/hello_zed.py +++ b/tutorials/hello_zed.py @@ -18,21 +18,20 @@ # ######################################################################## -import pyzed.camera as zcam -import pyzed.types as tp +import pyzed.sl as sl def main(): - # Create a PyZEDCamera object - zed = zcam.PyZEDCamera() + # Create a Camera object + zed = sl.Camera() - # Create a PyInitParameters object and set configuration parameters - init_params = zcam.PyInitParameters() + # Create a InitParameters object and set configuration parameters + init_params = sl.InitParameters() init_params.sdk_verbose = False # Open the camera err = zed.open(init_params) - if err != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: exit(1) # Get camera information (ZED serial number) diff --git a/tutorials/image_capture.py b/tutorials/image_capture.py index 726864b..98524c8 100644 --- a/tutorials/image_capture.py +++ b/tutorials/image_capture.py @@ -18,36 +18,33 @@ # ######################################################################## -import pyzed.camera as zcam -import pyzed.defines as sl -import pyzed.types as tp -import pyzed.core as core +import pyzed.sl as sl def main(): - # Create a PyZEDCamera object - zed = zcam.PyZEDCamera() + # Create a Camera object + zed = sl.Camera() - # Create a PyInitParameters object and set configuration parameters - init_params = zcam.PyInitParameters() - init_params.camera_resolution = sl.PyRESOLUTION.PyRESOLUTION_HD1080 # Use HD1080 video mode + # Create a InitParameters object and set configuration parameters + init_params = sl.InitParameters() + init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD1080 # Use HD1080 video mode init_params.camera_fps = 30 # Set fps at 30 # Open the camera err = zed.open(init_params) - if err != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: exit(1) # Capture 50 frames and stop i = 0 - image = core.PyMat() - runtime_parameters = zcam.PyRuntimeParameters() + image = sl.Mat() + runtime_parameters = sl.RuntimeParameters() while i < 50: - # Grab an image, a PyRuntimeParameters object must be given to grab() - if zed.grab(runtime_parameters) == tp.PyERROR_CODE.PySUCCESS: - # A new image is available if grab() returns PySUCCESS - zed.retrieve_image(image, sl.PyVIEW.PyVIEW_LEFT) - timestamp = zed.get_timestamp(sl.PyTIME_REFERENCE.PyTIME_REFERENCE_CURRENT) # Get the timestamp at the time the image was captured + # 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 + zed.retrieve_image(image, sl.VIEW.VIEW_LEFT) + timestamp = zed.get_timestamp(sl.TIME_REFERENCE.TIME_REFERENCE_CURRENT) # Get the timestamp at the time the image was captured print("Image resolution: {0} x {1} || Image timestamp: {2}\n".format(image.get_width(), image.get_height(), timestamp)) i = i + 1 diff --git a/tutorials/positional_tracking.py b/tutorials/positional_tracking.py index b6f8f9d..6b3e47f 100644 --- a/tutorials/positional_tracking.py +++ b/tutorials/positional_tracking.py @@ -18,56 +18,52 @@ # ######################################################################## -import pyzed.camera as zcam -import pyzed.defines as sl -import pyzed.types as tp -import pyzed.core as core - +import pyzed.sl as sl def main(): - # Create a PyZEDCamera object - zed = zcam.PyZEDCamera() + # Create a Camera object + zed = sl.Camera() - # 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) + # 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.PyCOORDINATE_SYSTEM.PyCOORDINATE_SYSTEM_RIGHT_HANDED_Y_UP - init_params.coordinate_units = sl.PyUNIT.PyUNIT_METER # Set units in meters + 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 != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: 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) + 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 != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: exit(1) # Track the camera position during 1000 frames i = 0 - zed_pose = zcam.PyPose() - zed_imu = zcam.PyIMUData() - runtime_parameters = zcam.PyRuntimeParameters() + zed_pose = sl.Pose() + zed_imu = sl.IMUData() + runtime_parameters = sl.RuntimeParameters() while i < 1000: - if zed.grab(runtime_parameters) == tp.PyERROR_CODE.PySUCCESS: + 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.PyREFERENCE_FRAME.PyREFERENCE_FRAME_WORLD) - zed.get_imu_data(zed_imu, sl.PyTIME_REFERENCE.PyTIME_REFERENCE_IMAGE) + 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 = core.PyTranslation() + 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)) # Display the orientation quaternion - py_orientation = core.PyOrientation() + 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) @@ -91,7 +87,7 @@ def main(): print("IMU Angular Velocity: Vx: {0}, Vy: {1}, Vz {2}\n".format(vx, vy, vz)) # Display the IMU orientation quaternion - imu_orientation = core.PyOrientation() + imu_orientation = sl.Orientation() ox = round(zed_imu.get_orientation(imu_orientation).get()[0], 3) oy = round(zed_imu.get_orientation(imu_orientation).get()[1], 3) oz = round(zed_imu.get_orientation(imu_orientation).get()[2], 3) diff --git a/tutorials/spatial_mapping.py b/tutorials/spatial_mapping.py index c7f618b..0a18204 100644 --- a/tutorials/spatial_mapping.py +++ b/tutorials/spatial_mapping.py @@ -18,51 +18,47 @@ # ######################################################################## -import pyzed.camera as zcam -import pyzed.defines as sl -import pyzed.types as tp -import pyzed.core as core -import pyzed.mesh as mesh +import pyzed.sl as sl def main(): - # Create a PyZEDCamera object - zed = zcam.PyZEDCamera() + # Create a ZEDCamera object + zed = sl.Camera() - # 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) + # 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.PyCOORDINATE_SYSTEM.PyCOORDINATE_SYSTEM_RIGHT_HANDED_Y_UP - init_params.coordinate_units = sl.PyUNIT.PyUNIT_METER # Set units in meters + 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 != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: exit(1) # Enable positional tracking with default parameters. # Positional tracking needs to be enabled before using spatial mapping - py_transform = core.PyTransform() - tracking_parameters = zcam.PyTrackingParameters(init_pos=py_transform) + py_transform = sl.Transform() + tracking_parameters = sl.TrackingParameters(init_pos=py_transform) err = zed.enable_tracking(tracking_parameters) - if err != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: exit(1) # Enable spatial mapping - mapping_parameters = zcam.PySpatialMappingParameters() + mapping_parameters = sl.SpatialMappingParameters() err = zed.enable_spatial_mapping(mapping_parameters) - if err != tp.PyERROR_CODE.PySUCCESS: + if err != sl.ERROR_CODE.SUCCESS: exit(1) # Grab data during 500 frames i = 0 - py_mesh = mesh.PyMesh() # Create a PyMesh object - runtime_parameters = zcam.PyRuntimeParameters() + py_mesh = sl.Mesh() # Create a Mesh object + runtime_parameters = sl.RuntimeParameters() while i < 500: # For each new grab, mesh data is updated - if zed.grab(runtime_parameters) == tp.PyERROR_CODE.PySUCCESS: + if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh mapping_state = zed.get_spatial_mapping_state() @@ -76,7 +72,7 @@ def main(): 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) + py_mesh.filter(sl.MeshFilterParameters()) # Filter the mesh (remove unnecessary vertices and faces) print("Saving Mesh...\n") py_mesh.save("mesh.obj")