diff --git a/README.md b/README.md index 0ea6826..17b7514 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ The changes were made to better reflect the C++ API and ease of use. Mainly all ### Prerequisites -- [ZED SDK 2.7](https://www.stereolabs.com/developers/) and its dependency +- [ZED SDK 2.8](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - Python 3.5+ (x64). ([Windows installer](https://www.python.org/ftp/python/3.6.2/python-3.6.2-amd64.exe)) - C++ compiler (VS2015 recommended) diff --git a/examples/README.md b/examples/README.md index dab0bac..396190d 100644 --- a/examples/README.md +++ b/examples/README.md @@ -41,3 +41,16 @@ Plane sample is searching for the floor in a video and extracts it into a mesh i ``` python examples/plane_example.py svo_file.svo ``` + +### Streaming + +These 2 samples show the local network streaming capabilities of the SDK. The sender is opening the camera and transmitting the images. +The receiver opens the network image stream and display the images. + +``` +python examples/camera_streaming/sender/streaming_sender.py +``` + +``` +python examples/camera_streaming/receiver/streaming_receiver.py 127.0.0.1 +``` \ No newline at end of file diff --git a/examples/camera_streaming/receiver/streaming_receiver.py b/examples/camera_streaming/receiver/streaming_receiver.py new file mode 100644 index 0000000..85b222e --- /dev/null +++ b/examples/camera_streaming/receiver/streaming_receiver.py @@ -0,0 +1,66 @@ +######################################################################## +# +# 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. +# +######################################################################## + +""" + Read SVO sample to read the video and the information of the camera. It can pick a frame of the svo and save it as + a JPEG or PNG file. Depth map and Point Cloud can also be saved into files. +""" +import sys +import pyzed.sl as sl +import cv2 + + +def main(): + + init = sl.InitParameters() + init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 + init.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_PERFORMANCE + + if (len(sys.argv) > 1) : + ip = sys.argv[1] + init.set_from_stream(ip) + else : + print('Usage : python3 streaming_receiver.py ip') + exit(1) + + cam = sl.Camera() + status = cam.open(init) + if status != sl.ERROR_CODE.SUCCESS: + print(repr(status)) + exit(1) + + runtime = sl.RuntimeParameters() + mat = sl.Mat() + + key = '' + print(" Quit : CTRL+C\n") + while key != 113: + err = cam.grab(runtime) + if (err == sl.ERROR_CODE.SUCCESS) : + cam.retrieve_image(mat, sl.VIEW.VIEW_LEFT) + cv2.imshow("ZED", mat.get_data()) + key = cv2.waitKey(1) + else : + key = cv2.waitKey(1) + + cam.close() + +if __name__ == "__main__": + main() diff --git a/examples/camera_streaming/sender/streaming_sender.py b/examples/camera_streaming/sender/streaming_sender.py new file mode 100644 index 0000000..8fc6a8e --- /dev/null +++ b/examples/camera_streaming/sender/streaming_sender.py @@ -0,0 +1,59 @@ +######################################################################## +# +# 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. +# +######################################################################## + +""" + Read SVO sample to read the video and the information of the camera. It can pick a frame of the svo and save it as + a JPEG or PNG file. Depth map and Point Cloud can also be saved into files. +""" +import sys +import pyzed.sl as sl +import cv2 + + +def main(): + + init = sl.InitParameters() + init.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 + init.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_NONE + cam = sl.Camera() + status = cam.open(init) + if status != sl.ERROR_CODE.SUCCESS: + print(repr(status)) + exit(1) + + runtime = sl.RuntimeParameters() + + stream = sl.StreamingParameters() + stream.codec = sl.STREAMING_CODEC.STREAMING_CODEC_AVCHD + stream.bitrate = 4000 + status = cam.enable_streaming(stream) + if status != sl.ERROR_CODE.SUCCESS: + print(repr(status)) + exit(1) + + print(" Quit : CTRL+C\n") + while True: + err = cam.grab(runtime) + + cam.disable_streaming() + cam.close() + +if __name__ == "__main__": + main() diff --git a/pyzed/sl.pyx b/pyzed/sl.pyx index 9ba6cdb..8de30d5 100644 --- a/pyzed/sl.pyx +++ b/pyzed/sl.pyx @@ -24,8 +24,7 @@ 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 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, PointCloudChunk as c_PointCloudChunk, FusedPointCloud as c_FusedPointCloud, Mesh as c_Mesh, Plane as c_Plane, CUctx_st, CUcontext, MAPPING_RESOLUTION as c_MAPPING_RESOLUTION, MAPPING_RANGE as c_MAPPING_RANGE, SPATIAL_MAP_TYPE as c_SPATIAL_MAP_TYPE, 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, StreamingParameters as c_StreamingParameters, STREAMING_CODEC as c_STREAMING_CODEC, StreamingProperties as c_StreamingProperties, FusedPointCloud as c_FusedPointCloud, SPATIAL_MAP_TYPE as c_SPATIAL_MAP_TYPE, PointCloudChunk as c_PointCloudChunk, saveDepthAs, savePointCloudAs, saveMatDepthAs, saveMatPointCloudAs from cython.operator cimport dereference as deref from libc.string cimport memcpy from cpython cimport bool @@ -728,6 +727,7 @@ class CAMERA_SETTINGS(enum.Enum): 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_LED_STATUS = c_CAMERA_SETTINGS.CAMERA_SETTINGS_LED_STATUS CAMERA_SETTINGS_LAST = c_CAMERA_SETTINGS.CAMERA_SETTINGS_LAST @@ -2985,6 +2985,43 @@ cdef class Texture: """ self.texture.clear() +cdef class PointCloudChunk : + cdef c_PointCloudChunk 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 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 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 Chunk: """ @@ -3079,6 +3116,64 @@ cdef class Chunk: """ self.chunk.clear() +cdef class FusedPointCloud : + cdef c_FusedPointCloud* fpc + def __cinit__(self): + self.fpc = new c_FusedPointCloud() + + def __dealloc__(self): + del self.fpc + + @property + def chunks(self): + list = [] + for i in range(self.mesh.chunks.size()): + py_chunk = PointCloudChunk() + py_chunk.chunk = self.fpc.chunks[i] + list.append(py_chunk) + return list + + def __getitem__(self, x): + return self.chunks[x] + + @property + def vertices(self): + cdef np.ndarray arr = np.zeros((self.fpc.vertices.size(), 3)) + for i in range(self.fpc.vertices.size()): + for j in range(3): + arr[i,j] = self.fpc.vertices[i].ptr()[j] + return arr + + @property + def normals(self): + cdef np.ndarray arr = np.zeros((self.fpc.normals.size(), 3)) + for i in range(self.fpc.normals.size()): + for j in range(3): + arr[i,j] = self.fpc.normals[i].ptr()[j] + return arr + + def save(self, str filename, typeMesh=MESH_FILE_FORMAT.MESH_FILE_OBJ, id=[]): + if isinstance(typeMesh, MESH_FILE_FORMAT): + return self.fpc.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.fpc.load(String(filename.encode()), update_mesh) + else: + raise TypeError("Argument is not of boolean type.") + + def clear(self): + self.fpc.clear() + + def update_from_chunklist(self, id=[]): + self.fpc.updateFromChunkList(id) + + def get_number_of_points(self): + return self.fpc.getNumberOfPoints() + + cdef class Mesh: """ A mesh contains the geometric (and optionally texture) data of the scene captured by spatial mapping. @@ -3510,6 +3605,10 @@ class MAPPING_RANGE(enum.Enum): MAPPING_RANGE_MEDIUM = c_MAPPING_RANGE.MAPPING_RANGE_MEDIUM MAPPING_RANGE_FAR = c_MAPPING_RANGE.MAPPING_RANGE_FAR +class SPATIAL_MAP_TYPE(enum.Enum): + SPATIAL_MAP_TYPE_MESH = c_SPATIAL_MAP_TYPE.SPATIAL_MAP_TYPE_MESH + SPATIAL_MAP_TYPE_FUSED_POINT_CLOUD = c_SPATIAL_MAP_TYPE.SPATIAL_MAP_TYPE_FUSED_POINT_CLOUD + cdef class InputType: """ Defines the input type used in the ZED SDK. Can be used to select a specific camera with ID or serial number, or a svo file. @@ -3545,6 +3644,10 @@ cdef class InputType: """ filename = svo_input_filename.encode() self.input.setFromSVOFile(String( filename)) + + def set_from_stream(self, str sender_ip, port=30000): + sender_ip_ = sender_ip.encode() + self.input.setFromStream(String(sender_ip_), port) cdef class InitParameters: @@ -4059,6 +4162,9 @@ cdef class InitParameters: filename = svo_input_filename.encode() self.init.input.setFromSVOFile(String( filename)) + def set_from_stream(self, str sender_ip, port=30000): + sender_ip_ = sender_ip.encode() + self.init.input.setFromStream(String(sender_ip_), port) cdef class RuntimeParameters: """ @@ -4320,10 +4426,135 @@ cdef class TrackingParameters: value_area = value.encode() self.tracking.area_file_path.set(value_area) +class STREAMING_CODEC(enum.Enum): + """ + List of possible camera state. + """ + STREAMING_CODEC_AVCHD = c_STREAMING_CODEC.STREAMING_CODEC_AVCHD + STREAMING_CODEC_HEVC = c_STREAMING_CODEC.STREAMING_CODEC_HEVC + STREAMING_CODEC_LAST = c_STREAMING_CODEC.STREAMING_CODEC_LAST + +cdef class StreamingProperties: + """ + Properties of all streaming devices + """ + cdef c_StreamingProperties c_streaming_properties + + @property + def ip(self): + return to_str(self.c_streaming_properties.ip).decode() + + @ip.setter + def ip(self, str ip_): + self.c_streaming_properties.ip = String(ip_.encode()) + + @property + def port(self): + return self.c_streaming_properties.port + + @port.setter + def port(self, port_): + self.c_streaming_properties.port = port_ + + +cdef class StreamingParameters: + """ + Sets the streaming parameters. + + The default constructor sets all parameters to their default settings. + + .. note:: + Parameters can be user adjusted. + """ + cdef c_StreamingParameters* streaming + def __cinit__(self, codec=STREAMING_CODEC.STREAMING_CODEC_HEVC, port=30000, bitrate=2000, gop_size=-1, adaptative_bitrate=False): + self.streaming = new c_StreamingParameters(codec.value, port, bitrate, gop_size, adaptative_bitrate) + + def __dealloc__(self): + del self.streaming + + @property + def codec(self): + """ + Defines the codec used for streaming. + + .. warning:: + If HEVC is used, make sure the receiving host is compatible with HEVC decoding (basically a pascal NVIDIA card). If not, prefer to use AVCHD since every compatible NVIDIA card supports AVCHD decoding + """ + return STREAMING_CODEC(self.streaming.codec) + + @codec.setter + def codec(self, codec): + self.streaming.codec = codec.value + + @property + def port(self): + """ + Defines the port the data will be streamed on. + .. warning:: + port must be an even number. Any odd number will be rejected. + """ + return self.streaming.port + + @port.setter + def port(self, unsigned short value): + self.streaming.port = value + + @property + def bitrate(self): + """ + Defines the port the data will be streamed on. + """ + return self.streaming.bitrate + + @bitrate.setter + def bitrate(self, unsigned int value): + self.streaming.bitrate = value + + @property + def adaptative_bitrate(self): + """ + Enable/Disable adaptive bitrate + + .. note:: + Bitrate will be adjusted regarding the number of packet loss during streaming. + + .. note:: + if activated, bitrate can vary between [bitrate/4, bitrate] + + .. warning:: + Bitrate will be adjusted regarding the number of packet loss during streaming. + """ + return self.streaming.adaptative_bitrate + + @adaptative_bitrate.setter + def adaptative_bitrate(self, bool value): + self.streaming.adaptative_bitrate = value + + @property + def gop_size(self): + """ + Defines the gop size in frame unit. + + .. note:: + if value is set to -1, the gop size will match 2 seconds, depending on camera fps. + + .. note:: + The gop size determines the maximum distance between IDR/I-frames. Very high GOP size will result in slightly more efficient compression, especially on static scene. But it can result in more latency if IDR/I-frame packet are lost during streaming. + + .. note:: + Default value is -1. Maximum allowed value is 256 (frames). + """ + + return self.streaming.gop_size + + @gop_size.setter + def gop_size(self, int value): + self.streaming.gop_size = value + cdef class SpatialMappingParameters: """ - Sets the spatial mapping parameters. A default constructor is enabled and set to its default parameters. @@ -4331,15 +4562,16 @@ cdef class SpatialMappingParameters: .. note:: Parameters can be user adjusted. + """ 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): + reverse_vertex_order=False, map_type=SPATIAL_MAP_TYPE.SPATIAL_MAP_TYPE_MESH): if (isinstance(resolution, MAPPING_RESOLUTION) and isinstance(mapping_range, MAPPING_RANGE) and - isinstance(use_chunk_only, bool) and isinstance(reverse_vertex_order, bool)): + isinstance(use_chunk_only, bool) and isinstance(reverse_vertex_order, bool) and isinstance(map_type, SPATIAL_MAP_TYPE)): self.spatial = new c_SpatialMappingParameters(resolution.value, mapping_range.value, max_memory_usage, save_texture, - use_chunk_only, reverse_vertex_order) + use_chunk_only, reverse_vertex_order, map_type.value) else: raise TypeError() @@ -4418,6 +4650,14 @@ cdef class SpatialMappingParameters: else: raise TypeError("Argument is not of MAPPING_RESOLUTION or float type.") + @property + def map_type(self): + return self.spatial.map_type + + @map_type.setter + def map_type(self, value): + self.spatial.map_type = value.value + @property def max_memory_usage(self): """ @@ -6107,6 +6347,29 @@ cdef class Camera: """ return ERROR_CODE(self.camera.retrieveMeshAsync(deref(py_mesh.mesh))) + def request_spatial_map_async(self): + self.camera.requestSpatialMapAsync() + + def get_spatial_map_request_status_async(self): + return ERROR_CODE(self.camera.getSpatialMapRequestStatusAsync()) + + def retrieve_spatial_map_async(self, py_mesh): + if isinstance(py_mesh, Mesh) : + return ERROR_CODE(self.camera.retrieveSpatialMapAsync(deref((py_mesh).mesh))) + elif isinstance(py_mesh, FusedPointCloud) : + py_mesh = py_mesh + return ERROR_CODE(self.camera.retrieveSpatialMapAsync(deref((py_mesh).fpc))) + else : + raise TypeError("Argument is not of Mesh or FusedPointCloud type.") + + def extract_whole_spatial_map(self, py_mesh): + if isinstance(py_mesh, Mesh) : + return ERROR_CODE(self.camera.extractWholeSpatialMap(deref((py_mesh).mesh))) + elif isinstance(py_mesh, FusedPointCloud) : + return ERROR_CODE(self.camera.extractWholeSpatialMap(deref((py_mesh).fpc))) + else : + raise TypeError("Argument is not of Mesh or FusedPointCloud type.") + def find_plane_at_hit(self, coord, Plane py_plane): """ Checks the plane at the given left image coordinates. @@ -6164,6 +6427,32 @@ cdef class Camera: """ self.camera.disableSpatialMapping() + + def enable_streaming(self, StreamingParameters streaming_parameters = StreamingParameters()) : + """ + Creates an streaming pipeline for images. + + **Parameters** + - **streaming_parameters** : the structure containing all the specific parameters for the streaming. + """ + return ERROR_CODE(self.camera.enableStreaming(deref(streaming_parameters.streaming))) + + def disable_streaming(self): + """ + Disables the streaming initiated by :meth:`~pyzed.sl.Camera.enable_streaming()` + + .. note:: + This function will automatically be called by :meth:`~pyzed.sl.Camera.close()` if :meth:`~pyzed.sl.Camera.enable_streaming()` was called. + """ + self.camera.disableStreaming() + + def is_streaming_enabled(self): + """ + Tells if the streaming is actually sending data (true) or still in configuration (false) + """ + return self.camera.isStreamingEnabled() + + def enable_recording(self, str video_filename, compression_mode=SVO_COMPRESSION_MODE.SVO_COMPRESSION_MODE_LOSSLESS): """ @@ -6317,6 +6606,34 @@ cdef class Camera: vect_python.append(prop) return vect_python + def get_streaming_device_list(cls): + """ + List all the streaming devices with their associated information. + + **Returns** + - The streaming properties for each connected camera + + .. warning:: + As this function returns an std::vector, it is only safe to use in Release mode (not Debug). + + This is due to a known compatibility issue between release (the SDK) and debug (your app) implementations of std::vector. + + .. warning:: + This function takes around 2seconds to make sure all network informations has been captured. Make sure to run this function in a thread. + """ + vect_ = cls.camera.getStreamingDeviceList() + vect_python = [] + for i in range(vect_.size()): + prop = StreamingProperties() + prop.ip = to_str(vect_[i].ip) + prop.port = vect_[i].port + prop.serial_number = vect_[i].serial_number + prop.current_bitrate = vect_[i].current_bitrate + prop.codec = vect_[i].codec + vect_python.append(prop) + return vect_python + + def save_camera_depth_as(Camera zed, format, str name, factor=1): """ Writes the current depth map into a file. diff --git a/pyzed/sl_c.pxd b/pyzed/sl_c.pxd index 8b8af2d..d056a66 100644 --- a/pyzed/sl_c.pxd +++ b/pyzed/sl_c.pxd @@ -267,6 +267,7 @@ cdef extern from "sl/defines.hpp" namespace "sl": CAMERA_SETTINGS_EXPOSURE CAMERA_SETTINGS_WHITEBALANCE CAMERA_SETTINGS_AUTO_WHITEBALANCE + CAMERA_SETTINGS_LED_STATUS CAMERA_SETTINGS_LAST String toString(CAMERA_SETTINGS o) @@ -716,7 +717,7 @@ cdef extern from "sl/Mesh.hpp" namespace "sl": cdef cppclass Chunk 'sl::Chunk': Chunk() - vector[Vector3[float]] vertices + vector[Vector4[float]] vertices vector[Vector3[uint]] triangles vector[Vector3[float]] normals vector[Vector2[float]] uv @@ -725,12 +726,21 @@ cdef extern from "sl/Mesh.hpp" namespace "sl": bool has_been_updated void clear() + cdef cppclass PointCloudChunk 'sl::PointCloudChunk': + PointCloudChunk() + vector[Vector4[float]] vertices + vector[Vector3[float]] normals + 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[Vector4[float]] vertices vector[Vector3[uint]] triangles vector[Vector3[float]] normals vector[Vector2[float]] uv @@ -747,6 +757,21 @@ cdef extern from "sl/Mesh.hpp" namespace "sl": bool load(const String filename, bool updateMesh) void clear() + cdef cppclass FusedPointCloud 'sl::FusedPointCloud': + ctypedef vector[size_t] chunkList + FusedPointCloud() + vector[PointCloudChunk] chunks + PointCloudChunk &operator[](int index) + vector[Vector4[float]] vertices + vector[Vector3[float]] normals + size_t getNumberOfPoints() + void updateFromChunkList(chunkList IDs) + 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 @@ -768,6 +793,10 @@ cdef extern from 'cuda.h' : cdef extern from 'sl/Camera.hpp' namespace 'sl': + ctypedef enum SPATIAL_MAP_TYPE 'sl::SpatialMappingParameters::SPATIAL_MAP_TYPE': + SPATIAL_MAP_TYPE_MESH 'sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::SPATIAL_MAP_TYPE_MESH' + SPATIAL_MAP_TYPE_FUSED_POINT_CLOUD 'sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::SPATIAL_MAP_TYPE_FUSED_POINT_CLOUD' + 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' @@ -786,6 +815,7 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl': void setFromCameraID(unsigned int id) void setFromSerialNumber(unsigned int serial_number) void setFromSVOFile(String svo_input_filename) + void setFromStream(String senderIP, unsigned short port) cdef cppclass InitParameters 'sl::InitParameters': RESOLUTION camera_resolution @@ -874,7 +904,8 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl': int max_memory_usage_, bool save_texture_, bool use_chunk_only_, - bool reverse_vertex_order_) + bool reverse_vertex_order_, + SPATIAL_MAP_TYPE map_type_) @staticmethod float get(MAPPING_RESOLUTION resolution) @@ -897,6 +928,8 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl': bool use_chunk_only bool reverse_vertex_order + SPATIAL_MAP_TYPE map_type + const interval allowed_range float range_meter const interval allowed_resolution @@ -905,6 +938,27 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl': bool save(String filename) bool load(String filename) + + cdef enum STREAMING_CODEC: + STREAMING_CODEC_AVCHD + STREAMING_CODEC_HEVC + STREAMING_CODEC_LAST + + cdef struct StreamingProperties: + String ip + unsigned short port + unsigned int serial_number + int current_bitrate + STREAMING_CODEC codec + + cdef cppclass StreamingParameters: + STREAMING_CODEC codec + unsigned short port + unsigned int bitrate + int gop_size + bool adaptative_bitrate + StreamingParameters(STREAMING_CODEC codec, unsigned short port_, unsigned int bitrate, int gop_size, bool adaptative_bitrate_) + cdef cppclass Pose: Pose() Pose(const Pose &pose) @@ -984,6 +1038,13 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl': ERROR_CODE retrieveMeshAsync(Mesh &mesh) void disableSpatialMapping() + void requestSpatialMapAsync() + ERROR_CODE getSpatialMapRequestStatusAsync() + ERROR_CODE retrieveSpatialMapAsync(Mesh &mesh) + ERROR_CODE retrieveSpatialMapAsync(FusedPointCloud &fpc) + ERROR_CODE extractWholeSpatialMap(Mesh &mesh) + ERROR_CODE extractWholeSpatialMap(FusedPointCloud &fpc) + 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) @@ -991,6 +1052,11 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl': RecordingState record() void disableRecording() + ERROR_CODE enableStreaming(StreamingParameters streaming_parameters) + void disableStreaming() + bool isStreamingEnabled() + + @staticmethod String getSDKVersion() @@ -1003,6 +1069,9 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl': @staticmethod vector[DeviceProperties] getDeviceList() + @staticmethod + vector[StreamingProperties] getStreamingDeviceList() + bool saveDepthAs(Camera &zed, DEPTH_FORMAT format, String name, float factor) bool savePointCloudAs(Camera &zed, POINT_CLOUD_FORMAT format, String name, bool with_color) diff --git a/setup.py b/setup.py index e1d9449..9cf24bd 100644 --- a/setup.py +++ b/setup.py @@ -37,7 +37,7 @@ cflags = "" ZED_SDK_MAJOR = "2" -ZED_SDK_MINOR = "7" +ZED_SDK_MINOR = "8" cuda_path = "/usr/local/cuda" @@ -188,7 +188,7 @@ def create_extension(name, sources): extensions.extend(extList) setup(name="pyzed", - version="2.7", + version= ZED_SDK_MAJOR + "." + ZED_SDK_MINOR, author_email="developers@stereolabs.com", description="Use the ZED SDK with Python", packages=py_packages, diff --git a/tutorials/spatial_mapping.py b/tutorials/spatial_mapping.py index 0a18204..4184e2a 100644 --- a/tutorials/spatial_mapping.py +++ b/tutorials/spatial_mapping.py @@ -46,35 +46,36 @@ def main(): exit(1) # Enable spatial mapping - mapping_parameters = sl.SpatialMappingParameters() + mapping_parameters = sl.SpatialMappingParameters(map_type=sl.SPATIAL_MAP_TYPE.SPATIAL_MAP_TYPE_FUSED_POINT_CLOUD) err = zed.enable_spatial_mapping(mapping_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) - # Grab data during 500 frames + # Grab data during 3000 frames i = 0 - py_mesh = sl.Mesh() # Create a Mesh object + py_fpc = sl.FusedPointCloud() # Create a Mesh object runtime_parameters = sl.RuntimeParameters() - while i < 500: + while i < 3000: # For each new grab, mesh data is updated if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS: # In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh mapping_state = zed.get_spatial_mapping_state() - print("\rImages captured: {0} / 500 || {1}".format(i, mapping_state)) + print("\rImages captured: {0} / 3000 || {1}".format(i, mapping_state)) i = i + 1 print("\n") # Extract, filter and save the mesh in an obj file - print("Extracting Mesh...\n") - zed.extract_whole_mesh(py_mesh) - print("Filtering Mesh...\n") - py_mesh.filter(sl.MeshFilterParameters()) # Filter the mesh (remove unnecessary vertices and faces) - print("Saving Mesh...\n") - py_mesh.save("mesh.obj") + print("Extracting Point Cloud...\n") + err = zed.extract_whole_spatial_map(py_fpc) + print(repr(err)) + #print("Filtering Mesh...\n") + #py_mesh.filter(sl.MeshFilterParameters()) # Filter the mesh (remove unnecessary vertices and faces) + print("Saving Point Cloud...\n") + py_fpc.save("fpc.obj") # Disable tracking and mapping and close the camera zed.disable_spatial_mapping()