From 4186123a6283600e91a4712d94e7c5e98da8c073 Mon Sep 17 00:00:00 2001 From: Benjamin Vallon Date: Fri, 23 Jun 2023 17:09:40 +0200 Subject: [PATCH] push to 4.0.4 --- ReadMe.md | 2 +- include/sl/c_api/types_c.h | 34 +++++++++++++++++++++++++++++++- include/sl/c_api/zed_interface.h | 13 +++++++----- src/ZEDController.cpp | 27 +++++++++++++++++-------- src/ZEDController.hpp | 8 ++++---- src/zed_interface.cpp | 18 +++++++++-------- 6 files changed, 75 insertions(+), 27 deletions(-) diff --git a/ReadMe.md b/ReadMe.md index 657bbc6..1d98076 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -11,7 +11,7 @@ This package lets you use the ZED stereo camera in C .The C API is a wrapper aro To start using the ZED SDK in C, you will need to install the following dependencies on your system: -- [ZED SDK 4.0](https://www.stereolabs.com/developers/release/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK 3.7](https://www.stereolabs.com/developers/release/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) ## Installing the C API diff --git a/include/sl/c_api/types_c.h b/include/sl/c_api/types_c.h index 06aa9f9..3b75a40 100644 --- a/include/sl/c_api/types_c.h +++ b/include/sl/c_api/types_c.h @@ -481,6 +481,20 @@ enum SL_POSITIONAL_TRACKING_STATE { SL_POSITIONAL_TRACKING_STATE_SEARCHING_FLOOR_PLANE, /**< The camera is searching for the floor plane to locate itself related to it, the REFERENCE_FRAME::WORLD will be set afterward.*/ }; +/** + \enum POSITIONAL_TRACKING_MODE + \ingroup PositionalTracking_group + \brief Lists the mode of positional tracking that can be used. + */ +enum SL_POSITIONAL_TRACKING_MODE { + SL_POSITIONAL_TRACKING_MODE_STANDARD, /**< Default mode, best compromise in performance and accuracy */ + SL_POSITIONAL_TRACKING_MODE_QUALITY, /**< Improve accuracy in more challening scenes such as outdoor repetitive patterns like extensive field. Curently works best with ULTRA depth mode, requires more compute power */ + ///@cond SHOWHIDDEN + SL_POSITIONAL_TRACKING_MODE_LAST + ///@endcond +}; + + /** \brief Lists the different states of spatial memory area export. */ @@ -719,7 +733,7 @@ enum SL_BODY_FORMAT * \brief 70 keypoint model. * Body model, including feet and full hands models (and simplified face) */ - SL_BODY_FORMAT_BODY_70, + SL_BODY_FORMAT_BODY_70, }; enum SL_BODY_KEYPOINTS_SELECTION @@ -1390,6 +1404,12 @@ d \warning: This mode requires more resources to run, but greatly improves track * @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 + * default : POSITIONAL_TRACKING_MODE::STANDARD + */ + enum SL_POSITIONAL_TRACKING_MODE mode; + }; /** @@ -1594,6 +1614,12 @@ struct SL_SpatialMappingParameters { \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 */ enum SL_SPATIAL_MAP_TYPE map_type; + /** + \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; }; @@ -1852,6 +1878,12 @@ struct SL_BodyTrackingRuntimeParameters { * it is useful for example to remove unstable fitting results when a skeleton is partially occluded. */ int minimum_keypoints_threshold; + /** + * @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; + */ + float skeleton_smoothing; }; /** diff --git a/include/sl/c_api/zed_interface.h b/include/sl/c_api/zed_interface.h index c7e6a2c..36d1fa5 100644 --- a/include/sl/c_api/zed_interface.h +++ b/include/sl/c_api/zed_interface.h @@ -657,12 +657,13 @@ extern "C" { \param camera_id : id of the camera instance. \param vertices : Vertices of the mesh \param triangles : Triangles of the mesh. + \param colors : (b,g,r) colors of each vertex. \param max_submeshes : Maximum number of submesh that can be handled. \param uvs : uvs of the texture. \param texture_ptr : Texture of the mesh (if enabled). \return SUCCESS if the mesh is retrieved. */ - INTERFACE_API int sl_retrieve_mesh(int camera_id, float* vertices, int* triangles, float* uvs, unsigned char* texture_ptr, const int max_submeshes); + INTERFACE_API int sl_retrieve_mesh(int camera_id, float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texture_ptr, const int max_submeshes); /** \brief Updates the internal version of the mesh and returns the sizes of the meshes. \param camera_id : id of the camera instance. @@ -683,11 +684,12 @@ extern "C" { \param max_submesh : Maximum number of submesh that can be handled. \param vertices : Vertices of the chunk \param triangles : Triangles of the chunk. + \param colors : b,g,r colors of the chunk \param uvs : uvs of the texture. \param texture_ptr : Texture of the mesh (if enabled). \return SUCCESS if the chunk is retrieved. */ - INTERFACE_API int sl_retrieve_chunks(int camera_id, float* vertices, int* triangles, float* uvs, unsigned char* texture_ptr, const int max_submesh); + INTERFACE_API int sl_retrieve_chunks(int camera_id, float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texture_ptr, const int max_submesh); /** \brief Updates the fused point cloud (if spatial map type was FUSED_POINT_CLOUD). @@ -792,13 +794,14 @@ extern "C" { \brief Retrieves the full mesh. Call update_mesh before calling this. Vertex and triangles arrays must be at least of the sizes returned by update_mesh (nb_vertices and nbTriangles). \param camera_id : id of the camera instance. - \param vertices : Vertices of the chunk - \param triangles : Triangles of the chunk. + \param vertices : Vertices of the mesh + \param triangles : Triangles of the mesh. + \param colors : (b,g,r) colors of the mesh. \param uvs : uvs of the texture. \param texture_ptr : Texture of the mesh (if enabled). \return SUCCESS if the chunk is retrieved. */ - INTERFACE_API int sl_retrieve_whole_mesh(int camera_id, float* vertices, int* triangles, float* uvs, unsigned char* texture_ptr); + INTERFACE_API int sl_retrieve_whole_mesh(int camera_id, float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texture_ptr); /** \brief Loads a saved mesh file. \param camera_id : id of the camera instance. diff --git a/src/ZEDController.cpp b/src/ZEDController.cpp index e60110e..17f5f7c 100644 --- a/src/ZEDController.cpp +++ b/src/ZEDController.cpp @@ -308,6 +308,7 @@ SL_PositionalTrackingParameters* ZEDController::getPositionalTrackingParameters( c_trackingParams->set_floor_as_origin = trackingParams.set_floor_as_origin; c_trackingParams->depth_min_range = trackingParams.depth_min_range; c_trackingParams->set_gravity_as_origin = trackingParams.set_gravity_as_origin; + c_trackingParams->mode = (SL_POSITIONAL_TRACKING_MODE)trackingParams.mode; return c_trackingParams; } @@ -338,7 +339,7 @@ void ZEDController::disableTracking(const char *path) { } sl::ERROR_CODE ZEDController::enableTracking(const SL_Quaternion *initial_world_rotation, const SL_Vector3 *initial_world_position, bool enable_area_memory, bool enable_pose_smoothing, bool set_floor_as_origin, - bool set_as_static, bool enable_imu_fusion, float depth_min_range, bool set_gravity_as_origin, const char* area_file_path) { + bool set_as_static, bool enable_imu_fusion, float depth_min_range, bool set_gravity_as_origin, SL_POSITIONAL_TRACKING_MODE mode, const char* area_file_path) { if (!isNull()) { sl::PositionalTrackingParameters params; sl::Transform motion; @@ -364,6 +365,7 @@ sl::ERROR_CODE ZEDController::enableTracking(const SL_Quaternion *initial_world_ params.enable_imu_fusion = enable_imu_fusion; params.depth_min_range = depth_min_range; params.set_gravity_as_origin = set_gravity_as_origin; + params.mode = (sl::POSITIONAL_TRACKING_MODE)mode; if (area_file_path != nullptr) { if (std::string(area_file_path) != "") { @@ -1090,6 +1092,7 @@ sl::ERROR_CODE ZEDController::enableSpatialMapping(struct SL_SpatialMappingParam params.range_meter = mapping_param.range_meter; params.use_chunk_only = mapping_param.use_chunk_only; params.reverse_vertex_order = mapping_param.reverse_vertex_order; + params.stability_counter = mapping_param.stability_counter; if (mapping_param.map_type == SL_SPATIAL_MAP_TYPE_MESH) { params.save_texture = mapping_param.save_texture; @@ -1181,10 +1184,10 @@ sl::ERROR_CODE ZEDController::updateChunks(int* numVertices, int* numTriangles, return sl::ERROR_CODE::CAMERA_NOT_DETECTED; } -sl::ERROR_CODE ZEDController::retrieveChunks(const int maxSubmesh, float* vertices, int* triangles, float* uvs, unsigned char* texturePtr) { +sl::ERROR_CODE ZEDController::retrieveChunks(const int maxSubmesh, float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texturePtr) { if (!isNull() && !isTextured) { if (isMeshUpdated) { - int offsetVertices = 0, offsetTriangles = 0, offsetUvs = 0, startIndexUV = 0; + int offsetVertices = 0, offsetTriangles = 0, offsetUvs = 0, startIndexUV = 0, offsetColors = 0; bool isTextureCalled = areTextureReady && uvs != nullptr && texturePtr != nullptr; if (isTextureCalled) { @@ -1194,8 +1197,11 @@ sl::ERROR_CODE ZEDController::retrieveChunks(const int maxSubmesh, float* vertic for (int i = 0; i < std::min(maxSubmesh, int(mesh.chunks.size())); i++) { 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()); if (isTextureCalled) { memcpy(&uvs[offsetUvs], &mesh.uv.data()[startIndexUV], sizeof(sl::float2) * int(mesh.chunks[i].uv.size())); offsetUvs += int(2 * mesh.chunks[i].uv.size()); @@ -1209,10 +1215,10 @@ sl::ERROR_CODE ZEDController::retrieveChunks(const int maxSubmesh, float* vertic return sl::ERROR_CODE::CAMERA_NOT_DETECTED; } -sl::ERROR_CODE ZEDController::retrieveMesh(float* vertices, int* triangles, const int maxSubmesh, float* uvs, unsigned char* texturePtr) { +sl::ERROR_CODE ZEDController::retrieveMesh(float* vertices, int* triangles, unsigned char* colors, const int maxSubmesh, float* uvs, unsigned char* texturePtr) { if (!isNull() && !isTextured) { if (isMeshUpdated) { - int offsetVertices = 0, offsetTriangles = 0, offsetUvs = 0; + int offsetVertices = 0, offsetTriangles = 0, offsetUvs = 0, offsetColors = 0; bool isTextureCalled = areTextureReady && uvs != nullptr && texturePtr != nullptr; cudaGraphicsResource_t pcuImageRes = nullptr; @@ -1258,8 +1264,11 @@ sl::ERROR_CODE ZEDController::retrieveMesh(float* vertices, int* triangles, cons if (mesh.chunks[i].has_been_updated) { 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()); + if (isTextureCalled) { memcpy(&uvs[offsetUvs], &mesh.uv.data()[startIndexUV], sizeof (sl::float2) * int(mesh.chunks[i].uv.size())); offsetUvs += int(2 * mesh.chunks[i].uv.size()); @@ -1480,7 +1489,7 @@ sl::ERROR_CODE ZEDController::updateWholeMesh(int* nb_vertices, int* nb_triangle } -sl::ERROR_CODE ZEDController::retrieveWholeMesh(float* vertices, int* triangles, float* uvs, unsigned char* texture_ptr) { +sl::ERROR_CODE ZEDController::retrieveWholeMesh(float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texture_ptr) { if (!isNull() && !isTextured) { if (isMeshUpdated) { @@ -1493,6 +1502,7 @@ sl::ERROR_CODE ZEDController::retrieveWholeMesh(float* vertices, int* triangles, memcpy(&vertices[0] , mesh.vertices.data() , sizeof(sl::float3) * int(mesh.vertices.size())); memcpy(&uvs[0], mesh.uv.data(), sizeof(sl::float2) * int(mesh.uv.size())); memcpy(&triangles[0], mesh.triangles.data() , sizeof(sl::uint3) * int(mesh.triangles.size())); + memcpy(&colors[0], mesh.colors.data(), sizeof(sl::uchar3) * int(mesh.colors.size())); if (areTextureReady && uvs != nullptr && texture_ptr != nullptr) { isTextured = true; @@ -1948,6 +1958,7 @@ sl::ERROR_CODE ZEDController::retrieveBodyTrackingData(SL_BodyTrackingRuntimePar sl::BodyTrackingRuntimeParameters runtime_params; runtime_params.detection_confidence_threshold = _bodyruntimeparams->detection_confidence_threshold; runtime_params.minimum_keypoints_threshold = _bodyruntimeparams->minimum_keypoints_threshold; + runtime_params.skeleton_smoothing = _bodyruntimeparams->skeleton_smoothing; sl::ERROR_CODE v = zed.retrieveBodies(bodies, runtime_params, instance_id); if (v == sl::ERROR_CODE::SUCCESS) @@ -2022,7 +2033,7 @@ sl::ERROR_CODE ZEDController::retrieveBodyTrackingData(SL_BodyTrackingRuntimePar data->body_list[count].head_position.y = p.head_position.y; data->body_list[count].head_position.z = p.head_position.z; - for (int i = 0; i < std::min((int)p.keypoint.size(), 70); i++) + for (int i = 0; i < p.keypoint.size(); i++) { data->body_list[count].keypoint_2d[i].x = p.keypoint_2d.at(i).x; data->body_list[count].keypoint_2d[i].y = p.keypoint_2d.at(i).y; @@ -2043,7 +2054,7 @@ sl::ERROR_CODE ZEDController::retrieveBodyTrackingData(SL_BodyTrackingRuntimePar data->body_list[count].global_root_orientation.z = p.global_root_orientation.z; data->body_list[count].global_root_orientation.w = p.global_root_orientation.w; - for (int i = 0; i < std::min((int)p.local_orientation_per_joint.size(), 70); i++) { // 38 or 70 + for (int i = 0; i < p.local_orientation_per_joint.size(); i++) { data->body_list[count].local_orientation_per_joint[i].x = p.local_orientation_per_joint[i].x; data->body_list[count].local_orientation_per_joint[i].y = p.local_orientation_per_joint[i].y; diff --git a/src/ZEDController.hpp b/src/ZEDController.hpp index b567485..d39ca66 100644 --- a/src/ZEDController.hpp +++ b/src/ZEDController.hpp @@ -82,7 +82,7 @@ class ZEDController { /************Tracking*******************/ sl::ERROR_CODE enableTracking(const SL_Quaternion *initial_world_rotation, const SL_Vector3 *initial_world_position, bool enable_area_memory, bool enable_pose_smoothing, bool set_floor_as_origin, bool set_as_static, - bool enable_imu_fusion, float depth_min_range, bool set_gravity_as_origin, const char* area_file_path); + bool enable_imu_fusion, float depth_min_range, bool set_gravity_as_origin, SL_POSITIONAL_TRACKING_MODE mode, const char* area_file_path); sl::POSITIONAL_TRACKING_STATE getPosition(SL_Quaternion *quat, SL_Vector3 *vec, sl::REFERENCE_FRAME mat_type); sl::POSITIONAL_TRACKING_STATE getPosition(SL_Quaternion *quat, SL_Vector3 *vec, SL_Vector3 *offset, SL_Quaternion *offsetRotation, int type); sl::POSITIONAL_TRACKING_STATE getPosition(SL_PoseData *pose, int reference_frame); @@ -102,12 +102,12 @@ class ZEDController { sl::ERROR_CODE updateMesh(int* numVertices, int* numTriangles, int* numSubmeshes, int* updatedIndices, int* numVerticesTot, int* numTrianglesTot, const int maxSubmesh); sl::ERROR_CODE extractWholeSpatialMap(); bool filterMesh(sl::MeshFilterParameters::MESH_FILTER filterParams, int* numVertices, int* numTriangles, int* numUpdatedSubmeshes, int* updatedIndices, int* numVerticesTot, int* numTrianglesTot, const int maxSubmesh); - sl::ERROR_CODE retrieveMesh(float* vertices, int* triangles, const int maxSubmesh, float* uvs, unsigned char* texturePtr); + sl::ERROR_CODE retrieveMesh(float* vertices, int* triangles, unsigned char* colors, const int maxSubmesh, float* uvs, unsigned char* texturePtr); sl::ERROR_CODE updateChunks(int* numVertices, int* numTriangles, int* numSubmeshes, int* updatedIndices, int* numVerticesTot, int* numTrianglesTot, const int maxSubmesh); - sl::ERROR_CODE retrieveChunks(const int maxSubmesh, float* vertices, int* triangles, float* uvs, unsigned char* texturePtr); + sl::ERROR_CODE retrieveChunks(const int maxSubmesh, float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texturePtr); sl::ERROR_CODE updateWholeMesh(int* nb_vertices, int* nb_triangles); - sl::ERROR_CODE retrieveWholeMesh(float* vertices, int* triangles, float* uvs, unsigned char* texture_ptr); + sl::ERROR_CODE retrieveWholeMesh(float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texture_ptr); bool loadWholeMesh(const char* filename, int* nb_vertices, int* nb_triangles, int* texture_size); bool applyWholeTexture(int* nb_vertices, int* nb_triangles, int* texture_size); bool filterWholeMesh(sl::MeshFilterParameters::MESH_FILTER filter_param, int* nb_vertices, int* nb_triangles); diff --git a/src/zed_interface.cpp b/src/zed_interface.cpp index de4f7d0..4cfb013 100644 --- a/src/zed_interface.cpp +++ b/src/zed_interface.cpp @@ -657,7 +657,7 @@ extern "C" { INTERFACE_API SL_ERROR_CODE sl_set_camera_settings(int c_id, enum SL_VIDEO_SETTINGS mode, int value) { if (!ZEDController::get(c_id)->isNull()) return (SL_ERROR_CODE)ZEDController::get(c_id)->zed.setCameraSettings((sl::VIDEO_SETTINGS)mode, value); - else SL_ERROR_CODE_FAILURE; + else return SL_ERROR_CODE_FAILURE; } INTERFACE_API SL_ERROR_CODE sl_set_roi_for_aec_agc(int c_id, enum SL_SIDE side, struct SL_Rect* roi, bool reset) { @@ -748,7 +748,7 @@ extern "C" { INTERFACE_API int sl_enable_positional_tracking(int c_id, SL_PositionalTrackingParameters* tracking_param, const char* area_path) { if (!ZEDController::get(c_id)->isNull()) { return (int)ZEDController::get(c_id)->enableTracking(&tracking_param->initial_world_rotation, &tracking_param->initial_world_position, tracking_param->enable_area_memory, tracking_param->enable_pose_smothing, tracking_param->set_floor_as_origin, - tracking_param->set_as_static, tracking_param->enable_imu_fusion, tracking_param->depth_min_range, tracking_param->set_gravity_as_origin, area_path); + tracking_param->set_as_static, tracking_param->enable_imu_fusion, tracking_param->depth_min_range, tracking_param->set_gravity_as_origin, tracking_param->mode, area_path); } else return (int)sl::ERROR_CODE::CAMERA_NOT_INITIALIZED; @@ -905,9 +905,9 @@ extern "C" { return -1; } - INTERFACE_API int sl_retrieve_mesh(int c_id, float* vertices, int* triangles, float* uvs, unsigned char* texturePtr, const int maxSubmesh) { + INTERFACE_API int sl_retrieve_mesh(int c_id, float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texturePtr, const int maxSubmesh) { if (!ZEDController::get(c_id)->isNull()) - return (int)ZEDController::get(c_id)->retrieveMesh(vertices, triangles, maxSubmesh, uvs, texturePtr); + return (int)ZEDController::get(c_id)->retrieveMesh(vertices, triangles, colors, maxSubmesh, uvs, texturePtr); else return -1; } @@ -919,9 +919,9 @@ extern "C" { return -1; } - INTERFACE_API int sl_retrieve_chunks(int c_id, float* vertices, int* triangles, float* uvs, unsigned char* texturePtr, const int maxSubmesh) { + INTERFACE_API int sl_retrieve_chunks(int c_id, float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texturePtr, const int maxSubmesh) { if (!ZEDController::get(c_id)->isNull()) - return (int)ZEDController::get(c_id)->retrieveChunks(maxSubmesh, vertices, triangles, uvs, texturePtr); + return (int)ZEDController::get(c_id)->retrieveChunks(maxSubmesh, vertices, triangles, colors, uvs, texturePtr); else return -1; } @@ -990,9 +990,9 @@ extern "C" { return false; } - INTERFACE_API int sl_retrieve_whole_mesh(int c_id, float* vertices, int* triangles, float* uvs, unsigned char* texture_ptr) { + INTERFACE_API int sl_retrieve_whole_mesh(int c_id, float* vertices, int* triangles, unsigned char* colors, float* uvs, unsigned char* texture_ptr) { if (!ZEDController::get(c_id)->isNull()) - return (int)ZEDController::get(c_id)->retrieveWholeMesh(vertices, triangles, uvs, texture_ptr); + return (int)ZEDController::get(c_id)->retrieveWholeMesh(vertices, triangles, colors, uvs, texture_ptr); else return false; } @@ -1424,6 +1424,8 @@ extern "C" { { return ZEDFusionController::get()->ingestGNSSData(gnss_data, radian); } + else + return SL_FUSION_ERROR_CODE_FAILURE; } INTERFACE_API enum SL_POSITIONAL_TRACKING_STATE sl_fusion_get_current_gnss_data(struct SL_GNSSData* data, bool radian)