diff --git a/Content/ZED/Blueprints/BodyTracking/ABP_ZED_Manny.uasset b/Content/ZED/Blueprints/BodyTracking/ABP_ZED_Manny.uasset index b921e325..3cb9ccb1 100644 Binary files a/Content/ZED/Blueprints/BodyTracking/ABP_ZED_Manny.uasset and b/Content/ZED/Blueprints/BodyTracking/ABP_ZED_Manny.uasset differ diff --git a/Content/ZED/Blueprints/PlaneDetection/BP_PlaneDetection_Mesh.uasset b/Content/ZED/Blueprints/PlaneDetection/BP_PlaneDetection_Mesh.uasset index 27d83185..480ef7e0 100644 Binary files a/Content/ZED/Blueprints/PlaneDetection/BP_PlaneDetection_Mesh.uasset and b/Content/ZED/Blueprints/PlaneDetection/BP_PlaneDetection_Mesh.uasset differ diff --git a/Content/ZED/Levels/L_BodyTrackingMulti.umap b/Content/ZED/Levels/L_BodyTrackingMulti.umap index 8406fb09..d3de0fc9 100644 Binary files a/Content/ZED/Levels/L_BodyTrackingMulti.umap and b/Content/ZED/Levels/L_BodyTrackingMulti.umap differ diff --git a/Content/ZED/Levels/L_BodyTrackingSingle.umap b/Content/ZED/Levels/L_BodyTrackingSingle.umap index 14a2df31..3a98b197 100644 Binary files a/Content/ZED/Levels/L_BodyTrackingSingle.umap and b/Content/ZED/Levels/L_BodyTrackingSingle.umap differ diff --git a/Content/ZED/Levels/L_CameraTracking.umap b/Content/ZED/Levels/L_CameraTracking.umap index 7653031f..2b505302 100644 Binary files a/Content/ZED/Levels/L_CameraTracking.umap and b/Content/ZED/Levels/L_CameraTracking.umap differ diff --git a/Content/ZED/Levels/L_DynamicCrosshairOnMouse.umap b/Content/ZED/Levels/L_DynamicCrosshairOnMouse.umap index 431fdcf1..91f702c3 100644 Binary files a/Content/ZED/Levels/L_DynamicCrosshairOnMouse.umap and b/Content/ZED/Levels/L_DynamicCrosshairOnMouse.umap differ diff --git a/Content/ZED/Levels/L_PlaneDetection.umap b/Content/ZED/Levels/L_PlaneDetection.umap index 8db5d678..700fbf18 100644 Binary files a/Content/ZED/Levels/L_PlaneDetection.umap and b/Content/ZED/Levels/L_PlaneDetection.umap differ diff --git a/Plugins/Stereolabs/Source/Stereolabs/Public/Core/StereolabsBaseTypes.h b/Plugins/Stereolabs/Source/Stereolabs/Public/Core/StereolabsBaseTypes.h index 10cc26ed..7076fb54 100644 --- a/Plugins/Stereolabs/Source/Stereolabs/Public/Core/StereolabsBaseTypes.h +++ b/Plugins/Stereolabs/Source/Stereolabs/Public/Core/StereolabsBaseTypes.h @@ -579,9 +579,9 @@ enum class ESlAIModels : uint8 AIM_HumanBody38FastDetection UMETA(DisplayName = "Human body 38 fast Detection"), AIM_HumanBody38MediumDetection UMETA(DisplayName = "Human body 38 medium Detection"), AIM_HumanBody38AccurateDetection UMETA(DisplayName = "Human body 38 accurate Detection"), - AIM_HumanBody70FastDetection UMETA(DisplayName = "Human body 70 fast Detection"), - AIM_HumanBody70MediumDetection UMETA(DisplayName = "Human body 70 medium Detection"), - AIM_HumanBody70AccurateDetection UMETA(DisplayName = "Human body 70 accurate Detection"), + //AIM_HumanBody70FastDetection UMETA(DisplayName = "Human body 70 fast Detection"), + //AIM_HumanBody70MediumDetection UMETA(DisplayName = "Human body 70 medium Detection"), + //AIM_HumanBody70AccurateDetection UMETA(DisplayName = "Human body 70 accurate Detection"), AIM_PersonHeadFastDetection UMETA(DisplayName = "Person head fast Detection"), AIM_PersonHeadAccurateDetection UMETA(DisplayName = "Person head accurate Detection"), AIM_REIDAssociation UMETA(DisplayName = "REID Association"), @@ -1037,7 +1037,8 @@ struct STEREOLABS_API FSlCameraParameters HFocal(0.0f), VFocal(0.0f), OpticalCenterX(0.0f), - OpticalCenterY(0.0f) + OpticalCenterY(0.0f), + FocalLengthMetric(0.0f) { } @@ -1072,6 +1073,10 @@ struct STEREOLABS_API FSlCameraParameters /** Vertical position of the optical center in pixels */ UPROPERTY(BlueprintReadOnly) float OpticalCenterY; + + /** Real focal length in millimeters */ + UPROPERTY(BlueprintReadOnly) + float FocalLengthMetric; }; /* @@ -1365,6 +1370,39 @@ struct STEREOLABS_API FSlSpatialMappingParameters bool bUseChunkOnly = false; }; +/** + \class PlaneDetectionParameters + \brief Sets the plane detection parameters. + + The default constructor sets all parameters to their default settings. + */ +USTRUCT(BlueprintType, Category = "Stereolabs|Types") +struct STEREOLABS_API FSlPlaneDetectionParameters +{ + GENERATED_BODY() + + FSlPlaneDetectionParameters() + : + MaxDistanceThreshold(0.15f), + NormalSimilarityThreshold(15.0f) + { + } + + /** + \brief controls the spread of plane by checking the position difference. + \n default: 0.15 meters + */ + UPROPERTY(BlueprintReadOnly) + float MaxDistanceThreshold; + + /** + \brief controls the spread of plane by checking the angle difference. + \n default: 15 degree + */ + UPROPERTY(BlueprintReadOnly) + float NormalSimilarityThreshold; +}; + /* * SDK recording state * see sl::RecordingState diff --git a/Plugins/Stereolabs/Source/Stereolabs/Public/Core/StereolabsCoreGlobals.h b/Plugins/Stereolabs/Source/Stereolabs/Public/Core/StereolabsCoreGlobals.h index c608d907..0ba3e4de 100644 --- a/Plugins/Stereolabs/Source/Stereolabs/Public/Core/StereolabsCoreGlobals.h +++ b/Plugins/Stereolabs/Source/Stereolabs/Public/Core/StereolabsCoreGlobals.h @@ -1589,6 +1589,7 @@ namespace sl CameraParameters.VFocal = SlData.fy; CameraParameters.OpticalCenterX = SlData.cx; CameraParameters.OpticalCenterY = SlData.cy; + CameraParameters.FocalLengthMetric = SlData.focal_length_metric; std::vector Tmp; Tmp.assign(SlData.disto, SlData.disto + 5); @@ -1612,6 +1613,7 @@ namespace sl CameraParameters.VFocal = SlData.fy; CameraParameters.OpticalCenterX = SlData.cx; CameraParameters.OpticalCenterY = SlData.cy; + CameraParameters.FocalLengthMetric = SlData.focal_length_metric; std::vector Tmp; Tmp.assign(SlData.disto, SlData.disto + 5); @@ -2295,6 +2297,19 @@ namespace sl return sl::MeshFilterParameters(sl::unreal::ToSlType(UnrealData.FilterIntensity)); } + /* + * Convert from FSlPlaneDetectionParameters to sl::PlaneDetectionParameters + */ + FORCEINLINE SL_PlaneDetectionParameters ToSlType(const FSlPlaneDetectionParameters& UnrealData) + { + struct SL_PlaneDetectionParameters PlaneDetectionParameters; + + PlaneDetectionParameters.max_distance_threshold = UnrealData.MaxDistanceThreshold; + PlaneDetectionParameters.normal_similarity_threshold = UnrealData.NormalSimilarityThreshold; + + return PlaneDetectionParameters; + } + /* * Convert from sl::CameraParameters to FSlCameraParameters */ @@ -2309,11 +2324,20 @@ namespace sl CameraParameters.cx = UnrealData.OpticalCenterX; CameraParameters.cy = UnrealData.OpticalCenterY; + CameraParameters.focal_length_metric = UnrealData.FocalLengthMetric; + CameraParameters.disto[0] = UnrealData.Disto[0]; CameraParameters.disto[1] = UnrealData.Disto[1]; CameraParameters.disto[2] = UnrealData.Disto[2]; CameraParameters.disto[3] = UnrealData.Disto[3]; CameraParameters.disto[4] = UnrealData.Disto[4]; + CameraParameters.disto[5] = UnrealData.Disto[5]; + CameraParameters.disto[6] = UnrealData.Disto[6]; + CameraParameters.disto[7] = UnrealData.Disto[7]; + CameraParameters.disto[8] = UnrealData.Disto[8]; + CameraParameters.disto[9] = UnrealData.Disto[9]; + CameraParameters.disto[10] = UnrealData.Disto[10]; + CameraParameters.disto[11] = UnrealData.Disto[11]; CameraParameters.image_size = sl::unreal::ToSlType2(UnrealData.Resolution); diff --git a/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/bin/sl_zed_c.dll b/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/bin/sl_zed_c.dll index 03381c6d..b821f5aa 100644 Binary files a/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/bin/sl_zed_c.dll and b/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/bin/sl_zed_c.dll differ diff --git a/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/include/sl/c_api/types_c.h b/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/include/sl/c_api/types_c.h index 9d80ae04..845d9ae7 100644 --- a/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/include/sl/c_api/types_c.h +++ b/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/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 @@ -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,10 +2332,11 @@ struct SL_InputType /////////////////////////////// FUSION API ///////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** +\enum 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 */ @@ -2321,7 +2346,10 @@ enum SL_FUSION_ERROR_CODE { 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_FUSION_FPS_TOO_LOW = 2, /**< At least one sender has fps lower than 10 FPS*/ + SL_FUSION_ERROR_CODE_INVALID_TIMESTAMP = 3, /** < Problem was detected with ingested timestamp*/ + SL_FUSION_ERROR_CODE_INVALID_COVARIANCE = 4, /** < Problem was detected with ingested covariance */ + SL_FUSION_ERROR_CODE_NO_NEW_DATA_AVAILABLE = 5 /** < All data from all sources were consumed, no new process available.*/ }; enum SL_SENDER_ERROR_CODE { @@ -2354,6 +2382,9 @@ struct SL_FusionConfiguration { }; +/** +\brief Holds the options used to initialize the \ref Fusion object. + */ struct SL_InitFusionParameters { /** @@ -2371,7 +2402,7 @@ struct SL_InitFusionParameters 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; @@ -2379,7 +2410,7 @@ struct SL_InitFusionParameters 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 + * \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. * */ @@ -2387,27 +2418,8 @@ struct SL_InitFusionParameters }; /** - * - * +\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 { /** @@ -2421,30 +2433,39 @@ struct SL_BodyTrackingFusionParameters 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 is -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 * */ int skeleton_minimum_allowed_camera; /** - * @brief this value controls the smoothing of the tracked or fitted fused skeleton. + * \brief this value controls the smoothing of the tracked or fitted fused skeleton. * it is ranged from 0 (low smoothing) and 1 (high smoothing) */ 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; @@ -2468,6 +2489,9 @@ struct SL_CameraMetrics 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 @@ -2484,6 +2508,19 @@ 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 @@ -2494,45 +2531,266 @@ struct SL_GNSSData double altitude; // Timestamp 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, 0, 0, 0, eph, 0, 0, 0, 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 Coordinates in LatLng format + * + */ struct SL_LatLng { + /** + * \brief latitude coordinate in radian + * + */ double latitude; + /** + * \brief longitude coordinate in radian + * + */ double longitude; + /** + * \brief altitude coordinate + * + */ double altitude; }; - +/** + * \brief Holds Geo data + * + */ struct SL_GeoPose { + /** + * pose in ENU (Translation) + */ struct SL_Vector3 translation; + /** + * pose in ENU (Rotation) + */ 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 timestamp of geopose + * + */ + unsigned long long timestamp; }; +/** + * \brief Coordinates in ECEF format + * + */ struct SL_ECEF { + /** + * \brief x coordinate of ECEF + * + */ double x; + /** + * \brief y coordinate of ECEF + * + */ double y; + /** + * \brief z coordinate of ECEF + * + */ double z; }; + +/** + * \brief Coordinate 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]; }; +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. By default, the threshold is set at 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. + \n By default, it is set to false. + */ + bool enable_translation_uncertainty_target; + /** + * \brief This parameter defines the target translation uncertainty at which the calibration process between GNSS and VIO terminates. By default, the threshold is set at 10 centimeters (10e-2). + * + */ + float target_translation_uncertainty; + /** + * \brief This initialization 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. + * By default, it is set to true. + */ + bool enable_reinitialization; + /** + * \brief This parameter determines the threshold for GNSS/VIO reinitialization. If the fused position deviates beyond the region defined by the product of the GNSS covariance and the gnss_vio_reinit_threshold, a reinitialization will be triggered. + * By default, it is set to 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. + * By default, it is set to true. + */ + bool enable_rolling_calibration; +}; + +/** + * + * + */ +struct SL_PositionalTrackingFusionParameters { + /** + * \brief Is the gnss fusion is enabled + * + */ + bool enable_GNSS_fusion; + /** + * \brief GNSS calibration parameter. Determine target threshold for GNSS / VIO calibration. + */ + 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/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/include/sl/c_api/zed_interface.h b/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/include/sl/c_api/zed_interface.h index 36d1fa56..0b80a784 100644 --- a/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/include/sl/c_api/zed_interface.h +++ b/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/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/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/lib/sl_zed_c.lib b/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/lib/sl_zed_c.lib index 0556c2e5..6cc23029 100644 Binary files a/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/lib/sl_zed_c.lib and b/Plugins/Stereolabs/Source/ThirdParty/sl_zed_c/lib/sl_zed_c.lib differ diff --git a/Plugins/Stereolabs/Source/ZED/Private/Utilities/ZEDFunctionLibrary.cpp b/Plugins/Stereolabs/Source/ZED/Private/Utilities/ZEDFunctionLibrary.cpp index 6af3ada2..9c6beb25 100644 --- a/Plugins/Stereolabs/Source/ZED/Private/Utilities/ZEDFunctionLibrary.cpp +++ b/Plugins/Stereolabs/Source/ZED/Private/Utilities/ZEDFunctionLibrary.cpp @@ -852,7 +852,7 @@ bool UZEDFunctionLibrary::GetFloorPlaneAtWorldLocation(AZEDPlayerController* Pla } -bool UZEDFunctionLibrary::CreateMeshFromFloorHit(AZEDPlayerController* PlayerController, const FIntPoint ScreenPosition, FSlMeshData& MeshData) { +bool UZEDFunctionLibrary::CreateMeshFromFloorHit(AZEDPlayerController* PlayerController, const FIntPoint ScreenPosition, FSlMeshData& MeshData, FSlPlaneDetectionParameters planeDetectionParameters) { if (!PlayerController->ViewportHelper.IsInViewport(ScreenPosition.X, ScreenPosition.Y)) { @@ -861,8 +861,10 @@ bool UZEDFunctionLibrary::CreateMeshFromFloorHit(AZEDPlayerController* PlayerCon } int CameraID = GSlCameraProxy->GetCameraID(); + SL_PlaneDetectionParameters sdk_params = sl::unreal::ToSlType(planeDetectionParameters); + FIntPoint ImagePosition = PlayerController->ViewportHelper.ConvertScreenSpaceToImageSpace(ScreenPosition); - SL_PlaneData* planeData = sl_find_plane_at_hit(CameraID, sl::unreal::ToSlType(ImagePosition), true); + SL_PlaneData* planeData = sl_find_plane_at_hit(CameraID, sl::unreal::ToSlType(ImagePosition), &sdk_params, true); ESlErrorCode ErrorCode = sl::unreal::ToUnrealType((SL_ERROR_CODE)planeData->error_code); if (ErrorCode != ESlErrorCode::EC_Success) { diff --git a/Plugins/Stereolabs/Source/ZED/Public/Core/AnimNode_ZEDPose.h b/Plugins/Stereolabs/Source/ZED/Public/Core/AnimNode_ZEDPose.h index e73c654b..a52348ad 100644 --- a/Plugins/Stereolabs/Source/ZED/Public/Core/AnimNode_ZEDPose.h +++ b/Plugins/Stereolabs/Source/ZED/Public/Core/AnimNode_ZEDPose.h @@ -125,9 +125,6 @@ template<> struct TStructOpsTypeTraits : public TStructOpsTyp }; - - - // Index of joints parent static TArray parents34Idx = TArray{ -1, @@ -894,7 +891,6 @@ static FName GetParent38BoneName(FName BoneName) return ParentBoneName; } - static FName GetParentBoneName(FName BoneName, int NbKeypoints = 34) { if (NbKeypoints == 34) diff --git a/Plugins/Stereolabs/Source/ZED/Public/Utilities/ZEDFunctionLibrary.h b/Plugins/Stereolabs/Source/ZED/Public/Utilities/ZEDFunctionLibrary.h index 0f59406a..43173f15 100644 --- a/Plugins/Stereolabs/Source/ZED/Public/Utilities/ZEDFunctionLibrary.h +++ b/Plugins/Stereolabs/Source/ZED/Public/Utilities/ZEDFunctionLibrary.h @@ -624,7 +624,7 @@ class ZED_API UZEDFunctionLibrary : public UBlueprintFunctionLibrary * @param MeshData The Mesh data */ UFUNCTION(BlueprintPure, meta = (DisplayName = "CreateMeshFromFloorHit", Keywords = "Find floor plane at hit and convert it to mesh"), Category = "Stereolabs|Zed") - static bool CreateMeshFromFloorHit(AZEDPlayerController* PlayerController, const FIntPoint ScreenPosition, FSlMeshData& MeshData); + static bool CreateMeshFromFloorHit(AZEDPlayerController* PlayerController, const FIntPoint ScreenPosition, FSlMeshData& MeshData, FSlPlaneDetectionParameters planeDetectionParameters = FSlPlaneDetectionParameters()); UFUNCTION(BlueprintPure, meta = (DisplayName = "CreateMeshFromFloorPlane", Keywords = "Find floor plane and convert it to mesh"), Category = "Stereolabs|Zed") static bool CreateMeshFromFloorPlane(FSlMeshData& MeshData);