Skip to content

Commit

Permalink
Merge pull request #8 from stereolabs/v4.0.4
Browse files Browse the repository at this point in the history 
push to sdk 4.0.4
  • Loading branch information
@Bvallon-sl
Bvallon-sl authored Jun 27, 2023
2 parents 2753158 + 4186123 commit 36ef5ac
Show file tree
Hide file tree
Showing 6 changed files with 75 additions and 27 deletions.
2 changes: 1 addition & 1 deletion ReadMe.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
34 changes: 33 additions & 1 deletion include/sl/c_api/types_c.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*/
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;

};

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


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

/**
Expand Down
13 changes: 8 additions & 5 deletions include/sl/c_api/zed_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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).
Expand Down Expand Up @@ -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.
Expand Down
27 changes: 19 additions & 8 deletions src/ZEDController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
Expand All @@ -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) != "") {
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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());
Expand All @@ -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;
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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) {

Expand All @@ -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;
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down
8 changes: 4 additions & 4 deletions src/ZEDController.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down
Loading

0 comments on commit 36ef5ac

Please sign in to comment.