Tips for improving robustness of LiDAR-inertial SLAM

[Roman Stadlhuber]

Hello all,

I am working on a LiDAR-inertial SLAM project, primarily for personal learning purposes.

• GitHub repository: https://github.com/RomanStadlhuber/livo-handheld/tree/main/mapping
• Pre-built doxygen documentation including derivations & explanations: https://stadlhuber.io/projects/livo-handheld/docs/

It is working and produces somewhat reasonable results on a custom dataset that I created from a handheld LiDAR device (also part of the project), but it is by no means robust.

I have tried to follow mainly those papers for the implementation (specifically for the LiDAR factor: PointToPlaneFactor.cpp)

• BA-LINS: A Frame-to-Frame Bundle Adjustment for LiDAR-Inertial Navigation
• MSC-LIO: An MSCKF-Based LiDAR-Inertial Odometry with Same-Plane Cluster Tracking

besides that, I used GTSAMs built-in IMU preintegration and the IncrementalFixedLagSmoother (i.e. iSAM2) for sliding window optimization.

The system currently estimates the NavState of each keyframe (IMU body pose + velocity) and I have added optional capabilities to estimate the temporal calibration between LiDAR and IMU (works well & gives reasonable results) and extrinsic calibration (doesn't work too good, so I am not using it).

The LiDAR features are points accumulated into planar clusters, the plane parameters (center, plane normal, thickness) are updated on each new keyframe from the updated pose estimates by using the keyframe points indices, as the cluster features themselves aren't part of the state.

Each keyframe can potentially add one new track per cluster, the residual formulation for the j-th cluster is:

In the default case where both online-calibrations are disabled, the system is still diverging easily, especially when navigating in turns that don't even need to be particularly sharp. E.g in the below Image you can see the entire trajectory in white and the sliding window part in green. After making a turn to the right, the active keyframe (white, red arrows are active cluster normals) is no longer aligning with the structure of the environment (this happened after about ~65 m of travel distance)

Besides the point-to-plane distance based checks outlined in the papers above, I had to add multiple additional checks to keep it from diverging nearly instantly, currently I am validating new tracks with

• planarity checks using the singular values (planarity & linearity)
• point-to-plane distance (this is what the papers do, too)
• plane-normal consistency check, i.e. reject a new track when it would change the direction of the normal vector too strongly (checked using dot-product)
• I wrapped the residual in a Geman-McClure kernel, otherwise it would diverge a lot faster
• I've noticed that the iSAM2 tuning is very brittle, currently I do 4 GN iterations, any more or less will easily lead to divergence in some cases, which is kind of a red flag.

I have added each of these bit-by-bit because I have found that the system will otherwise perform very poorly early on (e.g. diverge after ~ 20 m).

Overall, the tuning that I currently have (you can find the config values here) turned out to be very brittle and I found that tweaking even one variable (smoother iterations, pointcloud voxel size, check threshold values) has severe impact on the overall performance, which took me by surprise a little.

There are lots more things that I could add but I want to keep this post from exploding. I'm glad to share and discuss details if and when this finds any interest.

Here are some questions that I was hoping to find help with

• Tips & tricks how to go about tuning the parameters & modifying the algorithm (adding/removing checks, etc.) -  I have found that I can easily get stuck in a parameter tuning loop that improves performance in one dataset while being detrimental on others.
• Besides kernels, are there good options on the GTSAM side that I can use to make the LiDAR point checks looser or remove some of them such as to increase the number of constraints that I provide to the factor graph to not lose tracking so easily (like in the above case)?
• When using extrinsic calibration - would it be wise to apply any guard rails (e.g. in terms of noise covariance or bound the state)? Currently I have the feeling that the extrinsic calibration is "trying to explain away too much noise" -> amplifies divergence
• Any general advice for making point-to-plane SLAM more robust within the GTSAM framework?

If you have made it here, I want to thank you for your time. Should you be interested, I encourage you to check out the project documentation (including explanations & derivations) or just ask me here!

Wishing you a great weekend and all the best,

Roman

[Roman Stadlhuber]

Hi all, I have since implemented a secondary pose graph optimization for "Bundle Adjustment" to create a global map using Open3Ds pose graph optimization feature, which I then use for additional scan to map registration in my GTSAM tracking.

Since both libraries use TBB, I was able to conventiently separete the BA and LIO parts into task arenas, giving BA a lower priority and making sure the main pose tracking is never blocked!

You can find the Pull Request that implements this feature here: Global Map Bundle-Adjustment and Scan to Map Registration and the documentation for the module is available here: "Secondary Bundle Adjustment Thread"

The pull request also contains some more minor adjustments to the tracking logic which helped a bit with the issue, but only the scan to map registration brought a meaningful improvement over the previous iteration.

I'm assuming that noone cares, but I am still posting this here for future reference, in case anybody else is experiencing similar issues with their codebase and happens to stumble upon this.

Cheers!

[Dellaert, Frank]

Roman,

Back from ICRA in Vienna :-)

I care ! Very cool stuff. 

PS one thing you might want to check out is https://github.com/koide3/gtsam_points. It has several custom factors, some even GPU accelerated if I understand correctly, for LIDAR registration.

Best

Frank

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Roman Stadlhuber <roman.st...@gmail.com>
Sent: Sunday, May 31, 2026 11:02:06 PM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Re: Tips for improving robustness of LiDAR-inertial SLAM

 

You don't often get email from roman.st...@gmail.com. Learn why this is important

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/812dfebd-f101-4025-9098-4ab65d907e28n%40googlegroups.com.