Skip to content

Commit

Permalink
Merge pull request #10 from stereolabs/4.0.6
Browse files Browse the repository at this point in the history 
push to 4.0.6
  • Loading branch information
@Bvallon-sl
Bvallon-sl authored Aug 10, 2023
2 parents 8f655a3 + 4b61b05 commit 31bbf98
Show file tree
Hide file tree
Showing 7 changed files with 613 additions and 153 deletions.
530 changes: 442 additions & 88 deletions include/sl/c_api/types_c.h
View file

Large diffs are not rendered by default.

62 changes: 45 additions & 17 deletions include/sl/c_api/zed_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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);

/////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -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);

/**
Expand All @@ -1319,37 +1326,58 @@ 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
*/
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.
Expand Down
18 changes: 14 additions & 4 deletions src/ZEDController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -760,7 +763,7 @@ SL_PlaneData* ZEDController::findFloorPlane(SL_Quaternion *resetQuaternion, SL_V
return &currentFloorPlane;
}

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;
Expand All @@ -769,7 +772,12 @@ SL_PlaneData* ZEDController::findPlaneAtHit(SL_Vector2 pixels, bool thres) {
pos.y = (unsigned int) pixels.y;

memset(&currentPlaneAtHitSDK, 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;

Expand Down Expand Up @@ -925,14 +933,16 @@ 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;
output.v_fov = input.v_fov;
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;
}

Expand Down Expand Up @@ -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());
Expand Down
2 changes: 1 addition & 1 deletion src/ZEDController.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
67 changes: 49 additions & 18 deletions src/ZEDFusionController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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);

Expand All @@ -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));

Expand Down Expand Up @@ -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, &params->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;
Expand Down Expand Up @@ -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);
Expand All @@ -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;

Expand All @@ -445,21 +446,22 @@ 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;
pose->vertical_accuracy = sdk_pose.vertical_accuracy;
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;
Expand All @@ -480,17 +482,17 @@ 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));

sl::LatLng latLng;
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++)
Expand Down Expand Up @@ -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));

Expand All @@ -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;
Expand All @@ -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;
}
Loading

0 comments on commit 31bbf98

Please sign in to comment.