Adding new features from SDK 2.4 to 2.7 (#58)

* InputType class
* Plane class and plane detection functions
* camera_imu_transform in CameraInformation
* getRecommendedRange in spatialMappingParameters
* coordinate system RIGHT_HANDED_Z_UP_X_FWD
* optional_settings_path in initParameters
* pose_covariance in Pose
* SVO H264 / H265
* enable_imu_fusion
* Plane detection sample

Author: abastie

examples/plane_example.py

@@ -0,0 +1,102 @@
+########################################################################
+#
+# Copyright (c) 2018, 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.
+#
+########################################################################
+
+"""
+    Plane sample that save the floor plane detected as a mesh.
+"""
+import sys
+import pyzed.camera as zcam
+import pyzed.core as core
+import pyzed.mesh as mesh
+import pyzed.types as tp
+
+
+def main():
+    cam = zcam.PyZEDCamera()
+    init = zcam.PyInitParameters()
+    
+    if len(sys.argv) == 2:
+        filepath = sys.argv[1]
+        print("Reading SVO file: {0}".format(filepath))
+        init.set_from_svo_file(filepath)
+
+    status = cam.open(init)
+    if status != tp.PyERROR_CODE.PySUCCESS:
+        print(repr(status))
+        exit(1)
+
+    runtime = zcam.PyRuntimeParameters()
+    spatial = zcam.PySpatialMappingParameters()
+
+    transform = core.PyTransform()
+    tracking = zcam.PyTrackingParameters(transform)
+
+    cam.enable_tracking(tracking)
+    cam.enable_spatial_mapping(spatial)
+
+    pymesh = mesh.PyMesh()
+    pyplane = mesh.PyPlane()
+    reset_tracking_floor_frame = core.PyTransform()
+    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
+
+    if found == 0:
+        print('Floor was not found, please try with another SVO.')
+        cam.close()
+        exit(0)
+
+    cam.disable_tracking()
+    cam.disable_spatial_mapping()
+
+    save_mesh(pymesh)
+    cam.close()
+    print("\nFINISH")
+
+
+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:
+                filepath = input("Enter filepath name: ")
+                msh = pymesh.save(filepath)
+                print("Saving mesh: {0}".format(repr(msh)))
+                if msh:
+                    break
+                else:
+                    print("Help : you must enter the filepath + filename without extension.")
+            break
+        elif res == "n":
+            print("Mesh will not be saved.")
+            break
+        else:
+            print("Error, please enter [y/n].")
+
+if __name__ == "__main__":
+    main()

pyzed/camera.pxd

@@ -29,6 +29,10 @@ 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':
 
@@ -43,6 +47,13 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl':
         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
@@ -64,6 +75,10 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl':
 
         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,
@@ -80,7 +95,10 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl':
                        bool enable_right_side_measure,
                        int camera_buffer_count_linux,
                        types.String sdk_verbose_log_file,
-                       bool depth_stabilization)
+                       bool depth_stabilization,
+                       CUcontext sdk_cuda_ctx,
+                       InputType input,
+                       types.String optional_settings_path)
 
         bool save(types.String filename)
         bool load(types.String filename)
@@ -104,7 +122,10 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl':
     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,
@@ -113,7 +134,6 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl':
         bool save(types.String filename)
         bool load(types.String filename)
 
-
     cdef cppclass SpatialMappingParameters 'sl::SpatialMappingParameters':
         ctypedef pair[float, float] interval
 
@@ -132,6 +152,12 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl':
         @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
@@ -164,7 +190,7 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl':
         core.Transform pose_data
 
         int pose_confidence
-
+        float pose_covariance[36]
 
     cdef cppclass IMUData(Pose):
         IMUData()
@@ -227,6 +253,9 @@ cdef extern from 'sl/Camera.hpp' namespace 'sl':
         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()
@@ -253,6 +282,8 @@ cdef extern from "Utils.cpp" namespace "sl":
     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

pyzed/camera.pyx

@@ -44,6 +44,26 @@ class PyRANGE(enum.Enum):
     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 = <PyInputType> 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(<char*> filename))
+ 
 
 cdef class PyInitParameters:
     cdef InitParameters* init
@@ -54,7 +74,8 @@ cdef class PyInitParameters:
                   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):
+                  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
@@ -63,16 +84,19 @@ cdef class PyInitParameters:
             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)):
+            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(<char*> 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(<char*> filelog), depth_stabilization)
+                                           types.String(<char*> filelog), depth_stabilization,
+                                           <CUcontext> 0, input_t.input, types.String(<char*> fileoption))
         else:
             raise TypeError("Argument is not of right type.")
 
@@ -243,6 +267,38 @@ cdef class PyInitParameters:
     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(<char*>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(<char*> filename))
+
 
 cdef class PyRuntimeParameters:
     cdef RuntimeParameters* runtime
@@ -334,6 +390,30 @@ cdef class PyTrackingParameters:
     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():
@@ -386,6 +466,14 @@ cdef class PySpatialMappingParameters:
         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(<MAPPING_RESOLUTION> resolution.value, py_cam.camera)
+        elif isinstance(resolution, float):
+            return self.spatial.getRecommendedRange(<float> 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
@@ -499,6 +587,12 @@ cdef class PyPose:
     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
@@ -743,6 +837,13 @@ cdef class PyZEDCamera:
     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()
 

pyzed/core.pxd

@@ -58,6 +58,7 @@ cdef extern from "sl/Core.hpp" namespace "sl":
     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
@@ -291,3 +292,4 @@ cdef class PyCameraInformation:
     cdef unsigned int serial_number
     cdef unsigned int firmware_version
     cdef types.MODEL camera_model
+    cdef PyTransform py_camera_imu_transform