diff --git a/include/sl/c_api/types_c.h b/include/sl/c_api/types_c.h index 9d80ae0..20fe865 100644 --- a/include/sl/c_api/types_c.h +++ b/include/sl/c_api/types_c.h @@ -258,15 +258,16 @@ enum SL_ERROR_CODE { /** \brief Represents the available resolution defined in the \ref cameraResolution list. \note The VGA resolution does not respect the 640*480 standard to better fit the camera sensor (672*376 is used). +\warning All resolution are not available for every camera. You can find the available resolutions for each camera in our documentation. */ enum SL_RESOLUTION { SL_RESOLUTION_HD2K, /**< 2208*1242, available framerates: 15 fps.*/ - SL_RESOLUTION_HD1080, /**< 1920*1080, available framerates: 15, 30 fps.*/ - SL_RESOLUTION_HD1200, /**< 1920*1200 (x2), available framerates: 30,60 fps. (ZED-X(M) only)*/ + SL_RESOLUTION_HD1080, /**< 1920*1080, available framerates: 15, 30, 60 fps.*/ + SL_RESOLUTION_HD1200, /**< 1920*1200, available framerates: 15, 30, 60 fps.*/ SL_RESOLUTION_HD720, /**< 1280*720, available framerates: 15, 30, 60 fps.*/ - SL_RESOLUTION_SVGA, /**< 960*600 (x2), available framerates: 60, 120 fps. (ZED-X(M) only) */ + SL_RESOLUTION_SVGA, /**< 960*600, available framerates: 15, 30, 60, 120 fps.*/ SL_RESOLUTION_VGA, /**< 672*376, available framerates: 15, 30, 60, 100 fps.*/ - SL_RESOLUTION_AUTO, /**< Select the resolution compatible with camera, on ZEDX HD1200, HD720 otherwise */ + SL_RESOLUTION_AUTO, /**< Select the resolution compatible with camera, on ZED X HD1200, HD720 otherwise */ }; /** @@ -679,9 +680,9 @@ enum SL_AI_MODELS { SL_AI_MODELS_HUMAN_BODY_38_FAST_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_FAST SL_AI_MODELS_HUMAN_BODY_38_MEDIUM_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_FAST SL_AI_MODELS_HUMAN_BODY_38_ACCURATE_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_FAST - SL_AI_MODELS_HUMAN_BODY_70_FAST_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_FAST. - SL_AI_MODELS_HUMAN_BODY_70_MEDIUM_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_MEDIUM - SL_AI_MODELS_HUMAN_BODY_70_ACCURATE_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE + //SL_AI_MODELS_HUMAN_BODY_70_FAST_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_FAST. + //SL_AI_MODELS_HUMAN_BODY_70_MEDIUM_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_MEDIUM + //SL_AI_MODELS_HUMAN_BODY_70_ACCURATE_DETECTION, // related to sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE SL_AI_MODELS_PERSON_HEAD_DETECTION, // related to sl::DETECTION_MODEL::PERSON_HEAD_BOX SL_AI_MODELS_PERSON_HEAD_ACCURATE_DETECTION, // related to sl::DETECTION_MODEL::PERSON_HEAD_BOX_ACCURATE SL_AI_MODELS_REID_ASSOCIATION, // related to sl::BatchParameters::enable @@ -956,11 +957,11 @@ enum SL_BODY_70_PARTS #endif /** -\brief Change the type of outputed position for the Fusion positional tracking (raw data or fusion data projected into zed camera) +* \brief Change the type of outputted position (raw data or fusion data projected into zed camera). */ enum SL_POSITION_TYPE { - SL_POSITION_TYPE_RAW = 0, /*The output position will be the raw position data*/ - SL_POSITION_TYPE_FUSION, /*The output position will be the fused position projected into the requested camera repository*/ + SL_POSITION_TYPE_RAW,/**< The output position will be the raw position data. */ + SL_POSITION_TYPE_FUSION,/**< The output position will be the fused position projected into the requested camera repository. */ ///@cond SHOWHIDDEN SL_POSITION_TYPE_LAST ///@endcond @@ -1143,6 +1144,7 @@ struct SL_InitParameters The default behavior is synchronous, like previous ZED SDK versions */ bool async_grab_camera_recovery; + /** Define a computation upper limit to the grab frequency. This can be useful to get a known constant fixed rate or limit the computation load while keeping a short exposure time by setting a high camera capture framerate. @@ -1152,7 +1154,6 @@ struct SL_InitParameters default is 0. */ float grab_compute_capping_fps; - }; /** @@ -1232,11 +1233,12 @@ struct SL_CameraParameters { float fy; /**< Focal length in pixels along y axis. */ float cx; /**< Optical center along x axis, defined in pixels (usually close to width/2). */ float cy; /**< Optical center along y axis, defined in pixels (usually close to height/2). */ - double disto[5]; /**< Distortion factor : [ k1, k2, p1, p2, k3 ]. Radial (k1,k2,k3) and Tangential (p1,p2) distortion.*/ + double disto[12]; /**< Distortion factor : [ k1, k2, p1, p2, k3 ]. Radial (k1,k2,k3) and Tangential (p1,p2) distortion.*/ float v_fov; /**< Vertical field of view, in degrees. */ float h_fov; /**< Horizontal field of view, in degrees.*/ float d_fov; /**< Diagonal field of view, in degrees.*/ struct SL_Resolution image_size; /** size in pixels of the images given by the camera.*/ + float focal_length_metric; /**< real focal length in millimeters*/ }; /** @@ -1415,17 +1417,39 @@ d \warning: This mode requires more resources to run, but greatly improves track */ float depth_min_range; /** - * @brief This setting allows you to override 2 of the 3 rotations from initial_world_transform using the IMU gravity + * \brief This setting allows you to override 2 of the 3 rotations from initial_world_transform using the IMU gravity */ bool set_gravity_as_origin; /** - * @brief Positional tracking mode used. Can be used to improve accuracy in some type of scene at the cost of longer runtime + * \brief Positional tracking mode used. Can be used to improve accuracy in some type of scene at the cost of longer runtime * default : POSITIONAL_TRACKING_MODE::STANDARD */ enum SL_POSITIONAL_TRACKING_MODE mode; }; +/** + \class PlaneDetectionParameters + \brief Sets the plane detection parameters. + + The default constructor sets all parameters to their default settings. + */ + +struct SL_PlaneDetectionParameters { + + /** + \brief controls the spread of plane by checking the position difference. + \n default: 0.15 meters + */ + float max_distance_threshold; + + /** + \brief controls the spread of plane by checking the angle difference. + \n default: 15 degree + */ + float normal_similarity_threshold; +}; + /** \brief Recording structure that contains information about SVO. */ @@ -1716,7 +1740,7 @@ struct SL_ObjectDetectionParameters */ enum SL_OBJECT_FILTERING_MODE filtering_mode; /** - * @brief When an object is not detected anymore, the SDK will predict its positions during a short period of time before switching its state to SEARCHING. + * \brief When an object is not detected anymore, the SDK will predict its positions during a short period of time before switching its state to SEARCHING. * \n It prevents the jittering of the object state when there is a short misdetection. The user can define its own prediction time duration. * * \note During this time, the object will have OK state even if it is not detected. @@ -1848,7 +1872,7 @@ struct SL_BodyTrackingParameters { struct SL_BatchParameters batch_parameters; #endif /** - * @brief When an object is not detected anymore, the SDK will predict its positions during a short period of time before its state switched to SEARCHING. + * \brief When an object is not detected anymore, the SDK will predict its positions during a short period of time before its state switched to SEARCHING. * \n It prevents the jittering of the object state when there is a short misdetection. The user can define its own prediction time duration. * * \note During this time, the object will have OK state even if it is not detected. @@ -1893,7 +1917,7 @@ struct SL_BodyTrackingRuntimeParameters { */ int minimum_keypoints_threshold; /** - * @brief this value controls the smoothing of the fitted fused skeleton. + * \brief this value controls the smoothing of the fitted fused skeleton. * it is ranged from 0 (low smoothing) and 1 (high smoothing) * Default is 0; */ @@ -2308,33 +2332,41 @@ struct SL_InputType /////////////////////////////// FUSION API ///////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** +\enum SL_FUSION_ERROR_CODE +\brief Lists the types of error that can be raised by the Fusion. +*/ enum SL_FUSION_ERROR_CODE { - SL_FUSION_ERROR_CODE_NO_NEW_DATA_AVAILABLE = -10, /** < All data from all sources were consumed, no new process available.*/ - SL_FUSION_ERROR_CODE_INVALID_TIMESTAMP = -9, /** < Problem was detected with ingested timestamp*/ - SL_FUSION_ERROR_CODE_INVALID_COVARIANCE = -8, /** < Problem was detected with ingested covariance */ - SL_FUSION_ERROR_CODE_WRONG_BODY_FORMAT = -7, /**< The requested body tracking model is not available*/ - SL_FUSION_ERROR_CODE_NOT_ENABLE = -6, /**< The following module was not enabled*/ - SL_FUSION_ERROR_CODE_INPUT_FEED_MISMATCH = -5, /**< Some source are provided by SVO and some sources are provided by LIVE stream */ - SL_FUSION_ERROR_CODE_CONNECTION_TIMED_OUT = -4, /**< Connection timed out ... impossible to reach the sender... this may be due to ZedHub absence*/ - SL_FUSION_ERROR_CODE_MEMORY_ALREADY_USED = -3, /**< Detect multiple instance of SHARED_MEMORY communicator ... only one is authorised*/ - SL_FUSION_ERROR_CODE_BAD_IP_ADDRESS = -2, /**< The IP format provided is wrong, please provide IP in this format a.b.c.d where (a, b, c, d) are numbers between 0 and 255.*/ - SL_FUSION_ERROR_CODE_FAILURE = -1, /**< Standard code for unsuccessful behavior.*/ - SL_FUSION_ERROR_CODE_SUCCESS = 0, - SL_FUSION_ERROR_CODE_FUSION_ERRATIC_FPS = 1, /**< Some big differences has been observed between senders FPS*/ - SL_FUSION_ERROR_CODE_FUSION_FPS_TOO_LOW = 2 /**< At least one sender has fps lower than 10 FPS*/ -}; - + SL_FUSION_ERROR_CODE_WRONG_BODY_FORMAT = -7, /**< Senders are using different body formats. Consider changing them. */ + SL_FUSION_ERROR_CODE_NOT_ENABLE = -6, /**< The following module was not enabled. */ + SL_FUSION_ERROR_CODE_INPUT_FEED_MISMATCH = -5, /**< Some sources are provided by SVO and others by LIVE stream. */ + SL_FUSION_ERROR_CODE_CONNECTION_TIMED_OUT = -4, /**< Connection timed out. Unable to reach the sender. Verify the sender's IP/port. */ + SL_FUSION_ERROR_CODE_MEMORY_ALREADY_USED = -3, /**< Intra-process shared memory allocation issue. Multiple connections to the same data. */ + SL_FUSION_ERROR_CODE_BAD_IP_ADDRESS = -2, /**< The provided IP address format is incorrect. Please provide the IP in the format 'a.b.c.d', where (a, b, c, d) are numbers between 0 and 255. */ + SL_FUSION_ERROR_CODE_FAILURE = -1, /**< Standard code for unsuccessful behavior. */ + SL_FUSION_ERROR_CODE_SUCCESS = 0, /**< Standard code for successful behavior. */ + SL_FUSION_ERROR_CODE_FUSION_ERRATIC_FPS = 1, /**< Significant differences observed between sender's FPS. */ + SL_FUSION_ERROR_CODE_FUSION_FPS_TOO_LOW = 2, /**< At least one sender has an FPS lower than 10 FPS. */ + SL_FUSION_ERROR_CODE_INVALID_TIMESTAMP = 3, /**< Problem detected with ingested timestamp. Sample data will be ignored. */ + SL_FUSION_ERROR_CODE_INVALID_COVARIANCE = 4, /**< Problem detected with ingested covariance. Sample data will be ignored. */ + SL_FUSION_ERROR_CODE_NO_NEW_DATA_AVAILABLE = 5 /**< All data from all sources has been consumed. No new data is available for processing. */ +}; + +/** +\enum SL_SENDER_ERROR_CODE +\brief Lists the types of error that can be raised during the Fusion by senders. +*/ enum SL_SENDER_ERROR_CODE { - SL_SENDER_ERROR_CODE_DISCONNECTED = -1, /**< the sender has been disconnected*/ - SL_SENDER_ERROR_CODE_SUCCESS = 0, - SL_SENDER_ERROR_CODE_GRAB_ERROR = 1, /**< the sender has encountered an grab error*/ - SL_SENDER_ERROR_CODE_ERRATIC_FPS = 2, /**< the sender does not run with a constant frame rate*/ - SL_SENDER_ERROR_CODE_FPS_TOO_LOW = 3 /**< fps lower than 10 FPS*/ + SL_SENDER_ERROR_CODE_DISCONNECTED = -1, /**< The sender has been disconnected.*/ + SL_SENDER_ERROR_CODE_SUCCESS = 0, /**< Standard code for successful behavior.*/ + SL_SENDER_ERROR_CODE_GRAB_ERROR = 1, /**< The sender encountered a grab error.*/ + SL_SENDER_ERROR_CODE_ERRATIC_FPS = 2, /**< The sender does not run with a constant frame rate.*/ + SL_SENDER_ERROR_CODE_FPS_TOO_LOW = 3 /**< The frame rate of the sender is lower than 10 FPS.*/ }; enum SL_COMM_TYPE { - SL_COMM_TYPE_LOCAL_NETWORK, /* the sender and receiver are on the samed local network and communicate by RTP, communication can be affected by the network load.*/ + SL_COMM_TYPE_LOCAL_NETWORK, /* the sender and receiver are on the same local network and communicate by RTP, communication can be affected by the network load.*/ SL_COMM_TYPE_INTRA_PROCESS /* both sender and receiver are declared by the same process, can be in different threads, this communication is optimized.*/ }; @@ -2345,137 +2377,197 @@ struct SL_CommunicationParameters char ip_add[128]; }; +/** +\brief Useful struct to store the Fusion configuration, can be read from /write to a JSON file. + */ struct SL_FusionConfiguration { + /** + The serial number of the used ZED camera. + */ int serial_number; + + /** + The communication parameters to connect this camera to the Fusion. + */ struct SL_CommunicationParameters comm_param; + + /** + The WORLD SL_Vector3 of the camera for Fusion. + */ struct SL_Vector3 position; + + /** + The WORLD SL_Quaternion of the camera for Fusion. + */ struct SL_Quaternion rotation; + + /** + The input type for the current camera. + */ struct SL_InputType input_type; }; +/** +\brief Holds the options used to initialize the \ref Fusion object. + */ struct SL_InitFusionParameters { /** - This parameter allows you to select the unit to be used for all metric values of the SDK. (depth, point cloud, tracking, mesh, and others). - \n default : \ref UNIT "UNIT::MILLIMETER" + * \brief This parameter allows you to select the unit to be used for all metric values of the SDK (depth, point cloud, tracking, mesh, and others). + * + * Default : \ref SL_UNIT "SL_UNIT_MILLIMETER" */ enum SL_UNIT coordinate_units; /** - Positional tracking, point clouds and many other features require a given \ref COORDINATE_SYSTEM to be used as reference. - This parameter allows you to select the \ref COORDINATE_SYSTEM used by the \ref Camera to return its measures. - \n This defines the order and the direction of the axis of the coordinate system. - \n default : \ref COORDINATE_SYSTEM "COORDINATE_SYSTEM::IMAGE" + * \brief Positional tracking, point clouds and many other features require a given \ref SL_COORDINATE_SYSTEM to be used as reference. + * This parameter allows you to select the \ref COORDINATE_SYSTEM used by the \ref SL_Camera to return its measures. + * + * This defines the order and the direction of the axis of the coordinate system. + * \n Default : \ref SL_COORDINATE_SYSTEM "SL_COORDINATE_SYSTEM_IMAGE" */ enum SL_COORDINATE_SYSTEM coordinate_system; /** - * @brief it allows users to extract some stats of the Fusion API like drop frame of each camera, latency, etc + * \brief It allows users to extract some stats of the Fusion API like drop frame of each camera, latency, etc... * */ bool output_performance_metrics; + /** + * \brief Enable the verbosity mode of the SDK. + * + */ bool verbose; /** * @brief If specified change the number of period necessary for a source to go in timeout without data. For example, if you set this to 5 * then, if any source do not receive data during 5 period, these sources will go to timeout and will be ignored. - * + * */ unsigned int timeout_period_number; }; /** - * - * +\brief Holds the options used to initialize the body tracking module of the \ref Fusion. */ -struct SL_PositionalTrackingFusionParameters { - /** - * @brief Is the gnss fusion is enabled - * - */ - bool enable_GNSS_fusion; - /** - * @brief Distance necessary for initializing the transformation between cameras coordinate system and GNSS coordinate system (north aligned) - * - */ - float gnss_initialisation_distance; - /** - * @brief Value used by Fusion for ignoring high covariance input GNSS data - * - */ - float gnss_ignore_threshold; -}; - struct SL_BodyTrackingFusionParameters { /** - * \brief Defines if the object detection will track objects across images flow + * \brief Defines if the object detection will track objects across images flow. + * + * Default: true */ bool enable_tracking; /** * \brief Defines if the body fitting will be applied + * + * Default: false + * \note If you enable it and the camera provides data as BODY_18 the fused body format will be BODY_34. */ bool enable_body_fitting; }; +/** +\brief Holds the options used to change the behavior of the body tracking module at runtime. + */ struct SL_BodyTrackingFusionRuntimeParameters { /** - * @brief if the fused skeleton has less than skeleton_minimum_allowed_keypoints keypoints, it will be discarded. Default is -1. - * + * \brief If the fused skeleton has less than skeleton_minimum_allowed_keypoints keypoints, it will be discarded. + * + * Default: -1. */ int skeleton_minimum_allowed_keypoints; /** - * @brief if a skeleton was detected in less than skeleton_minimum_allowed_camera cameras, it will be discarded + * \brief If a skeleton was detected in less than skeleton_minimum_allowed_camera cameras, it will be discarded. * + * Default: -1. */ int skeleton_minimum_allowed_camera; /** - * @brief this value controls the smoothing of the tracked or fitted fused skeleton. - * it is ranged from 0 (low smoothing) and 1 (high smoothing) + * \brief This value controls the smoothing of the tracked or fitted fused skeleton. + * + * It is ranged from 0 (low smoothing) and 1 (high smoothing). + * \n Default: 0. */ float skeleton_smoothing; }; +/** +\brief Used to identify a specific camera in the Fusion API + */ struct SL_CameraIdentifier { unsigned long long int sn; }; +/** +\brief Holds the metrics of a sender in the fusion process. + */ struct SL_CameraMetrics { struct SL_CameraIdentifier uuid; - // gives the fps of the received datas + /** + * \brief FPS of the received data. + * + */ float received_fps; - // gives the latency (in second) of the received datas + /** + * \brief Latency (in seconds) of the received data. + * + */ float received_latency; - // gives the latency (in second) after Fusion synchronization + /** + * \brief Latency (in seconds) after Fusion synchronization. + * + */ float synced_latency; - // if no data present is set to false + /** + * \brief If no data present is set to false. + * + */ bool is_present; - // percent of detection par image during the last second in %, a low values means few detections occurs lately + /** + * \brief Gives the percent of detection par image during the last second in %, a low value means few detections occurs lately. + * + */ float ratio_detection; - // percent of detection par image during the last second in %, a low values means few detections occurs lately + /** + * \brief Average time difference for the current fused data. + * + */ float delta_ts; }; +/** +\brief Holds the metrics of the fusion process. + */ struct SL_FusionMetrics { - - // mean number of camera that provides data during the past second + /** + * \brief Mean number of camera that provides data during the past second. + * + */ float mean_camera_fused; - // mean number of camera that provides data during the past second + /** + * \brief Standard deviation of the data timestamp fused, the lower the better. + * + */ float mean_stdev_between_camera; + /** + * \brief Sender metrics. + * + */ struct SL_CameraMetrics camera_individual_stats[MAX_FUSED_CAMERAS]; }; @@ -2484,55 +2576,317 @@ struct SL_FusionMetrics { /////////////////////////////// GNSS API ////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** +\brief Current state of GNSS fusion. +*/ +enum SL_GNSS_CALIBRATION_STATE { + SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED = 0, /**< The GNSS/VIO calibration has not been completed yet. Please continue moving the robot while ingesting GNSS data to perform the calibration.*/ + SL_GNSS_CALIBRATION_STATE_CALIBRATED = 1, /**< The GNSS/VIO calibration is completed.*/ + SL_GNSS_CALIBRATION_STATE_RE_CALIBRATION_IN_PROGRESS = 2 /**< A GNSS/VIO re-calibration is in progress in the background. Current geo-tracking services may not be entirely accurate.*/ +}; + + +/** +\brief Contains all GNSS data to be used for positional tracking as prior. + */ struct SL_GNSSData { - // longitude in radian + /** + * \brief Longitude in radian. + * + */ double longitude; - // latitude in radian + /** + * \brief Latitude in radian. + * + */ double latitude; - // altitude in meter + /** + * \brief Altitude in meters. + * + */ double altitude; - // Timestamp + /** + * \brief Timestamp of GNSS position, must be aligned with camera time reference. + * + */ unsigned long long ts; - + /** + * \brief Position covariance in meter must be expressed in ENU coordinate system. + * For eph, epv GNSS sensors, set it as follow: {eph*eph, 0, 0, 0, eph*eph, 0, 0, 0, epv*epv}. + * + */ double position_covariance[9]; - + /** + * \brief Longitude standard deviation. + * + */ double longitude_std; + /** + * \brief Latitude standard deviation. + * + */ double latitude_std; + /** + * \brief Altitude standard deviation. + * + */ double altitude_std; }; +/** + * \brief Represents a world position in LatLng format. + * + */ struct SL_LatLng { + /** + * \brief Latitude coordinate in radian. + * + */ double latitude; + /** + * \brief Longitude coordinate in radian. + * + */ double longitude; + /** + * \brief Altitude coordinate in meters. + * + */ double altitude; }; - +/** + * \brief Holds Geo reference position. + * + */ struct SL_GeoPose { + /** + * The translation defining the pose in ENU. + */ struct SL_Vector3 translation; + /** + * The rotation defining the pose in ENU. + */ struct SL_Quaternion rotation; + /** + * The pose covariance in ENU. + */ float pose_covariance[36]; + /** + * The horizontal accuracy. + */ double horizontal_accuracy; + /** + * The vertical accuracy. + */ double vertical_accuracy; + /** + * The latitude, longitude, altitude. + */ struct SL_LatLng latlng_coordinates; + /** + * The heading. + */ double heading; + /** + * \brief The timestamp of SL_GeoPose. + * + */ + unsigned long long timestamp; }; +/** + * \brief Represents a world position in ECEF format. + * + */ struct SL_ECEF { + /** + * \brief x coordinate of SL_ECEF. + * + */ double x; + /** + * \brief y coordinate of SL_ECEF. + * + */ double y; + /** + * \brief z coordinate of SL_ECEF. + * + */ double z; }; + +/** + * \brief Represents a world position in UTM format. + * + */ struct SL_UTM { + /** + * \brief Northing coordinate. + * + */ double northing; + /** + * \brief Easting coordinate. + * + */ double easting; + /** + * \brief Gamma coordinate. + * + */ double gamma; + /** + * \brief UTMZone of the coordinate. + * + */ char UTMZone[256]; }; +/** + * \brief Holds the options used for calibrating GNSS / VIO. +*/ +struct SL_GNSSCalibrationParameters { + /** + * \brief This parameter defines the target yaw uncertainty at which the calibration process between GNSS and VIO concludes. + * The unit of this parameter is in radian. + * + * Default: 0.1 radians + */ + float target_yaw_uncertainty; + /** + * @brief When this parameter is enabled (set to true), the calibration process between GNSS and VIO accounts for the uncertainty in the determined translation, thereby facilitating the calibration termination. + * The maximum allowable uncertainty is controlled by the 'target_translation_uncertainty' parameter. + * + * Default: false + */ + bool enable_translation_uncertainty_target; + /** + * \brief This parameter defines the target translation uncertainty at which the calibration process between GNSS and VIO concludes. + * + * Default: 10e-2 (10 centimeters) + */ + float target_translation_uncertainty; + /** + * \brief This parameter determines whether reinitialization should be performed between GNSS and VIO fusion when a significant disparity is detected between GNSS data and the current fusion data. + * It becomes particularly crucial during prolonged GNSS signal loss scenarios. + * + * Default: true + */ + bool enable_reinitialization; + /** + * \brief This parameter determines the threshold for GNSS/VIO reinitialization. + * If the fused position deviates beyond out of the region defined by the product of the GNSS covariance and the gnss_vio_reinit_threshold, a reinitialization will be triggered. + * + * Default: 5 + */ + float gnss_vio_reinit_threshold; + /** + * \brief If this parameter is set to true, the fusion algorithm will used a rough VIO / GNSS calibration at first and then refine it. + * This allow you to quickly get a fused position. + * + * Default: true + */ + bool enable_rolling_calibration; +}; + +/** + * \brief Holds the options used for initializing the positional tracking fusion module. + * + */ +struct SL_PositionalTrackingFusionParameters { + /** + * \brief This attribute is responsible for enabling or not GNSS positional tracking fusion. + * + * Default: false + */ + bool enable_GNSS_fusion; + /** + * \brief Control the VIO / GNSS calibration process. + * + */ + struct SL_GNSSCalibrationParameters gnss_calibration_parameters; +}; + +#if 0 + +/////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////// Spatial Mapping //////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/** +\class SpatialMappingFusionParameters +\ingroup Fusion_group +\brief Sets the spatial mapping parameters. + +Instantiating with the default constructor will set all parameters to their default values. +\n You can customize these values to fit your application, and then save them to a preset to be loaded in future executions. + +\note Users can adjust these parameters as they see fit. + */ +struct SL_SpatialMappingFusionParameters +{ + /** + \brief Spatial mapping resolution in meters. Should fit \ref allowed_resolution. + * Default is 0.05f meter. + */ + float resolution_meter; + + /** + \brief Depth range in meters. + Can be different from the value set by \ref sl::InitParameters::depth_maximum_distance. + \n Set to 0 by default. In this case, the range is computed from resolution_meter + and from the current internal parameters to fit your application. + */ + float range_meter; + + /** + \brief Set to false if you want to ensure consistency between the mesh and its inner chunk data. + * Set to false by Default. + \note Updating the mesh is time-consuming. Setting this to true results in better performance. + */ + bool use_chunk_only; + + /** + \brief The maximum CPU memory (in MB) allocated for the meshing process. + * Default is 2048 MB. + */ + int max_memory_usage; + + /** + * \brief Control the disparity noise (standard deviation) in px. set a very small value (<0.1) if the depth map of the scene is accurate. + * set a big value (>0.5) if the depth map is noisy. + * Default is 0.3f. + */ + float disparity_std; + + /** + \brief Adjust the weighting factor for the current depth during the integration process. + By default, the value is set to 1, which results in the complete integration and fusion of the current depth with the previously integrated depth. + Setting it to 0 discards all previous data and solely integrates the current depth. + */ + float decay; + + bool enable_forget_past; + + /** + \brief Control the integration rate of the current depth into the mapping process. + This parameter controls how many times a stable 3D points should be seen before it is integrated into the spatial mapping. + Default value is 0, this will define the stability counter based on the mesh resolution, the higher the resolution, the higher the stability counter. + */ + int stability_counter; + /** + \brief The type of spatial map to be created. This dictates the format that will be used for the mapping(e.g. mesh, point cloud). See \ref SPATIAL_MAP_TYPE + * Default value is SL_SPATIAL_MAP_TYPE_MESH + */ + enum SL_SPATIAL_MAP_TYPE map_type; +}; +#endif + #endif diff --git a/include/sl/c_api/zed_interface.h b/include/sl/c_api/zed_interface.h index 36d1fa5..0b80a78 100644 --- a/include/sl/c_api/zed_interface.h +++ b/include/sl/c_api/zed_interface.h @@ -116,7 +116,7 @@ extern "C" { /** \brief Defines a region of interest to focus on for all the SDK, discarding other parts. \param camera_id of the camera instance. - \param roi_mask: the Mat defining the requested region of interest, all pixel set to 0 will be discard. If empty, set all pixels as valid, otherwise should fit the resolution of the current instance and its type should be U8_C1. + \param roi_mask: the Mat defining the requested region of interest, pixels lower than 127 will be discard. If empty, set all pixels as valid, otherwise should fit the resolution of the current instance and its type should be U8_C1/C3/C4. \return An ERROR_CODE if something went wrong. */ INTERFACE_API int sl_set_region_of_interest(int camera_id, void* roi_mask); @@ -853,7 +853,7 @@ extern "C" { \param thres : Check if area is enough for Unity. If true, removes smaller planes. \return Data of the detected plane. */ - INTERFACE_API struct SL_PlaneData* sl_find_plane_at_hit(int camera_id, struct SL_Vector2 pixel, bool thres); + INTERFACE_API struct SL_PlaneData* sl_find_plane_at_hit(int camera_id, struct SL_Vector2 pixel, struct SL_PlaneDetectionParameters* params, bool thres); /** \brief Using data from a detected floor plane, updates supplied vertex and triangles arrays with data needed to make a mesh that represents it. \param camera_id : id of the camera instance. @@ -1236,14 +1236,21 @@ extern "C" { INTERFACE_API enum SL_FUSION_ERROR_CODE sl_fusion_update_pose(struct SL_CameraIdentifier* uuid, struct SL_Vector3* pose_translation, struct SL_Quaternion* pose_rotation); /* - * \brief update the pose of the camera in the fusion coordinate space - * \param [in] uuid : unique ID that is associated with the camera for easy access. - * \param [in] pose_translation : new position of the camera - * \param [in] pose_rotation : new orientation of the camera - * \return SL_FUSION_ERROR_CODE + * \brief Returns the state of a connected data sender. + * \param [in] uuid : Identifier of the camera. + * \return SL_SENDER_ERROR_CODE : State of the sender * */ INTERFACE_API enum SL_SENDER_ERROR_CODE sl_fusion_get_sender_state(struct SL_CameraIdentifier* uuid); + /** + \brief Read a Configuration JSON file to configure a fusion process. + \param json_config_filename : The name of the JSON file containing the configuration + \param coord_sys : The COORDINATE_SYSTEM in which you want the World Pose to be in. + \param unit : The UNIT in which you want the World Pose to be in. + + \return a vector of \ref SL_FusionConfiguration for all the camera present in the file. + \note empty if no data were found for the requested camera. + */ INTERFACE_API void sl_fusion_read_configuration_file(char json_config_filename[256], enum SL_COORDINATE_SYSTEM coord_system, enum SL_UNIT unit, struct SL_FusionConfiguration* configs, int* nb_cameras); ///////////////////////////////////////////////////////////////////// @@ -1299,7 +1306,7 @@ extern "C" { * \param uuid Camera identifier * \return POSITIONAL_TRACKING_STATE is the current state of the tracking process */ - INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_get_position(struct SL_PoseData* pose, enum SL_REFERENCE_FRAME reference_frame, enum SL_COORDINATE_SYSTEM coordinate_system, enum SL_UNIT unit, + INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_get_position(struct SL_PoseData* pose, enum SL_REFERENCE_FRAME reference_frame, enum SL_UNIT unit, struct SL_CameraIdentifier* uuid, enum SL_POSITION_TYPE retrieve_type); /** @@ -1319,7 +1326,7 @@ extern "C" { INTERFACE_API enum SL_FUSION_ERROR_CODE sl_fusion_ingest_gnss_data(struct SL_GNSSData* gnss_data, bool radian); /** - * @brief returns the current GNSS data + * \brief returns the current GNSS data * \param out [out]: the current GNSS data * \param radian [in] : true if the gnss data is set in radian * \return POSITIONAL_TRACKING_STATE is the current state of the tracking process @@ -1327,29 +1334,50 @@ extern "C" { INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_get_current_gnss_data(struct SL_GNSSData* data, bool radian); /** - * @brief returns the current GeoPose + * \brief returns the current GeoPose * \param pose [out]: the current GeoPose * \param radian [in] : true if the geopose is set in radian. - * \return POSITIONAL_TRACKING_STATE is the current state of the tracking process + * \return GNSS_CALIBRATION_STATE is the current state of the tracking process */ - INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_get_geo_pose(struct SL_GeoPose* pose, bool radian); + INTERFACE_API enum SL_GNSS_CALIBRATION_STATE sl_fusion_get_geo_pose(struct SL_GeoPose* pose, bool radian); /** * \brief Convert latitude / longitude into position in sl::Fusion coordinate system. * \param in: the current GeoPose * \param out [out]: the current Pose * \param radian [in] : true if the geopose is set in radian. - * \return POSITIONAL_TRACKING_STATE is the current state of the tracking process + * \return GNSS_CALIBRATION_STATE is the current state of the tracking process */ - INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_geo_to_camera(struct SL_LatLng* in, struct SL_PoseData* out, bool radian); + INTERFACE_API enum SL_GNSS_CALIBRATION_STATE sl_fusion_geo_to_camera(struct SL_LatLng* in, struct SL_PoseData* out, bool radian); /** - * @brief returns the current GeoPose + * \brief returns the current GeoPose * \param pose [out]: the current GeoPose * \param radian [in] : true if the geopose is set in radian. - * \return POSITIONAL_TRACKING_STATE is the current state of the tracking process + * \return GNSS_CALIBRATION_STATE is the current state of the tracking process + */ + INTERFACE_API enum SL_GNSS_CALIBRATION_STATE sl_fusion_camera_to_geo(struct SL_PoseData* in, struct SL_GeoPose* out, bool radian); + + /** + * @brief returns the current timestamp + * \return the current timestamp in nanoseconds. + */ + INTERFACE_API unsigned long long sl_fusion_get_current_timestamp(); + + /** + * \brief Get the current calibration uncertainty defined during calibration process + * + * @param yaw_std [out] yaw uncertainty + * @param x_std [out] position uncertainty + * \return sl_GNSS_CALIBRATION_STATE representing current initialisation status + */ + INTERFACE_API enum SL_GNSS_CALIBRATION_STATE sl_fusion_get_current_gnss_calibration_std(float* yaw_std, struct SL_Vector3* position_std); + + /** + * \brief Get the calibration found between VIO and GNSS + * \return sl::Transform transform containing calibration found between VIO and GNSS */ - INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_camera_to_geo(struct SL_PoseData* in, struct SL_GeoPose* out, bool radian); + INTERFACE_API void sl_fusion_get_geo_tracking_calibration(struct SL_Vector3* translation, struct SL_Quaternion* rotation); /** \brief Close Multi Camera instance. diff --git a/src/ZEDController.cpp b/src/ZEDController.cpp index 291c415..3df1392 100644 --- a/src/ZEDController.cpp +++ b/src/ZEDController.cpp @@ -267,6 +267,9 @@ SL_InitParameters* ZEDController::getInitParameters() { initParams->async_grab_camera_recovery = initParameters.async_grab_camera_recovery; + initParams->grab_compute_capping_fps = initParameters.grab_compute_capping_fps; + + return initParams; } @@ -760,7 +763,7 @@ SL_PlaneData* ZEDController::findFloorPlane(SL_Quaternion *resetQuaternion, SL_V return ¤tFloorPlane; } -SL_PlaneData* ZEDController::findPlaneAtHit(SL_Vector2 pixels, bool thres) { +SL_PlaneData* ZEDController::findPlaneAtHit(SL_Vector2 pixels, struct SL_PlaneDetectionParameters* params, bool thres) { if (!isNull()) { sdk_mutex.lock(); sl::Transform resetTransform; @@ -769,7 +772,12 @@ SL_PlaneData* ZEDController::findPlaneAtHit(SL_Vector2 pixels, bool thres) { pos.y = (unsigned int) pixels.y; memset(¤tPlaneAtHitSDK, 0, sizeof (sl::Plane)); - sl::ERROR_CODE res = zed.findPlaneAtHit(pos, currentPlaneAtHitSDK); + + sl::PlaneDetectionParameters sdk_params; + sdk_params.max_distance_threshold = params->max_distance_threshold; + sdk_params.normal_similarity_threshold = params->normal_similarity_threshold; + + sl::ERROR_CODE res = zed.findPlaneAtHit(pos, currentPlaneAtHitSDK, sdk_params); currentPlaneAtHit.error_code = (int) res; @@ -925,7 +933,7 @@ SL_CameraParameters convertCamParameters(sl::CameraParameters input) { output.cy = input.cy; output.fx = input.fx; output.fy = input.fy; - memcpy(&output.disto[0], &input.disto[0], sizeof(double) * 5); + memcpy(&output.disto[0], &input.disto[0], sizeof(double) * 12); output.d_fov = input.d_fov; output.h_fov = input.h_fov; @@ -933,6 +941,8 @@ SL_CameraParameters convertCamParameters(sl::CameraParameters input) { output.image_size.height = input.image_size.height; output.image_size.width = input.image_size.width; + output.focal_length_metric = input.focal_length_metric; + return output; } @@ -1200,7 +1210,7 @@ sl::ERROR_CODE ZEDController::retrieveChunks(const int maxSubmesh, float* vertic memcpy(&vertices[offsetVertices], mesh.chunks[i].vertices.data(), sizeof (sl::float3) * int(mesh.chunks[i].vertices.size())); memcpy(&triangles[offsetTriangles], mesh.chunks[i].triangles.data(), sizeof (sl::uint3) * int(mesh.chunks[i].triangles.size())); memcpy(&colors[offsetColors], mesh.chunks[i].colors.data(), sizeof(sl::uchar3) * int(mesh.chunks[i].colors.size())); - + offsetVertices += int(3 * mesh.chunks[i].vertices.size()); offsetTriangles += int(3 * mesh.chunks[i].triangles.size()); offsetColors += int(3 * mesh.chunks[i].colors.size()); diff --git a/src/ZEDController.hpp b/src/ZEDController.hpp index d39ca66..783eacd 100644 --- a/src/ZEDController.hpp +++ b/src/ZEDController.hpp @@ -140,7 +140,7 @@ class ZEDController { ******* Plane Detection ********* *************************************/ SL_PlaneData* findFloorPlane(SL_Quaternion *resetQuaternion, SL_Vector3* resetTranslation, SL_Quaternion priorQuaternion, SL_Vector3 priorTranslation); - SL_PlaneData* findPlaneAtHit(SL_Vector2 pixel, bool thres); + SL_PlaneData* findPlaneAtHit(SL_Vector2 pixel, struct SL_PlaneDetectionParameters* params, bool thres); sl::ERROR_CODE convertCurrentFloorPlaneToChunk(float* vertices, int* triangles, int* numVerticesTot, int* numTrianglesTot); sl::ERROR_CODE convertCurrentHitPlaneToChunk(float* vertices, int* triangles, int* numVerticesTot, int* numTrianglesTot); diff --git a/src/ZEDFusionController.cpp b/src/ZEDFusionController.cpp index 464069a..5cbfa66 100644 --- a/src/ZEDFusionController.cpp +++ b/src/ZEDFusionController.cpp @@ -213,7 +213,7 @@ SL_FUSION_ERROR_CODE ZEDFusionController::retrieveBodies(struct SL_Bodies* data, } -SL_FUSION_ERROR_CODE ZEDFusionController::getProcessMetrics(SL_FusionMetrics* metrics) +SL_FUSION_ERROR_CODE ZEDFusionController::getProcessMetrics(struct SL_FusionMetrics* metrics) { memset(metrics, 0, sizeof(SL_FusionMetrics)); sl::FusionMetrics sdk_metrics; @@ -253,7 +253,7 @@ SL_FUSION_ERROR_CODE ZEDFusionController::getProcessMetrics(SL_FusionMetrics* me return (SL_FUSION_ERROR_CODE)err; } -SL_SENDER_ERROR_CODE ZEDFusionController::getSenderState(SL_CameraIdentifier* uuid) +SL_SENDER_ERROR_CODE ZEDFusionController::getSenderState(struct SL_CameraIdentifier* uuid) { SL_SENDER_ERROR_CODE err = SL_SENDER_ERROR_CODE_DISCONNECTED; @@ -322,7 +322,7 @@ inline SL_InputType getInput(sl::InputType::INPUT_TYPE type, sl::String conf) { return input; } -void ZEDFusionController::readFusionConfigFile(char json_config_filename[256], SL_COORDINATE_SYSTEM coord_system, SL_UNIT unit, SL_FusionConfiguration* configs, int& nb_cameras) +void ZEDFusionController::readFusionConfigFile(char json_config_filename[256], enum SL_COORDINATE_SYSTEM coord_system, enum SL_UNIT unit, struct SL_FusionConfiguration* configs, int& nb_cameras) { auto sdk_configs = sl::readFusionConfigurationFile(std::string(json_config_filename), (sl::COORDINATE_SYSTEM)coord_system, (sl::UNIT)unit); @@ -334,6 +334,7 @@ void ZEDFusionController::readFusionConfigFile(char json_config_filename[256], S { sl::FusionConfiguration sdk_config = sdk_configs[i]; + SL_FusionConfiguration fusion_config; memset(&fusion_config, 0, sizeof(SL_FusionConfiguration)); @@ -361,17 +362,17 @@ void ZEDFusionController::readFusionConfigFile(char json_config_filename[256], S } } -SL_FUSION_ERROR_CODE ZEDFusionController::enablePositionalTracking(SL_PositionalTrackingFusionParameters* params) +SL_FUSION_ERROR_CODE ZEDFusionController::enablePositionalTracking(struct SL_PositionalTrackingFusionParameters* params) { sl::PositionalTrackingFusionParameters sdk_params; sdk_params.enable_GNSS_fusion = params->enable_GNSS_fusion; - sdk_params.gnss_ignore_threshold = params->gnss_ignore_threshold; - sdk_params.gnss_initialisation_distance = params->gnss_initialisation_distance; + + memcpy(&sdk_params.gnss_calibration_parameters, ¶ms->gnss_calibration_parameters, sizeof(SL_GNSSCalibrationParameters)); return (SL_FUSION_ERROR_CODE)fusion.enablePositionalTracking(sdk_params); } -SL_POSITIONAL_TRACKING_STATE ZEDFusionController::getPosition(SL_PoseData* poseData, enum SL_REFERENCE_FRAME reference_frame, struct SL_CameraIdentifier* uuid, SL_POSITION_TYPE retrieve_type) +SL_POSITIONAL_TRACKING_STATE ZEDFusionController::getPosition(SL_PoseData* poseData, enum SL_REFERENCE_FRAME reference_frame, struct SL_CameraIdentifier* uuid, enum SL_POSITION_TYPE retrieve_type) { sl::CameraIdentifier sdk_uuid; sdk_uuid.sn = uuid->sn; @@ -405,7 +406,7 @@ void ZEDFusionController::disablePositionalTracking() { fusion.disablePositionalTracking(); } -SL_FUSION_ERROR_CODE ZEDFusionController::ingestGNSSData(SL_GNSSData* data, bool radian) +SL_FUSION_ERROR_CODE ZEDFusionController::ingestGNSSData(struct SL_GNSSData* data, bool radian) { sl::GNSSData sdk_gnss; sdk_gnss.setCoordinates(data->latitude, data->longitude, data->altitude, radian); @@ -422,7 +423,7 @@ SL_FUSION_ERROR_CODE ZEDFusionController::ingestGNSSData(SL_GNSSData* data, bool return (SL_FUSION_ERROR_CODE)fusion.ingestGNSSData(sdk_gnss); } -SL_POSITIONAL_TRACKING_STATE ZEDFusionController::getCurrentGNSSData(SL_GNSSData* data, bool radian) +SL_POSITIONAL_TRACKING_STATE ZEDFusionController::getCurrentGNSSData(struct SL_GNSSData* data, bool radian) { SL_POSITIONAL_TRACKING_STATE state = SL_POSITIONAL_TRACKING_STATE_OFF; @@ -445,14 +446,14 @@ SL_POSITIONAL_TRACKING_STATE ZEDFusionController::getCurrentGNSSData(SL_GNSSData return state; } -SL_POSITIONAL_TRACKING_STATE ZEDFusionController::getGeoPose(SL_GeoPose* pose, bool radian) +enum SL_GNSS_CALIBRATION_STATE ZEDFusionController::getGeoPose(struct SL_GeoPose* pose, bool radian) { - SL_POSITIONAL_TRACKING_STATE state = SL_POSITIONAL_TRACKING_STATE_OFF; + SL_GNSS_CALIBRATION_STATE state = SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED; memset(pose, 0, sizeof(SL_GeoPose)); sl::GeoPose sdk_pose; - state = (SL_POSITIONAL_TRACKING_STATE)fusion.getGeoPose(sdk_pose); + state = (SL_GNSS_CALIBRATION_STATE)fusion.getGeoPose(sdk_pose); pose->heading = sdk_pose.heading; pose->horizontal_accuracy = sdk_pose.horizontal_accuracy; @@ -460,6 +461,7 @@ SL_POSITIONAL_TRACKING_STATE ZEDFusionController::getGeoPose(SL_GeoPose* pose, b pose->latlng_coordinates.latitude = sdk_pose.latlng_coordinates.getLatitude(radian); pose->latlng_coordinates.longitude = sdk_pose.latlng_coordinates.getLongitude(radian); pose->latlng_coordinates.altitude = sdk_pose.latlng_coordinates.getAltitude(); + pose->timestamp = sdk_pose.timestamp.getNanoseconds(); sl::float3 sdk_position = sdk_pose.pose_data.getTranslation(); pose->translation.x = sdk_position.x; @@ -480,9 +482,9 @@ SL_POSITIONAL_TRACKING_STATE ZEDFusionController::getGeoPose(SL_GeoPose* pose, b return state; } -SL_POSITIONAL_TRACKING_STATE ZEDFusionController::geoToCamera(SL_LatLng* in, SL_PoseData* out, bool radian) +enum SL_GNSS_CALIBRATION_STATE ZEDFusionController::geoToCamera(struct SL_LatLng* in, struct SL_PoseData* out, bool radian) { - SL_POSITIONAL_TRACKING_STATE state = SL_POSITIONAL_TRACKING_STATE_OFF; + enum SL_GNSS_CALIBRATION_STATE state = SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED; memset(out, 0, sizeof(SL_PoseData)); @@ -490,7 +492,7 @@ SL_POSITIONAL_TRACKING_STATE ZEDFusionController::geoToCamera(SL_LatLng* in, SL_ latLng.setCoordinates(in->latitude, in->longitude, in->altitude, radian); sl::Pose pose; - state = (SL_POSITIONAL_TRACKING_STATE)fusion.Geo2Camera(latLng, pose); + state = (enum SL_GNSS_CALIBRATION_STATE)fusion.Geo2Camera(latLng, pose); out->pose_confidence = pose.pose_confidence; for (int i = 0; i < 36; i++) @@ -522,9 +524,9 @@ SL_POSITIONAL_TRACKING_STATE ZEDFusionController::geoToCamera(SL_LatLng* in, SL_ return state; } -SL_POSITIONAL_TRACKING_STATE ZEDFusionController::cameraToGeo(SL_PoseData* in, SL_GeoPose* out, bool radian) +enum SL_GNSS_CALIBRATION_STATE ZEDFusionController::cameraToGeo(struct SL_PoseData* in, struct SL_GeoPose* out, bool radian) { - SL_POSITIONAL_TRACKING_STATE state = SL_POSITIONAL_TRACKING_STATE_OFF; + SL_GNSS_CALIBRATION_STATE state = SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED; memset(out, 0, sizeof(SL_GeoPose)); @@ -548,7 +550,7 @@ SL_POSITIONAL_TRACKING_STATE ZEDFusionController::cameraToGeo(SL_PoseData* in, S pose.valid = in->valid; - state = (SL_POSITIONAL_TRACKING_STATE)fusion.Camera2Geo(pose, sdk_pose); + state = (SL_GNSS_CALIBRATION_STATE)fusion.Camera2Geo(pose, sdk_pose); out->heading = sdk_pose.heading; out->horizontal_accuracy = sdk_pose.horizontal_accuracy; @@ -574,4 +576,33 @@ SL_POSITIONAL_TRACKING_STATE ZEDFusionController::cameraToGeo(SL_PoseData* in, S } return state; +} + +enum SL_GNSS_CALIBRATION_STATE ZEDFusionController::getCurrentGNSSCalibrationSTD(float* yaw_std, struct SL_Vector3* position_std) +{ + SL_GNSS_CALIBRATION_STATE state = SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED; + sl::float3 sdk_position_std; + state = (SL_GNSS_CALIBRATION_STATE)fusion.getCurrentGNSSCalibrationSTD(*yaw_std, sdk_position_std); + + position_std->x = sdk_position_std.x; + position_std->y = sdk_position_std.y; + position_std->z = sdk_position_std.z; + + return state; +} + +void ZEDFusionController::getGeoTrackingCalibration(struct SL_Vector3* translation, struct SL_Quaternion* rotation) +{ + sl::Transform transform = fusion.getGeoTrackingCalibration(); + + sl::float3 sdk_position = transform.getTranslation(); + translation->x = sdk_position.x; + translation->y = sdk_position.y; + translation->z = sdk_position.z; + + sl::float4 sdk_rotation = transform.getOrientation(); + rotation->x = sdk_rotation.x; + rotation->y = sdk_rotation.y; + rotation->z = sdk_rotation.z; + rotation->w = sdk_rotation.w; } \ No newline at end of file diff --git a/src/ZEDFusionController.hpp b/src/ZEDFusionController.hpp index bb659b9..4d8843d 100644 --- a/src/ZEDFusionController.hpp +++ b/src/ZEDFusionController.hpp @@ -38,15 +38,15 @@ class ZEDFusionController { void close(); - SL_FUSION_ERROR_CODE process(); + enum SL_FUSION_ERROR_CODE process(); - SL_FUSION_ERROR_CODE subscribe(struct SL_CameraIdentifier* uuid, struct SL_CommunicationParameters* params, struct SL_Vector3* pose_translation, struct SL_Quaternion* pose_rotation); + enum SL_FUSION_ERROR_CODE subscribe(struct SL_CameraIdentifier* uuid, struct SL_CommunicationParameters* params, struct SL_Vector3* pose_translation, struct SL_Quaternion* pose_rotation); - SL_FUSION_ERROR_CODE updatePose(struct SL_CameraIdentifier* uuid, struct SL_Vector3* pose_translation, struct SL_Quaternion* pose_rotation); + enum SL_FUSION_ERROR_CODE updatePose(struct SL_CameraIdentifier* uuid, struct SL_Vector3* pose_translation, struct SL_Quaternion* pose_rotation); - SL_SENDER_ERROR_CODE getSenderState(SL_CameraIdentifier* uuid); + enum SL_SENDER_ERROR_CODE getSenderState(struct SL_CameraIdentifier* uuid); - void readFusionConfigFile(char json_config_filename[256], SL_COORDINATE_SYSTEM coord_system, SL_UNIT unit, SL_FusionConfiguration* configs,int& nb_cameras); + void readFusionConfigFile(char json_config_filename[256], enum SL_COORDINATE_SYSTEM coord_system, enum SL_UNIT unit, struct SL_FusionConfiguration* configs,int& nb_cameras); ///////////////////////////////////////////////////////////////////// ///////////////////// Object Detection Fusion /////////////////////// @@ -57,14 +57,14 @@ class ZEDFusionController { /// \param [in] init parameters /// \return SL_FUSION_ERROR_CODE /// - SL_FUSION_ERROR_CODE init(struct SL_InitFusionParameters* init_parameters); + enum SL_FUSION_ERROR_CODE init(struct SL_InitFusionParameters* init_parameters); /// /// \brief enables body tracking fusion module /// \param [in] parameters defined by \ref SL_BodyTrackingFusionParameters /// \return SL_FUSION_ERROR_CODE /// - SL_FUSION_ERROR_CODE enableBodyTracking(struct SL_BodyTrackingFusionParameters* params); + enum SL_FUSION_ERROR_CODE enableBodyTracking(struct SL_BodyTrackingFusionParameters* params); /** * \brief get the stats of a given camera in the Fusion API side @@ -72,7 +72,7 @@ class ZEDFusionController { * \param metrics : structure containing all the metrics available * \return SL_FUSION_ERROR_CODE */ - SL_FUSION_ERROR_CODE getProcessMetrics(SL_FusionMetrics* metrics); + enum SL_FUSION_ERROR_CODE getProcessMetrics(struct SL_FusionMetrics* metrics); /// /// \brief disable Object detection fusion module @@ -86,7 +86,7 @@ class ZEDFusionController { /// \param [out] bodies: list of objects seen by all available cameras /// \note Only the 3d informations is available in the returned object. /// - SL_FUSION_ERROR_CODE retrieveBodies(struct SL_Bodies* bodies, struct SL_BodyTrackingFusionRuntimeParameters* rt, struct SL_CameraIdentifier uuid); + enum SL_FUSION_ERROR_CODE retrieveBodies(struct SL_Bodies* bodies, struct SL_BodyTrackingFusionRuntimeParameters* rt, struct SL_CameraIdentifier uuid); ///////////////////////////////////////////////////////////////////// ///////////////////// Positional Tracking Fusion /////////////////////// @@ -99,7 +99,7 @@ class ZEDFusionController { * * \return ERROR_CODE */ - SL_FUSION_ERROR_CODE enablePositionalTracking(SL_PositionalTrackingFusionParameters* params); + enum SL_FUSION_ERROR_CODE enablePositionalTracking(struct SL_PositionalTrackingFusionParameters* params); /** * \brief Get the Fused Position of the camera system @@ -109,7 +109,7 @@ class ZEDFusionController { * \param uuid Camera identifier * \return POSITIONAL_TRACKING_STATE is the current state of the tracking process */ - SL_POSITIONAL_TRACKING_STATE getPosition(SL_PoseData* pose, enum SL_REFERENCE_FRAME reference_frame, struct SL_CameraIdentifier* uuid, SL_POSITION_TYPE retrieve_type); + enum SL_POSITIONAL_TRACKING_STATE getPosition(struct SL_PoseData* pose, enum SL_REFERENCE_FRAME reference_frame, struct SL_CameraIdentifier* uuid, enum SL_POSITION_TYPE retrieve_type); /// /// \brief disable Positional Tracking fusion module @@ -118,15 +118,19 @@ class ZEDFusionController { void disablePositionalTracking(); - SL_FUSION_ERROR_CODE ingestGNSSData(SL_GNSSData* data, bool radian); + enum SL_FUSION_ERROR_CODE ingestGNSSData(struct SL_GNSSData* data, bool radian); - SL_POSITIONAL_TRACKING_STATE getCurrentGNSSData(SL_GNSSData* data, bool radian); + enum SL_POSITIONAL_TRACKING_STATE getCurrentGNSSData(struct SL_GNSSData* data, bool radian); - SL_POSITIONAL_TRACKING_STATE getGeoPose(SL_GeoPose* pose, bool radian); + enum SL_GNSS_CALIBRATION_STATE getGeoPose(struct SL_GeoPose* pose, bool radian); - SL_POSITIONAL_TRACKING_STATE geoToCamera(SL_LatLng* in, SL_PoseData* out, bool radian); + enum SL_GNSS_CALIBRATION_STATE geoToCamera(struct SL_LatLng* in, struct SL_PoseData* out, bool radian); - SL_POSITIONAL_TRACKING_STATE cameraToGeo(SL_PoseData* in, SL_GeoPose* out, bool radian); + enum SL_GNSS_CALIBRATION_STATE cameraToGeo(struct SL_PoseData* in, struct SL_GeoPose* out, bool radian); + + enum SL_GNSS_CALIBRATION_STATE getCurrentGNSSCalibrationSTD(float* yaw_std, struct SL_Vector3* position_std); + + void getGeoTrackingCalibration(struct SL_Vector3* translation, struct SL_Quaternion* rotation); void destroy(); diff --git a/src/zed_interface.cpp b/src/zed_interface.cpp index 4cfb013..ee9eb5a 100644 --- a/src/zed_interface.cpp +++ b/src/zed_interface.cpp @@ -1030,9 +1030,9 @@ extern "C" { return nullptr; } - INTERFACE_API SL_PlaneData* sl_find_plane_at_hit(int c_id, SL_Vector2 pixel, bool refine) { + INTERFACE_API SL_PlaneData* sl_find_plane_at_hit(int c_id, SL_Vector2 pixel, struct SL_PlaneDetectionParameters* params, bool refine) { if (!ZEDController::get(c_id)->isNull()) { - return ZEDController::get(c_id)->findPlaneAtHit(pixel, refine); + return ZEDController::get(c_id)->findPlaneAtHit(pixel, params, refine); } return nullptr; } @@ -1396,7 +1396,7 @@ extern "C" { } } - INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_get_position(SL_PoseData* pose, enum SL_REFERENCE_FRAME reference_frame, enum SL_COORDINATE_SYSTEM coordinate_system, enum SL_UNIT unit, + INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_get_position(SL_PoseData* pose, enum SL_REFERENCE_FRAME reference_frame, enum SL_UNIT unit, struct SL_CameraIdentifier* uuid, enum SL_POSITION_TYPE retrieve_type) { if (!ZEDFusionController::get()->isNotCreated()) @@ -1440,7 +1440,7 @@ extern "C" { } } - INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_get_geo_pose(SL_GeoPose* pose, bool radian) + INTERFACE_API enum SL_GNSS_CALIBRATION_STATE sl_fusion_get_geo_pose(SL_GeoPose* pose, bool radian) { if (!ZEDFusionController::get()->isNotCreated()) { @@ -1448,11 +1448,11 @@ extern "C" { } else { - return SL_POSITIONAL_TRACKING_STATE_OFF; + return SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED; } } - INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_geo_to_camera(struct SL_LatLng* in, struct SL_PoseData* out, bool radian) + INTERFACE_API enum SL_GNSS_CALIBRATION_STATE sl_fusion_geo_to_camera(struct SL_LatLng* in, struct SL_PoseData* out, bool radian) { if (!ZEDFusionController::get()->isNotCreated()) { @@ -1460,11 +1460,11 @@ extern "C" { } else { - return SL_POSITIONAL_TRACKING_STATE_OFF; + return SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED; } } - INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_camera_to_geo(struct SL_PoseData* in, struct SL_GeoPose* out, bool radian) + INTERFACE_API enum SL_GNSS_CALIBRATION_STATE sl_fusion_camera_to_geo(struct SL_PoseData* in, struct SL_GeoPose* out, bool radian) { if (!ZEDFusionController::get()->isNotCreated()) { @@ -1472,7 +1472,40 @@ extern "C" { } else { - return SL_POSITIONAL_TRACKING_STATE_OFF; + return SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED; + } + } + + INTERFACE_API unsigned long long sl_fusion_get_current_timestamp() + { + if (!ZEDFusionController::get()->isNotCreated()) + { + return ZEDFusionController::get()->fusion.getCurrentTimeStamp().getNanoseconds(); + } + else + { + return 0; + } + } + + INTERFACE_API enum SL_GNSS_CALIBRATION_STATE sl_fusion_get_current_gnss_calibration_std(float* yaw_std, struct SL_Vector3* position_std) + { + if (!ZEDFusionController::get()->isNotCreated()) + { + return ZEDFusionController::get()->getCurrentGNSSCalibrationSTD(yaw_std, position_std); + } + else + { + return SL_GNSS_CALIBRATION_STATE_NOT_CALIBRATED; + } + } + + + INTERFACE_API void sl_fusion_get_geo_tracking_calibration(struct SL_Vector3* translation, struct SL_Quaternion* rotation) + { + if (!ZEDFusionController::get()->isNotCreated()) + { + return ZEDFusionController::get()->getGeoTrackingCalibration(translation, rotation); } }