Optimizing Multi-Camera Mobile Mapping Systems with Pose Graph and Feature-Based Approaches

Abstract

Multi-camera Visual Simultaneous Localization and Mapping (V-SLAM) increases spatial coverage through multi-view image streams, improving localization accuracy and reducing data acquisition time. Despite its speed and generally robustness, V-SLAM often struggles to achieve precise camera poses necessary for accurate 3D reconstruction, especially in complex environments. This study introduces two novel multi-camera optimization methods to enhance pose accuracy, reduce drift, and ensure loop closures. These methods refine multi-camera V-SLAM outputs within existing frameworks and are evaluated in two configurations: (1) multiple independent stereo V-SLAM instances operating on separate camera pairs; and (2) multi-view odometry processing all camera streams simultaneously. The proposed optimizations include (1) a multi-view feature-based optimization that integrates V-SLAM poses with rigid inter-camera constraints and bundle adjustment; and (2) a multi-camera pose graph optimization that fuses multiple trajectories using relative pose constraints and robust noise models. Validation is conducted through two complex 3D surveys using the ATOM-ANT3D multi-camera fisheye mobile mapping system. Results demonstrate survey-grade accuracy comparable to traditional photogrammetry, with reduced computational time, advancing toward near real-time 3D mapping of challenging environments.

1. Introduction

Three-dimensional spatial data, such as dense point clouds and textured models, offers unique insights that go beyond traditional 2D formats, unlocking new opportunities for analysis and exploration across various fields. Close-range photogrammetry and terrestrial laser scanning (TLS) are widely adopted survey techniques that provide high geometric accuracy, but are relatively slow in data acquisition and require significant post-processing, especially in large-scale and complex environments [1,2,3,4,5,6]. To address these limitations, Visual- and LiDAR (Light Detection and Ranging)-based Mobile Mapping Systems (MMS), which integrate Simultaneous Localization and Mapping (SLAM) [7], have gained popularity for their acquisition speed and flexibility, enabling rapid 3D data acquisition and real-time positioning [7,8,9,10,11]. However, these systems may generate output data with lower density and accuracy compared to traditional methods [12]. They can vary significantly in terms of cost, weight, size, sensor quality (e.g., LiDAR sensor accuracy or camera resolution), and algorithm efficiency, depending on the target application. Furthermore, many MMS implementations rely on heterogeneous sensor fusion (e.g., LiDAR, camera, inertial measurement unit, etc.) or require external reference data (e.g., ground control points) and often involve extensive post-processing to ensure the accuracy and completeness of the output, especially in complex environments [13,14].

SLAM originated in robotics to enable autonomous localization and mapping in unknown environments [15,16]. Feature-based V-SLAM utilizes sequential image data to estimate camera poses and reconstruct the sensor trajectory by detecting, matching, and tracking local features [17]. Through feature triangulation, it incrementally builds a sparse 3D representation of the environment, which is used to estimate and refine the motion trajectory [18,19,20,21].

The integration of V-SLAM with Visual MMS [8,9,22,23] offers promising advances in real-time localization and mapping, improving speed, flexibility [8,22], efficiency, and accuracy [24]. V-SLAM-aided photogrammetry, a hybrid approach that integrates V-SLAM-generated camera poses and keyframe selection into the photogrammetric pipeline to establish spatial and temporal reference, has also shown great potential for enhancing the speed of 3D spatial data processing and possibly the accuracy of outputs [23]. Despite the growing popularity of SLAM-based vision mobile mapping systems, significant challenges remain, particularly related to the capabilities of V-SLAM algorithms to achieve metric accuracy and operational reliability required for geospatial applications.

1.1. Related Works

Various V-SLAM algorithms have been developed with different configurations based on the number of cameras used. Monocular V-SLAM offers a simple and lightweight setup, relying solely on a single camera to infer motion and the scene structure. Several key monocular V-SLAM algorithms have been developed over the years, each addressing different aspects of accuracy, robustness, and computational efficiency. MonoSLAM [25] pioneered the field using a constant velocity model combined with an Extended Kalman Filter for feature initialization; however, it remained susceptible to scale ambiguity and tracking loss. PTAM [18] introduced the concept of parallel tracking and mapping, incorporating local bundle adjustment (BA) to improve accuracy, although it required manual initialization. DTAM [19] followed a direct-based approach to achieve dense 3D surface reconstruction, but incurred high computational costs and lacked loop closure capabilities. SVO-SLAM [26] combined feature-based tracking with direct methods and offered improved speed and accuracy, but remained vulnerable to cumulative drift over time. ORB-SLAM [20] introduced a breakthrough by presenting a feature-based framework utilizing ORB descriptors for tracking, mapping, re-localization, and loop closure, making it one of the most widely adopted and influential V-SLAM frameworks. Building upon direct methods, DSO-SLAM [27] introduced a direct sparse approach that optimized camera motion and scene geometry through photometric constraints, improving accuracy but lacking loop closure detection. CubeSLAM [28] integrated semantic object detection, leveraging cuboid-shaped objects for scale recovery, but similar to other direct methods, it did not include a loop closure mechanism. SLAM3R [29], a monocular RGB system that regresses dense 3D data using a feed-forward network, performs real-time global alignment; however, the accuracy of its camera pose estimates may be limited compared to other SLAM solutions, due to the lack of global bundle adjustment with camera parameters refinement.

Stereo V-SLAM overcomes the scale ambiguity inherent to monocular approaches and improves the depth estimation accuracy. A notable advancement in this field was ORB-SLAM2.0 [30], an extension of ORB-SLAM that incorporated support for stereo and RGB-D cameras, enabling improved localization, mapping, and loop closure performance. It introduced an additional optimization thread dedicated to full bundle adjustment (BA), improving global consistency and SLAM performance in both indoor and outdoor environments. To address the challenge of feature scarcity in low-texture scenes, PL-SLAM [31] extended stereo V-SLAM by incorporating both point and line features into visual odometry and BA, while leveraging the Bag-of-Words model for loop closure detection. To further improve the stereo V-SLAM speed, HOOFR-SLAM [32] focused on maximizing parallelization by implementing hardware-software mapping on a CPU-GPU system to achieve faster processing while preserving accuracy. TIMA-SLAM [33] introduced a multi-camera approach inspired by ORB-SLAM2.0, where the cameras operate independently without requiring pre-calibration while sharing a common map. The system estimated and refined extrinsic calibrations during mapping to improve accuracy. However, it remained susceptible to scale ambiguity when using monocular cameras, which was mitigated by integrating RGB-D (i.e., red, green, blue channels, and depth) sensors for on-device depth estimation. AQUA-SLAM [34] introduced a tightly-coupled underwater SLAM system that fuses Doppler Velocity Log (DVL), stereo camera, and inertial measurement unit (IMU) within a graph optimization framework supplemented by a multi-sensor calibration method. RGB-D SLAM frameworks incorporate depth measurements obtained from active light sensors, such as structured light or time-of-flight sensors, enabling direct and dense depth recovery alongside visual information [35,36,37].

Recent research has increasingly focused on enhancing the accuracy and robustness of V-SLAM systems by integrating advanced computational techniques, including artificial intelligence (AI) components. EC-SLAM [38] integrates Neural Radiance Fields (NeRF) into a real-time dense RGB-D SLAM framework, using implicit NeRF-based loop closures and globally constrained bundle adjustment to improve tracking and map consistency. To address challenges posed by dynamic environments, RoDyn-SLAM [39] proposed an RGB-D SLAM framework that employs a motion mask generation method that combines optical flow and semantic segmentation to filter out dynamic regions by rejecting sampled points along camera rays that intersect moving objects in the 3D scene. A more uncertainty-aware approach was presented by CG-SLAM [40], which utilizes a 3D Gaussian field-based representation to enhance both tracking efficiency and mapping robustness. Additionally, Chai et al. [41] introduced a pedestrian dead-reckoning-aided visual-inertial SLAM system, which incorporates vanishing point observations as external references to correct for system drift and improve localization stability. VINGS-Mono [42] introduced a monocular visual-inertial SLAM framework that integrates Gaussian Splatting with dense bundle adjustment, NVS (Novel View Synthesis)-based loop closure, and dynamic-object filtering to achieve real-time, kilometer-scale mapping.

Semantic V-SLAM extends traditional SLAM frameworks by integrating deep learning-based object detection and semantic segmentation to improve localization accuracy, particularly in dynamic environments. Liu et al. [43] proposed a real-time optimization method for V-SLAM combining YOLOv8 with geometric constraints to improve tracking in dynamic scenes. Yang et al. [44] introduced a lightweight YOLOv8-based dynamic V-SLAM algorithm that focuses on computational efficiency while maintaining reliable tracking performance in challenging environments. Li et al. [45] developed a semantic stereo V-SLAM system based on ORB-SLAM2.0, tailored for outdoor dynamic environments to improve mapping consistency and reduce tracking errors. MVS-SLAM [46] builds upon ORB-SLAM3.0 by integrating YOLOv7-based semantic object detection to strengthen feature selection and ego-motion estimation and to improve the SLAM robustness in highly dynamic environments.

Recent trend in V-SLAM research has focused on the use of multi-camera systems, which integrate multiple synchronized cameras and, in some cases, asynchronous and additional sensors, to expand the field-of-view (FoV) and spatial awareness, thereby increasing robustness against tracking failures and improving localization and mapping performance. Yeh et al. [47] proposed a fusion method that integrates V-SLAM with Inertial Navigation Systems and Global Navigation Satellite Systems (GNSS), achieving lane-level positioning accuracy of 1.5 m in dynamic vehicular environments. Similarly, BundledSLAM [48] extended ORB-SLAM2.0 by introducing a virtual camera model, the BundledFrame, which fuses measurements from multiple cameras into a unified structure to improve pose tracking and bundle adjustment. However, the system still faces challenges in motion-blurred and texture-deficient scenarios. Focusing on scalability, Kaveti et al. [24] introduced a generalized MultiCam Visual Odometry framework that treats multiple cameras as a single imaging device using a generalized camera model, enabling efficient cross-matching across overlapping FoVs while minimizing computational overhead. MAVIS [49], an optimization-based Visual-Inertial-SLAM framework, extends traditional monocular and stereo SLAM to multi-camera configurations by combining wide FoV imaging with IMU-based metric scale measurements. A key innovation of MAVIS is its exact IMU (Inertial Measurement Unit) pre-integration formulation, based on the Lie Group SE2(3) [49], representing the extended poses (position, orientation, and velocity) in the 3D space, to improve tracking performance during fast rotations. Additionally, MAVIS optimizes front-end tracking and back-end pose estimation to accommodate multi-camera systems, improving the mapping accuracy while maintaining computational efficiency. Other works explore asynchronous multi-camera setups and advanced semantic integration. For example, AMV-SLAM [50] introduces a generalized SLAM framework capable of handling asynchronous sensor observations by employing a continuous-time motion model with cubic B-splines across multi-frame data during tracking, local mapping, and loop closure. Lastly, Multicam-SLAM [51] eliminates the need for FoV overlap by accurately estimating pose relationships among multiple RGB-D cameras, leveraging a multi-camera model and multi-keyframe architecture for enhanced scalability.

Despite significant advancements, persistent limitations include susceptibility to tracking failures due to challenging conditions such as motion blur, extreme and low lighting, or texture-poor environments, eventually leading to the accumulation of errors. Although stereo and multi-camera setups mitigate some of these challenges by providing additional geometric constraints and observational redundancy, drift remains an issue. Loop closure techniques [52,53,54], while beneficial, can fail due to insufficient or inconsistent visual correspondences, viewpoint changes, environmental variations, or repetitive structures. False-positive loop closures can introduce significant errors, whereas false negatives allow drift to persist, highlighting the need for more efficient error-reduction and drift-mitigation strategies. Additionally, incorporating supplementary heterogeneous sensors such as inertial measurement units (IMUs), Global Navigation Satellite Systems (GNSS), and RGB-D cameras, although beneficial, increases hardware complexity, power consumption, and operational costs. Furthermore, the lack of fully open-source frameworks or comprehensive documentation hinders reproducibility and extensibility.

Collectively, these challenges emphasize ongoing research needs in improving the accuracy, robustness, scalability, and adaptability of V-SLAM systems across various real-world scenarios.

1.2. Research Objectives and Contributions

This study aims to adapt and extend V-SLAM approaches to enhance their applicability for geomatics and 3D reconstruction tasks by improving the accuracy of the V-SLAM estimated camera poses. To this end, two novel multi-camera V-SLAM pose optimization approaches are introduced: (1) a Multi-View Feature-Based Optimization (Section 2.2.1); and (2) a Multi-Camera Pose Graph Optimization (Section 2.2.2).

The proposed optimization approaches are integrated within two multi-camera V-SLAM processing paradigms: (1) Multiple Single-Instance Stereo V-SLAM (Section 2.1.1), which independently processes synchronized stereo pairs; and (2) Multi-View Odometry (Section 2.1.2), which jointly processes all five camera views to produce a unified trajectory.

Building on this integration, we present two enhanced multi-camera V-SLAM approaches: (1) Multi-Instance Stereo V-SLAM (Section 2.3.1), which merges multiple stereo trajectories into a globally consistent solution utilizing both proposed optimization techniques; and (2) Multi-View V-SLAM (Section 2.3.2), which enhances simultaneous multi-view processing through feature-based optimization, reducing drift and adding a loop closure capability.

The proposed approaches are evaluated on the portable handheld ATOM-ANT3D system [10,23], equipped with five synchronized and overlapping wide-FoV (190°) fisheye cameras, configured to support four stereo-pair arrangements (Figure 1).

The evaluation is conducted in two complex real-world environments, demonstrating the practical applicability of the proposed approaches in producing accurate and metrically reliable results. This study focuses exclusively on a vision-based SLAM framework, utilizing only multi-camera image data without integrating additional sensors such as IMUs, GNSS, or LiDAR. The core novelty of the proposed optimizations lies in their ability to jointly refine multi-camera V-SLAM poses by leveraging synchronized and overlapping views from the multi-camera rig, allowing the incorporation of both inter-camera visual feature correspondences and pre-calibrated rig camera configurations (i.e., known rigid relative poses between the cameras). This enables an optimization framework that is robust, metrically consistent, and particularly well suited for multi-camera systems where auxiliary sensors are unavailable or difficult to integrate.

2. Multi-Camera V-SLAM Approaches and Optimization Algorithms

To process datasets acquired with multi-camera imaging systems and estimate the multi-camera trajectory required for high-quality 3D reconstructions, two initial approaches are evaluated (Section 2.1): Multiple Single-Instance Stereo V-SLAM and Multi-View Odometry. From our tests, both approaches exhibited limitations and produced suboptimal trajectory estimates. Therefore, to address these shortcomings, this study introduces two novel optimization methods, as follows:

(1) a Multi-View Feature-Based Optimization (Section 2.2.1);

(2) a Multi-Camera Pose Graph Optimization (Section 2.2.2).

These optimization methods are integrated into the initial approaches, resulting in two enhanced multi-camera V-SLAM approaches: Multi-Instance Stereo V-SLAM (Section 2.3.1) and Multi-View V-SLAM (Section 2.3.2). The proposed integration significantly reduces trajectory errors and enhances the overall accuracy. Figure 2 outlines the proposed optimization methodologies and their integration into the investigated multi-camera V-SLAM approaches.

2.1. Multi-Camera V-SLAM Approaches

2.1.1. Multiple Single-Instance Stereo V-SLAM

The multiple single-instance stereo V-SLAM approach is applied to the ATOM-ANT3D system [23], using ORBSLAM3.0 [55] as the underlying SLAM foundation. This approach leverages the known relative poses between overlapping camera pairs (Figure 1) to independently generate scaled 3D reconstructions for each stereo configuration. Consequently, multiple trajectories are estimated for the multi-camera system, each with varying levels of accuracy. To optimize and merge independently computed stereo trajectories into a single globally optimized solution, this approach is integrated with the two proposed optimization strategies (Section 2.2).

2.1.2. Multi-View Odometry

An alternative to running multiple independent stereo V-SLAM instances is to jointly process images from all cameras using a Multi-View Odometry approach. This approach increases tie point redundancy and enriches the observation set. To implement this, COLMAP-SLAM [56] was selected due to its support for generic multi-camera system configurations. The internal logic of the approach operates as follows. The keyframe selection is performed based on a designated primary camera; a new keyframe is created when the median optical flow of tie points between the current and previous primary keyframe exceeds a predefined threshold (e.g., 5 pixels). The corresponding secondary frames, captured at the same epoch, are also selected as keyframes. Each primary or secondary keyframe is subsequently matched spatially at the same epoch with adjacent cameras and temporally with previous frames from the same camera. COLMAP-SLAM supports a variety of deep learning-based feature extraction and matching algorithms. For this study, ALIKE [57] was selected due to its robust performance under challenging illumination conditions and faster matching capabilities. Camera orientations are incrementally estimated using the COLMAP API, with iterative local and global bundle adjustment (BA) performed throughout the process. However, COLMAP-SLAM does not natively support loop closure detection, a critical limitation for improving the accuracy of tracking and trajectory estimation. To address this, the novel Multi-View Feature-Based Optimization (Section 2.2.1) is integrated, serving as an additional loop closure mechanism, reducing accumulated drift and enhancing global trajectory consistency.

2.2. Optimization Methods

Two novel multi-camera V-SLAM optimization methods, applicable either independently or sequentially, are introduced in the following sections.

2.2.1. Multi-View Feature-Based Optimization

Our method is implemented through the Agisoft Metashape Software (v2.1) and its Python (v3.10) API [58]. We explored other open-source libraries; however, they currently do not offer full support for multi-camera fisheye systems with fixed rig constraints, which are essential to our application. The goal of the Multi-View Feature-Based Optimization is to reduce accumulated drift in the V-SLAM trajectory and refine the camera poses by incorporating additional spatial constraints within a bundle adjustment (BA) (Figure 3).

The initial trajectory, which is typically estimated by the SLAM system for a designated master camera, serves as a reference. Based on this reference, the poses of all five cameras in the ATOM-ANT3D system are determined, under the assumption of prior system calibration and known inter-camera transformations (i.e., fixed baselines and orientations).

In the Multiple Single-Instance Stereo V-SLAM approach, any of the four stereo trajectories can be used to initialize the full multi-camera pose estimates. In contrast, the Multi-View Odometry strategy directly outputs a unified trajectory. Each approach (Section 2.1) results in a trajectory with distinct keyframe selections and pose accuracies, influencing the subsequent optimization.

To implement the optimization, the multi-camera fisheye rig is first defined, with fixed baselines and relative orientations between overlapping cameras. Since the cameras are pre-calibrated under controlled laboratory conditions, we set a low uncertainty (i.e., standard deviation) of the rig’s cameras’ relative poses. These values serve as constraints during the bundle adjustment, allowing the entire camera rig to be adjusted as a rigid body. While the internal implementation is not publicly disclosed, the general optimization concepts follow standard constrained bundle adjustment principles, where additional priors or constraints are introduced to preserve known relative transformations among cameras [59,60].

Feature extraction and matching are performed internally using SIFT-like descriptors. The imported V-SLAM trajectory plays a key role in guiding spatial proximity during keyframe pair selection for feature matching. While Metashape (v2.1) does not expose a user-defined search radius, it internally leverages the spatial relationships encoded in the initial trajectory to prioritize image pairs for matching. This process is further supported by the fixed multi-camera rig configuration, which provides known relative orientations between the camera poses. As a result, image matching over the V-SLAM trajectory is performed both within each multi-camera rig instance and between keyframes captured at different times but located in close spatial proximity. This implicitly enables loop-closure-like behavior without relying on explicit global loop detection, similar to strategies commonly adopted in LiDAR-SLAM frameworks [61]. In our case, the use of a fisheye multi-camera system further improves observations correspondence, as the wide field-of-view increases scene coverage and overlap across cameras, enhancing the chances of successful feature matching even under moderate drift conditions. This improves robustness while keeping computational cost manageable. Following this, multi-view triangulation is performed to reconstruct a sparse 3D point cloud of tie points observed from multiple overlapping viewpoints.

The triangulated tie points, along with the defined multi-camera rig constraints, are then used to iteratively refine the camera poses via bundle adjustment. After triangulation and prior to bundle adjustment (BA), tie points exhibiting a reprojection error RMSE greater than 1 pixel are filtered out. During the iterative bundle adjustment process, outliers are progressively reduced by applying thresholding criteria. Specifically, at each iteration, the reconstruction uncertainty threshold is decreased by increments of 10–20, while the projection accuracy threshold is reduced by increments of 5–10, directly influencing outlier points filtering. Following each threshold reduction, optimization is performed. The iterative process continues until convergence criteria are met, defined by achieving a reprojection error RMSE of tie points below 1 pixel and observing no significant further changes in the optimized camera poses. This process ensures geometric consistency and improves the accuracy of the final multi-camera trajectory and resulting 3D reconstruction.

2.2.2. Multi-Camera Pose Graph Optimization (PGO)

In the Multiple Single-Instance Stereo V-SLAM approach (Section 2.1.1), multi-camera data acquisition produces redundant camera pose estimates (i.e., up to four trajectory estimations). To integrate these estimates into a unified, globally consistent solution, we propose a novel pose graph optimization (PGO) algorithm, implemented using the Georgia Tech Smoothing and Mapping Library (GTSAM v4.2.0) [62].

A pose graph is a common representation in SLAM, modeling camera poses (i.e., positions and orientations) as nodes, and their relative constraints (i.e., measurements and observations) as edges. A corresponding nonlinear factor graph mathematically represents these relationships. In our framework, the nodes of the graph correspond to the full keyframe poses (i.e., position and orientation) estimated independently from up to four stereo trajectories, and are treated as the unknowns of the pose graph. Each node is assigned an initial estimate, with the first node (i.e., the first keyframe in the trajectory) fixed, anchoring the model against gauge freedom. Edges are defined by relative pose constraints between consecutive keyframes within each trajectory. Due to the unavailability of uncertainty estimates for individual pose measurements (e.g., standard deviations of 3D poses), an isotropic noise model is adopted. To mitigate the influence of noisy measurements and outliers, while leveraging the redundancy of multi-trajectory constraints, a robust noise model based on the Cauchy loss function is employed, enhancing optimization stability and preventing divergence. The scale parameter of the loss function was empirically tuned to the value of 0.3, balancing the strong reduction of outlying observations’ weights and the stability of the optimization process.

Once the factor graph is constructed, a Gauss-Newton optimizer iteratively minimizes the squared error of the non-linear system to refine the graph and optimize the pose estimates. The optimization problem can be expressed as the minimization of the sum of relative pose errors across all trajectory edges, as shown in Equation (1) [63]. The final output is a single global trajectory. A simplified illustration of the factor graph structure is provided in Figure 4.

$$\underset{\left\{{\mathrm{T}}_i\right\}}{\min }\sum _{(i,j)ϵ\epsilon }\rho \left({{‖\mathit{log}(Z_{ij}^{-1}.(T_i^{-1}.T_j))‖}^2}_{{\sum }_{ij}^{-1} }\right)$$

(1)

where

○

$\rho$(⋅): Robust loss function

○

T_i ϵ SE(3): Estimated absolute pose at node i

○

Zij ϵ SE(3): Measured relative pose from node i to node j

○

∑ij: Covariance of measurement Zij

○

Log(∙): Log map from SE(3) to the Lie algebra

○

‖e‖²_∑⁻¹ = e^T∑⁻¹e: Mahalanobis form

Figure 4. The proposed multi-camera pose graph optimization scheme fuses ATOM-ANT3D individual estimated stereo pairs’ trajectories into a single final optimized trajectory.

Figure 4. The proposed multi-camera pose graph optimization scheme fuses ATOM-ANT3D individual estimated stereo pairs’ trajectories into a single final optimized trajectory.

Beyond merging stereo trajectories, the proposed PGO framework is highly extensible. Additional constraints, such as known inter-camera baselines, shared tie points, and loop closure detections, can be seamlessly integrated. Moreover, the framework allows for the incorporation of other sensor modalities, such as IMUs or LiDAR odometry [64], making it a scalable solution for robust multi-camera V-SLAM.

A key consideration for the robustness of the optimization process is the availability of sufficient common keyframes across individual trajectories. Since each stereo independently selects keyframes based on tracking quality and keyframe count thresholds, this might create sparsity in common keyframes, potentially reducing the strength of inter-trajectory constraints. V-SLAM uses all frames for camera pose estimation; however, to maintain computational efficiency and enable real-time performance, only keyframes are retained during the optimization, and non-keyframes are excluded. Nonetheless, non-keyframes are abundant and can offer valuable improvement to the graph constraints. To leverage this, we enhance the pose graph by incorporating both keyframe-based and frame-to-frame (i.e., non-keyframe) constraints. Specifically, between-factors are introduced to connect consecutive frames within each trajectory, with keyframes acting as high-certainty anchors. This enriched constrained structure increases the density of shared keyframes, improving inter-trajectory consistency and enhancing the performance and stability of the proposed optimization framework.

2.3. Integration Methodology

The proposed optimizations (Section 2.2) are integrated into both V-SLAM approaches (Section 2.1), resulting in two new multi-camera V-SLAM solutions: (1) Multi-Instance Stereo V-SLAM and (2) Multi-View V-SLAM.

2.3.1. Multi-Instance Stereo V-SLAM

In the ATOM-ANT3D system, each stereo pair independently generates a V-SLAM trajectory, which may exhibit varying levels of drift despite the application of loop closure mechanisms. The integration of Multi-View Feature-Based Optimization (Section 2.2.1) refines these individual trajectories, improving their accuracy and spatial consistency by expanding the estimated stereo V-SLAM poses into five cameras’ 360° FoV, leveraging the fixed rigid baselines and orientations in a constrained iterative BA. This optimization enhances the internal accuracy of each trajectory. To merge them into a globally consistent solution, the Multi-Camera Pose Graph Optimization (Section 2.2.2) is applied, enforcing both intra- and inter-trajectory constraints to produce a unified, optimized multi-camera trajectory.

While the order of applying both optimizations can be reversed in principle, we intentionally apply the feature-based optimization first to support the goals of our evaluation. Our study investigates two different multi-camera V-SLAM configurations, and applying the feature-based optimization directly to the output of both configurations maintains a consistent and fair evaluation framework. This allows us to assess how each SLAM configuration behaves when refined independently by the same optimization method. In the case of multi-instance stereo V-SLAM, each stereo pair generates a trajectory with its unique drift profile, viewpoint-specific coverage, and keyframe distribution. Applying feature-based optimization at this stage not only improves local spatial consistency but also enables us to analyze the refined trajectories individually. This is valuable for understanding the performance and error characteristics of each SLAM instance before merging. If the pose graph optimization were to be applied first, this level of insight would be lost, as the feature-based refinement would then operate on a single, merged trajectory. Finally, applying the pose graph optimization after the feature-based optimization allows us to assess whether the optimization produces a balanced compromise between the independently optimized trajectories or further enhances the globally consistent solution. This ordering ultimately provides both deeper diagnostic insight and a more transparent understanding of the contributions of each optimization stage.

2.3.2. Multi-View V-SLAM

The Multi-View Odometry approach (Section 2.1.2) generates a single trajectory using all five cameras simultaneously; however, it lacks a dedicated loop detection thread. To overcome this limitation, the Multi-View Feature-Based Optimization (Section 2.2.1) is integrated to enhance the image network connectivity, ensure global coherence during BA, reduce drift, and enable loop closures when applicable, thereby transitioning from odometry estimation into a V-SLAM approach. Multi-View V-SLAM is primarily designed as a development and testing platform, making it better suited for post-processing V-SLAM sequences rather than real-time field deployment.

For the development and assessment of this study, the multi-camera pose graph optimization was implemented using the Python wrapper of the GTSAM library (v4.2.0) (Georgia Tech Smoothing and Mapping Library) [62]. Trajectory metric evaluation was conducted with the Evo Python package [65]. Additionally, we utilized two photogrammetric software, Agisoft Metashape (v2.1) (2024) [58], and COLMAP (v3.8) [66]. Finally, Cloud Compare (v2.13) [67] was used for 3D point cloud processing.

3. Case Studies and Evaluation Approach

This section presents the two case studies used to evaluate the proposed approaches, detailing the data collection process, survey methodologies, ground truth generation, and evaluation strategy.

Apart from ground truth data used for validation, no Ground Control Points (GCPs) or external constraints were used during the processing of the proposed approaches. We relied solely on the fixed, pre-calibrated rigid transformation between cameras, with known baselines and orientations, to achieve a scaled metric 3D reconstruction. This setup enabled a fully self-contained evaluation of our pipeline from acquisition to final output.

3.1. Case Study 1: Sordine of the Duomo Di Milano

The Sordine of the Duomo di Milano (Italy), located below the rooftop terrace, is an intricately designed U-shaped architectural space. It spans approximately 70 m in length with an average path width of 1 m (Figure 5). The environment consists of nine interconnected rooms featuring curved dome-like structures, linked by narrow passages as small as 75 cm wide. This case study presents several challenges, including complex geometries, tight corridors, intricate architectural elements, restricted maneuverability, and highly variable lighting conditions ranging from well-lit areas to near-dim lighting.

These factors pose difficulties for conventional photogrammetry and are time-consuming for static laser scanning. The primary challenge lies in achieving a coherent reconstruction of the U-shape trajectory while ensuring adequate coverage, particularly across the connecting passages between compartments.

The survey followed a U-shaped trajectory, with forward and backward passes beginning and ending at the same location to enable loop closure (Figure 5c). Over approximately fifteen minutes of field surveying, a total of 13,655 images were acquired (2731 images per camera). The acquisition rate was configured at fifteen frames per second, with each camera saving three images per second.

3.2. Case Study 2: The Minguzzi Spiral Staircase of the Duomo Di Milano

The Minguzzi spiral staircase (Figure 6), located at the front-right corner of the cathedral, is a marble structure extending vertically from ground level to the rooftop, reaching a height of ca. 25 m.

Its interior features a narrow passageway, approximately 70 cm wide, spiraling around a central column with a diameter of 40 cm. The confined space, limited line of sight, texture, and absence of loop closures present substantial challenges for 3D localization and mapping. This case study thus provides an ideal testbed for evaluating the robustness and accuracy of the proposed approaches. The dataset was originally collected during a previous study [23], following an ascending spiral trajectory starting at the lower entrance and ending at the rooftop exit. Over ca. 8 min of field acquisition, a total of 7905 images were captured (1581 images per camera).

3.3. Ground Truth and Evaluation Methodology

For the validation of the Sordine case study, 16 natural GCPs were sampled from a high-resolution terrestrial laser scan survey (Figure 5) covering the entire area. The TLS data scan-to-object distance ranged from ca. 1 to 5 m, where the GCPs were sampled, yielding a calculated point-cloud sampling interval of ca. 1 mm at a manufacturer-specified accuracy of 2 mm at 10 m scan distance. Such high resolution ensures that naturally occurring features and high-contrast surface textures are both visible and defined on the TLS point cloud. On the other hand, the image data acquisition with a scan-to-object distance of ca. 1 to 2 m, where the GCPs were sampled, produced a ground sampling distance (GSD) of less than ca. 1 cm; therefore, these TLS-derived natural points constitute valid GCPs for the photogrammetric reconstruction.

These points were used to scale, reference, and constrain a photogrammetric reconstruction of the entire dataset (13,655 images), establishing the ground truth reference for evaluation. Figure 7 presents the distribution of the GCPs at the start, middle, and end of the Sordine area, equally divided into two groups: checkpoints and control points. The Root Mean Square Error (RMSE) analysis of the natural GCPs yielded an RMSE of ca. 1 cm on the reference XYZ coordinates for both groups.

The consistency between the photogrammetric reconstruction and the TLS point cloud was evaluated by fitting least-squares planes to each point cloud and performing a cloud-to-cloud (C2C) distance analysis in CloudCompare. Distances from each point in one cloud were projected onto the fitted plane of the other cloud, enabling a comparison that captured deviations between corresponding surfaces. The C2C analysis yielded a mean distance of approximately 1.8 cm, with 95% of the points falling within 3.7 cm. Notably, large glass surfaces, dome-like curved geometries, low-texture regions, and high-elevation areas in the surveyed environment introduced challenges for the photogrammetric process, often resulting in local deviations and increased variability in pointwise distances, even when the overall alignment remained consistent. Figure 8 presents the computed C2C absolute distances as a scalar field coupled with the histogram, demonstrating that the majority of points exhibit minimal discrepancies, with deviations remaining relatively modest across the scene.

Despite potential local noise in the photogrammetric reconstruction, its agreement with the TLS point cloud is acceptable at the centimeter level. Consequently, the trajectory derived from the photogrammetric reconstruction is considered valid as ground truth for this case study.

For the Minguzzi case study, a set of 20 natural points, originally identified in a previous photogrammetric survey [68], was utilized. These points were evenly distributed along the staircase (Figure 7) and served to constrain a photogrammetric reconstruction of the complete image dataset.

The RMSE computed on the XYZ coordinates was 0.4 cm for control points and 2 cm for check points. Considering the complexity of the staircase environment and the challenges posed to alternative surveying methods, this level of accuracy is deemed acceptable for establishing a reliable ground truth in the context of this study.

The presented trajectory accuracy evaluations in the following sections compare the V-SLAM estimated and optimized trajectories against the ground truth trajectories. Specifically, the accuracy assessment metrics are performed using RMSE (i.e., the average deviation between estimated and reference camera poses over the entire trajectory) of the following:

(1)

Absolute Pose Error (APE): assessing the global consistency of the trajectory with respect to translational deviations.

(2)

Relative Poses Error (RPE): assessing the local consistency of the trajectory on translation and orientation differences, with the reported error on the translational deviations.

4. Results

The outputs from the proposed V-SLAM approaches are the estimated camera poses and trajectories, as well as their corresponding sparse point clouds. Due to the inherent keyframe selection mechanism of V-SLAM, the number of retained images varied in each approach, as reported in

Table 1. Total number of images used by each of the proposed approaches. S.I. = stereo instance.

		Sordine Study Case	Minguzzi Study Case
Total Dataset		2731 img/cam	1581 img/cam
Multi-Instance Stereo V-SLAM	Left S.I.	636 img/cam	592 img/cam
	Right S.I.	765 img/cam	489 img/cam
	Front-right S.I.	987 img/cam	434 img/cam
	Front-left S.I.	N.A	321 img/cam
Multi-View V-SLAM		1955 img/cam	1526 img/cam
Photogrammetry		2731 img/cam	1581 img/cam
V-SLAM-aided photogrammetry		1955 img/cam	1526 img/cam

.

The photogrammetric reconstruction is performed with the complete image datasets for both case studies, simulating a workflow without the V-SLAM keyframe selection mechanism. In addition to traditional photogrammetry, a V-SLAM-aided photogrammetry [23] workflow is evaluated, using estimated camera poses from V-SLAM to improve spatial matching between adjacent cameras and reduce computational time. To achieve this, we used camera poses generated by the Multi-View V-SLAM method (Section 2.3.2), as it retained a larger number of keyframes, nearly matching the full image dataset in the Minguzzi case study, which allows a robust evaluation of the trade-off between computational efficiency and reconstruction accuracy. Both photogrammetric approaches are run on an Intel CPU i9-11900H @ 2.5GHz and NVIDIA GeForce RTX 3080, and serve as baselines for comparing accuracy and approximate computational time.

4.1. Multi-Instance Stereo V-SLAM Optimization

Due to hardware limitations and the rapid increase in memory consumption during map construction, only two stereo instances (i.e., corresponding to two stereo pairs) could be processed simultaneously in real-time on-field. To ensure fair and consistent comparison across all configurations, each of the four stereo instances in both case studies was reprocessed offline at a fixed rate of three frames per second on an Intel CPU i9-11900H @ 2.5 GHz and NVIDIA GeForce RTX 3080. This offline setting approximately simulates the time required for the V-SLAM operation on the field.

In the Sordine dataset, the front-left stereo instance could not provide and maintain reliable feature tracking due to limited illumination and insufficient texture within its field of view. These conditions impaired the system’s ability to re-localize and merge new observations with previously mapped areas, resulting in disconnected trajectory segments and poor camera pose estimation. As a result, this instance was manually excluded from further analysis by the operator based on the real-time feedback from the system. This highlights the advantage of multi-camera systems, where the redundancy of observations allows other stereo instances to maintain consistent tracking and mapping, preserving dataset continuity even in the presence of partial failures.

Table 2. RMSE results (in meters) for the proposed multi-camera V-SLAM approaches, reported before and after optimization, and compared against traditional photogrammetry and V-SLAM-aided photogrammetry. S.I. = stereo instance.

Approaches		Cameras Absolute Poses Root Mean Square Error [m]
		Sordine case study			Minguzzi case study
V-SLAM		Before Optim.	Feature-Based Optim.	Pose Graph Optim.	Before Optim.	Feature-Based Optim.	Pose Graph Optim.
Single- instance stereo V-SLAM	Left S.I.	0.45 (2 cam)	0.03 (5 cam)	-	0.92 (2 cam)	0.12 (5 cam)	-
	Right S.I.	1.27 (2 cam)	0.02 (5 cam)	-	0.34 (2 cam)	0.05 (5 cam)	-
	Front-right S.I.	1.10 (2 cam)	0.07 (5 cam)	-	0.53 (2 cam)	0.05 (5 cam)	-
	Front-left S.I.	N.A	N.A	-	0.60 (2 cam)	0.26 (5 cam)	-
Multi-View Odometry/V-SLAM (5 cam)		Odometry 0.64	V-SLAM 0.08	-	Odometry 0.11	V-SLAM 0.08	-
Multi-Instance Stereo V-SLAM		-		0.04	-		0.03
Baselines
Photogrammetry		0.03			0.05
V-SLAM-aided Photogrammetry		0.04			0.06

presents the RMSE of Absolute Pose Error (APE) in both case studies for the proposed multi-camera V-SLAM approaches, before and after applying the optimization methods. For comparative purposes, the results of traditional photogrammetry and V-SLAM-aided photogrammetry are reported. The APE RMSE for photogrammetry is 0.03 m in the Sordine case and 0.05 m in the Minguzzi case, while the V-SLAM-aided photogrammetry yielded 0.04 m and 0.06 m, respectively.

The results presented in Table 2 reveal notable variability in pre-optimization trajectory accuracy among the stereo instances. In the Sordine case study, the left stereo instance achieves the lowest pre-optimization RMSE at 0.45 m, whereas the right stereo instance records the highest error at 1.27 m. The front-right stereo instance also exhibits a relatively high error of 1.10 m.

In contrast, the Minguzzi case study demonstrates a different distribution of errors. The right stereo instance attains the lowest RMSE at 0.34 m, while the left stereo instance shows a considerably higher error of 0.92 m. The front-left and front-right stereo instances report intermediate errors of 0.60 m and 0.53 m, respectively.

Following the application of the proposed Multi-View Feature-Based Optimization, a substantial improvement in camera pose accuracy was observed across all individual stereo instances. In Sordine, the left stereo RMSE drops from 0.45 m to 0.03 m, and the right stereo improves from 1.27 m to 0.02 m. Similarly, the front-right stereo instance improves from 1.10 m to 0.07 m.

Comparable improvements are recorded in Minguzzi. The left stereo instance RMSE was reduced from 0.92 m to 0.12 m, and the right and front-right stereo instances improved from 0.34 m and 0.53 m to 0.05 m, respectively. However, the front-left stereo instance continued to exhibit a relatively high RMSE of 0.26 m after optimization. This residual error is attributed to a combination of limiting factors, including a feature-poor viewpoint, exposure to strong artificial lighting, and the lowest number of retained keyframes, all of which contribute to potentially weaker visual correspondences. Although this stereo configuration is geometrically divergent, the divergence alone does not fully justify the error, as the other divergent instance (i.e., front-right) achieves better performance. Figure 9 illustrates the initial drift discrepancies between the left and right stereo trajectories in the Sordine before optimization. Figure 10 demonstrates the improvements achieved post-optimization, with refined camera poses and reduced accumulated drift leading to improved global trajectory consistency.

Beyond optimizing individual instances, the proposed Multi-Instance Stereo V-SLAM approach integrates multiple single-instance stereo trajectories using the Multi-Camera Pose Graph Optimization. This integration enforces global consistency across multiple viewpoints, producing a unified and optimized trajectory for the entire multi-camera system. The final achieved RMSE values for both case studies are 0.04 m in the Sordine and 0.03 m in Minguzzi, confirming the effectiveness of the proposed approach.

In terms of computational time efficiency, the average processing time of the Multi-Instance Stereo V-SLAM is ca. 0.53 h for Sordine and 0.27 h for Minguzzi. These computation times are lower than those of traditional photogrammetry, ca. 9.5 h for the Sordine case and ca. 3.0 h for the Minguzzi case, as well as V-SLAM-aided photogrammetry, which required ca. 2.0 h and 1.25 h, respectively.

4.2. Multi-View V-SLAM Optimization

To evaluate the effectiveness of the proposed Multi-View V-SLAM pipeline, the results of the underlying Multi-View Odometry are presented before integrating the Multi-View Feature-Based Optimization. Figure 11 presents the results of the Multi-view Odometry approach run on an Intel CPU i9-10900F @ 5.2GHz and a NVIDIA GeForce GTX 1080 GPU, showing a visual comparison of trajectories generated using two, three, and five cameras. The corresponding APE RMSE values for these configurations are summarized in

Table 3. Comparison of Absolute Pose Errors RMSE between two, three, and five cameras in the Multi-View Odometry approach before applying the proposed optimization.

		Cameras Absolute Poses Root Mean Square Error [m]
		Sordine Study Case	Minguzzi Study Case
Multi-View Odometry	Two cameras	1.932	0.107
	Three cameras	0.505	0.113
	Five cameras	0.643	0.110

.

In the Sordine case study, a substantial reduction in the RMSE is observed when the number of cameras is increased from two to three, with the error decreasing from 1.93 m to 0.50 m. However, increasing the number of cameras to five increased the RMSE to 0.64 m, suggesting a potential trade-off between enhanced viewpoint redundancy and the introduction of additional feature mismatches or noise that accumulate and propagate through the incremental estimation process.

In contrast, the Minguzzi case study exhibits consistently low RMSE values of ca. 0.11 m across all configurations, indicating greater reconstruction stability in this environment. To further interpret the outcomes, Figure 12 presents a component-wise (X, Y, Z) comparison of the pose deviations across increasing camera counts relative to the ground truth. The Sordine dataset demonstrates a larger variation across all axes, whereas the confined structure of the Minguzzi staircase results in a more stable performance with minimal deviation. Notably, the application of loop closure detection via the proposed Multi-View Feature-Based Optimization significantly enhanced the final trajectory accuracy. As presented in Table 2, the post-optimization RMSE values improved to approximately 0.08 m for both case studies.

Furthermore, Figure 13 demonstrates successful loop closure detection in the Sordine case study, confirming the trajectory estimation improvement and drift reduction achieved. The Multi-View V-SLAM, including both SLAM processing and optimization, required ca. 1.4 h and 0.7 h for the Sordine and Minguzzi case studies, respectively. A comparison of the multi-camera V-SLAM approaches before and after optimization is presented in Figure 14, which illustrates the variation plots of the APE residuals along each trajectory, benchmarked against the corresponding ground truth data across the two case studies.

In the Sordine case study (Figure 14a), the pre-optimization trajectories of the left, right, and front-right stereo configurations exhibit a gradual accumulation of APE, followed by sharp vertical drops at specific timestamps. This behavior corresponds to significant positional drift events and irregular pose estimation caused by outlier observations and loss of reliable visual features. The subsequent steep drops in APE reflect partial realignment events associated with loop closures or re-localization. In the Minguzzi case study (Figure 14b), pre-optimization trajectories display consistent, repetitive oscillations in APE, which are indicative of periodic geometric ambiguities and drift, particularly during repeated rotations within the spiral staircase traversal. This cyclic drift behavior is further highlighted for the Minguzzi case study in Figure 15a–d before optimization, which illustrates the recurring pose estimation errors.

Overall, the results of integrating the proposed optimization approaches demonstrate the effectiveness in refining camera poses and reducing accumulated drift in the overall trajectory estimations, as evident by the post-optimization plots in Figure 14 and Figure 15.

5. Discussion

Prior to optimization, the V-SLAM trajectories exhibited substantial camera pose errors and drift, with the RMSE in APE exceeding 1.0 m in some cases. Although the initial V-SLAM estimates often demonstrated relatively low median RPE values (Figure 16), the presence of high-magnitude outliers contributed to the cumulative drift, thereby increasing the global APE metric. In particular, this effect is evident in the Sordine case study, where the right stereo instance exhibited the highest APE RMSE of 1.27 m before optimization, despite having a median RPE value of ca. 0.05 m. This highlights how the frame-to-frame accuracy does not reflect the overall consistency of the trajectory owing to the buildup of drift.

Following the application of the proposed Multi-View Feature-Based Optimization, significant improvements were achieved. In Sordine, the right stereo instance error decreased from 1.27 m to 0.02 m, whereas in Minguzzi, the left stereo instance error was reduced from 0.92 m to 0.12 m. The box plots of RPE (Figure 16) illustrate these improvements, highlighting a marked reduction in both the median RPE and the variance across all samples. After optimization, the RPE outliers were significantly reduced, leading to more stable and consistent trajectory estimations and achieving centimeter-level APE accuracy in nearly all cases.

The Multi-Instance Stereo V-SLAM approach, integrated with Multi-Camera Pose Graph Optimization, successfully fused individual stereo trajectories into a globally consistent solution by enforcing inter-trajectory pose constraints. In the Minguzzi case, this strategy yielded better overall trajectory accuracy than individually optimized stereo instances, while in the Sordine case, it stabilized performance across all configurations, emphasizing the benefits of global graph-based refinement.

The impact of increasing the number of cameras within the Multi-View Odometry framework varied between the case studies. In the Sordine case study, expanding from two to three cameras substantially reduced the APE RMSE from 1.93 m to 0.50 m. However, further expansion to five cameras introduced a slight accuracy degradation with the RMSE increasing to 0.64 m, likely due to feature mismatches, outlier propagation, and the incremental nature of the pipeline accumulating the drift. Conversely, in the Minguzzi case study, the APE RMSE remained consistent at ca. 0.11 m across all camera configurations. While the use of two cameras may offer computational savings compared to three- and five-camera configurations, this observation is specific to the Minguzzi case study, where the constrained geometry and stable feature tracks using the ALIKE feature extractor potentially minimized drift across configurations. However, when loop closure is required, utilizing all five cameras is advisable, as the increased viewpoint redundancy introduces additional constraints in the final optimization, enhancing the robustness and generalizability of the solution, particularly in more complex or less structured environments. The integration of loop closure detection through the Multi-View Feature-Based Optimization further improved the final APE values to approximately 8 cm in both case studies, demonstrating the robustness and generalizability of the proposed optimization framework, reducing the accumulated drift error and providing a loop closure mechanism when applicable.

The computational efficiency of the proposed multi-camera V-SLAM approaches, along with V-SLAM-aided photogrammetry, was compared to the traditional photogrammetry workflow. The results indicate a reduction in processing time, with the V-SLAM methods achieving ca. 77–94% faster computation compared to conventional photogrammetry, while maintaining comparable reconstruction accuracy. Additionally, the V-SLAM-aided photogrammetry approach, leveraging a pre-selected subset of the V-SLAM-derived keyframes, further optimized processing, reducing computation time by ca. 58–79% relative to the full photogrammetric pipeline.

6. Conclusions

This study presented two novel multi-camera V-SLAM optimization methods (Multi-View Feature-Based Optimization and Multi-Camera Pose Graph Optimization) developed and integrated within two multi-camera V-SLAM approaches (Multi-Instance Stereo V-SLAM and Multi-View V-SLAM). These approaches were designed to improve camera pose accuracy, reduce drift, and enable reliable loop closure in multi-camera V-SLAM applications, thereby supporting more accurate and robust 3D reconstructions in complex and challenging environments.

The proposed approaches were evaluated using extensive and complex datasets acquired with ATOM-ANT3D, a synchronized wide FoV fisheye multi-camera mobile mapping system featuring a pre-calibrated rigid camera configuration. The Multi-Instance Stereo V-SLAM approach demonstrated that despite possible failures in individual trajectories estimation, the redundancy provided by the multi-camera setup ensured tracking and mapping continuity. The Multi-View Feature-Based Optimization improved the single instance trajectories, significantly reducing errors and achieving centimeter-level accuracy in V-SLAM camera poses. Furthermore, the integration of Multi-Camera Pose Graph Optimization merges individual stereo trajectories into a globally consistent and refined single solution.

In contrast, the Multi-View V-SLAM approach, which simultaneously processes all five cameras and integrates Multi-View Feature-Based Optimization, demonstrates a clear advantage over the Multi-View Odometry-based approach in scenarios where loop closure is required to correct the drift and improve the accuracy of the estimated trajectory.

Overall, this study demonstrates that multi-camera V-SLAM, when combined with dedicated optimization techniques, can achieve accurate 3D reconstructions, bridging the gap between the real-time SLAM capability and the accuracy of traditional photogrammetric methods. The proposed approaches significantly outperform conventional V-SLAM in terms of accuracy, achieving centimeter-level accuracy while requiring less computational time than traditional photogrammetry.

7. Patents

The ANT3D multi-camera involved in this study resulted in patent proposal no 102021000000812. The patent was licensed on 24 January 2023.