Cannot get precision with ZED 2i

Hello.

I have a ZED 2i camera connected to my jetson orin nano 8GB board.
I also have a GNSS receiver on that board, and NTRIP software to get RTK fix.

I have setup a second tty USB port to the receiver where i can enable multiple NMEA messages.

I then subscribe to them with gpsd to get the data into the global navigation example. however, I don’t get any time stamps.
I have enabled GPGSV, GLGSV, GAGSV, GBGSV, GNGLL, GNTXT, GNVTG, GNGGA. All with 20 Hz.

I have also tried to make my own reader, where i get the position data directly. but without a covariances, so i get something from my kalman filter i then have to scale. But no matter how low or high i scale the variance i get very unprecise position. look at attached figure.
The orange data is meter grid from GNSS, and the blue it lat long scaled to match somewhat with the orange. I’m not worried about the scale or translation. But i’m worried about the line quality. cause, as you can see on the orange, the lines is very straight, but the blue are not.

I don’t know if it would help to use the gpsd reader, but doing it manually doesn’t work.

Any help is appreciated,

Hello @tmrpj,
Welcome to the community :wave:

It seems that you have problem in your GNSS that can explain the fusion position orientation problem. Moreover where to you placed the ZED camera compared to the GNSS antenna ? Whitout GNSS signal the fused position rely only on the VIO that is returned relative to ZED left camera sensor.

Regards,
Tanguy

Hello @TanguyHardelin
Thanks for the answer.

There is approximately 10 cm forward and 6 cm sideways from the gnss anteanna to the front of the left camera. so the antenna is behind the camera.

I verify that the gnss data comes in by
input_gnss.getCoordinates(latitude, longitude, altitude, false);

Then i do:
auto ingest_error = fusion.ingestGNSSData(input_gnss);
to add the gnss data to the fusion.

Then finally to check the output i do:
auto current_geopose_satus = fusion.getGeoPose(current_geopose);

hope this makes sense?

Hello @tmrpj,
Thanks for you response !
The difference that you saw may comes from the transformation between GNSS antenna and ZED. Have you provided the gnss_antenna_position in the API? This parameter plays a crucial role in the calibration process, as detailed in the API reference for C++. Once GNSS and VIO are calibrated, the fused position should align closely with the quality of your GNSS data.

For an accurate calibration, ensure that you provide precise timestamps for your GNSSData alongside the appropriate covariance values. Throughout the calibration process, you should observe the decrease in yaw_std, which can be obtained using getCurrentGNSSCalibrationSTD.

For further insights, I recommend our official documentation, especially this section.

Last but not least, what is your positional tracking mode ? If your application is based on GEN_1, you should try GEN_2 that provide a way better VIO.

I hope this information proves helpful insight,
Best regards,
Tanguy

Okay so i have put in some calibration for the gnss_antenna_position, and is also checking the getCurrentGNSSCalibrationSTD.
But the STD’s are all just -nan.
I’m still using the gpsd, however it cannot get a timestamp so i set a timestamp manually. But can also see with cgps that the Err fields are all n/a except for 2D Err. Is gpsd not working then?

My whole init:

sl::Camera zed;
sl::InitParameters init_params;
init_params.sdk_verbose = 1;
init_params.depth_mode = sl::DEPTH_MODE::ULTRA;
init_params.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP;
init_params.coordinate_units = sl::UNIT::METER;
sl::ERROR_CODE camera_open_error = zed.open(init_params);
if (camera_open_error != sl::ERROR_CODE::SUCCESS)
{
    std::cerr << "[ZED][ERROR] Can't open ZED camera" << std::endl;
    return EXIT_FAILURE;
}

// Enable positional tracking:
sl::PositionalTrackingParameters pose_tracking_params;
pose_tracking_params.mode = sl::POSITIONAL_TRACKING_MODE::GEN_2;
pose_tracking_params.enable_area_memory = false;
auto positional_init = zed.enablePositionalTracking(pose_tracking_params);
if (positional_init != sl::ERROR_CODE::SUCCESS)
{
    std::cerr << "[ZED][ERROR] Can't start tracking of camera" << std::endl;
    return EXIT_FAILURE;
}

// Create Fusion object:
sl::Fusion fusion;
sl::InitFusionParameters init_fusion_param;
init_fusion_param.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP ;
init_fusion_param.coordinate_units = sl::UNIT::METER;
init_fusion_param.verbose = true;
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;
}

sl::GNSSCalibrationParameters gnss_calibration_parameter;
gnss_calibration_parameter.enable_reinitialization = false;
gnss_calibration_parameter.enable_translation_uncertainty_target = false;
gnss_calibration_parameter.gnss_vio_reinit_threshold = 0.05;
gnss_calibration_parameter.target_yaw_uncertainty = 1e-2;
gnss_calibration_parameter.gnss_antenna_position = sl::float3(0.06, -0.065, 0.08); // Set your antenna position

// Enable odometry publishing:
zed.startPublishing();
// Enable GNSS data producing:
GPSDReader gnss_reader;
gnss_reader.initialize();

// Subscribe to Odometry
sl::CameraIdentifier uuid(zed.getCameraInformation().serial_number);
fusion.subscribe(uuid);

// Enable positional tracking for Fusion object
sl::PositionalTrackingFusionParameters positional_tracking_fusion_parameters;
positional_tracking_fusion_parameters.enable_GNSS_fusion = true;
positional_tracking_fusion_parameters.gnss_calibration_parameters = gnss_calibration_parameter;
sl::FUSION_ERROR_CODE tracking_error_code = fusion.enablePositionalTracking(positional_tracking_fusion_parameters);
if(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;
}

I have made my own code to read the gga data from the reciever and input into zed sdk. the covariance of the gnss data is as low as it can be before the sdk starts to complain about the variance being too small. i then drove a 4x4 square for some time and then drove a longer route while checking the std value of sdk.

Here are the resulting plots. the lat longs has been shifted by a static value.
The std are static at std x,y,z: [0.018258972, 0.00247082, 0.002852182]

Hello @tmrpj,
Indeed your results seem bad. Is it possible to record an SVO2 for testing on our side ? Furthermore is it possible to also have the yaw std given by the sdk ?

Regards,
Tanguy

I’m trying to record some data where i simply add this to the init steps:

    std::string svo_path = "ZED_SN" + std::to_string(zed.getCameraInformation().serial_number) + "_" + getCurrentDatetime() + ".svo2";
    sl::String path_output(svo_path.c_str());
    sl::RecordingParameters recordingParameters;
    recordingParameters.compression_mode = sl::SVO_COMPRESSION_MODE::LOSSLESS;
    recordingParameters.video_filename = path_output;
    auto returned_state = zed.enableRecording(recordingParameters);
    if (returned_state != sl::ERROR_CODE::SUCCESS) {
        std::cerr << "Recording ZED : " << returned_state << std::endl;
        zed.close();
        return EXIT_FAILURE;
    }

It seems to work since i get some data files, but today is very rainy so i keep getting the error:
Fix lost: reinit GNSS
terminate called without an active exception
Aborted
And then the program closes.
Anyway, here are what i managed to create, but since i’m on a jetson orin board, i had to record in SVO_COMPRESSION_MODE::LOSSLESS

Hi @tmrpj,
Sorry for the delayed response; the beginning of the week has been very busy. A few remarks regarding your SVO2. When I look at the saved image, a large portion of the optics is water-saturated. This could largely explain the odometry issues you were facing. Furthermore, no GNSS data is present in the SVO2 you sent. Did you use the recording sample?

Finally, we released today a new version 4.2.4 that contains a lot of fixes that can be useful to you. Do not hesitate to test it :slight_smile:

Regards,
Tanguy

Hello @TanguyHardelin
Also, sorry for my late response :slight_smile:
I have been playing around with to better understand what goes on. not much wiser, however i have noticed alot of NO_NEW_DATA_AVAILABLE and GNSS_DATA_COVARIANCE_MUST_VARY.
I’m pretty sure i get data, otherwise i will not be able to plot them, but i might not ingest them correctly, do i need to do more than:

auto ingest_error = fusion.ingestGNSSData(input_gnss);

or might it be because of the sanity checks for covariance, and can that be disabled? i do vary it with our internal kalman filter believe, but it doesn’t vary a lot. I can however guarantee that the positions I send to the node is of good quality.

I have made a new recording with the saveKMLData aswell. I cannot use your sample, since i do not have a screen, or any GUI enabled on that machine, however, my program looks alot like the sample, except for the viewer parts.
The data is bigger this time, so i have to send via google drive instead. you can simply request access through this link:
ZedCameraData