Integrating GNSS data to the graph.

[jpennecot]

Hi Mathieu,

I’m working on an iPhone Pro application that captures the environment using the phone’s camera, LiDAR, IMU, and an external GNSS-RTK antenna. The goal is to generate a geo-referenced 3D textured mesh and point cloud with high positional accuracy. Because our geo-location requirements are strict, we use an external RTK antenna that theoretically provides centimeter-level accuracy.

I would like to integrate this GNSS-RTK data directly into RTAB-Map’s graph optimizer—not only to assist with loop closures, but also to anchor the reconstructed map in an absolute geo-referenced frame and improve the overall optimization.

From what I understood, we add a new graph node with:

rtabmap::SensorData data(rgbImage, depthImage, id, timestamp); rtabmap.process(data);

And then I attach the available IMU and GPS information:

data.setGPS(gps); data.setIMU(imu);

Challenge: Sensor Rate Mismatch

Since each sensor has a different capture rate, the RGB, depth, IMU, and GNSS timestamps are not perfectly aligned. If I simply attach the latest GNSS reading to each SensorData object, the GNSS pose may be off by several centimeters, which would degrade the accuracy we expect from RTK.

Solution 1: Interpolation

One approach would be:

• Cache SensorData objects until GNSS measurements before/after their timestamps are available.
• Interpolate the GNSS pose to match the exact timestamp of the RGB/depth frames.
• Optionally create separate nodes for RGB and depth frames, each with their own interpolated GNSS pose.

Would this be an appropriate strategy, or unnecessarily complex?

Solution 2: Adding GNSS Constraints Manually

Another option would be to add GNSS constraints directly into the graph:

Link link(fromId, toId, Link::kGlobalClosure, gpsPose, informationMatrix); rtabmap.addLink(link);

My interpretation of the parameters:

• fromId: ID of the node for which the GPS measurement applies
• toId: Should this be 0 (world frame) or the same node ID?
• linkType: Link::kGlobalClosure
• gpsPose: GNSS-derived pose
• informationMatrix: uncertainty of the GNSS measurement

This seems functionally similar to Solution 1, but allows more precise uncertainty modeling. Is this the recommended method?

Graph Structure Question

From my understanding, each RTAB-Map node is tied to a SensorData object. Is it possible to create a node containing only a pose and timestamp (without images)? Or must every node be associated with an image/depth frame?

Main Question

Is this the correct way to integrate high-accuracy GNSS-RTK data into RTAB-Map’s optimization pipeline?

Thanks a lot for your help,
Julien Pennecot

[matlabbe]

Hi Julien,

Solution 1 would be the easiest one if you can feed the interpolated GPS position to SensorData.  What is the frame rate of the GPS and the latency? Rtabmap adds by default a new SensorData at 1 Hz, one way would be to buffer the latest SensorData till a gps value with more recent timestamp is received, interpolate at SensorData timestamp and publish it to rtabmap core.

It is possible to add nodes in the graph without image (just pose and GPS). However, they cannot be added in the past in case the GPS has some latency.

What is the format of the RTK data? Is it GPS (longitude/latitude/bearing) format or 6DoF "local" (from original GPS corodinate) pose format? The GPS format used by rtabmap is assumed to be only XYZ parameters (no orientation). If your RTK gives 6DoF poses, you may add a Prior constraint (SensorData::setGlobalPose()) instead of adding the GPS (you can still provide GPS to help loop closure detection based on GPS and export data in GPS coordinates, but prior will be used in the optimizer instead of the GPS). The Prior constraint lets you set your own covariance.

Based on the SLAM datasets using a RTK(GPS-IMU fusion) or Vicon/OptiTrack system to provide a 6DoF ground truth, the global pose is the way to go.

cheers,
Mathieu