Unsure how to modify spatial mapping C example to save point cloud instead

Software
1

I am a student using the ZED 2i for a robotics project. I would like to modify the spatial mapping example to output a fused point cloud (.ply) instead of a mesh. This is the modification I have done on the C example. The device is a Jetson Nano with the [ZED SDK for L4T 32.7 (Jetpack 4.6.X)] installed.

All i changed was the very bottom where I now save point cloud instead of mesh. What I get is a blank .ply file with just the header information.

Thank you in advance and for your patience, I know this is definitely something simple I am overlooking.

/*

  • Author : Josef Miller
  • Date : 2/17/2023
  • Project : Control_System
  • Device : Tegra X1 SoC
  • Software : Visual Studio Code
  • Compiler : ???
  • C Dialect : ???
    */

#include <sl/c_api/zed_interface.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>

#define FRAME_COUNT 20

int main()
{
int camera_id = 0;
sl_create_camera(camera_id);

struct SL_InitParameters init_param;
init_param.input_type                = SL_INPUT_TYPE_USB;
init_param.resolution                = SL_RESOLUTION_HD720;
init_param.camera_fps                = 30;
init_param.camera_device_id          = camera_id;
init_param.camera_image_flip         = SL_FLIP_MODE_AUTO; 
init_param.camera_disable_self_calib = false;
init_param.enable_right_side_measure = false;
init_param.svo_real_time_mode        = true;
init_param.depth_mode                = SL_DEPTH_MODE_PERFORMANCE;
init_param.depth_stabilization       = 1;
init_param.depth_minimum_distance    = 0;
init_param.depth_maximum_distance    = 40;
init_param.coordinate_unit           = SL_UNIT_METER;
init_param.coordinate_system         = SL_COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD;
init_param.sdk_gpu_id                = -1;
init_param.sdk_verbose               = true;
init_param.sensors_required          = false;
init_param.enable_image_enhancement  = false;
init_param.open_timeout_sec          = 0;

if (sl_open_camera(camera_id, &init_param, "", "", 0, "", "", "") == SL_ERROR_CODE_FAILURE)
{
	printf("Error Open \n");
    return SL_ERROR_CODE_FAILURE;
}

struct SL_Vector3    position = {.x = 0, .y = 0, .z = 0};
struct SL_Quaternion rotation = {.x = 0, .y = 0, .z = 0, .w = 1};

struct SL_PositionalTrackingParameters tracking_param;
tracking_param.initial_world_rotation = rotation;
tracking_param.initial_world_position = position;
tracking_param.enable_area_memory     = true;
tracking_param.enable_pose_smothing   = false;
tracking_param.set_floor_as_origin    = false;
tracking_param.set_as_static          = false;
tracking_param.enable_imu_fusion      = true;
tracking_param.depth_min_range        = -1;
tracking_param.set_gravity_as_origin  = true;

if (sl_enable_positional_tracking(camera_id, &tracking_param, "") == SL_ERROR_CODE_FAILURE)
{
	printf("Error Enable Positional Tracking \n");
	return SL_ERROR_CODE_FAILURE;
}

struct SL_SpatialMappingParameters mapping_param;
mapping_param.resolution_meter     = 0.05f;
mapping_param.range_meter          = 0;
mapping_param.save_texture         = false;
mapping_param.use_chunk_only       = true;
mapping_param.max_memory_usage     = 2048;
mapping_param.reverse_vertex_order = false;
mapping_param.map_type             = SL_SPATIAL_MAP_TYPE_FUSED_POINT_CLOUD;

if (sl_enable_spatial_mapping(camera_id, &mapping_param) == SL_ERROR_CODE_FAILURE)
{
	printf("Error Enable Spatial Mapping \n");
	return SL_ERROR_CODE_FAILURE;
}

struct SL_RuntimeParameters rt_param;
rt_param.sensing_mode                 = SL_SENSING_MODE_STANDARD;
rt_param.reference_frame              = SL_REFERENCE_FRAME_CAMERA;
rt_param.enable_depth                 = true;
rt_param.confidence_threshold         = 100;
rt_param.texture_confidence_threshold = 100;
rt_param.remove_saturated_areas       = true;

int width  = sl_get_width(camera_id);
int height = sl_get_height(camera_id);

enum SL_SPATIAL_MAPPING_STATE map_state;

for (int i = 1; i <= FRAME_COUNT; ++i)
{
	if (sl_grab(camera_id, &rt_param) == SL_ERROR_CODE_SUCCESS)
	{
		map_state = sl_get_spatial_mapping_state(camera_id);
		printf("\r Images captured: %i || Spatial mapping state : %i \t \n", i, map_state);
	}
}
printf("Extracting spatial map...\n");
sl_extract_whole_spatial_map(camera_id);

printf("Saving point cloud ... \n");
sl_save_point_cloud(camera_id, "cloud.ply", SL_MESH_FILE_FORMAT_PLY);

sl_disable_spatial_mapping(camera_id);
sl_disable_positional_tracking(camera_id, "");
sl_close_camera(camera_id);
return 0;

}

2

Hi,
You have to change some functions to make this sample output a Fused Point Cloud, here is how to:


int main() {

    // Create a ZED camera object
	int camera_id = 0;
	sl_create_camera(camera_id);

	struct SL_InitParameters init_param;
	init_param.camera_fps = 30;
	init_param.resolution = SL_RESOLUTION_HD1080;
	init_param.input_type = SL_INPUT_TYPE_USB;
	init_param.camera_device_id = camera_id;
	init_param.camera_image_flip = SL_FLIP_MODE_AUTO;
	init_param.camera_disable_self_calib = false;
	init_param.enable_image_enhancement = true;
	init_param.svo_real_time_mode = true;
	init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE;
	init_param.depth_stabilization = 1;
	init_param.depth_maximum_distance = 40;
	init_param.depth_minimum_distance = -1;
	init_param.coordinate_unit = SL_UNIT_METER;
	init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP;
	init_param.sdk_gpu_id = -1;
	init_param.sdk_verbose = false;
	init_param.sensors_required = false;
	init_param.enable_right_side_measure = false;
	init_param.open_timeout_sec = 5.0f;
	init_param.async_grab_camera_recovery = false;

	// Open the camera
	int state = sl_open_camera(camera_id, &init_param, 0, "", "", 0, "", "", "");

    if (state != 0) {
		printf("Error Open \n");
        return 1;
    }

	//Enable Positional tracking
	struct SL_PositionalTrackingParameters tracking_param;
	tracking_param.enable_area_memory = true;
	tracking_param.enable_imu_fusion = true;
	tracking_param.enable_pose_smothing = false;
	tracking_param.depth_min_range = -1;

	struct SL_Vector3  position;
	position = (struct SL_Vector3) { .x = 0, .y = 0, .z = 0 };
	struct SL_Quaternion  rotation;
	rotation = (struct SL_Quaternion) { .x = 0, .y = 0, .z = 0, .w = 1 };

	tracking_param.initial_world_position = position;
	tracking_param.initial_world_rotation = rotation;
	tracking_param.set_as_static = false;
	tracking_param.set_floor_as_origin = false;
	tracking_param.set_gravity_as_origin = true;

	state = sl_enable_positional_tracking(camera_id, &tracking_param, "");
	if (state != 0) {
		printf("Error Enable Tracking %i , exit program.\n", state);
		return 0;
	}

	struct SL_SpatialMappingParameters mapping_param;
	mapping_param.map_type = SL_SPATIAL_MAP_TYPE_FUSED_POINT_CLOUD;
	mapping_param.max_memory_usage = 2048;
	mapping_param.range_meter = 0;
	mapping_param.resolution_meter = 0.05f;
	mapping_param.save_texture = false;
	mapping_param.use_chunk_only = false;
	mapping_param.reverse_vertex_order = false;

	sl_enable_spatial_mapping(camera_id, &mapping_param);

	struct SL_RuntimeParameters rt_param;
	rt_param.enable_depth = true;
	rt_param.confidence_threshold = 95;
	rt_param.reference_frame = SL_REFERENCE_FRAME_CAMERA;
	rt_param.texture_confidence_threshold = 100;
	rt_param.remove_saturated_areas = true;

	int width = sl_get_width(camera_id);
	int height = sl_get_height(camera_id);

	int i = 1;
	// Grab data during 500 frames
	while (i <= 100) {
		// Grab an image
		state = sl_grab(camera_id, &rt_param);
		// A new image is available if grab() returns ERROR_CODE::SUCCESS
		if (state == 0) {

			// In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh
			enum SL_SPATIAL_MAPPING_STATE map_state = sl_get_spatial_mapping_state(camera_id);

			printf("\r Images captured: %i / 500 || Spatial mapping state : %i \t \n", i, map_state);
			i++;
		}
	}

	printf("Extracting Mesh...\n");
	// Extract the whole mesh.
	sl_request_mesh_async(camera_id);
	// Filter the mesh

	while (sl_get_mesh_request_status_async(camera_id) != 0) {
		_sleep(100);
	}

	int nb_v = 0;
	sl_update_fused_point_cloud(camera_id, &nb_v);
	printf("Nb Vertices %d \n", nb_v);

	sl_save_point_cloud(camera_id, "MyPointCloud.ply", SL_MESH_FILE_FORMAT_PLY);

	sl_disable_spatial_mapping(camera_id);
	sl_disable_positional_tracking(camera_id, "");
	sl_close_camera(camera_id);
    return 0;
}
3

Thank you I understand now. Have a good day :slight_smile: