Ingesting independently recorded GNSS NMEA messages to Zed 2 SVO recordings

Software Python
1

Hi,

My goal is to create a geolocated 3D point cloud using the stereo camera. The project information:

  • An RTK GNSS receiver, a Zed 2, and a Zed box.

  • I have all the GNSS positions and their estimated accuracy at 10-Hz through NMEA messages, i.e., NMEA-GGA message for the time, latitude, longitude and altitude, and NMEA-GST message for the time and position errors.

  • The time shift between GNSS messages and the camera recordings as SVO files is known, and I can give the position and the corresponding errors at each frame’s timestamp of the SVO.

  • The covariance matrix is defined using: covariance = [eph * eph, 0, 0, 0, eph * eph, 0, 0, 0, epv * epv], where eph is the horizontal error (typically below 0.05 m) and epv is the altitude error (typically below 0.1 m). One example of the matrix: [0.0034, 0.0, 0.0, 0.0, 0.0034, 0.0, 0.0, 0.0, 0.0292]. The matrix is not singular and is invertible (determinant ~ 1e-7).

  • I use the SDK 4.03 for the playback and fusion.

Now, my problem is that when I ingest the gnss_data to fusion module, it returns: “INVALID COVARIANCE”. I tried to change the covariance matrix to the default covariance = [1, 0.1, 0.1, 0.1, 1, 0.1, 0.1, 0.1, 1], then it returned “INVALID TIMESTAMP”.

Could you please help me understand what is the problem?
Is it possible to add an example recorded GNSS JSON + SVO files to your sample codes on: zed-sdk/geotracking/playback at master · stereolabs/zed-sdk · GitHub to understand how it works?

my_geotracking.py (9.8 KB)
gnss_tools.py (13.1 KB)
nmea_messages_definition.yaml (1.6 KB)

2

Hello Mostafa,
Welcome to stereolabs community ! First of all the Fusion SDK do not support very low covariance for now. The minimum authorized for eph or epv is 0.1 for now. We will release a version that allow to set minimum covariance allowed inside Fusion Initparameters.
Concerning your second problem (INVALID TIMESTAMP) could you activate the SDK verbose and tell me what is reported ?
You can enable verbose mode through InitParameters : InitParameters Class Reference | API Reference | Stereolabs

Best regards,
Tanguy

3

Hello,
Thanks for your reply.
I turned on the verbose option. Before changing the values for the covariance matrix I get this WARNING based on eph = 0.06 and epv = 0.17 and covariance = [0.0034, 0, 0, 0, 0.0034, 0, 0, 0, 0.0292]:

[ZED][INFO] Logging level INFO
[ZED][INFO] [Init] Depth mode: PERFORMANCE
[ZED][INFO] [Init] Serial Number: S/N 34284780
Subscribing to 34284780 COMM_TYPE.INTRA_PROCESS
[ZED][WARNING] [GeoTracking] The data you are trying to ingest has very low covariance (less than 1 millimeter).

This raises the question about the unit of the entries in the covariance matrix. If “eph” or “epv” are in meters then the covariance entries should have m^2 unit, but apparently they are in cm^2. Is that true? Even covariance = [0.99, 0, 0, 0, 0.99, 0, 0, 0, 0.99] triggers the same warning.

Now to pass this warning I use covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1]. Now I get:

[ZED][WARNING] [GeoTracking] The timestamp of the data you are trying to ingest is very different from the current synchronization timestamp. Please verify that you are using the correct timestamp units. Timestamp difference: 1683883686557 ms

I assigned the odometry position’s timestamp to gnss_data, i.e.:

ts = odometry_pose.timestamp
…
gnss_data.ts = ts
gnss_data.set_coordinates(lat, lon, alt, in_radian=False)

At the point that I receive the latter warning, I checked the timestamp of the gnss_data using the following in the debug console:

> gnss_data.ts.get_milliseconds()
1683883686557

Apparently, either gnss_data.ts or the current synchronization timestamp is zero. I don’t know why.

my_geotracking.py (9.9 KB)

4

Concerning your first question you are right the covariance are supposed to be in m² not in cm². It seems to be linked to an int approximation problem present in python wrapper (it is not present in C++). This issue will be fixed in next release.

Regarding your timestamp issue do you get this message after the first fusion.process() ? Indeed the synchronization start running during the first fusion.process(). In current sample the first ingest_gnss_data may failed but the next one should succeed. Could you confirm that ?

Regard,
Tanguy

5

You are right, after the second frame the warning disappears and the ingest returns SUCCESS. But now the problem is that when I want to retrieve geopose, it remains in SEARCHING mode. I use:

pose_data = camera_pose.pose_data(sl.Transform())
geopose = sl.GeoPose()
status = fusion.camera_to_geo(camera_pose, geopose)

and the status is SEARCHING. Attached is the stdout of the script. Another point is that it exits the code after the first geopose output!
my_geotracking.py (10.2 KB)
my_geotracking_log.txt (15.7 KB)

6

@mostafa.hoseini I think that the fusion mode is taking in those values, it is computing the inverse in order to generate the error matrix for the pose graph routines under the hood. I might try 1. Making sure your units are correct 2. Taking the inverse of that diagonal (1/x for all elements in this case) and see if that fixes the error.