Pose Graph Optimization Algorithm Based on Loop-Closure Edges Residual Focusing Weight Model

Pose Graph Optimization Algorithm Based on Loop-Closure Edges Residual Focusing Weight Model

Introduction

Simultaneous Localization and Mapping (SLAM) is a fundamental technology that enables autonomous systems to navigate and map unknown environments. It has found extensive applications in robotics, autonomous vehicles, and 3D reconstruction. A critical component of SLAM systems is the backend optimization, where pose graph optimization (PGO) plays a central role in refining the estimated poses of the robot or sensor over time. However, the presence of loop-closure edges with large noise can severely degrade the performance of PGO, leading to inaccurate localization and inconsistent maps.

Traditional PGO methods rely on iterative optimization techniques such as Gauss-Newton, Levenberg-Marquardt, and trust-region methods. While these approaches work well under moderate noise conditions, they struggle when faced with large noise in loop-closure edges. Such noise can arise from sensor inaccuracies, environmental factors, or incorrect feature matching. To address this challenge, robust initialization and optimization techniques are essential to ensure convergence to a globally optimal solution.

This paper introduces a novel algorithm called Residual Weighted Enhancement for Recursive Least Squares Pose Graph Optimization (RW-RLSPGO), which leverages K-means clustering to classify loop-closure edge residuals and dynamically adjust their weights during optimization. By reducing the influence of noisy loop closures, the proposed method enhances both the accuracy and robustness of PGO in high-noise environments.

Background and Related Work

PGO has evolved significantly since its inception. Early approaches, such as those by Lu and Milios, introduced the concept of globally consistent range scan alignment, laying the groundwork for modern graph-based SLAM. Subsequent developments, including GraphSLAM by Thrun et al., formalized the use of pose graphs, where nodes represent robot poses and edges encode relative measurements between them.

Initialization methods for PGO have also seen considerable advancements. Simple heuristics like odometry-based initialization and minimum spanning tree search provide computationally efficient but often suboptimal initial estimates. More sophisticated techniques, such as the Cauchy algorithm and MASAT, offer better performance under low-noise conditions but falter when noise levels increase. Recent work by Nasiri et al. proposed the Recursive Least Squares (RS) algorithm, which demonstrates improved robustness by decoupling rotation and translation estimation.

Despite these advances, handling large noise in loop-closure edges remains a challenge. Existing methods often rely on fixed-weight schemes or outlier rejection heuristics, which may not adapt well to varying noise levels. The RW-RLSPGO algorithm addresses this limitation by introducing an adaptive weighting mechanism based on residual clustering.

Methodology

The RW-RLSPGO algorithm consists of three main components: residual clustering, residual focusing weight modeling, and iterative reweighted least squares optimization.

Residual Clustering

The first step involves classifying loop-closure edges based on their residual errors. Residuals are computed for each loop-closure edge by comparing the measured relative pose with the estimated pose derived from odometry or other initial estimates. These residuals are then clustered using the K-means algorithm.

To determine the optimal number of clusters, the elbow method is employed. This technique evaluates the within-cluster sum of squared errors (SSE) as a function of the number of clusters. The point where the rate of decrease in SSE sharply changes (the “elbow”) indicates the appropriate number of clusters. By clustering residuals, the algorithm can distinguish between reliable and noisy loop closures.

Residual Focusing Weight Model

Once residuals are clustered, a weight is assigned to each loop-closure edge based on its deviation from the cluster centroid. The weight is computed as the absolute difference between the residual and its cluster centroid, normalized by the maximum of the two values. This ensures that edges with large residuals (indicative of high noise) receive lower weights, reducing their influence on the optimization process.

The weight model is designed to be adaptive, meaning that as noise levels change, the weights adjust accordingly. This flexibility allows the algorithm to maintain robustness across varying noise conditions.

Iterative Reweighted Least Squares Optimization

With the weights assigned, the PGO problem is reformulated as a weighted least squares optimization. The algorithm iteratively refines the pose estimates by solving two linear least squares problems: one for rotation and another for translation. The rotation problem is solved first, assuming an initial estimate, and then the translation problem is addressed using the refined rotation estimates.

The iterative process continues until convergence, with weights updated in each iteration based on the latest residual evaluations. This approach ensures that the optimization remains focused on reliable measurements while gradually correcting for errors introduced by noisy loop closures.

Experimental Results

The performance of RW-RLSPGO was evaluated on both simulated and real-world PGO datasets, including sphere-a, torus, cube, garage, cubicle, and rim. These datasets were chosen to represent a range of scenarios, from synthetic environments to real-world SLAM challenges.

To simulate high-noise conditions, Gaussian noise with varying standard deviations (ranging from 0.1 to 1.0 radians) was added to the loop-closure edges. Monte Carlo experiments were conducted to ensure statistical significance, with each experiment repeated 50 times to account for randomness in noise injection.

Comparison with Baseline Methods

RW-RLSPGO was compared against four state-of-the-art initialization algorithms: Odometry, Spanning Tree, MASAT, and RS. The evaluation metrics included total loss function value, rotation loss, translation loss, computation time, and iteration count.

The results demonstrated that RW-RLSPGO consistently outperformed the baseline methods, particularly in high-noise scenarios. For instance, in the torus dataset with a noise level of 0.5 radians, RW-RLSPGO achieved a 78.69% reduction in translation loss compared to RS. Similar improvements were observed across other datasets, with rotation loss reductions exceeding 80% in some cases.

Robustness to Noise

A key advantage of RW-RLSPGO is its ability to maintain performance as noise levels increase. While traditional methods like Odometry and Spanning Tree exhibited significant degradation in accuracy under high noise, RW-RLSPGO showed only marginal increases in loss values. This robustness is attributed to the adaptive weighting mechanism, which effectively downweights unreliable loop closures.

Computational Efficiency

Despite its advanced clustering and weighting steps, RW-RLSPGO remains computationally efficient. The algorithm’s iteration count and runtime were comparable to or better than those of RS, indicating that the additional complexity does not impose a significant overhead. This makes RW-RLSPGO suitable for real-time applications where both accuracy and speed are critical.

Qualitative Results

Visual comparisons of optimized trajectories further validated the superiority of RW-RLSPGO. In datasets like torus and rim, trajectories optimized using RW-RLSPGO closely matched the ground truth, even under severe noise conditions. In contrast, trajectories from other methods often diverged or exhibited large drifts, particularly when noise exceeded 0.5 radians.

Discussion

The success of RW-RLSPGO can be attributed to its innovative use of residual clustering and adaptive weighting. By dynamically adjusting the influence of loop-closure edges, the algorithm mitigates the impact of outliers and noisy measurements. This is particularly important in real-world SLAM applications, where sensor noise and environmental variability are inevitable.

One limitation of the current approach is the reliance on K-means clustering, which can be sensitive to initial centroid selection. Future work could explore more robust clustering techniques, such as density-based clustering or hierarchical methods, to further improve stability. Additionally, integrating RW-RLSPGO with real-time SLAM systems and evaluating its performance in dynamic environments would be valuable.

Conclusion

This paper presented RW-RLSPGO, a novel pose graph optimization algorithm designed to handle large noise in loop-closure edges. By leveraging K-means clustering and an adaptive residual focusing weight model, the algorithm significantly improves the accuracy and robustness of PGO. Extensive experiments on both simulated and real-world datasets demonstrated its superiority over existing methods, particularly in high-noise scenarios.

RW-RLSPGO represents a meaningful advancement in SLAM backend optimization, offering a practical solution for applications where sensor noise and environmental challenges are prevalent. Future research directions include enhancing the clustering stability and extending the algorithm to handle more complex noise distributions.

doi.org/10.19734/j.issn.1001-3695.2024.04.0170

Was this helpful?

YesNo

0 / 0