MCHS-SLAM: A Multi-Constraint Hybrid Strategy SLAM Framework for AUV-Based Seafloor Terrain Mapping

Abstract

During seafloor terrain mapping missions conducted by AUVs, positioning error accumulation occurs inevitably over long distances due to the unavailability of global satellite navigation signals underwater. Moreover, the alternating distribution of flat and undulating regions on the seafloor renders single-constraint-based bathymetric SLAM methods prone to performance degradation in complex environments. To address these challenges, this paper proposes a multi-constraint hybrid strategy SLAM framework for AUV-based seafloor terrain mapping, grounded in an analysis of error accumulation mechanisms and constraint failure characteristics. The framework establishes a hierarchical and progressive constraint architecture to enable collaborative optimization across different spatial scales and topographic conditions. At the foundational pose estimation stage, multi-source trajectory information is fused to ensure continuity and stability in pose computation. In the local consistency constraint stage, an improved point cloud registration method combined with a neighborhood survey-line constraint mechanism is introduced to enhance geometric consistency among survey lines in feature-sparse regions. At the global optimization stage, a loop closure detection strategy is designed based on topographic statistical features, incorporating adaptive thresholds and correlation metrics to achieve robust introduction of global constraints. By flexibly integrating direct registration and feature-matching strategies according to topographic characteristics, the framework fully leverages the advantages of multi-constraint cooperative optimization. The proposed method is validated by the field data. Experimental results on real lake-trial data show that, relative to the baseline configurations evaluated under identical noise-injection conditions, the MCHS-SLAM framework yields more concentrated consistency-error distributions with markedly shorter large-error tails, and exhibits improved error suppression relative to the reference trajectory. This work presents a methodological framework for high-quality seafloor terrain mapping under heterogeneous terrain conditions, providing a basis for future extensions toward onboard real-time deployment.

1. Introduction

Seafloor terrain mapping is a fundamental task in ocean engineering and resource development, possessing irreplaceable strategic value for major engineering activities such as submarine pipeline laying, transoceanic cable installation, and deep-sea resource exploration [1]. Conventional underwater survey operations are typically characterized by long mission durations and high costs. Consequently, reducing offshore operational time and personnel requirements while improving overall survey efficiency has become an urgent challenge to address. In recent years, AUVs have become an important technological platform for autonomous seafloor terrain mapping, owing to their strong environmental adaptability, autonomous operational capabilities, and suitability for large-scale marine tasks [2,3]. In underwater environments where external absolute positioning signals are unavailable, AUV platforms place higher demands on robust navigation and mapping methods, motivating dedicated research on bathymetric SLAM frameworks that can remain stable across heterogeneous seafloor terrain.

However, under long-range, deep-water operating conditions and in the absence of external positioning aids (e.g., ship-based support), the positioning accuracy of AUVs remains a significant challenge. Due to the absence of external absolute reference signals such as GPS in the underwater environment, AUVs typically rely on dead reckoning (DR) and Doppler velocity logs (DVLs) for navigation. Owing to sensor noise and the accumulation of integration errors, the positioning errors of these methods increase continuously over time, thereby substantially degrading the accuracy and reliability of seafloor terrain map construction. Although existing post-processing techniques such as track correction, swath matching, and mosaic stitching, implemented in mapping software, can mitigate error accumulation to some extent, they generally require extensive manual intervention, involve lengthy computation cycles, and are often unable to fully compensate for systematic drift accumulated over large-scale survey areas. Consequently, achieving real-time accurate trajectory estimation and online optimization of AUV navigation is of critical practical importance for ensuring complete coverage of surveyed regions and generating high-quality topographic maps that meet stringent engineering accuracy requirements.

Bathymetric simultaneous localization and map (BSLAM) has demonstrated excellent performance in terrain mapping across various underwater scenarios. In practical seabed survey missions, as illustrated in Figure 1, AUVs typically employ a classic lawnmower pattern trajectory for area coverage [4].

Figure 1. Schematic illustration of coverage path planning. Different colors represent different survey lines, D denotes the line spacing between adjacent survey lines, and the dotted lines indicate the planned coverage trajectory.

However, the structural mismatch between regular survey tracks and spatially heterogeneous seafloor terrain poses significant challenges to BSLAM in the data association and registration stages. These challenges primarily manifest in the following three aspects:

(1)

Degradation of data association due to insufficient features. In large-scale flat regions, although adjacent survey lines maintain a certain degree of overlap, the lack of discriminative terrain features makes it difficult to establish stable data associations. As a result, positioning errors tend to accumulate rapidly.

(2)

Registration strategy failure caused by uneven feature distribution. In locally complex terrains, while richer topographic features may be available, their spatial distribution is highly non-uniform. This heterogeneity prevents a uniform registration strategy from effectively adapting to geomorphological variations across different scales, increasing the risk of converging to local optima and thereby reducing overall registration accuracy.

(3)

Long-term stability degradation induced by evolving terrain characteristics. As the operational area expands and mission duration increases, terrain features exhibit continuous variation in scale, density, and spatial distribution. A single, fixed registration strategy struggles to maintain consistent performance over the entire mission, leading to deteriorated navigation accuracy and reduced consistency in mapping results.

Although various underwater SLAM schemes have been proposed in existing research [5,6], most of these approaches emphasize theoretical innovation at the algorithmic level or performance validation under controlled environments. They often pay insufficient attention to the engineering challenges posed by spatial heterogeneity in seafloor terrain. Existing methodologies predominantly employ a single registration strategy. While effective in feature-rich or otherwise favorable environments, these approaches exhibit limited adaptability to the complex and varied conditions typical of real-world seafloor terrains, which are often characterized by an interlaced distribution of flat and structurally complex regions. This limitation becomes particularly pronounced during long-duration, large-scale surveys employing lawnmower-pattern coverage. A critical unresolved challenge is thus the adaptive selection of registration strategies based on localized terrain characteristics, so as to ensure consistent localization accuracy and reliable mapping performance throughout an entire mission.

This paper addresses the key challenges posed by spatial heterogeneity in seabed topography under the lawnmower survey pattern by proposing an adaptive seafloor terrain SLAM framework. The framework establishes a three-layer cooperative constraint architecture and integrates a hybrid strategy combining direct point cloud registration and feature-based terrain matching. It further introduces a terrain complexity-driven adaptive selection mechanism, which leverages the robustness of direct methods in flat regions while activating the high-precision capability of feature-based methods in complex terrains. This approach effectively mitigates issues such as feature scarcity in flat areas, registration imbalance in complex zones, and degradation of long-term stability. As a result, the framework achieves robust localization and high-quality mapping under conditions of uneven feature distribution.

The contribution of this work does not lie in a new fundamental point cloud registration operator, but in a system level design philosophy for bathymetric SLAM under heterogeneous seafloor terrain. Existing bathymetric SLAM methods typically adopt a single constraint strategy, either direct registration or feature based matching, and apply it uniformly across the entire survey area. Such a uniform strategy is inherently mismatched with the spatial heterogeneity of real seafloor environments, where flat regions provide insufficient features for reliable matching, while locally complex regions exhibit uneven geometric distributions that may destabilize direct registration. The core idea of this paper is that the choice of constraint strategy should be governed by local terrain observability rather than fixed in advance. Specifically, local feature density is used as a data driven proxy for terrain observability, and this indicator determines, for each candidate registration, whether the system relies on geometric alignment through direct GICP or on feature correspondence through TSD keypoint matching. In addition, accepted constraints are not incorporated into graph optimization with uniform weights. Instead, each constraint is incorporated through a quality aware information matrix derived from its own registration indicators, including the inlier ratio, the mean squared error of the overlap region, and the number of valid overlapping points, so that high quality constraints dominate the optimization while low quality constraints are naturally down weighted. The novelty of this work therefore lies in two coupled design choices, namely observability driven strategy selection and quality aware constraint modeling, as well as in their joint effect of producing a system that remains stable across the alternating flat and complex structure of a complete lawnmower survey, rather than being optimal for any single registration instance in isolation.

The main contributions of this paper are as follows:

(1)

An observability driven constraint switching mechanism for heterogeneous bathymetric terrain is proposed. A data driven criterion is introduced in which local feature density, defined as the ratio of extracted TSD key points to valid grid cells within a submap, is used as a proxy indicator of terrain observability. When this indicator falls below a threshold, the local terrain is considered insufficient to support reliable feature matching, and the system falls back to direct GICP registration to preserve robustness. Otherwise, the feature aided registration path is activated to exploit richer local geometric information. This mechanism provides an explicit and reproducible answer to the question of when features should be trusted and when geometry should be trusted under heterogeneous seafloor terrain.

(2)

A terrain standard deviation-based feature extraction and matching method for 2.5D seafloor topography is developed. Stable key points are extracted using multiscale TSD maps and adaptive percentile thresholds. A cascaded matching framework is introduced by incorporating an MSE based similarity metric, spatial consistency constraints, and bidirectional validation. For robust transformation estimation, an enhanced RANSAC algorithm is developed by integrating PROSAC sampling and MAD based adaptive inlier thresholding.

(3)

Quality aware constraint modeling through registration quality dependent information matrices is introduced. Rather than incorporating all inter submap and loop closure constraints into the pose graph with uniform weights, each accepted registration is incorporated through an information matrix whose diagonal entries are determined by its own quality indicators, specifically the RANSAC inlier ratio, the mean squared error of the overlap region, and the number of valid overlapping points. Registrations that fail to satisfy the acceptance thresholds are rejected directly, whereas accepted registrations enter the optimization with weights that scale with their quality. This quality aware modeling ensures that high confidence constraints dominate global optimization, while low confidence constraints are naturally down weighted rather than being allowed to degrade the solution.

(4)

System level validation is conducted on real lake trial data under a complete lawnmower survey. The proposed framework is validated on a real multibeam lake trial dataset covering an area of 738 m × 294 m with alternating flat and locally undulating regions, under a systematic primary plus check survey pattern that activates all three types of constraints, namely odometry, K-adjacent survey line registration, and loop closure. The experimental results are interpreted not in terms of a single scalar accuracy value, but in terms of the shape of the consistency error distribution over all overlap regions, which provides an appropriate global characterization of stability for heterogeneous terrain mapping tasks. This provides system level evidence that observability driven switching combined with quality aware constraint modeling yields more concentrated error distributions and markedly shorter large error tails than the baseline configurations.

The remainder of this paper is organized as follows: Section 2 reviews related work. Section 3 presents the design of the adaptive BSLAM framework in detail, including the overall architecture, odometry-constrained initial pose estimation, adaptive registration strategy for adjacent survey lines, and loop closure detection with the establishment of registration constraints. Section 4 describes the multi-constraint factor graph optimization method. Section 5 presents experimental results using a lake-trial dataset. Finally, Section 6 concludes the paper with a summary and outlook.

2. Related Work

Pose graph-based SLAM optimization algorithms are primarily divided into a frontend and a backend [7]. The frontend is responsible for bathymetric point cloud registration, which can be categorized into three main approaches: direct matching, feature-based matching, and deep learning-based registration. Through point cloud registration, relative constraints between nodes in the pose graph are established. The backend then performs global optimization over the constraint network constructed by the frontend, further reducing errors in the initially estimated relative pose constraints. Among these three registration paradigms, direct matching methods have received the earliest and most extensive research attention in underwater applications due to their ability to operate without explicit feature extraction, thus making them particularly suitable for unstructured underwater environments.

Direct matching methods perform registration directly using raw terrain point clouds, without the need to extract intermediate features. Although these methods typically involve higher computational complexity, they exhibit strong adaptability in unstructured underwater environments that lack prominent textural or geometric features, thus making them highly attractive and widely studied. Roman et al. [8,9] advanced a bathymetric SLAM (BSLAM) framework, primarily employing an Extended Kalman Filter (EKF) to track the historical AUV trajectory and estimate the current vehicle pose. After collecting short-duration multibeam bathymetric submaps, the method registers them through correlation analysis and the Iterative Closest Point (ICP) algorithm. The effectiveness of this approach in constructing high-precision topographic maps was validated using real-world field data. Palomer et al. [10] proposed an EKF-SLAM method integrated with two-stage probabilistic ICP, incorporating point-to-point and point-to-plane registration, and employed octree and surface normal-based subsampling for unstructured underwater scenarios, reducing ICP complexity through a heuristic algorithm. Norgren and Skjetne [11] applied two improved BSLAM weighting methods and particle filter-weighted ICP in iceberg mapping, enabling batch processing of multibeam measurements, supporting real-time data processing without loss of accuracy, achieved good experimental results on both simulated and real iceberg datasets, and further validated the feasibility of the algorithm using the HUGIN HUS AUV. Bichucher et al. [12] proposed a factor graph-based sparse point cloud bathymetric SLAM framework, in which the AUV forms a sparse point cloud by accumulating height measurements from DVL over time, aligns overlapping submaps via GICP in the frontend, and introduces Dynamic Covariance Scaling (DCS) in the backend to handle incorrect data associations, improving the robustness of bathymetric SLAM. Massot-Campos et al. [13] proposed a submap-based bathymetric SLAM using a single-line laser structured light system, segmenting the point cloud data returned by the structured light system into smaller submaps, determining the relative pose between submaps through pairwise submap registration, compensating for drift in position estimation, and optimizing overall localization accuracy via pose graph-based SLAM. In the study by Torroba et al. [14], submaps establish weak inter-frame constraints through a Gaussian process model, use GICP to establish local constraints for overlapping regions of submaps, obtain a complete pose graph after local optimization, and perform global optimization of AUV trajectory error using the Ceres Solver optimization library. Chang et al. [15] proposed a novel joint graph optimization method that fuses bathymetric and magnetic beacon observations to construct a joint factor graph. In the frontend, bathymetric data are processed through a two-stage data association of multi-scale vector field coarse alignment and GICP, magnetic beacon positions are detected using Euler deconvolution and clustering methods, and a robust backend optimizer is designed to obtain the optimal AUV trajectory and magnetic beacon locations. However, direct registration methods still have limitations in computational efficiency and sensitivity to initial estimates. Especially in regions with significant topographic variations, it is difficult to establish stable correspondences based solely on point cloud geometric information, which motivates researchers to explore feature-based registration strategies.

The seafloor environment, as a typical unstructured scenario, poses significant technical challenges in extracting robust feature points from bathymetric data and establishing reliable correspondences. However, in recent years, solutions specifically targeting feature extraction for seabed terrain have been proposed. Hammond and Rock [16] extracted high-curvature points from multibeam bathymetric point clouds as feature points for data association, constructed a graph-based structure, and corrected accumulated drift after loop closure. Subsequently, Hammond et al. [17] projected 3D point clouds onto 2D images to extract Scale-Invariant Feature Transform (SIFT) features. However, this method ignores the depth information of the point cloud and thus struggles to find effective keypoints in relatively flat environments. Hitchcox and Forbes [18] proposed a Gaussian process (GP)-based point cloud alignment method, using Gaussian regression to extract keypoints, identifying consistent correspondences through a weighted network alignment algorithm, and finally performing fine alignment via ICP. Sture and Ludvigsen [19] proposed a robust Gaussian process-based AUV survey-line feature matching method, gridding multibeam echo sounder data into regular image grids, and using the SIFT algorithm to extract keypoints and feature descriptors. The method matches SIFT features between different survey lines to identify points corresponding to the same seafloor topographic features, and models the discrepancies between matched feature points using a linear mixture model and Gaussian process. Moreover, Zhang and Kim [6] proposed a terrain gradient feature-based bathymetric SLAM framework, termed TTT SLAM. The method first grids the raw bathymetric data, computes the terrain standard deviation for each grid cell, extracts grid points with relatively large standard deviation values as terrain gradient feature points, and directly defines the gridded bathymetric map patches containing these feature locations as descriptors. Rotation invariance is achieved by identifying the dominant orientation of features through directional histograms and aligning the descriptors to these principal directions. Overlapping submaps are then aligned using the TEASER++ robust point cloud registration method, and pose graph optimization is performed using asymptotically non-convex truncated least squares optimization, demonstrating faster performance while maintaining high accuracy.

Feature-based methods require extracting specific keypoints to perform feature matching for establishing loop closure constraints. When AUVs conduct area mapping missions using lawn-mower coverage patterns, although adjacent survey lines maintain certain overlap, purely feature-based approaches struggle to establish effective co-visibility constraints in these regions due to the local flatness of the seafloor terrain. When an AUV travels over a long duration and revisits previously traversed locations, loop closure constraints are established to eliminate accumulated errors caused by long-range navigation. Although feature-based methods have demonstrated strong performance in loop closure detection, their effectiveness heavily relies on hand-crafted feature extractors and exhibits limited generalization capability across different seafloor geomorphic conditions. This limitation has driven researchers to turn toward deep learning approaches with adaptive feature learning capabilities.

Deep learning leverages the powerful representation learning capability of neural networks to automatically extract hierarchical feature representations from raw bathymetric data, and simultaneously optimizes the keypoint detection and matching process through end-to-end training, effectively overcoming the limitation of traditional methods that require hand-crafted feature extractors. Peng et al. [20] first proposed PL-Net, a 3D sonar point cloud method for underwater terrain matching and localization. The network extracts high-level terrain features through a deep neural network and incorporates a self-attention mechanism to improve localization accuracy. Tan et al. [21] proposed a deep learning network architecture suitable for seafloor environments, combining a Siamese network and triplet loss to achieve effective matching and loop closure detection of bathymetric point clouds, significantly improving the real-time performance and accuracy of the SLAM system. Underwater bathymetric data acquisition is challenging due to complex underwater environments and equipment limitations, and the high computational resource requirements of deep learning lead to time-consuming training. Under these dual constraints, current research is primarily focused on two categories of methods for bathymetric data matching: direct point cloud matching and feature-based registration.

In summary, existing underwater SLAM methods each have their strengths but also exhibit clear limitations. Direct registration methods are stable but computationally intensive and sensitive to initial estimates; feature-based methods perform well in complex terrains but struggle to extract effective features in flat regions; deep learning-based methods possess strong feature learning capabilities but are constrained by limited underwater data availability and high computational resource requirements. Therefore, how to organically integrate the advantages of different methods and design a hybrid SLAM framework capable of adaptively selecting registration strategies based on seafloor terrain characteristics has become a key technical challenge for improving underwater mapping quality.

Before proceeding to the framework description, it is necessary to clarify the scope within which the proposed framework is compared with existing methods. This study is positioned as an exploratory methodological investigation of bathymetric SLAM under heterogeneous seafloor terrain, and its value is evaluated in terms of whether the proposed design philosophy is effective on real survey data, rather than through numerical ranking against specific existing systems. Direct quantitative comparison with the stronger methods reviewed above is constrained by two practical factors. First, representative feature based and robust back end bathymetric SLAM methods, including the EKF ICP method of Palomer et al. [10], the dynamic covariance scaling back end of Bichucher et al. [12], the Gaussian process aided feature method of Sture and Ludvigsen [19], and the TTT-SLAM method of Zhang and Kim [6], do not provide publicly available reproducible implementations. In addition, deep learning-based methods [20,21] require training data and sensor calibrations that are not compatible with the present dataset. Second, the sensor configurations and data formats differ substantially across these methods. For example, the sparse DVL point clouds used in [12] are fundamentally different from the dense MBES point clouds used in this study, and the DoN based preprocessing pipeline of [10] depends on parameters specific to its original dataset. Reproducing these methods with sufficient fidelity for fair numerical comparison would amount to a separate benchmarking study. Accordingly, the proposed framework is compared with two deliberately selected internal configurations, namely pure odometry and GICP-SLAM, which correspond to the two extreme cases of no external constraint and a single direct registration path, respectively. These two configurations isolate the contribution of graph optimization with direct registration and, subsequently, the contribution of feature aided registration together with observability driven switching and quality aware weighting. Differences in design philosophy from the stronger methods listed above are discussed conceptually where relevant, but are not converted into numerical performance rankings, consistent with the exploratory positioning of this study.

3. MCHS-SLAM Framework

To address the issues of local insufficiency, mid-range drift, and long-range accumulated error caused by spatial heterogeneity of seafloor terrain under the lawn-mower survey pattern, this paper proposes a multi-constraint hybrid strategy SLAM framework for AUV-based seafloor terrain mapping. The framework achieves hierarchical suppression of errors at different scales by constructing three types of constraints: odometry, adjacent survey-line, and loop closure detection, and obtains a globally consistent AUV trajectory and terrain map through unified optimization in a factor graph.

3.1. Overall Framework

MCHS-SLAM is based on a pose graph structure, and the overall system architecture and data flow are illustrated in Figure 2. It consists of a frontend processing module and a backend optimization module. In the frontend, bathymetric point clouds acquired by the multibeam echosounder system (MBES) and pose data output by the inertial navigation system (INS) are first time-synchronized, and the bathymetric information is organized in the form of local submaps. Subsequently, voxel grid downsampling is applied to mitigate the effects of uneven point cloud density and improve the efficiency of subsequent registration. Based on this, the frontend generates three types of constraints—short-range, mid-range, and long-range—according to mission requirements, providing structured information for backend optimization.

Figure 2. Overall framework of the MCHS-SLAM system.

The backend module models the above multi-source constraints as a unified factor graph and performs global optimization using a robust nonlinear optimization algorithm, thereby obtaining a globally consistent AUV trajectory and seafloor terrain mapping.

The three types of constraints used in this framework, namely odometry, K-adjacent survey line registration, and loop closure, are individually well established in the bathymetric SLAM literature. The nontrivial aspect of the proposed design lies in how these constraints interact under the spatial heterogeneity of real seafloor terrain. This interaction can be understood through three representative scenarios commonly encountered in a lawnmower survey.

In flat regions, feature based matching is unreliable because local topographic variation is insufficient to produce distinctive keypoints, while odometry alone accumulates drift over long straight survey legs. In this case, the feature density criterion automatically falls below its threshold, the system adopts direct GICP registration between overlapping K-adjacent survey lines, and the information matrix of the GICP constraint is determined according to the GICP convergence residual and the number of valid overlapping points. The value of this design lies not in the novelty of GICP itself, but in the fact that the framework recognizes the corresponding regime and assigns the resulting constraint a weight consistent with its actual reliability rather than with a global default.

In locally complex regions, direct registration becomes sensitive to nonuniform geometric distributions, and a small number of highly variable structures may dominate the alignment residual and bias the estimated transformation. In this case, the feature density criterion rises above the threshold, the feature aided registration path is activated, and TSD keypoint matching provides a coarse transformation that stabilizes the subsequent GICP refinement. The resulting constraint is incorporated with an information matrix derived from its inlier ratio and the mean squared error of the overlap region, so that a well-matched complex region enters the optimization with a higher weight than a marginally accepted flat region.

Over long-range navigation, neither adjacent survey line constraints nor odometry can prevent the slow accumulation of heading drift. Loop closure constraints from revisited areas provide the only mechanism for correcting this global drift. Because a spurious loop closure would be disproportionately damaging at this scale, the framework applies the same quality aware information matrix construction to loop closure edges, admitting only those whose RANSAC inlier ratio, overlap region mean squared error, and physical plausibility of the transformation all satisfy the acceptance thresholds.

The complementarity across these three scenarios distinguishes the proposed framework from a linear combination of existing modules. Any single type of constraint, when used alone, fails in some regime: features fail in flat regions, direct registration fails under nonuniform geometry, and odometry fails over long range navigation. However, these failures occur in different regimes. The observability driven switching mechanism together with quality aware weighting ensures that, in each regime, the dominant constraint is the one whose assumptions are actually satisfied. It is this coupled behavior, rather than any individual module, that constitutes the contribution of the framework.

3.2. Odometry Constraint

Initial pose estimation based on odometry constraints provides the pose prior foundation for the MCHS-SLAM framework. The system employs an integrated navigation module composed of internal sensors including INS, DVL, and depth sensor, which generates continuous trajectory estimates through dead reckoning, providing a basic pose prior for the BSLAM framework. The system utilizes odometry information to construct an initial pose graph, in which each node represents the AUV’s pose state at a specific time and each edge encodes the relative motion constraint between adjacent time steps. Unlike terrestrial mobile robots that can leverage external reference sources such as GPS or LiDAR for localization correction, underwater SLAM systems rely entirely on internal sensors for autonomous navigation. This makes sensor noise and error accumulation key factors limiting system performance. Therefore, establishing a robust odometry constraint is not only essential for maintaining local consistency in BSLAM systems, but also provides reliable initial values for subsequent point cloud registration and global optimization.

The AUV’s dead reckoning follows the following kinematic model:

$$x_i=f(x_{i-1},u_i)+w_i$$

(1)

where $x_i$ denotes the AUV’s pose at the i-th time step, $u_i$ represents the motion control input, and $w_i$ is the system process noise.

The constraint between two consecutive frames is established through the DR constraint. For consecutive pose nodes, the DR constraint is defined as

$$e_{i,j}\left(x_i,x_j\right)=f\left(x_i,x_{i+1}\right)-T\left(x_i,x_j\right) \mathrm{with} {\Omega }_{i,j}={\Sigma }_i^{-1}$$

(2)

where $f(x_i,x_{i+1})$ denotes the relative pose transformation derived from odometry measurements, $T(x_i,x_j)$ represents the geometric transformation between consecutive pose estimates, ${\Omega }_{i,j}$ is the corresponding information matrix, and ${\Sigma }_i^{-1}$ is the inverse of the covariance matrix.

The DR constraint, together with the subsequently constructed multi-layer constraints, constitutes the edge set of the pose graph. The globally optimal pose sequence is solved through Maximum A Posteriori (MAP) estimation, thereby achieving global consistency optimization while maintaining local continuity of the trajectory.

3.3. K-Adjacent Survey Line Registration Constraint

In practical operations, accumulated inertial navigation errors and oceanic environmental disturbances can introduce systematic biases between adjacent survey lines, degrading the quality of terrain map construction. Aiming at the characteristics of sparse, high-noise, and feature-poor seafloor point cloud data, this paper employs the GICP algorithm to establish geometric constraints between adjacent survey lines. During the registration of adjacent survey lines, GICP introduces a probabilistic distance metric function that effectively integrates the complementary advantages of point-to-point and point-to-plane distance metrics, constructing a more robust error model. This approach demonstrates superior convergence properties in processing low-density point cloud data. By minimizing the probabilistic registration error function, the transformation parameters are progressively optimized, effectively compensating for pose deviations caused by navigation errors and environmental disturbances.

The GICP algorithm models each point and its local neighborhood as a Gaussian distribution. Let the source point cloud be $P={\left\{\left.b_i^P\right\}\right.}_{i=1,\dots ,N}$ and the target point cloud be $Q={\left\{\left.b_i^Q\right\}\right.}_{i=1,\dots ,N}$. The rigid-body transformation between them is formulated as

$$b_i^P=R\cdot b_i^Q+t=\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)\cdot b_i^Q=T\cdot b_i^Q$$

(3)

Here, $R\in SO\left(3\right)$ denotes the rotation matrix, $t\in R^3$ the translation vector, and $T\in SE3$ the transformation matrix that combines both to describe the rigid-body transformation. Let ${\widehat{b}}_i^P$ and ${\widehat{b}}_i^Q$ represent the true spatial distributions of the source and target point clouds, respectively. The observed point clouds obtained through actual measurements are affected by sensor noise, and their associated covariance matrices quantify the measurement uncertainty. Specifically, covariance matrices ${\mathit{C}}_i^P$ and ${\mathit{C}}_i^Q$ characterize the magnitude of measurement noise in the respective point clouds. Under this probabilistic framework, the probability distribution of the registration error $d_i^{\left(T\right)}$ is formulated as

$${\mathit{b}}_i^P~N\left({\widehat{\mathit{b}}}_i^P,{\mathit{C}}_i^P\right),{\mathit{b}}_i^Q~N\left({\widehat{\mathit{b}}}_i^Q,{\mathit{C}}_i^Q\right).$$

(4)

$$d_i^{\left(T\right)}={\mathit{b}}_i^P-\mathit{T}\cdot {\mathit{b}}_i^Q$$

(5)

$${\mathbf{d}}_1^Q=\mathbf{N}\left({\mathbf{b}}_1^Q-{\mathbf{T}}^{\ast }{\mathbf{b}}_1,{\mathbf{C}}_1^Q+\left({\mathbf{T}}^{\ast }\right){\mathbf{C}}_1^P({\mathbf{T}}^{\ast }{)}^T\right)=\mathbf{N}\left(0,{\mathbf{C}}_1^Q+\left({\mathbf{T}}^{\ast }\right){\mathbf{C}}_1^P({\mathbf{T}}^{\ast }{)}^T\right)$$

(6)

The objective function is derived through maximum likelihood estimation, and the optimal transformation parameters $T^{\ast }$ are obtained by iteratively solving a nonlinear least-squares problem.

$$T=\underset{T}{\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{a}\mathrm{x}}{\prod }_i\mathrm{p}\left({\mathrm{d}}_i^{\left(T\right)}\right)=\underset{T}{\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{a}\mathrm{x}}{\prod }_i\log \left(\mathrm{p}\left({\mathrm{d}}_1^{\left(T\right)}\right)\right)=\underset{T}{\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}}{{\prod }_i\left({\mathrm{d}}_1^{\left(T\right)}\right)}^T$$

(7)

$${\left({\mathrm{C}}_1^0+\left(T^{\ast }\right)\hspace{0.17em}{\mathrm{C}}_1^p(T^{\ast }{)}^T\right)}^T{\mathrm{d}}_1^{\left(T\right)}$$

(8)

The convergence performance of the GICP algorithm is highly dependent on the accuracy of the initial transformation estimate. In lawn-mower survey patterns, due to the relatively short data acquisition period per single survey line, error accumulation in the inertial navigation system is effectively constrained, enabling the odometry-based pose constraints to provide high-quality initialization parameters for the GICP algorithm. This initialization strategy not only effectively reduces the parameter search space of the optimization algorithm, significantly lowering computational complexity, but also enhances the convergence robustness of the algorithm in complex seafloor terrain environments.

To further improve the stability and accuracy of point cloud registration, as shown in Figure 3, this paper proposes a survey-line submap constraint strategy based on K-neighborhood. This strategy considers not only the registration relationship between directly adjacent survey lines, but also constructs K overlapping submaps from neighboring survey lines, enabling refined registration under multiple geometric constraints. This multi-submap collaborative registration mechanism, based on overlapping regions, enhances the density of local constraints and effectively suppresses error accumulation that may arise from individual pairwise registrations, significantly improving the local consistency and global coherence of the registration results.

Figure 3. Schematic illustration of K-adjacent survey line registration constraint. Blue circles represent submaps, black solid arrows indicate sequential connections between adjacent submaps along the survey line, black dotted arrows denote omitted intermediate sequential connections, and green arrows represent covisibility constraints.

3.4. Loop Closure Detection Registration Constraint

Although adaptive registration constraints between adjacent survey lines can effectively solve the alignment problem between local survey lines, relying solely on local constraints is insufficient to ensure global consistency as operation time extends and travel distance increases. To address the issue of error accumulation in long-duration missions, the system introduces a terrain feature-based loop closure detection mechanism to achieve global optimization by constructing long-range spatial constraints. Considering that the convergence performance of the GICP algorithm is sensitive to the initial transformation estimate, the system adopts a coarse-to-fine two-stage registration strategy. First, a feature-based coarse registration is performed on candidate loop closure submap pairs to obtain an initial transformation estimate, which is then used as the initial value for GICP fine registration, thereby ensuring the convergence stability of the algorithm.

The spatial heterogeneity of seafloor terrain provides a theoretical basis for feature-based registration methods. Terrain Standard Deviation (TSD), as an effective metric for quantifying local terrain complexity, can accurately identify regions with significant topographic variations. The proposed TSD-feature-driven coarse registration method first regularizes the raw point cloud through grid mapping and constructs a continuous terrain surface using inverse distance weighting interpolation. Subsequently, local terrain standard deviation is computed at multiple predefined window scales to generate a multi-scale TSD map. Keypoint detection employs a data-driven adaptive percentile thresholding strategy, which dynamically determines threshold parameters based on the TSD statistical distribution of each submap, with the addition of multi-scale consistency constraints-requiring candidate points to satisfy significance criteria across all observation scales. To achieve rotation invariance, the dominant orientation of each keypoint is estimated by constructing a gradient direction histogram, and neighborhood grids are rotationally normalized accordingly. The normalized terrain elevation values directly form the feature descriptor, effectively preserving the original geometric information of the terrain. The pseudocode for keypoint extraction is provided in Algorithm 1.

Algorithm 1: Keypoint detection and descriptor construction

Input: Gridded terrain surface S; set of multi-scale window sizes Scales; percentile threshold P_th; descriptor window size w Output: Keypoint set K; Descriptor set D 1  Initialize TSD_maps ← Empty set 2  For each scale s in Scales do 3    Compute TSD_map ← Compute terrain standard deviation (S, s) 4    Add TSD_map to TSD_maps 5  End for 6  Initialize K ← Empty set 7  For each grid point p in S do 8    Set is_keypoint ← True 9    For each TSD_map in TSD_maps do 10      Compute τ ← Get percentile threshold (TSD_map, P_th) 11      If TSD_map(p) < τ then 12        Set is_keypoint ← False 13        Break loop 14      End if 15    End for 16    If is_keypoint is true then 17      Add p to K 18    End if 19  End for 20  Initialize D ← Empty set 21  For each keypoint k in K do 22    Obtain N_k ← Get neighborhood grid (S, k, w) 23    Construct Hist ← Orientation histogram (N_k) 24    Compute θ_main ← Get dominant orientation (Hist) 25    Compute d_k ← Rotate alignment (N_k, θ_main) 26    Add d_k to D 27  End for 28  Return K, D

The feature matching stage employs a multi-stage verification mechanism to enhance matching reliability. Descriptor similarity is evaluated using mean squared error (MSE) as the metric, which exhibits stronger robustness to local noise compared to traditional Euclidean distance. Spatial consistency constraints are implemented via a KD-tree data structure to enable efficient nearest-neighbor search, restricting potential matches within a reasonable spatial neighborhood. Only bidirectional-verified correspondences are retained, and a uniqueness constraint is applied to ensure a one-to-one mapping relationship. The algorithm pseudocode is provided in Algorithm 2.

$$TSD=\sqrt{{\displaystyle \frac{1}{MN}}{\sum }_{i=1}^M{\sum }_{j=1}^N(h_{ij}-\stackrel{-}{h}{)}^2}$$

(9)

$$\mathrm{NCC}({\mathbf{D}}_i,{\mathit{D}}_j)={\displaystyle \frac{{\sum }_{p,q}\left({\mathit{D}}_i\right(p,q)-{\overline{\mathit{D}}}_i)\left({\mathit{D}}_j\right(p,q)-{\overline{\mathit{D}}}_j)}{\sqrt{{\sum }_{p,q}\left({\mathit{D}}_i\right(p,q)-{\overline{\mathit{D}}}_i{)}^2}\sqrt{{\sum }_{p,q}\left({\mathit{D}}_j\right(p,q)-{\overline{\mathit{D}}}_j{)}^2}}}$$

(10)

$\mathrm{NCC}({\mathbf{D}}_i,{\mathit{D}}_j)$: Normalized cross-correlation coefficient between feature descriptors ${\mathit{D}}_i$ and ${\mathit{D}}_j$

${\mathit{D}}_i,{\mathit{D}}_j\in {\mathbb{R}}^{d\times d}$: Two terrain feature descriptor matrices, of size $d\times d$

$p,q\in \{1,2,\dots ,d\}$: Row and column indices within the descriptor matrix

${\mathit{D}}_i(p,q)$: Seafloor depth value (in meters) of descriptor ${\mathit{D}}_i$ at position $(p,q)$

${\mathit{D}}_j(p,q)$: Seafloor depth value (in meters) of descriptor ${\mathit{D}}_j$ at position $(p,q)$

${\overline{\mathit{D}}}_i={\displaystyle \frac{1}{d^2}}{\sum }_{p=1}^d{\sum }_{q=1}^d{\mathit{D}}_i(p,q)$: Mean depth value of descriptor ${\mathit{D}}_i$

${\overline{\mathit{D}}}_j={\displaystyle \frac{1}{d^2}}{\sum }_{p=1}^d{\sum }_{q=1}^d{\mathit{D}}_j(p,q)$: Mean depth value of descriptor ${\mathit{D}}_j$

Algorithm 2: Robust feature matching and verification

Input: Source feature set F_src; target feature set F_trg; MSE threshold τ_mse; spatial distance threshold d_max Output: Verified correspondence set M 1  Initialize M_forward ← Empty set 2  Initialize M_backward ← Empty set 3  Construct KDTree_trg ← Build KD-tree(F_trg) 4  For each feature f_i in F_src do 5    Obtain Candidates ← Radius search (KDTree_trg, f_i, d_max) 6    For each candidate feature c_j in Candidates do 7      Compute mse ← Compute MSE (f_i.descriptor, c_j.descriptor) 8      If mse < τ_mse then 9        Add (i, j, mse) to M_forward 10       End if 11    End for 12  End for 13  Construct KDTree_src ← Build KD-tree (F_src) 14  For each feature f_j in F_trg do 15    Obtain Candidates ← Radius search (KDTree_src, f_j, d_max) 16    For each candidate feature c_i in Candidates do 17      Compute mse ← Compute MSE (c_i.descriptor, f_j.descriptor) 18      If mse < τ_mse then 19        Add (i, j, mse) to M_backward 20         End if 21    End for 22  End for 23  Initialize M ← Empty set 24  Initialize src_matched ← Fake array (|F_src|) 25  Initialize trg_matched ← Fake array (|F_trg|) 26  For each match (i, j, mse) in M_forward do 27    If there exists a match (i, j) in M_backward then 28      If src_matched[i] = False and trg_matched[j] = False then 29        Add (i, j) to M 30        Set src_matched[i] ← True 31        Set trg_matched[j] ← True 32      End if 33    End if 34  End for 35  Return M

The transformation parameter estimation employs an improved Random Sample Consensus (RANSAC) framework integrated with Progressive Sample Consensus (PROSAC) and an adaptive thresholding mechanism. The PROSAC strategy ranks the candidate match set based on matching quality scores, prioritizing high-confidence matches in initial iterations and progressively expanding the sampling scope as iterations proceed, thereby effectively improving the efficiency of model estimation. The adaptive thresholding mechanism dynamically adjusts the inlier determination criterion based on the Median Absolute Deviation (MAD) of inlier errors, enabling adaptation to varying noise levels. Rigid-body transformation parameters are solved via Singular Value Decomposition (SVD), and once sufficient inlier support is obtained, a least-squares optimization is performed over all inliers to globally refine the transformation estimate, further enhancing accuracy. The detailed algorithm pseudocode is provided in Algorithm 3.

Algorithm 3: Adaptive RANSAC transformation estimation

Input: Matching pair set M; number of sample points n_sample; minimum inlier count n_min; initial inlier threshold ε_init; maximum number of iterations iter_max Output: Optimal transformation matrix T_best; inlier set I_best 1  Initialize T_best ← Identity matrix 2  Initialize I_best ← Empty set 3  Initialize best_count ← 0 4  Initialize ε_adaptive ← ε_init 5  Initialize iter ← 0 6  Compute target_inliers ← |M| × Target inlier ratio 7  While iter < iter_max and best_count < target_inliers do 8      Compute max_idx ← Min(|M|, Max(n_sample, iter/10 + n_sample)) 9      Obtain Sample ← Random sample(M[0:max_idx], n_sample)//PROSACstrategy 10    Extract src_points, trg_points ← Extract sampled point pairs (Sample) 11    Compute src_centroid ← Compute centroid (src_points) 12    Compute trg_centroid ← Compute centroid (trg_points) 13    Compute src_centered ← src_points − src_centroid 14    Compute trg_centered ← trg_points − trg_centroid 15    Compute H ← src_centered × ${trg\_centered}^T$ 16    Perform U, Σ, V ← SVD decomposition(H) 17    Compute R ← V × U^T 18    If determinant(R) < 0 then 19      V[:,2] ←−V[:,2] 20      R ← V × U^T 21    End if 22    Compute t ← trg_centroid - R × src_centroid 23    Construct T_candidate ← [R|t] 24    Initialize Inliers ← Empty set 25    Initialize Errors ← Empty set 26    For each match m in M do 27      Compute error ← ||T_candidate × m.src - m.trg|| 28      Add error to Errors 29      If error < ε_adaptive then 30        Add m to Inliers 31      End if 32    End for 33    If |Inliers| > best_count then 34      Set best_count ← |Inliers| 35      Set I_best ← Inliers 36      Set T_best ← T_candidate 37      If |Inliers| > threshold update minimum count then 38        Compute inlier_errors ← Get inlier errors (Errors, Inliers) 39        Compute median_error ← Median (inlier_errors) 40        Update ε_adaptive ← Min(ε_init, median_error × scaling factor) 41      End if 42    End if 43    Set iter ← iter + 1 44  End if 45  If |I_best| > n_min then 46    Recompute T_best using all point pairs in I_best (SVD-based least squares) 47  End if 48  Return T_best, I_best

The system’s loop closure detection workflow is illustrated in Figure 4. First, the potential overlap ratio between submaps is calculated based on dead reckoning information. When this metric exceeds a predefined threshold, the loop closure detection module is activated. Candidate submap pairs sequentially undergo feature extraction, robust matching, and transformation estimation to generate a coarse registration transformation. This transformation is then used as the initial estimate for GICP optimization, yielding an accurate geometric constraint. Constraint quality evaluation comprehensively considers multiple metrics, including RANSAC inlier ratio, GICP convergence residual, mean squared error over the overlapping region, and physical plausibility of the transformation parameters. The system dynamically adjusts the constraint confidence based on the evaluation results, ensuring that high-quality constraints dominate the backend optimization. For registration results that do not meet the quality criteria, the system adopts a conservative strategy or directly rejects them, maintaining the stability of the overall optimization process.

Figure 4. Flowchart of loop closure detection.

So far, this paper has established a complete hierarchical constraint system: odometry constraints maintain temporal continuity, adjacent survey-line constraints enhance geometric consistency at medium scale, and loop closure detection constraints correct accumulated errors during long-term operations. Due to significant differences among the three types of constraints in terms of error characteristics, confidence levels, and effective scales, effectively fusing these heterogeneous constraints within a unified framework, constructing a consistent optimization objective, and designing an efficient solving strategy become the core challenges of backend optimization. Therefore, multi-constraint factor graph optimization plays a key role in transforming frontend constraint information into globally optimal state estimates, and its design quality directly affects the final performance of the entire BSLAM system.

4. Multi-Constraint Factor Graph Optimization

In the MCHS-SLAM system, pose graph optimization achieves globally consistent map construction by building a graph-structured state estimation framework, as shown in Figure 5, where nodes represent the AUV’s pose states and edges encode spatial constraint relationships of different types.

Figure 5. Factor graph representation of the MCHS-SLAM framework.

The factor graph incorporates three types of heterogeneous constraints: odometry constraints provide temporally continuous pose predictions based on the inertial navigation system, adjacent survey-line registration constraints establish lateral geometric associations between survey lines through the GICP algorithm, and loop closure constraints identify revisited areas via terrain feature matching and construct global topological constraints. The core of pose graph optimization lies in finding the optimal state configuration within a nonlinear least squares framework, such that the weighted sum of squared residuals across all constraints in the graph is globally minimized, thereby achieving joint optimization of trajectory estimation and map reconstruction.

These nonlinear least squares problem can be formulated as

$$X^{\ast }=\mathrm{a}\mathrm{r}\mathrm{g} \underset{X}{\mathrm{m}\mathrm{i}\mathrm{n}}\left\{{\sum }_{(i,j)\in E_o}(e_{ij}^o{)}^{\mathrm{\top }}{\mathsf{\Omega }}_{ij}^o{e}_{ij}^o+{\sum }_{(k,l)\in E_r}(e_{kl}^r{)}^{\mathrm{\top }}{\mathsf{\Omega }}_{kl}^r{e}_{kl}^r+{\sum }_{(m,n)\in E_l}(e_{mn}^l{)}^{\mathrm{\top }}{\mathsf{\Omega }}_{mn}^l{e}_{mn}^l\right\}$$

(11)

where $E_o$, $E_r$, and $E_l$ denote the sets of odometry edges, K-adjacent survey line registration edges, and loop closure edges, respectively; $e_{ij}^o$, $e_{kl}^r$, and $e_{mn}^l$ denote the corresponding residual functions; and ${\mathsf{\Omega }}_{ij}^o$, ${\mathsf{\Omega }}_{kl}^r$, and ${\mathsf{\Omega }}_{mn}^l$ denote the information matrices that characterize the uncertainty of each constraint. In this formulation, the relative influence of different types of constraints is determined by their information matrices rather than by a set of global scalar weighting coefficients.

The information matrices are constructed in a constraint specific and quality aware manner. For odometry edges, ${\mathsf{\Omega }}_{ij}^o$ is defined as the inverse of the DR noise covariance, namely, ${\mathsf{\Omega }}_{ij}^o=\mathrm{diag}\left({\sigma }_x^{-2},{\sigma }_y^{-2},{\sigma }_z^{-2},{\sigma }_{roll}^{-2},{\sigma }_{pitch}^{-2},{\sigma }_{yaw}^{-2}\right)$, where the noise standard deviations are listed in the parameter table in Section 5.2. Under the depth hold and altitude hold mapping assumption, ${\sigma }_{\mathrm{r}\mathrm{o}\mathrm{l}\mathrm{l}}$ and ${\sigma }_{\mathrm{p}\mathrm{i}\mathrm{t}\mathrm{c}\mathrm{h}}$ are assigned small values to reflect the relatively high confidence in these two degrees of freedom, whereas ${\sigma }_x$, ${\sigma }_y$, and ${\sigma }_{\mathrm{y}\mathrm{a}\mathrm{w}}$ are assigned larger values in accordance with the dominant horizontal drift and heading drift characteristics of long range AUV navigation.

For K-adjacent survey line registration edges and loop closure edges, the information matrices ${\mathsf{\Omega }}_{kl}^r$ and ${\mathsf{\Omega }}_{mn}^l$ are constructed dynamically from the quality indicators of the corresponding registration result, specifically the RANSAC inlier ratio ${\rho }_{\mathrm{i}\mathrm{n}\mathrm{l}\mathrm{i}\mathrm{e}\mathrm{r}}$, the overlap region mean squared error ${\mathrm{M}\mathrm{S}\mathrm{E}}_{\mathrm{o}\mathrm{v}\mathrm{e}\mathrm{r}\mathrm{l}\mathrm{a}\mathrm{p}}$, and the number of valid overlapping points $N_{\mathrm{v}\mathrm{a}\mathrm{l}\mathrm{i}\mathrm{d}}$. A candidate registration is incorporated into the graph only when all three indicators simultaneously satisfy the acceptance thresholds listed in Section 5.2; otherwise, it is rejected and does not contribute an edge to the optimization. For accepted registrations, the diagonal entries of the information matrix are assigned in a piecewise manner that increases monotonically with registration quality, so that a registration with a high inlier ratio and low MSE exerts a stronger influence on the final solution than a marginally accepted one. Although the same quality aware construction principle is used for both types of registration constraints, they operate at different spatial scales: ${\mathsf{\Omega }}_{kl}^r$ governs mid-range geometric consistency between neighboring survey lines, whereas ${\mathsf{\Omega }}_{mn}^l$ governs long range global drift correction through revisited areas.

This construction replaces the conventional hierarchical weighting scheme, in which all odometry edges share one global weight, all registration edges share another, and all loop closure edges share a third, with an edge specific weighting scheme driven by the quality of each individual constraint. As a result, the optimization is robust in two complementary respects: globally consistent and high-quality constraints dominate the solution regardless of their type, whereas locally unreliable constraints, whether arising from degenerate odometry propagation, feature sparse registration, or weakly matched loop closures, are naturally down weighted rather than allowed to degrade the estimate.

A standard precision and recall (PR) evaluation of the loop closure component requires manually annotated ground truth loop pairs in the test data. However, such annotations are not available for the real lake trial dataset used in this study and, to the best of our knowledge, are also unavailable in publicly released real AUV bathymetric datasets of comparable scale. Therefore, instead of reporting PR values based on unverifiable annotations, the proposed framework makes the loop closure acceptance mechanism fully explicit. A candidate loop is incorporated only when its RANSAC inlier ratio, overlap region mean squared error, and transformation plausibility all satisfy the thresholds listed in Section 5.2, and each accepted loop is then incorporated into the optimization through a quality aware information matrix, as described above. Under this design, the behavior of the loop closure component on a given dataset is determined entirely by the documented acceptance thresholds and information matrix construction rules, which are reproducible without requiring loop pair annotations. Standardized PR evaluation on annotated datasets will be considered in future work.

5. Experiments

5.1. Evaluation Metrics

This paper adopts two types of metrics-terrain consistency error and trajectory estimation accuracy-to comprehensively evaluate the performance of the SLAM system, quantifying the algorithm’s engineering application value from the two dimensions of mapping quality and localization accuracy.

Due to the lack of reference trajectory references in underwater environments, this paper adopts the self-consistency evaluation criterion proposed by Roman and Singh [6] as the primary accuracy metric. This criterion assesses mapping quality by quantifying terrain consistency in overlapping survey-line regions. Its fundamental principle is that an accurate SLAM algorithm should ensure consistent observations of the same seafloor area across different time instances. The metric computes elevation differences at corresponding locations within overlapping regions and uses the root mean square error (RMSE) of these discrepancies as a measure of consistency. It effectively reflects the SLAM algorithm’s ability to eliminate accumulated errors and maintain global consistency, and has become a standard evaluation method in the field of underwater SLAM.

Trajectory estimation accuracy is evaluated with respect to a reference trajectory derived from the system’s initial pose chain, rather than an external absolute ground truth, which cannot be obtained in real underwater lake trials. The reference trajectory does not represent absolute truth, but instead serves as a common baseline under which different methods are compared within the same error propagation scenario generated by controlled noise injection, as described in Section 5.3.2. Under this evaluation framework, the trajectory error of each method reflects its ability to suppress the same injected drift toward the reference trajectory, rather than its absolute localization accuracy in the global frame. The root mean square error (RMSE) is used to measure the overall deviation between the optimized trajectory and the reference trajectory, while the standard deviation of the per pose error is used to characterize the dispersion of the error distribution. Together, these two metrics provide a comparative rather than absolute characterization of the ability of each method to resist accumulated drift during heterogeneous terrain missions.

$$RMSE=\sqrt{{\displaystyle \frac{1}{M}}{\sum }_{i=1}^M{‖{\mathit{p}}_i-{\widehat{\mathit{p}}}_i‖}^2}$$

(12)

$$\sigma =\sqrt{{\displaystyle \frac{1}{M}}{\sum }_{i=1}^M(e_i-\stackrel{-}{e}{)}^2}$$

(13)

5.2. Field Experiment

To validate the performance of the proposed method in real underwater environments, this study conducted a comprehensive lake trial experiment in July 2024 at Qipan Mountain Reservoir, Shenyang City, Liaoning Province. The experimental design employed densely arranged survey lines and a systematic re-visiting strategy, aiming to fully evaluate the mapping accuracy and robustness of the algorithm under multiple constraint conditions.

The experimental platform employed an autonomous underwater vehicle (AUV) independently developed by the Shenyang Institute of Automation, Chinese Academy of Sciences, integrated with a high-precision underwater mapping sensor system. The AUV platform used in the field experiment is shown in Figure 6. A Doppler velocity log (DVL) provided real-time velocity measurements, an inertial navigation system (INS) ensured stable attitude estimation, and a SeaBat T20-S multibeam bathymetric sonar (Teledyne RESON, Slangerup, Denmark) was used to acquire high-resolution terrain data. The AUV performed pre-programmed mapping missions in a test area with an average water depth of 10 m, with tightly coupled DVL/INS providing the fundamental navigation information for the system.

Figure 6. AUV used in the field experiment.

The test area covers a region of 738 m × 294 m and features representative terrain, including flat lakebed areas and locally undulating regions. The survey line layout follows a systematic design principle: the primary survey lines consist of seven east–west oriented parallel lines spaced 30 m apart, ensuring an overlap ratio of more than 30% between adjacent lines; the check lines include three north–south oriented perpendicular lines that intersect the primary lines at multiple points, providing sufficient conditions for validating loop closure detection. This configuration not only conforms to the standard lawn-mower survey pattern but also ensures effective activation of different constraint mechanisms. Based on the collected multibeam data, offline playback was conducted to comparatively analyze the performance differences among three methods: pure odometry, GICP-SLAM, and MCHS-SLAM, providing an objective basis for quantitative evaluation of the algorithm’s advantages.

During the experiment, the operating environment and parameter settings for all three methods were kept strictly consistent. All algorithms were executed in single-threaded mode on the same computing platform, using a unified data interface and time synchronization mechanism. This controlled-variable experimental design enables accurate quantification of performance differences among the various technical approaches, providing reliable experimental evidence to validate the superiority of the MCHS-SLAM framework in complex underwater environments. The parameters used in the proposed framework can be classified into three functional layers according to the decisions they govern: candidate activation, registration acceptance, and optimization modeling. Candidate activation determines whether a registration procedure is initiated for a given pair of submaps. Registration acceptance determines whether a computed local estimate is converted into a graph constraint. Optimization modeling determines how an accepted constraint is incorporated into factor graph optimization through its information matrix. Table 1 summarizes the key parameters in these three layers and lists their values for GICP-SLAM and the complete MCHS-SLAM framework. The main algorithm parameters are listed in the following table:

Table 1. Algorithm design parameters.

Layer	Parameter	GICP-SLAM	MCHS-SLAM
(a) Candidate activation	Submap size	100 pings	100 pings
	Voxel downsampling size	0.2 × 0.2 × 0.1 m	0.2 × 0.2 × 0.1 m
	Number of K-adjacent survey lines	3	3
	Overlap coverage threshold	0.9	0.9
	Feature-density threshold for strategy switching	—	0.005
(b) Registration acceptance	TSD window scales	—	{3, 7, 9}
	Keypoint TSD percentile threshold	—	70th percentile
	Descriptor window size	—	11 × 11
	NCC correlation threshold	—	0.85
	Ratio-test threshold	—	0.8
	Spatial distance threshold	—	30.0 m
	Minimum match count	—	30
	RANSAC max iterations	—	3000
	Initial inlier threshold (adaptive)	—	3.0 m
	Overlap-region MSE threshold	—	5.0 m2
	Minimum inlier ratio	—	0.25
	Minimum valid overlapping points	—	500
	Translation sanity bound	—	100.0 m
(c) Optimization modeling	GICP max correspondence distance	3.0 m	3.0 m
	GICP max iterations	300	300
	GICP convergence threshold	1 × 10−3	1 × 10−3
	Normal-estimation neighbors	20	20
	DR translation noise (σ_x, σ_y, σ_z)	(5.0, 5.0, 1.0) m	(5.0, 5.0, 1.0) m
	DR rotation noise (σ_roll, σ_pitch, σ_yaw)	(1 × 10−4, 1 × 10−4, 1 × 10−2) rad	(1 × 10−4, 1 × 10−4, 1 × 10−2) rad

5.3. Playback Experiment Results

The three configurations evaluated in this section, namely pure odometry, GICP-SLAM, and the complete MCHS-SLAM framework, are regarded as a hierarchical system level ablation of the proposed design. Pure odometry retains only the DR baseline, with all external constraints disabled. GICP-SLAM further incorporates direct point cloud registration constraints between K-adjacent survey lines together with a complete factor graph back end, but does not include the feature aided registration path, the terrain observability driven switching strategy, or the quality aware construction of information matrices, and all registration edges are incorporated with a uniform default weight. On this basis, MCHS-SLAM further introduces the feature aided path, the observability driven switching mechanism, and the quality aware information matrix construction. Accordingly, the comparison between pure odometry and GICP-SLAM reflects the contribution of the factor graph back end together with direct registration, whereas the comparison between GICP-SLAM and MCHS-SLAM reflects the additional contribution of the feature aided path, the switching mechanism, and the quality aware weighting strategy. Therefore, both the qualitative map reconstruction results in Section 5.3.1 and the quantitative error analysis in Section 5.3.3 can be interpreted within this hierarchical ablation framework.

5.3.1. Comparison of Terrain Map Reconstruction Quality

Figure 7, Figure 8, Figure 9, Figure 10, Figure 11 and Figure 12 present the seafloor topographic maps generated by the three methods, along with the corresponding frequency distribution plots of consistency errors. Visually, it is evident that the pure odometry method, lacking external constraints, suffers from significant accumulated error as mission duration increases, resulting in noticeable misalignments between adjacent survey lines and failure to correctly align terrain features. The GICP-based SLAM method improves line alignment through point cloud registration but still experiences registration failures in feature-sparse regions. In contrast, the proposed MCHS-SLAM method achieves the best terrain reconstruction performance by leveraging multi-layer constraint fusion and cooperative optimization-survey lines are smoothly aligned, and terrain features appear clear and continuous.

Figure 7. Seafloor terrain map reconstructed using the pure odometry baseline (DR only, with all external constraints disabled). Visible misalignments are observed between adjacent survey lines, reflecting the accumulation of DR drift over the course of the mission.

Figure 8. Histogram of consistency error frequency distribution built from odometry.

Figure 9. Seafloor terrain map reconstructed using the GICP-SLAM baseline (direct point cloud registration between K-adjacent survey lines and a factor graph back end, with the feature aided registration path, observability driven strategy switching, and quality aware information matrix construction all disabled).

Figure 10. Histogram of consistency error frequency distribution for the GICP-based SLAM method.

Figure 11. Seafloor terrain map reconstructed using the full MCHS-SLAM framework (direct registration between K-adjacent survey lines, feature-aided registration path, observability-driven strategy switching, and quality-aware information-matrix construction all active).

Figure 12. Histogram of consistency error frequency distribution for MCHS-SLAM.

Beyond the visual differences observed in the reconstructed terrain maps, the consistency error distributions over all overlap regions provide a more informative global characterization of mapping stability under heterogeneous terrain conditions. As shown in Figure 8, Figure 10 and Figure 12, the frequency distributions of consistency errors for pure odometry, GICP-SLAM, and MCHS-SLAM exhibit a clear progressive change in shape. The distribution of pure odometry is broad and long tailed, with a non-negligible proportion of large error samples appearing in the right tail, indicating the accumulation of severe local misalignments over the course of the mission. Compared with pure odometry, the distribution of GICP-SLAM shows a narrower main peak, but a visible large error tail still remains, suggesting that a single direct registration path with uniform weighting is still unstable in feature sparse or geometrically degenerate regions. By contrast, the distribution of MCHS-SLAM exhibits a further concentration of the main peak together with a markedly shortened large error tail, indicating that observability driven switching and quality aware weighting effectively suppress most of the severe local misalignment cases that remain in GICP-SLAM. This transition in distribution shape, from a broad long tailed distribution to a more concentrated one, provides a global statistical indication of improved stability under heterogeneous terrain, as it reflects the overall behavior across flat regions, sloped regions, and track intersection areas. A more detailed quantitative interpretation of these distributions is provided in Section 5.3.3.

5.3.2. Reference Trajectory Establishment and Its Limitations

Before presenting the quantitative error analysis, the construction of the reference trajectory used for trajectory error evaluation, the evaluation logic it supports, and its limitations are clarified. This clarification is necessary because continuous absolute reference trajectories cannot be obtained in real underwater lake trials. GPS is unavailable underwater, and acoustic tracking systems such as USBL cannot provide the synchronization and accuracy required for a long-range mission of the scale considered in this study.

The reference trajectory used in this work, denoted as ${\mathrm{poses}}_{\mathrm{original}}$, is derived from the system’s initial pose chain, specifically the pose sequence generated by DR together with the initial pose graph structure before any noise injection or optimization is applied. It is used as a common baseline for comparing different methods under the same error propagation scenario, rather than as an external absolute ground truth.

The trajectory error analysis in Section 5.3.3 is based on three trajectories derived from this common baseline. The reference trajectory ${\mathrm{poses}}_{\mathrm{original}}$ serves as the baseline. The noise corrupted trajectory ${\mathrm{poses}}_{\mathrm{corrupted}}$ is obtained by injecting controlled Gaussian noise into the relative pose measurements along the reference trajectory, as described in Section 5.3.3, and serves as the input to the back end of each method. The optimized trajectory ${\mathrm{poses}}_{\mathrm{optimized}}$ is the output of the corresponding back end. Accordingly, the reported trajectory errors quantify the deviation of ${\mathrm{poses}}_{\mathrm{optimized}}$ from ${\mathrm{poses}}_{\mathrm{original}}$, that is, the ability of each method to correct the same injected drift toward the common baseline, rather than the deviation of ${\mathrm{poses}}_{\mathrm{optimized}}$ from an absolute ground truth trajectory.

It should be noted that the reference trajectory itself still inherits residual uncertainty from the raw DR solution and the initial pose graph approximation, and therefore does not represent absolute ground truth. Accordingly, the trajectory error metrics reported in this paper should be interpreted as relative improvement with respect to the reference trajectory under controlled noise propagation, rather than as an upper bound on absolute localization accuracy in the global frame. Consistent with this interpretation, the revised manuscript avoids the expressions “ground truth trajectory” and “high precision reference trajectory”, and instead uses the term “reference trajectory” throughout, or “pseudo ground truth trajectory” where additional emphasis is required.

5.3.3. Quantitative Error Analysis

To quantitatively evaluate the comprehensive performance of different methods in seafloor mapping tasks, and considering the limitations imposed by underwater environments-where high-precision GPS reference trajectories are unavailable during AUV operations-this paper adopts a noise-free DR trajectory as the reference. A noisy odometry trajectory is then generated by adding Gaussian noise to this reference, enabling controlled assessment of the backend optimization algorithm’s trajectory estimation accuracy.

Considering that AUVs in depth-hold or altitude-hold mapping missions can be approximated as undergoing 2D planar motion, with pose errors primarily manifesting in the heading direction, this paper introduces zero-mean Gaussian noise only to the heading component of the DR relative poses, while neglecting the effects of roll and pitch angles. The pure odometry trajectory constructed in this manner simulates trajectory drift caused by accumulated heading errors during actual operations and serves as input for subsequent trajectory optimization and accuracy comparison.

The procedure used to generate the corrupted trajectory is described as follows. Let the DR relative pose between two consecutive pose nodes $i$ and $i+1$ be

$$T_{i,i+1}=\left[\begin{array}{cc}{R}_{i,i+1}& t_{i,i+1}\\ \mathbf{0}& 1\end{array}\right]\in SE(3)$$

(14)

Since the AUV operates in depth hold and altitude hold survey mode throughout the mission, its motion can be reasonably approximated as planar motion in two dimensions. Under this assumption, pose errors are dominated by accumulated heading drift. Accordingly, the corrupted relative pose measurement is generated by injecting zero mean Gaussian perturbations into the translation component and the yaw component of each relative pose:

$${\tilde{T}}_{i,i+1}=\left[\begin{array}{cc}\Delta R_i{R}_{i,i+1}& t_{i,i+1}+\Delta t_i\\ \mathbf{0}& 1\end{array}\right]$$

(15)

where $\Delta t_i\sim \mathcal{N}(\mathbf{0},{\sigma }_t^2{I}_3),\Delta R_i=\mathrm{e}\mathrm{x}\mathrm{p}(\Delta {\theta }_i^{\wedge }),\Delta {\theta }_i=(0, 0, {\delta }_{\mathrm{y}\mathrm{a}\mathrm{w}}{)}^{\mathrm{\top }},{\delta }_{\mathrm{y}\mathrm{a}\mathrm{w}}\sim \mathcal{N}(0,{\sigma }_r^2).$

Under the two-dimensional planar motion assumption, the perturbation components in roll and pitch are set to zero. At the relative pose level, the perturbation standard deviations are set to ${\sigma }_t=0.01$ m and ${\sigma }_r=0.01$ rad. A fixed random seed is used to ensure full reproducibility of the corrupted trajectory across different methods. Although the perturbation magnitude at each step is small, its accumulation along the 700 m scale trajectory results in substantial absolute drift at the trajectory endpoint, which is consistent with the pure odometry mean error of 17.53 m and RMSE of 20.91 m reported later in this section.

It should be noted that the noise injection parameters described above, which are used to generate the corrupted trajectory as the common input to all methods, are different from the DR edge information matrix parameters introduced in Section 4, which determine how DR edges are incorporated into factor graph optimization. The latter are set to $\left({\sigma }_x,{\sigma }_y,{\sigma }_z)=(5.0, 5.0, 1.0\right)$ m for translation and $\left({\sigma }_{\mathrm{r}\mathrm{o}\mathrm{l}\mathrm{l}},{\sigma }_{\mathrm{p}\mathrm{i}\mathrm{t}\mathrm{c}\mathrm{h}},{\sigma }_{\mathrm{y}\mathrm{a}\mathrm{w}})=({10}^{-4}, {10}^{-4}, {10}^{-2}\right)$ rad for rotation (Table 1, layer (c)). These settings reflect the dominance of horizontal and heading uncertainty in long range AUV navigation, as well as the relatively high confidence in roll and pitch under the planar motion assumption.

The scope and limitation of this noise model should also be clarified. The model is a controlled simplification for comparative evaluation, rather than a complete statistical reproduction of real DVL/INS navigation errors. Its purpose is to construct a uniform and reproducible error propagation scenario, dominated by heading drift, under which different methods can be compared on identical input. It does not account for random walk processes, slowly varying sensor biases, sensor correlated colored noise, or environment induced disturbances. Therefore, the trajectory correction results reported below should be interpreted as a relative comparison among methods under the same controlled error propagation scenario, rather than as a complete reproduction of real navigation error processes or as claims of absolute localization accuracy.

This study conducts a comparative analysis of the pure odometry, GICP-SLAM, and MCHS-SLAM methods from two aspects: mapping consistency and trajectory estimation accuracy.

The consistency error frequency distributions of the three configurations, namely pure odometry (Figure 8), GICP-SLAM (Figure 10), and MCHS-SLAM (Figure 12), which were presented in Section 5.3.1, are further analyzed here from a quantitative perspective. For heterogeneous terrain mapping, the shape of the distribution provides more informative evidence of system performance than the mean value alone. The distribution of pure odometry is broad and exhibits a pronounced long tail, with a nonnegligible proportion of large error samples located in the right tail. This result indicates that, in the absence of external constraints, accumulated DR error leads to severe local misalignments in multiple overlap regions throughout the survey. Compared with pure odometry, the distribution of GICP-SLAM shows a narrower main peak, indicating that the factor graph back end together with direct K-adjacent survey line registration eliminates part of the large error samples. However, a visible large error tail still remains, suggesting that the single direct registration path with uniform weighting remains unstable in some overlap regions, particularly in feature sparse flat areas or locally degenerate geometric regions. By contrast, the distribution of MCHS-SLAM exhibits a further concentration of the main peak and, more importantly, a substantially shortened large error tail. This indicates that most of the severe local misalignment cases that remain in GICP-SLAM are effectively suppressed after the introduction of observability driven switching and quality aware weighting. In terms of summary statistics, the RMSE of the consistency error is 1.55 m for pure odometry, 1.42 m for GICP-SLAM, and 1.39 m for MCHS-SLAM. Although the RMSE reduction from GICP-SLAM to MCHS-SLAM is modest in absolute terms, it coincides with the most informative change in distribution shape, namely the shortening of the large error tail, and therefore should not be interpreted only through the mean level of error reduction. Taken together, these pairwise comparisons are consistent with the system level ablation framework introduced at the beginning of Section 5.3. The transition from pure odometry to GICP-SLAM reflects the contribution of the factor graph back end with direct registration, whereas the transition from GICP-SLAM to MCHS-SLAM reflects the contribution of feature aided registration, observability driven switching, and quality aware information matrix construction. The latter contribution is manifested mainly in the reduction in the large error tail, rather than in a substantial further decrease in the mean error.

Trajectory estimation accuracy under controlled noise propagation. The optimized trajectory of each method was compared with the reference trajectory ${\mathrm{poses}}_{\mathrm{original}}$ defined in Section 5.3.2 (Figure 13). The resulting per-pose 2D position errors are presented in Figure 14, and their distributions are further illustrated by the histograms in Figure 15. The resulting per-pose 2D position errors are presented in Figure 14, and their distributions are further illustrated by the histograms in Figure 15. For pure odometry, the mean error, RMSE, and standard deviation of the per pose error were 17.53 m, 20.91 m, and 11.41 m, respectively. These results confirm that, in the absence of external constraints, the injected relative pose perturbations, although small at each step, accumulated into substantial absolute drift over the 700 m scale mission. For GICP-SLAM, the mean error and RMSE decreased to 11.13 m and 14.68 m, respectively, corresponding to reductions of 36.5% and 29.8% relative to pure odometry, whereas the standard deviation of the per pose error remained relatively high at 9.56 m. This suggests that, although the factor graph back end corrected a considerable portion of the accumulated drift, the single direct registration path with uniform weighting still allowed local misalignments to increase the per pose trajectory error in some regions. For MCHS-SLAM, the mean error and RMSE were further reduced to 6.65 m and 7.58 m, respectively, corresponding to reductions of 63.8% and 63.7% relative to pure odometry, and 40.3% and 48.4% relative to GICP-SLAM. Meanwhile, the standard deviation of the per pose error decreased to 3.65 m, showing a larger relative reduction than that of the mean error. This result is consistent with the reduction in the large error tail observed in the consistency error distribution in Section 5.3.3. It shows that observability driven switching combined with quality aware weighting not only reduced the average drift, but also effectively suppressed local error excursions that would otherwise dominate the dispersion of the distribution. Consistent with the evaluation logic described in Section 5.1 and Section 5.3.2, these results should be interpreted as the comparative ability of each method to suppress the same injected drift toward the reference trajectory, rather than as absolute localization accuracy in the global frame. To further compare the dispersion and outliers of trajectory consistency errors among different methods, the corresponding box plots are shown in Figure 16.

Figure 13. Trajectory comparison plot.

Figure 14. Position error versus pose index.

Figure 15. Histogram of trajectory errors. (a) Pure odometry; (b) GICP-SLAM; (c) MCHS-SLAM.

Figure 16. Box plot of trajectory consistency errors.

The current implementation is intended for offline validation and methodological comparison, and has not been specifically optimized for embedded deployment or strict real time operation. All three configurations were executed in single thread mode on the same platform described in Section 5.2, ensuring that the accuracy comparisons reported above are attributable to algorithmic design rather than to implementation or platform differences. In its present form, the framework is therefore more appropriately evaluated as an offline or near online bathymetric SLAM system. The feasibility of onboard real time deployment, particularly in long duration missions with continuously expanding pose graphs, would require further investigation of keyframe selection, submap pruning, and incremental factor graph optimization strategies, which are discussed as directions for future work in Section 6.

6. Conclusions and Future Work

This paper proposes a multi-constraint hybrid strategy for underwater SLAM, named MCHS-SLAM, and investigates key challenges in lawn-mower pattern seafloor mapping missions, including error accumulation, feature scarcity, and terrain heterogeneity. The main conclusions are as follows:

(1)

A multi-constraint collaborative MCHS SLAM framework is proposed. By constructing a three level hierarchical constraint system composed of odometry, K-adjacent survey line registration, and loop closure detection, and by incorporating these constraints into the factor graph through quality aware information matrices rather than uniform weights assigned at the constraint type level, the framework effectively suppresses error accumulation across different spatial scales, yielding more concentrated consistency error distributions and improved error suppression relative to the reference trajectory in lawnmower pattern surveys over heterogeneous terrain.

(2)

A terrain standard deviation (TSD)-based adaptive feature extraction method is designed. By employing multi-scale TSD mapping, a data-driven thresholding strategy, and rotation-invariant descriptors, the method enhances the stability and robustness of feature detection in unstructured seafloor environments.

(3)

A terrain-adaptive hybrid registration mechanism is established. The system adaptively switches between direct and feature-based registration methods according to local terrain complexity, achieving a dynamic balance between registration robustness and accuracy, thereby addressing performance inconsistencies across varying terrain conditions.

(4)

A multi-constraint factor graph optimization framework is proposed. To address the challenge of unifying heterogeneous constraints into a single model, a hierarchical weighting scheme and robust kernel functions are integrated to construct a unified optimization objective, enhancing the system’s global optimization capability.

We emphasize that the value of this framework should be interpreted at the system level rather than at the level of any single registration instance or any single scalar accuracy metric. The experimental results show only a modest reduction in mean consistency error relative to the GICP SLAM baseline, but the shape of the consistency error distribution changes substantially: the main peak becomes more concentrated and the large error tail becomes markedly shorter. This is indicative of a system whose behavior has shifted from being good on average with occasional severe local misalignments to being consistently stable across flat, complex, and transition regions. For bathymetric mapping under heterogeneous terrain, the latter property determines whether a reconstructed map is actually usable in downstream applications, because a map that is accurate on average but locally inconsistent in flat regions may still fail to satisfy engineering quality requirements. The improvement reported in this paper should therefore be understood as an improvement in stability under terrain heterogeneity, which is an emergent property of observability driven switching combined with quality aware constraint modeling, rather than as an improvement in peak accuracy for any particular submap pair. This framing also clarifies the scope of the contribution: the framework is not proposed as a universally superior registration algorithm, but as a design philosophy whose benefit is most evident in missions that traverse heterogeneous seafloor topography over long distances.

Although MCHS-SLAM has achieved promising results in improving the accuracy and consistency of AUV terrain mapping, several aspects warrant further investigation. Future work will focus on the following directions:

(1)

Real-time terrain complexity assessment, Introducing lightweight probabilistic neural networks to enable online adaptive switching of registration strategies based on dynamic environmental conditions.

(2)

Multi-vehicle collaborative SLAM, Extending MCHS-SLAM to multi-AUV cooperative mapping scenarios to enhance robustness and operational efficiency in dynamic underwater environments.

(3)

Efficient optimization strategies for real-time onboard deployment. The present implementation is designed for offline validation and has not been optimized for embedded platforms or strict real-time operation. Future work will explore keyframe selection, submap pruning, sparsification of the pose graph, and incremental factor-graph optimization methods, with the goal of reducing computational overhead to a level compatible with onboard real-time deployment on long-duration, large-scale mapping missions.