Hi @adujardin @Myzhar @mattrouss @TanguyHardelin @obraun-sl
Any of you guys please help me out the only way find out to get this escalated by tagging others to this thread, I have tried almost all the possible ways and suggestions that the support member have provided but nothing works, even though I have re-written the code in C++ just to ensure the previously mentioned function (this post) will help me out in archiveing my goal but it seem like SDK does not have that much capabilities to serve.
My problem is something which will directly help others too, once solved, so I beelive that a proper resolution is very much helpfull.
Again providing the deep context on this problem.
- Get a SVO2 recording file which have GNSS data already ingested and load it to zed object using below code with also initializing PositionTracking module.
int main() {
// Create a ZED camera object
Camera zed;
sl::FusedPointCloud finalObj;
// Set configuration parameters
InitParameters init_parameters;
init_parameters.coordinate_system = COORDINATE_SYSTEM::IMAGE; // Use a right-handed Y-up coordinate system
init_parameters.coordinate_units = UNIT::METER; // Set units in meters
init_parameters.depth_mode = DEPTH_MODE::ULTRA; // Set units in meters
init_parameters.input.setFromSVOFile("zed-sdk/global localization/recording/python/output/ZED_SN35169074_25-03-2025_17-08-22.svo2");
init_parameters.sdk_verbose = 1;
// Open the camera
auto returned_state = zed.open(init_parameters);
if (returned_state != ERROR_CODE::SUCCESS) {
cout << "Error " << returned_state << ", exit program.\n";
return EXIT_FAILURE;
}
// Enable positional tracking with default parameters. Positional tracking needs to be enabled before using spatial mapping
sl::PositionalTrackingParameters tracking_parameters;
tracking_parameters.mode = sl::POSITIONAL_TRACKING_MODE::GEN_2;
returned_state = zed.enablePositionalTracking(tracking_parameters);
if (returned_state != ERROR_CODE::SUCCESS) {
cout << "Error " << returned_state << ", exit program.\n";
return EXIT_FAILURE;
}
auto display_resolution = sl::Resolution(1280, 720);
sl::Fusion fusion;
sl::InitFusionParameters init_fusion_param;
init_fusion_param.coordinate_units = UNIT::METER;
init_fusion_param.coordinate_system = COORDINATE_SYSTEM::IMAGE;
init_fusion_param.output_performance_metrics = true;
init_fusion_param.verbose = true;
init_fusion_param.timeout_period_number = 66;
sl::FUSION_ERROR_CODE fusion_init_code = fusion.init(init_fusion_param);
if (fusion_init_code != sl::FUSION_ERROR_CODE::SUCCESS) {
std::cerr << "[Fusion][ERROR] Failed to initialize fusion, error: " << fusion_init_code << std::endl;
return EXIT_FAILURE;
}
// Enable odometer publishing:
sl::CommunicationParameters configuration;
zed.startPublishing(configuration);
/// Run a first grab for starting sending data:
while (zed.grab() != sl::ERROR_CODE::SUCCESS) {
}
sl::CameraIdentifier uuid(zed.getCameraInformation().serial_number);
fusion.subscribe(uuid, configuration, sl::Transform());
// Enable positional tracking for Fusion object
sl::PositionalTrackingFusionParameters tracking_fusion_param;
tracking_fusion_param.enable_GNSS_fusion = true;
sl::GNSSCalibrationParameters gnss_calibration_parameters;
gnss_calibration_parameters.target_yaw_uncertainty = 1e-2;
gnss_calibration_parameters.enable_translation_uncertainty_target = false;
gnss_calibration_parameters.target_translation_uncertainty = 15e-2;
gnss_calibration_parameters.enable_reinitialization = false;
gnss_calibration_parameters.gnss_vio_reinit_threshold = 5;
gnss_calibration_parameters.enable_rolling_calibration = true;
gnss_calibration_parameters.gnss_antenna_position = sl::float3 (1.0, 0.0, 0.25);
tracking_fusion_param.gnss_calibration_parameters = gnss_calibration_parameters;
if(sl::FUSION_ERROR_CODE tracking_error_code = fusion.enablePositionalTracking(tracking_fusion_param); tracking_error_code != sl::FUSION_ERROR_CODE::SUCCESS){
std::cout << "[Fusion][ERROR] Could not start tracking, error: " << tracking_error_code << std::endl;
return EXIT_FAILURE;
}
SpatialMappingParameters spatial_mapping_parameters;
spatial_mapping_parameters.set(SpatialMappingParameters::MAPPING_RANGE::AUTO);
spatial_mapping_parameters.set(SpatialMappingParameters::MAPPING_RESOLUTION::HIGH);
spatial_mapping_parameters.max_memory_usage = 2560;
spatial_mapping_parameters.save_texture = false;
spatial_mapping_parameters.use_chunk_only = true;
spatial_mapping_parameters.reverse_vertex_order = false;
spatial_mapping_parameters.map_type = SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD;
spatial_mapping_parameters.stability_counter = 2;
FusedPointCloud zed_mesh;
FusedPointCloud fusion_mesh;
RuntimeParameters runtime_parameters;
// Use low depth confidence to avoid introducing noise in the constructed model
runtime_parameters.measure3D_reference_frame = REFERENCE_FRAME::WORLD;
runtime_parameters.enable_depth = true;
runtime_parameters.confidence_threshold = 50;
Mat left_image(display_resolution, MAT_TYPE::U8_C4, sl::MEM::GPU);
Mat point_cloud(display_resolution, MAT_TYPE::F32_C4, sl::MEM::GPU);
POSITIONAL_TRACKING_STATE tracking_state = POSITIONAL_TRACKING_STATE::OFF;
Pose pose;
sl::Timestamp last_timestamp = 0;
bool is_running = true;
std::string gnss_key;
try {
gnss_key = zed.getSVODataKeys()[0];
}catch (const std::runtime_error &e) {
std::cerr << "Error while Locating GNSS Key in SVO File: " << e.what() << std::endl;
}
- Looping and grabbing frames from Zed Object and retreiveing image, matching timestamp and ingesting GNSS data to fusion object then enabling spatialMapping and trying to get a referenced Point cloud but don’t know how to save referenced point cloud data and how reference point cloud is created and where does it stored. Below is the contiuos of code along side the explanation.
bool mapping_enable = false;
bool spatial_mapping_fusion_status = false;
while(is_running) {
if (zed.grab(runtime_parameters) == ERROR_CODE::SUCCESS) {
// Retrieve the left image
zed.retrieveImage(left_image, VIEW::LEFT, MEM::GPU, display_resolution);
zed.retrieveMeasure(point_cloud, MEASURE::XYZBGRA, MEM::GPU, display_resolution);
// Retrieve the camera pose data
tracking_state = zed.getPosition(pose, REFERENCE_FRAME::WORLD);
if (tracking_state != POSITIONAL_TRACKING_STATE::OK) {
std::cout << "Waiting for Positional Tracking to Begin" << std::endl;
}
auto current_ts = zed.getTimestamp(TIME_REFERENCE::IMAGE);
// auto status use status in real problem
if (gnss_key.empty()) {
std::cout << "GNSS Key is empty. Killing Program" << std::endl;
break;
}
std::map<sl::Timestamp, sl::SVOData> data_map;
zed.retrieveSVOData(gnss_key,
data_map,
last_timestamp,
current_ts);
sl::GNSSData input_gnss;
for (auto &[fst, snd] : data_map) {
SVOData& data = snd; // Accessing the value (SVOData)
std::string s;
bool data_string = data.getContent(s);
auto tmp_array = json::array();
try {
auto gnss_data_point = json::parse(s);
auto gnss_data_point_formatted = json::object();
if (!gnss_data_point["coordinates"].is_null()) {
gnss_data_point_formatted["coordinates"] = {
{"latitude", gnss_data_point["coordinates"]["latitude"].get<float>()},
{"longitude", gnss_data_point["coordinates"]["longitude"].get<float>()},
{"altitude", gnss_data_point["coordinates"]["altitude"].get<float>()},
};
gnss_data_point_formatted["ts"] = gnss_data_point["ts"];
float latitude_std = gnss_data_point["latitude_std"];
float longitude_std = gnss_data_point["longitude_std"];
float altitude_std = gnss_data_point["altitude_std"];
gnss_data_point_formatted["latitude_std"] = sqrt(latitude_std);
gnss_data_point_formatted["longitude_std"] = sqrt(longitude_std);
gnss_data_point_formatted["altitude_std"] = sqrt(altitude_std);
gnss_data_point_formatted["position_covariance"] = json::array({
longitude_std, 0, 0, 0, latitude_std, 0, 0, 0, altitude_std
});
gnss_data_point_formatted["original_gnss_data"] = gnss_data_point;
} else if (!gnss_data_point["coordinates"].is_null() && !gnss_data_point["latitude_std"].is_null() && !gnss_data_point["longitude_std"].is_null()) {
// no conversion
gnss_data_point_formatted = gnss_data_point;
}
tmp_array.push_back(gnss_data_point_formatted);
} catch (const std::runtime_error &e) {
std::cerr << "Error while reading GNSS data: " << e.what() << std::endl;
}
input_gnss.gnss_mode = GNSS_MODE::FIX_2D;
input_gnss.gnss_status = GNSS_STATUS::SINGLE;
input_gnss.longitude_std = tmp_array[0]["longitude_std"];
input_gnss.latitude_std = tmp_array[0]["latitude_std"];
input_gnss.altitude_std = tmp_array[0]["altitude_std"];
input_gnss.position_covariance = tmp_array[0]["position_covariance"];
if (is_microseconds(tmp_array[0]["ts"].get<uint64_t>())){
input_gnss.ts.setMicroseconds(tmp_array[0]["ts"].get<uint64_t>());
}
else if (is_nanoseconds(tmp_array[0]["ts"].get<uint64_t>())){
input_gnss.ts.setNanoseconds(tmp_array[0]["ts"].get<uint64_t>());
}
input_gnss.setCoordinates(tmp_array[0]["coordinates"]["latitude"],
tmp_array[0]["coordinates"]["longitude"],
tmp_array[0]["coordinates"]["altitude"],
false
);
sl::FUSION_ERROR_CODE ingest_status = fusion.ingestGNSSData(input_gnss);
if (ingest_status != sl::FUSION_ERROR_CODE::SUCCESS and ingest_status != sl::FUSION_ERROR_CODE::NO_NEW_DATA_AVAILABLE) {
std::cout << "Ingest error occurred when ingesting GNSSData: " << ingest_status << std::endl;
}else {
std::cout << "Ingest OK" << std::endl;
break;
}
}
last_timestamp = current_ts;
if (fusion.process() == sl::FUSION_ERROR_CODE::SUCCESS){
if (!mapping_enable) {
sl::ERROR_CODE mapping_status = zed.enableSpatialMapping(spatial_mapping_parameters);
if (mapping_status == sl::ERROR_CODE::SUCCESS) {
mapping_enable = true;
}
}
if (!spatial_mapping_fusion_status) {
sl::SpatialMappingFusionParameters spatial_mapping_fusion_parameters;
spatial_mapping_fusion_parameters.set(SpatialMappingParameters::MAPPING_RANGE::AUTO);
spatial_mapping_fusion_parameters.set(SpatialMappingParameters::MAPPING_RESOLUTION::HIGH);
spatial_mapping_fusion_parameters.max_memory_usage = 2000;
spatial_mapping_fusion_parameters.use_chunk_only = true;
spatial_mapping_fusion_parameters.map_type = SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD;
spatial_mapping_fusion_parameters.stability_counter = 2;
sl::FUSION_ERROR_CODE fusion_mapping_status = fusion.enableSpatialMapping(spatial_mapping_fusion_parameters);
if (fusion_mapping_status == sl::FUSION_ERROR_CODE::SUCCESS) {
spatial_mapping_fusion_status = true;
}
}
// How to work from here ?
sl::Pose fused_pose;
sl::POSITIONAL_TRACKING_STATE state = fusion.getPosition(fused_pose);
float yaw_std;
sl::float3 position_std;
fusion.getCurrentGNSSCalibrationSTD(yaw_std, position_std);
if (yaw_std != -1.f) {
std::cout << ": calibration uncertainty yaw_std " << yaw_std << " rad position_std " << position_std
[0] << " m, " << position_std[1] << " m, " << position_std[2] << " m\t\t\t\r";
}
if (state == sl::POSITIONAL_TRACKING_STATE::OK) {
sl::GeoPose fused_geo_pose;
sl::GNSS_FUSION_STATUS gnss_fusion_status = fusion.getGeoPose(fused_geo_pose);
sl::GNSS_FUSION_STATUS geo_fusion_status = fusion.Camera2Geo(fused_pose, fused_geo_pose);
// zed.requestSpatialMapAsync();
fusion.requestSpatialMapAsync();
if (fusion.getSpatialMapRequestStatusAsync() == sl::FUSION_ERROR_CODE::SUCCESS) {
fusion.retrieveSpatialMapAsync(fusion_mesh);
fusion_mesh.save("test");
}
// zed.retrieveSpatialMapAsync(zed_mesh);
// fusion.getGeoPose();
// finalObj.updateFromChunkList(fused_geo_pose.pose_data)
}
}
}
else if (zed.grab() == ERROR_CODE::END_OF_SVOFILE_REACHED) {
is_running = false;
std::cout << "End of file";
break;
}
else {
is_running = false;
}
}
// zed.extractWholeSpatialMap(zed_mesh);
// zed_mesh.save("/home/Documents/test_cpp_zed.ply");
fusion_mesh.save("/home/Documents/test_cpp_fused.ply");
fusion.disablePositionalTracking();
zed.disablePositionalTracking();
zed.disableSpatialMapping();
fusion.close();
zed.close();
}
Also, I have attached the code here 
main.cpp (13.7 KB)
Please help me out, as this is urgent and need attention we have 6 zed camera to do a lot of stuff and we are stuck on our first goal.
If possible please suggest a time where you can reply promptly to assist us better or can we have a meeting to discuss the problem.
