How to create GPS-aligned 2D LIDAR maps in ROS using RTK GPS ground truth?

Question

The Problem

I need to create precise 2D maps of an orchard that are perfectly aligned with UTM coordinates. While I have centimeter-accurate RTK GPS, existing SLAM packages prefer their own pose estimation, leading to drift and misalignment over large distances.

Platform

• ROS Noetic (20.04)
• RTK GPS: 2x ZED-F9P in RTK/differential configuration (we get reliable cm-level accuracy with GBAS)
• RTK heading data
• 2D LIDAR: RPLidar S2
• Wheel odometry
• Multiple IMUs
• Platform: 4-wheel ground robot with ackermann steering

Use Case Requirements

1. Map permanent orchard features (tree trunks, posts) during low-canopy season
2. Maps must align perfectly with UTM coordinates for integration with photogrammetry pipeline
3. Maps will be used for localization when canopy becomes dense and GPS degrades
4. Different mapping sessions must produce perfectly aligned maps based on GPS coordinates

Attempted Solutions

1. slam_toolbox

   • Produces good quality maps
   • Can be transformed to UTM frame with a bit of manual coaxing
   • No GPS anchoring will show unacceptable distortion over hundreds of meters

2. Cartographer with GPS input

   • Somehow worse results than slam_toolbox
   • Poor loop closure and map cohesion

3. Various 3D SLAM packages (GTSAM etc.)

   • Can't get it to work with 2D pointclouds without weird results

4. Custom ICP-based scan matching

   • I tried my own python ICP implementation and this seems way above my skill level..

5. costmap_2d approach

   • Issues with dynamic obstacle clearing
   • Yaw estimation errors caused scan smearing

Core Questions

1. How can I modify existing SLAM packages to:

   • Use absolute GPS coordinates for positioning
   • Only estimate yaw/heading (though RTK heading is available)
   • Prevent drift in XY coordinates

2. Alternatively, what approaches would work for:

   • Building 2D occupancy grids directly from RTK GPS + LIDAR data
   • Ensuring perfect UTM alignment
   • Handling dynamic obstacles appropriately

Any suggestions on approaches or existing packages that might help achieve this would be MUCH appreciated!

Answer 1

I'm struggling to understand what downside you see to the standard way to handle this. Assuming a transform tree like utm -> map -> odom -> base_link, you would let SLAM use an arbitrary map origin, and you calculate the utm -> map transform (a couple calibration points would make this trivial.) This approach requires no software changes to SLAM code.

Your question implies you want to make utm -> map the identity transform. You could do this if you initialize the SLAM start location correctly, and also properly set the bounds of its map frame. But I can't say how difficult it would be to add these features to any particular SLAM code.

The odom to map frame transform is an ekf that takes the absolute gps heading (we get 5hz yaw from the two differential gps antennae), fused with odom / imu / magnetometer (as differential, not absolute) at a higher rate.

This comment sounds like ordinary Localization, not SLAM. Typically with SLAM, I believe the map->odom transform is generated by the SLAM block, based on a best-fit alignment of the current lidar scan with the existing map data. So if you want GPS data to override the lidar-matching, you would want to modify the SLAM code so that it uses the GPS data for the position part of the robot pose (while the rotation part would still be determined by lidar-best-fit.)