FORM: Fixed-Lag Odometry with Reparative Mapping utilizing Rotating LiDAR Sensors

TL;DR

FORM addresses the challenge of real-time lidar odometry by performing fixed-lag smoothing over a window of past poses and reparative mapping against a single evolving map. It builds a densely connected factor graph with both planar and point features, uses a semi-linearized optimization for speed followed by a full nonlinear refinement, and regenerates the map from smoothed poses to repair prior errors. Across 64 trajectories in seven datasets, FORM achieves real-time performance while delivering competitive short- and long-horizon drift (RTE) and significantly smoother trajectories, outperforming several state-of-the-art lidar odometry methods in robustness. The approach, grounded in a flexible factor-graph formulation, is amenable to sensor fusion and can be extended to include IMUs and other modalities, enhancing practical autonomous navigation systems.

Abstract

Light Detection and Ranging (LiDAR) sensors have become a de-facto sensor for many robot state estimation tasks, spurring development of many LiDAR Odometry (LO) methods in recent years. While some smoothing-based LO methods have been proposed, most require matching against multiple scans, resulting in sub-real-time performance. Due to this, most prior works estimate a single state at a time and are ``submap''-based. This architecture propagates any error in pose estimation to the fixed submap and can cause jittery trajectories and degrade future registrations. We propose Fixed-Lag Odometry with Reparative Mapping (FORM), a LO method that performs smoothing over a densely connected factor graph while utilizing a single iterative map for matching. This allows for both real-time performance and active correction of the local map as pose estimates are further refined. We evaluate on a wide variety of datasets to show that FORM is robust, accurate, real-time, and provides smooth trajectory estimates when compared to prior state-of-the-art LO methods.

Figures (6)

• Figure 1: An example map built using ours from the Oxford Spires taoOxfordSpiresDataset2024 dataset. Point colors represent which scan the point originated from with black points being from the current scan. The point matches are added as factors to a dense factor graph over a window of prior poses and are optimized in a dense optimization. At each timestep, the map is regenerated with the new, optimized poses. This scheme allows for reparative mapping to occur and minimizes overall pose error.
• Figure 2: The components that make up the ours pipeline as described in Section \ref{['sec:methods']}. First, point and planar features are extracted. Next, the pose is initialized, icp iterations are performed utilizing a semi-linearized dense optimization over past poses, and a full nonlinear optimization follows. Finally, keyscan management is done, poses are marginalized, and a new map is generated from the smoothed poses.
• Figure 3: Example of the dense factor graph used in ours. Shown are keyscans, recent scans, and the recent scan that is being considered to become a keyscan. During semi-linearized optimization, all the previous matches are linearized to use less compute. The newest matches are also recomputed at every icp step.
• Figure 4: An example ours map, with planar features in blue and point features in orange. Note the trees on the left captured by point features, and walls and ground captured by planar features.
• Figure 5: wrte small window size demonstration on a trajectory in the os dataset. In the above plot, many filtering methods result in higher than expected wrte for small windows sizes. This can be seen in the jittery trajectories shown in the other plots, with the exception of ours.