Keyframe-Factor-Based Graph Optimization for Efficient Lidar-Inertial Odometry and Mapping

1. Introduction

In recent years, unmanned aerial vehicles (UAVs) have been increasingly applied to tasks such as autonomous inspection, environmental monitoring, and logistics delivery. Reliable autonomous operation in these scenarios critically depends on accurate localization and mapping capabilities, especially in complex and dynamic environments. Among various perception techniques, LiDAR-based simultaneous localization and mapping (SLAM) has demonstrated notable advantages in terms of accuracy and robustness. Recent survey studies have systematically summarized the progress of LiDAR-based SLAM in robotics and autonomous systems, highlighting its effectiveness in large-scale, GPS-denied, and complex environments [1,2,3,4]. However, in real-world UAV operations, external disturbances such as airflow turbulence and wind gusts often induce high-frequency vibrations, which can severely corrupt LiDAR measurements and lead to unstable pose estimation or even complete SLAM failure.

To address motion-induced measurement degradation, a variety of LiDAR and LiDAR–inertial SLAM systems have been developed. Early LiDAR-only methods, such as LOAM [5], decompose the SLAM problem into high-frequency odometry estimation and low-frequency mapping optimization to achieve real-time performance. Building upon this framework, LeGO-LOAM [6] introduces ground-based constraints to improve computational efficiency on lightweight platforms. More recently, tightly coupled LiDAR–inertial SLAM approaches, including LIO-SAM [7], iEKF-based fusion methods [8], and multi-modal systems such as LVI-SAM [9], integrate inertial or visual information into optimization or filtering frameworks, significantly enhancing motion compensation and robustness under aggressive maneuvers. Comprehensive reviews on multi-sensor fusion SLAM further indicate that LiDAR–IMU–camera integration has become a dominant paradigm for improving robustness in highly dynamic scenarios [17,18,19,20].

Despite these advances, most existing LiDAR-based SLAM systems still rely primarily on sequential scan-to-scan or frame-to-keyframe alignment strategies. Under high-frequency vibration and rapid attitude changes, such local registration schemes are prone to accumulating drift over time, as corrupted measurements cannot be effectively corrected using limited temporal context. Moreover, aligning the current frame with only the most recent keyframe restricts the utilization of historical spatial information, reducing the system’s ability to achieve globally consistent pose estimation in challenging flight conditions. Similar limitations have been identified in recent comparative evaluations and large-scale LiDAR SLAM analyses [21,22,23].

Graph-based optimization has become a fundamental paradigm in modern SLAM, formulating state estimation as a nonlinear least-squares problem on a factor graph. Efficient solvers such as g2o [12], Ceres Solver [11], and incremental methods like iSAM2 [15] enable large-scale and real-time optimization, and have been successfully applied in practical systems including Google Cartographer [14] and ORB-SLAM [10]. Survey studies on SLAM architectures further emphasize that factor graph optimization provides a principled way to incorporate heterogeneous constraints and long-term temporal information [24]. However, in many existing implementations, graph optimization is still constructed upon locally constrained measurements, limiting its effectiveness when facing severe vibration-induced noise and rapid UAV motion. To overcome these limitations, this paper proposes a multi-keyframe graph optimization-based SLAM method specifically tailored for UAV operations under high-vibration conditions. Instead of relying on a single reference keyframe, the proposed approach aligns each incoming LiDAR frame with multiple historical keyframes and formulates the resulting constraints as a factor graph for joint pose optimization. An adaptive weighting strategy that jointly considers spatial distance and temporal interval is further introduced to balance the contributions of different keyframes. The resulting nonlinear least-squares problem is efficiently solved using the Gauss–Newton method, leading to more stable and globally consistent pose estimation under challenging flight conditions.

Comprehensive experiments are conducted on the NTU VIRAL dataset [25] as well as on a real UAV platform. The results demonstrate that the proposed method significantly improves localization accuracy and mapping consistency compared with LeGO-LOAM, particularly in scenarios involving severe UAV vibrations and rapid motion.

The remainder of this paper is organized as follows. Section 2 introduces the materials and methods used in this study. Section 3 presents the experimental results. Section 4 discusses the implications of the results, and Section 5 concludes the paper.

2. Materials and Methods

2.1. System Framework

The proposed keyframe-based graph optimization method is designed to enhance the robustness and accuracy of LiDAR SLAM for UAVs operating under vibration disturbances. As illustrated in Figure 1, the overall system framework consists of four main modules: data preprocessing, multi-keyframe alignment, graph construction and optimization, and result output.

In the data preprocessing module, raw LiDAR measurements are first temporally synchronized and compensated for motion distortion caused by UAV movement. Subsequently, feature extraction is performed to identify edge and planar feature points, which serve as inputs for the subsequent point cloud registration process.

The multi-keyframe alignment module aligns the current LiDAR frame not only with the most recent keyframe, but also with multiple historical keyframes within a certain temporal window. For each selected keyframe, a relative transformation between the current frame and the keyframe is estimated through point cloud registration. By leveraging multiple keyframes, this module generates several local pose constraints, which provide richer and more redundant information for robust pose estimation.

In the graph construction and optimization module, the relative pose constraints obtained from multi-keyframe alignment are integrated into a unified factor graph. In this graph, nodes represent the poses of the current frame and selected keyframes, while edges encode the relative transformations between them. The overall pose estimation problem is formulated as a nonlinear least-squares optimization, which is solved using a Gauss–Newton-based approach to jointly optimize all involved poses. This global optimization process effectively mitigates noise accumulation and drift induced by high-frequency UAV vibrations, leading to improved pose consistency.

Finally, in the result output module, the optimized pose of the current frame is extracted and incorporated into the global map. The updated map and pose estimates are then provided as outputs for downstream UAV navigation and mapping tasks. All system modules are implemented in a modular and extensible manner, facilitating reproducibility and independent evaluation.

2.2. Factor Graph Construction

In keyframe-based factor graph optimization, the construction of reliable constraints plays a crucial role in achieving stable and accurate pose estimation. By aligning the current LiDAR frame with multiple historical keyframes and exploiting the existing inter-keyframe relationships, a factor graph composed of multiple vertices and edges is constructed. In this representation, each vertex corresponds to a pose variable to be optimized, while each edge encodes a spatial constraint derived from point cloud registration residuals.

In a SLAM framework, relative pose constraints between keyframes are commonly obtained through pairwise point cloud alignment. Let $X_i$ and $X_j$ denote two keyframes associated with point clouds $P_i$ and $P_j$, respectively. The relative transformation $T_{i,j}$ between them can be estimated by minimizing the point-to-point registration error:

$$T_{i,j}=arg\underset{T_{i,j}}{min}\sum _k{∥P_i^k-T_{i,j}·P_j^k∥}^2.$$

(1)

Here, $P_i^k$ and $P_j^k$ represent the k-th corresponding points in the point clouds of keyframes $X_i$ and $X_j$. This formulation applies not only to relative pose estimation between consecutive historical keyframes, such as $(X_{i-2},X_{i-1},X_i)$, but also to the alignment between the current frame $F_{\mathrm{cur}}$ and multiple preceding keyframes. The resulting set of relative transformations $\left\{T_{i,j}\right\}$ constitutes the edge constraints of the factor graph.

By incorporating multiple spatial and temporal constraints from both recent and earlier keyframes, the proposed method improves the connectivity and redundancy of the factor graph, thereby enhancing the robustness of pose estimation under noisy measurements and dynamic UAV motion.

In conventional multi-keyframe alignment strategies, all historical keyframes are typically treated with equal importance when estimating the pose of the current frame. However, in real-world UAV operations, the influence of each keyframe varies depending on its spatial proximity and temporal relevance. To address this issue, a dual-factor adaptive weighting strategy is introduced, which assigns different weights to each keyframe based on both spatial distance and temporal interval.

The distance weight $w_{d_j}$ between the current frame $F_{\mathrm{cur}}$ and a historical keyframe $X_j$$(j=i,i-1,i-2)$ is defined as:

$$w_{d_j}={\displaystyle \frac{1}{1+\alpha ·\mathrm{dist}(F_{\mathrm{cur}},X_j)}},$$

(2)

where $\mathrm{dist}(F_{\mathrm{cur}},X_j)$ denotes the Euclidean distance between the poses of $F_{\mathrm{cur}}$ and $X_j$, and $\alpha$ is a tunable parameter controlling the decay rate of the distance weight.

Similarly, the temporal weight $w_{t_j}$ is defined as:

$$w_{t_j}={\displaystyle \frac{\mathsf{\Delta }{t}_{max}-\mathsf{\Delta }{t}_j}{\mathsf{\Delta }{t}_{max}-\mathsf{\Delta }{t}_{min}}},$$

(3)

where $\mathsf{\Delta }{t}_j$ represents the time interval between $F_{\mathrm{cur}}$ and $X_j$, and $\mathsf{\Delta }{t}_{max}$ and $\mathsf{\Delta }{t}_{min}$ denote the maximum and minimum time intervals among all selected keyframes, respectively.

The combined weight $w_j$ for each keyframe is computed as:

$$w_j=\beta ·w_{d_j}+(1-\beta )·w_{t_j},$$

(4)

where $\beta \in [0,1]$ is a fusion coefficient that balances the contributions of spatial and temporal factors.

Using these adaptive weights, the relative transformation between the current frame and each historical keyframe $X_j$ is estimated as:

$$T_{\mathrm{cur},j}=arg\underset{T_{\mathrm{cur},j}}{min}{w}_j\sum _k{∥P_{\mathrm{cur}}^k-T_{\mathrm{cur},j}·P_j^k∥}^2.$$

(5)

In addition, relative transformations between adjacent historical keyframes are refined using corresponding inter-keyframe weights:

$$T_{i-1,i}=arg\underset{T_{i-1,i}}{min}{w}_{i-1,i}\sum _k{∥P_i^k-T_{i-1,i}·P_{i-1}^k∥}^2,$$

(6)

where $w_{i-1,i}$ is computed based on the spatial and temporal relationships between the two keyframes. These weighted constraints are directly incorporated into the factor graph optimization process in both dataset-based evaluations and real UAV experiments, as described in Results Section.

2.3. Factor Graph Structure

Based on the constraints described in the previous subsection, a local factor graph is constructed to jointly optimize the poses of the current LiDAR frame and multiple historical keyframes. In this graph, each node represents a 6-DoF pose of a keyframe or the current frame, while each edge encodes a relative transformation constraint obtained from weighted point cloud registration, as illustrated in Figure 1. The factor graph is maintained in a sliding-window manner, allowing the system to balance computational efficiency and robustness.

Specifically, the constructed factor graph contains the following types of edges:

• An edge between $X_{i-2}$ and $X_{i-1}$, representing the relative transformation $T_{i-2,i-1}$;
• An edge between $X_{i-1}$ and $X_i$, representing the relative transformation $T_{i-1,i}$;
• An edge between the current frame $F_{cur}$ and the historical keyframe $X_{i-2}$, representing $T_{cur,i-2}$;
• An edge between $F_{cur}$ and $X_{i-1}$, representing $T_{cur,i-1}$;
• An edge between $F_{cur}$ and $X_i$, representing $T_{cur,i}$.

By connecting the current frame with multiple recent keyframes, the proposed graph structure introduces redundant spatial–temporal constraints that are particularly beneficial under high-frequency vibration and rapid motion. At the same time, limiting the graph to a small set of temporally adjacent keyframes avoids excessive computational overhead and reduces the risk of incorporating outdated or inconsistent constraints. All edges in the factor graph are weighted according to the adaptive spatial–temporal strategy described in the previous subsection, ensuring that more relevant keyframes contribute more significantly to the overall optimization.

This factor graph structure enables robust and locally consistent pose estimation by effectively leveraging both short-term motion continuity and longer-term spatial consistency, thereby mitigating vibration-induced noise and pose drift in UAV LiDAR SLAM.

2.4. Graph Optimization

In the proposed multi-keyframe SLAM framework, graph optimization aims to jointly minimize the residuals of all weighted edges in a local sliding window of the factor graph. Let the graph contain n edges and m nodes within the current optimization window. The objective function is defined as:

$$E\left(x\right)=\sum _{k=1}^n{e}_k{\left(x_k\right)}^T{\Omega }_k{e}_k\left(x_k\right)$$

(7)

where

• $x=\{x_1,x_2,\dots ,x_m\}$ denotes the set of node poses within the sliding window, with $x_i$ representing the state of the i-th node;
• $e_k\left(x_k\right)$ is the error function for the k-th edge, which may involve the current frame and one or more historical keyframes;
• ${\Omega }_k$ is the information matrix that incorporates the adaptive spatial-temporal weights described in Factor Graph Structure Subsection, ensuring that edges corresponding to more relevant keyframes have higher influence.

Specifically, the edge residuals are defined on $\mathrm{SE}\left(3\right)$ as follows:

$$e_{j-1,j}(X_{j-1},X_j)=log\left(T_{j-1,j}^{-1}·T{\left(X_{j-1}\right)}^{-1}·T\left(X_j\right)\right)$$

(8)

$$e_{cur,j}(F_{cur},X_j)=log\left(T_{cur,j}^{-1}·T{\left(F_{cur}\right)}^{-1}·T\left(X_j\right)\right),j\in \{i-2,i-1,i\}$$

(9)

As in standard pose graph optimization, the factor graph is sparse: each node is connected only to a subset of edges in the sliding window. This sparsity allows efficient computation by reducing the number of non-zero elements in the Hessian matrix.

To solve the nonlinear least-squares problem, the Gauss–Newton method is employed iteratively within the sliding window. The main steps are:

1.

Jacobian computation: calculate partial derivatives of each edge residual with respect to the connected node poses, forming the Jacobian $J_k$;

2.

Linearization: approximate each residual using first-order Taylor expansion around the current estimate $x^0$:

$$e_k(x^0+\mathsf{\Delta }x)\approx e_k\left(x^0\right)+J_k\mathsf{\Delta }x$$

(10)

3.

Linear system formation: accumulate the contributions from all edges in the sliding window to form

$$\left(\sum _{k=1}^n{H}_k\right)\mathsf{\Delta }x=-\sum _{k=1}^n{b}_k,H_k=J_k^T{\Omega }_k{J}_k,b_k=J_k^T{\Omega }_k{e}_k\left(x^0\right)$$

(11)

4.

State update: solve for $\mathsf{\Delta }x$ and update all node poses within the sliding window:

$$x\leftarrow x^0+\mathsf{\Delta }x$$

(12)

5.

Convergence check: evaluate residual reduction and iterate until convergence criteria are met.

By performing Gauss–Newton optimization on this weighted multi-keyframe factor graph, the system achieves robust and globally consistent pose estimation, effectively mitigating the impact of UAV vibrations and high-frequency measurement noise while maintaining computational efficiency.

3. Results

3.1. Experimental Evaluation on Datasets

To evaluate the performance of the proposed optimization method, experiments were conducted on the NTU VIRAL dataset. The NTU VIRAL dataset, developed by the research team at Nanyang Technological University in Singapore, is designed to meet the complex requirements of autonomous aerial vehicles. It integrates a variety of sensor configurations, including two 3D LiDAR units, stereo time-synchronized cameras, multiple inertial measurement units (IMUs), and four sets of ultra-wideband nodes, forming a comprehensive perception system. These sensors work collaboratively to capture environmental information from diverse perspectives, providing high-quality multimodal data for algorithm evaluation. Flight tests were conducted in various indoor and outdoor environments, covering scenarios from urban streets to complex indoor spaces, making the dataset a comprehensive benchmark for UAV SLAM performance assessment.

To quantitatively assess the performance of the proposed method, we compared it with the widely-used LeGO-LOAM algorithm, as it represents a strong baseline for LiDAR-only SLAM under vibration-prone conditions. LeGO-LOAM has demonstrated strong performance in LiDAR-based SLAM, particularly in point cloud processing and map construction. For a reliable evaluation benchmark, ground-truth trajectories were generated using an IMU-assisted SLAM system. The IMU provides real-time pose and acceleration data, which, when fused with LiDAR measurements, yields trajectories that closely approximate the true motion of the UAV, enabling a more objective and accurate comparison between algorithms.

Figure 4 further compares the trajectory and rotational estimation results obtained using the proposed method and LeGO-LOAM. During the experiments, UAVs were subject to external disturbances, such as vibration, posing a significant challenge for pose estimation. LeGO-LOAM exhibits noticeable deviations under such conditions, resulting in less accurate environmental reconstruction. The reconstructed maps closely match the real environment, clearly preserving key structural and geometric features. This indicates that the proposed approach is highly resilient to UAV-induced perturbations, ensuring precise localization and reliable mapping in challenging scenarios.

Figure 2. Comparison of mapping results in outdoor environments.

Figure 2. Comparison of mapping results in outdoor environments.

Figure 3. Comparison of mapping results in indoor environments.

Figure 3. Comparison of mapping results in indoor environments.

Figure 4. Comparison of trajectory and rotational estimation between the proposed method and LeGO-LOAM. Subfigures (a)–(d) respectively show indoor translation, indoor rotation, outdoor translation, and outdoor rotation results.

Figure 4. Comparison of trajectory and rotational estimation between the proposed method and LeGO-LOAM. Subfigures (a)–(d) respectively show indoor translation, indoor rotation, outdoor translation, and outdoor rotation results.

In addition to these qualitative mapping results, we further evaluate the trajectory and rotational accuracy quantitatively using ground-truth data. Table 1 and Table 2 provide quantitative comparisons of trajectory and rotational errors. In outdoor scenarios, the trajectory RMSE for LeGO-LOAM reaches 8.00 m, whereas the proposed method reduces it to 2.32 m, corresponding to an improvement of approximately 71%. The rotational RMSE is reduced from 45.01° to 7.06° (∼84% improvement), reflecting the effectiveness of multi-keyframe constraints and spatial-temporal weighting in suppressing vibration-induced orientation errors. In indoor environments, trajectory RMSE is reduced from 0.49 m to 0.35 m (∼29% improvement), while rotational RMSE decreases from 2.98° to 1.71° (∼43% improvement).

These results demonstrate that the proposed method significantly outperforms LeGO-LOAM in both localization accuracy and rotational consistency. The multi-keyframe graph optimization with spatial-temporal weighting effectively reduces drift and suppresses vibration-induced noise, ensuring reliable and precise mapping performance in real-world UAV applications.

3.2. Algorithm Deployment

To comprehensively validate the performance of the proposed graph-based multi-keyframe joint optimization method in real-world scenarios, we deployed the proposed algorithm on an unmanned aerial vehicle (UAV) platform and performed field experiments to evaluate its practical effectiveness. This section presents the hardware configuration, data collection process, and experimental results.

As shown in Figure 5, the UAV platform mainly consists of the following components:

1.

Velodyne VLP-16 LiDAR: Serves as the primary environmental perception sensor.

2.

IMU: Provides high-frequency attitude and acceleration data to assist LiDAR data compensation.

3.

NUC Mini Computer: Executes the proposed SLAM algorithm in real time. Its compact design and low power consumption make it ideal for UAV applications.

4.

UAV Platform: Provides stable flight control and payload capability to carry the LiDAR, IMU, and NUC.

Figure 6 presents the 3D map reconstructed by the proposed algorithm. Key structural and geometric features, including building contours, road layouts, and vegetation distributions, are accurately preserved, demonstrating the algorithm’s ability to capture both large-scale structures and fine details. The point cloud exhibits high density and uniform distribution, demonstrating the algorithm’s accuracy and robustness in environmental perception and map construction.

Figure 7 shows the experimental UAV platform. The UAV performed flights over diverse environments, including open spaces, building surroundings, and vegetation-rich areas, enabling comprehensive evaluation of the algorithm’s robustness.

Table 3 presents quantitative errors in trajectory and rotation. The low RMSE, mean, and standard deviation values indicate that the proposed algorithm preserves accurate pose estimates under real-world UAV conditions, consistent with the dataset-based evaluation. Table 3 and Fig. Figure 8 illustrate that the estimated UAV trajectory closely aligns with the ground truth, exhibiting minimal deviation even in complex environments. These results confirm that the proposed algorithm maintains high localization accuracy and stable tracking performance, ensuring reliable UAV localization and supporting autonomous navigation. The overall mapping and trajectory validation confirm the strong real-world applicability of the proposed SLAM system.

4. Discussion

The experimental results demonstrate that the proposed multi-keyframe graph optimization approach substantially improves localization accuracy and mapping consistency under high-vibration UAV conditions. Compared with LeGO-LOAM, the method achieves reduced trajectory drift and enhanced robustness, particularly during rapid attitude changes. For instance, the outdoor trajectory RMSE decreased from 8.00 m to 2.32 m, while rotational RMSE dropped from 45.01° to 7.06°, highlighting the effectiveness of the approach in challenging scenarios.

The performance improvement primarily arises from joint optimization over multiple historical keyframes. By incorporating spatially and temporally distributed constraints into the factor graph, the method effectively mitigates the adverse effects of vibration-induced LiDAR measurement noise. In contrast, traditional scan-to-scan or single-keyframe-based approaches tend to accumulate errors when local alignment becomes unreliable.

The proposed adaptive weighting strategy further enhances system stability by dynamically balancing the contributions of different keyframes. Keyframes that are spatially closer and temporally more relevant exert stronger influence during optimization, which helps maintain local consistency while preserving the global map structure. This combination of multi-keyframe constraints and adaptive weighting ensures accurate pose estimation and consistent map reconstruction even under rapid UAV maneuvers and external disturbances.

Despite these advantages, certain limitations remain. Increasing the number of keyframes involved in optimization inevitably raises computational cost compared with lightweight SLAM frameworks. Although real-time performance is demonstrated on the onboard NUC computer, further optimization or hardware acceleration may be required for smaller UAV platforms with stricter payload constraints.

Overall, these findings indicate that the proposed approach provides a favorable trade-off between accuracy and computational complexity, making it well-suited for practical UAV applications in dynamic and vibration-prone environments.

5. Conclusions

This paper addressed the limited accuracy and stability of UAV-based SLAM systems under severe motion disturbances. The proposed method exploits spatial-temporal correlations in historical observations by aligning the current frame with multiple historical keyframes and constructing a pose graph, thereby substantially improving pose estimation accuracy and robustness.

Extensive experiments on the NTU VIRAL dataset demonstrated that the proposed method outperforms LeGO-LOAM, reducing trajectory RMSE by up to 71% and rotational RMSE by up to 84%, particularly under rapid motion and oscillatory disturbances. Real-world UAV experiments further confirmed the practical applicability of the method, demonstrating real-time processing and accurate mapping in complex environments, including buildings, vegetation, and ground structures.

Overall, the graph-based multi-keyframe optimization framework effectively mitigates UAV vibration effects, enhances localization accuracy and mapping consistency, and exhibits strong potential for deployment in practical aerial applications.