FORM: Fixed-Lag Odometry & Reparative Mapping

• The paper demonstrates that FORM’s dense factor graph smoothing and active map repair yield accurate, consistent, real-time trajectory estimates.
• It utilizes iterative map management that retroactively refines past scan poses and keypoints, correcting registration errors effectively.
• Reparative mapping in FORM reduces trajectory jitter and enhances navigation reliability in diverse environments as shown on multiple benchmark datasets.

Fixed-Lag Odometry with Reparative Mapping (FORM) encompasses a class of methods that improve state estimation and trajectory consistency by performing odometry smoothing over a fixed window of past states and actively repairing the local map as new information arrives. The approach is motivated by the limitations of conventional LiDAR odometry (LO) and filtering-based SLAM systems, where misestimated poses propagate irreversibly into downstream submaps, degrading accuracy and introducing undesirable trajectory jitter. FORM employs dense factor graph optimization, iterative mapping, and active map correction mechanisms to achieve real-time performance, robust tracking, and smooth trajectories in diverse environments (Potokar et al., 11 Oct 2025).

1. Dense Factor Graph Smoothing

FORM builds a fixed-lag smoothing architecture over a densely connected factor graph. Rather than estimating poses scan-by-scan in a filtering paradigm, FORM maintains a sliding window of multiple recent poses and scan measurements, each represented as nodes in the factor graph. Edges correspond to residuals from scan-to-scan or scan-to-map matches, leveraging both planar and point features.

Optimization is performed jointly over all poses within the window using non-linear least squares methods such as Levenberg–Marquardt. A key differentiator from submap-based architectures is that new measurements can retroactively refine past pose estimates, leveraging temporal correlations and improving consistency.

The semi-linearized approach accelerates computation: after an initial ICP matching loop, only current scan factors are kept non-linear while older residuals are linearized. Once ICP converges, a full non-linear optimization is applied across the window.

<table> <tr> <th>Component</th> <th>Role</th> </tr> <tr> <td>Node</td> <td>Pose at each scan</td> </tr> <tr> <td>Edge</td> <td>Scan-to-scan or scan-to-map residuals</td> </tr> <tr> <td>Optimization</td> <td>Jointly over window using non-linear least squares</td> </tr> </table>

2. Iterative Map Management and Matching

Every LiDAR scan is processed to extract geometrically salient features: planar keypoints are selected from scanlines by computing local curvature ($$\hat{k}_i = ||\frac{1}{N_{neighbor}}\sum_{j=1}^{N_{neighbor}} (p_{i+j} - 2p_i + p_{i-j})||$$), and point features are also identified, which improves robustness in unstructured settings.

These features are aggregated into a single “iterative map” $M$, where each keypoint is stored in its scan’s local frame and re-projected into the world frame via the current optimized poses. Matching from the current scan to $M$ uses point-to-plane and point-to-point residuals; matched keypoints with correspondence distances below threshold are inserted into $M$.

The constant update of $M$ using the latest smoothed or corrected pose estimates is central to reparative mapping, preventing the propagation of registration errors into the map. In typical submap-based methods, once features have been assigned a pose, errors become entrenched and compromise future registrations; FORM repairs the map dynamically.

3. Error Correction and Reparative Mapping

A central innovation of FORM is its reparative mapping strategy. As the factor graph within the fixed lag window is optimized, prior scan poses—along with their extracted keypoints—are updated. The map $M$ is rebuilt by transforming all points from their original scan frames into the world frame using the latest smoothed pose estimates.

This active correction ensures that the local map reflects the best available trajectory estimate rather than being a fixed record of potentially erroneous scans. In contrast to submap architectures where errors propagate irreversibly, FORM corrects both current and past registration errors, resulting in smoother velocities and more reliable trajectory estimates.

Such reparative updates are critical for real-time robotic planning and control, as they reduce local jitter and provide high-fidelity input to downstream motion modules.

4. Mathematical Formulations

Robustness and tractability of FORM are supported by explicit mathematical derivations:

• Curvature estimation for keypoint selection:

$$\hat{k}_i = \left\lVert \frac{1}{N_{neighbor}} \sum_{j=1}^{N_{neighbor}} (p_{i+j} - 2p_i + p_{i-j}) \right\rVert$$

• Normal calculation via neighborhood covariance:

$\Sigma_f = \sum_{p \in N} (p - f)(p - f)^T$

with the normal $n_f$ extracted as the eigenvector corresponding to the smallest eigenvalue.

• Residual construction for scan matching:
  • For planar features (point-to-plane residual):

  $r_{planar}(X_k,X_i) = (R_k n_k)^T(X_ip_i - X_kp_k)$ - For point features (point-to-point residual):

  $r_{point}(X_k,X_i) = X_ip_i - X_kp_k$

• Keyscan selection criteria for retaining geometrically important scans:

$$\text{Select scan } i \text{ if} \ \frac{|\text{matches to recent scans}|}{N_{recent} \cdot |P_i|} > \delta_{key}$$

These formulations underpin the factor graph optimization, map repair process, and efficient real-time operation.

5. Performance and Evaluation

FORM has been evaluated on a diverse set of datasets—Oxford Spires, Newer Stereo-Cam, Hilti 2022, Multi-Campus, Botanic Gardens—demonstrating competitive long-term drift performance (measured by long-window Relative Trajectory Error) while particularly excelling in local smoothness (short-window RTE).

The reparative mapping substantially reduces trajectory jitter compared to filtering and submap-based methods. Semi-linearization and optimization over bounded windows yield real-time performance suitable for deployment with commodity CPU hardware and standard LiDAR sensors (Potokar et al., 11 Oct 2025).

Local map correction via smoothing provides a more stable and accurate trajectory estimate for downstream control and planning algorithms.

6. Comparison to Conventional LO Architectures

Traditional LO and SLAM solutions frequently estimate one pose per scan, fixing feature locations as soon as they are first observed. This produces irreversible errors whenever pose estimation is corrupted—by sensor dropouts or poor registration—and these errors propagate into the submap, adversely affecting subsequent scan integrations and leading to jittery trajectories.

By contrast, FORM’s fixed-lag smoothing and reparative mapping enable previous and current poses—along with the map itself—to be actively corrected whenever optimization yields improved estimates. This continuous correction mechanism is especially valuable for real-time robotic systems, where trajectory smoothness and map reliability are critical for robust navigation and safety.

The semi-linearized optimization and efficient window management allow FORM to operate in real time, offering a practical balance between accuracy and computational tractability.

7. Practical Implications and Limitations

FORM’s architecture is robust, accurate, and provides smooth trajectory estimates suitable for planning and control. Reparative mapping, dense factor graph optimization, and iterative mapping deliver significant improvements over filtering-based and submap-based LO approaches, particularly by mitigating local jitter and enabling real-time performance (Potokar et al., 11 Oct 2025).

The main computational trade-off lies in amortizing optimization over the fixed window: while more complex than one-step filtering, semi-linearization and windowed factor graph strategies keep processing rates above standard sensor update frequencies.

Grid and feature selection thresholds (e.g., keyscan promotion criteria) influence both the computational cost and the quality of estimation; careful parameterization is required for different environments.

A plausible implication is that FORM generalizes well to varied LiDAR and navigation scenarios, provided the window length and map repair process are chosen to match environmental characteristics and hardware constraints.

---

In summary, Fixed-Lag Odometry with Reparative Mapping (FORM) fuses dense windowed pose optimization and map repair to overcome the entrenched trajectory errors of conventional LiDAR odometry schemes. The approach yields consistent, smooth, and real-time trajectory estimates and actively corrects local map errors, showing robust performance across diverse robotic datasets and environments.