Always waiting for a valid GNSS pose in GNSS fusion!

Hi, I was trying to use ZED2i to conduct a GNSS fusion with GPS. But after I check the message If found the mGeoPoseStatus is not OK in the component.cpp, so in the terminal It print “waiting for a valid GNSS fused pose…” all the time. I just want to know how can I fix it and make the rest part of processGeoPose run.

here is a terminal log, I’m 100% sure that the GPS fusion module is set and the fix is successfully called.

As mGeoPoseStatus = mFusion.getGeoPose(mLastGeoPose); and mLastGeoPose is a reference, I can’t find any initialization of mLastGeoPose in the wrapper, So I just guess maybe there should initialize that first before running? Just small try.

How Can I solve it?
Many thanks in advance!

And the common.yaml file as well as the echo of navsatfix is here

Plz help me!

Hi @EnchengLiu
Welcome to the Stereolabs community.

According to the terminal log, there is an issue with the timestamping of the GNSS data.

@TanguyHardelin can you help this user to understand the cause of his issue?

Indeed it looks like a timestamp issue. Could you print the timestamp of GNSSData ingested to fusion and the current sl::Camera timestamp ? You could use getTimestamp and ts field of sl::GNSSData.


Hi, I add these two timestamp checking at the callback of gpsfix, And in the image is the output, can you check it and tell me how to fix that?

Hello,
You seem to have an overflow issue in your print. The timestamp must be a positive number (number of nanoseconds since epoch). Basically in C++ it a uint64_t or unsigned long long type. Could you print the timestamp with the following code:

DEBUG_GNSS("mZed CURRENT Timestamp %llu nsec", mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT).getNanoseconds());
DEBUG_GNSS("mZed IMAGE Timestamp %llu nsec", mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds());

Regards,
Tanguy


Hi, here is the correct timestamp, can you help me fix it?
Many thanks!

Indeed it seems to be a valid timestamp. In your last logs the warning log changed and now it seems that you are ingesting data with low covariance value (the message Always waiting for a valid GNSS pose in GNSS fusion! - #5 by EnchengLiu) and in your last message it seems that you do not have warning messages anymore.
Do you still encounter problems ?

Hi, I think things now back to the original problem on the first floor I raised. The status for ~/geo_pose/status and ~/pose/filtered/status is always 0 when I topic echo them. There is no /geo_pose and /pose/filtered neither. So I think the GeoTracking part is not correctly called.

And there is always a periodical “Waiting for a valid GNSS fused pose…”);" in the function processGeoPose() in zed_camera_Component.cpp.

You can check the log file here:

Is there anything I can help or anything I could do to improve it?

Thanks,
Encheng

Hello!
Indeed it seems that your ingested sl::GNSSData have covariance problems. In your log I saw many [ZED][WARNING] Warning: The data you are attempting to ingest for [GeoTracking] has a low covariance value (less than 1 millimeter). It will be clamped for stability purposes.
Did you check the covariance set for the ingested sl::GNSSData ?

Hi, I printed out my msg->position with convariance here. The msg is from Sensor_msgs/NavSatFix topic in Ros2.

And the message in my terminal said that:

  • msg->position_covariance_type (no output here)
    Position_covariance -X 40.9811
    Position_covariance -Y 154.595
    Position_covariance -Z 1e-08
    And there is also a low covariance error here.

Can you tell me what to do next?
Thank you!

Indeed from your log I saw that you ingest very low covariance on the Z axis (1e-8) could you try something more reasonable values (like one meter for testing) ?

Thank you for your reply!
This time I checked the configuration file common.yaml and Set the Setting gnss_zero_altitude to False, to this time it can output a normal Position Covariance.

So this time, there is no Low Covariance Warning in our code. And the message topic ~/geo_pose/status and ~/pose/filtered/status are true now. Thank you!

But there is one more problem when I try to plot the path of the fused position, as you mentioned in [~/path_map]in Adding Positional Tracking in ROS 2 - Stereolabs and ~/path_odom. The after-fused path I plotted in RVIZ are scatted and nonuniform, much more worse than the unfused one. The link here is my current log . And current path plot. Can you tell me how to make the after fused path correct? Or can you give me some examples to illustrate it?

You can also checked my configuration and code setting in that github repository
Best,
Encheng

Hi Tanguy,
Merry Christmas!
Could you tell me how to fix that?

Hello,
Apologies for the delayed response; there was a public holiday in France. I trust you had a wonderful Christmas and a happy New Year celebration.

Upon reviewing your trajectory, it appears that there are intermittent jumps. This phenomenon could be attributed to two primary factors:

  1. Rolling Calibration:
    It seems you have enabled enable_rolling_calibration, causing the fused position to jump with each new calibration estimation. While this behavior helps in obtaining calibration quickly, it may introduce uncertainty, leading to jumps. You can deactivate this behavior by setting enable_rolling_calibration to false. The relevant configuration can be adjusted here.
  2. Calibration Quality:
    If the calibration is not sufficiently accurate, the system may enter “re-initialization mode.” In your current setup, the target yaw uncertainty is set at 0.1 radians, which might be a bit high. Consider lowering it to 0.01 to improve calibration. During the initialization phase, the fusion module estimates the yaw difference between the VIO path orientation and the GNSS path orientation. This process runs in the background until the solution uncertainty falls below target_yaw_uncertainty. You can make this adjustment here.

Despite these challenges, the end of your trajectory seems satisfactory. Have you experimented with slight movements to check if any new jumps occur?

I hope this help,
Regards,
Tanguy

Hi Tanguy!

Also Happy new year to you!
Your suggestion sounds so reasoning, but Sorry I will be no access to ZED camera for about two weeks. I will try your suggestion later!

Best,
Encheng