D²SLAM: Decentralized and Distributed Collaborative Visual-Inertial SLAM System for Aerial Swarm

Hao Xu , Peize Liu , Graduate Student Member, IEEE, Xinyi Chen , Graduate Student Member, IEEE, and Shaojie Shen , Member, IEEE

Abstract—Collaborative simultaneous localization and mapping (CSLAM) is essential for autonomous aerial swarms, laying the foundation for downstream algorithms, such as planning and control. To address existing CSLAM systems’ limitations in relative localization accuracy, crucial for close-range UAV collaboration, this article introduces $D^2\mathbf{S}\mathbf{L}\mathbf{A}\mathbf{M}\mathbf{-}\mathbf{a}$ novel decentralized and distributed CSLAM system. D²SLAM innovatively manages nearfield estimation for precise relative state estimation in proximity and far-field estimation for consistent global trajectories. Its adaptable front-end supports both stereo and omnidirectional cameras, catering to various operational needs and overcoming field-of-view challenges in aerial swarms. Experiments demonstrate D²SLAM’s effectiveness in accurate ego-motion estimation, relative localization, and global consistency. Enhanced by distributed optimization algorithms, $D^2\mathbf{S}\mathbf{I}$ AM exhibits remarkable scalability and resilience to network delays, making it well suited for a wide range ofreal-world aerial swarm applications. We believe the adaptability and proven performance of ${\overline{D}}^2\mathbf{S}\mathbf{I}$ AM signify a notable advancement in autonomous aerial swarm technology.

Index Terms—Aerial systems: perception and autonomy, multirobot systems, simultaneous localization and mapping (SLAM), swarms.

I. INTRODUCTION

OCALIZATION technology is critical for highly auswarm robots are required to estimate not only their own state but also the states of other robots in the swarm. In recent years, simultaneous localization and mapping technology (SLAM) [1], [2], [3], [4] for swarm robots, including aerial swarms [5], [6], has been greatly developed. These methods are well known as collaborative SLAM (CSLAM) or multirobot SLAM.

Considering the size, weight, and power (SWaP) constraints of aerial robot platforms, the primary goal of most SLAM algorithms on aerial robots is to provide input (such as trajectories and ESDF map) for planning and control, as shown in Fig. 1, rather than producing high-quality mesh. Let us explore a few categories of practical tasks of aerial swarms to summarize the technology requirements of CSLAM. The first category is self-assemble aerial swarm [7] and cooperative transportation [8] using multiple UAVs, in this case, UAVs in the swarm require very precise relative localization (centimeter level) to cooperate with each other at a very close distance (usually less than a meter). The second category is inter-UAV collision avoidance [9] and formation flight [10], in this case, high accuracy relative localization (centimeter-level to decimeter-level) at a few meters’ distance is required to avoid collision and maintain flight formation. Another typical task of aerial swarm is cooperative exploration of unknown space. In some typical task, e.g., unknown space exploration [6], it is essential to have a high accuracy relative localization when UAVs are near to each other to avoid inter-UAV collision for this task. However, when UAVs start to explore the unknown space and fly far away from each other, the relative localization accuracy is not important, and the global consistency of the estimated trajectories is more important to build up the global map of the unknown space.

(a)

(b)
Fig. 1. Demonstration of $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ in the HKUST RI dataset: (a) The dense map generated by $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ using TSDF reconstruction, showing only the surface voxels from TSDF. (b) The estimated trajectories of $D^2\overline{\mathrm{V}}$ INS (for near-field state estimation) and D²PGO (for far-field state estimation) in a three-UAV scenario.

In summary, for close-range operations, aerial swarms require high-precision relative localization. At longer distances, however, this precision becomes less critical, with a greater emphasis on the global consistency of state estimation. In addition, accurate ego-motion estimation is vital for stable flight in all tasks.

CSLAM algorithms are influenced not only by task-specific requirements but also by communication constraints. Common communication modes in robot swarms typically encompass centralized systems, such as wireless LAN (WLAN), mainly used in laboratory settings, and wireless ad hoc networks. The latter, a decentralized approach, is particularly suitable for complex real-world scenarios where setting up a router for centralized communication is not feasible. In the latter approach, typically seen in high-mobility aerial swarms [1], robots form mesh networks with communication limited by factors like occlusion, distance, and interference. While these ad hoc networks provide effective communication at close ranges, they face challenges in bandwidth and stability over longer distances.

Moreover, the computational architecture of CSLAM may vary, primarily encompassing two major forms for robot swarms: 1) centralized and 2) decentralized. Centralized CSLAM processes information on a ground-station server, requiring stable network connectivity, while decentralized CSLAM [1], [3] adapts to a broader range of environments. A well-designed decentralized approach is less dependent on stable network connectivity. However, decentralized methods may only partially address the issue. Some approaches, such as [1] and [11], are considered decentralized due to the lack of a central node, yet they replicate the same computations across multiple robots. As the number of UAVs in a swarm increases, these methods are likely to encounter computational bottlenecks. Compared to these approaches, distributed CSLAM offers the advantage ofreduced (but not cost-free) computational overhead per UAV, and enhanced privacy, as only partial information needs to be shared [4]. The key advantage of distributed SLAM lies in its scalability, enabling applications in large-scale swarms.

Taking into account communication constraints and SWaP limitations, and driven by the notion that precise relative localization is essential only for nearby robots while global consistency gains importance as robots move farther apart, we propose $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}.$ . This innovative system represents a novel approach to decentralized and distributed visual-inertial SLAM. Its state estimation combines two parts: 1) near-field and 2) far-field state estimation for aerial swarms. Near-field state estimation in $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ involves estimating high-precision real-time local state [e.g., visual-inertial odometry (VIO)] and relative state between UAVs when their onboard sensors have field-of-view (FoV) overlap and good communication. Far-field state estimation involves estimating trajectories with global consistency when UAVs are far apart or in nonline-of-sight, with limited communication. These two state estimation approaches effectively address the challenges associated with state estimation in aerial swarms, as previously outlined.

The near-field estimation module in $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}.$ named $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ (decentralized and distributed visual-inertial navigation system), is a collaborative multirobot VIO system using sparse features. Similar to standard single-robot VIO systems like VINS-Mono [12], $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ maintains a local map using a sliding window and employs graph-based optimization for state estimation. Recent studies have shown significant advancements in applying the alternating direction method of multipliers (ADMM) [13], [14] to distributed bundle adjustment (BA) [14], [15]. Inspired by these developments, we have adopted ADMM in $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ to effectively address distributed VIO challenges.

Given visual SLAM limitations, $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ requires overlapping sensor FoVs for accurate real-time relative state estimation at close range. Omnidirectional vision systems [1], especially with our enhanced omnidirectional front end, effectively address these issues in aerial swarms. With omnidirectional cameras, $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ is not constrained by the UAVs’ yaw angles, enabling precise relative localization when UAVs maintain line of sight, share environmental features, and communicate reliably. Commercial stereo cameras like the Intel Realsense $\mathrm{D}435\mathrm{i},{}^1$ widely used in advanced aerial swarms [5], [6], are also supported by $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ . Stereo cameras, requiring less computational power, are well suited for UAVs with limited resources. However, their optimal usage, particularly adjusting UAV yaw for FoV overlap, falls beyond the scope of this article.

The core of far-field estimation is $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ (decentralized and distributed pose graph optimization). $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ facilitates both relative and global localization through PGO, particularly when UAVs are distant or out of each other’s line of sight. Unlike $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}.$ , which requires good communication conditions, $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ functions effectively even in poor communication scenarios, characterized by unstable network latency and low bandwidth. Peng et al.’s [16] distributed optimization method, ARock, adeptly handles network latency and asynchronous updates, effectively countering the network instability and delays in PGO. Thus, we introduce an ARock-based asynchronous distributed PGO algorithm in Section VII.

The main contributions of this article are as follows.

1) Introduction of $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ , a novel decentralized and distributed SLAM system, capable of achieving highaccuracy ego-motion and relative state estimation for nearby UAVs, as well as globally consistent trajectory estimation for distant or nonvisible UAVs.

2) We introduce $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S},$ a distributed visual-inertial state estimator for multirobot systems utilizing the ADMM approach. This system represents the first instance of a tightly coupled VIO estimator based on distributed optimization, providing both ego-motion estimation and high-accuracy relative localization.

3) $D^2\mathrm{P}\mathrm{G}\mathrm{O},$ , an ARock-based, asynchronous, distributed PGO algorithm, specifically designed for multiple robots is introduced. The adaptability of $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ to communication delays and its proficiency in managing nonlinear issues make it an ideal solution for robot swarms. This is also the first attempt of using ARock in solving pose graph problems.

4) We conduct extensive testing of $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ on an aerial swarming platform, encompassing both dataset evaluations and real-world experiments. In addition, we have open-sourced the code and custom datasets.²

II. RELATED WORKS

A. Distributed SLAM Techniques

This subsection explores fundamental techniques in distributed SLAM, namely distributed PGO and distributed BA.

1) Distributed Pose Graph Optimization: PGO [17], [18], a technique derived from the factor graph theory [19], is crucial in SLAM for relocalization [12], [20] and dense mapping [21]. When SLAM expanded to multirobot collaboration, PGO was naturally incorporated [22], [23], [24]. Initial PGO applications in CSLAM utilized centralized servers to solve the pose graph problem [24], but faced scalability and communication challenges in swarm robot systems. Cunningham et al. [22] proposed the first distributed SLAM method, DDF-SAM, using a constrained factor graph, a concept further refined in DDF-SAM2 [23]. Nevertheless, these methods maintain a neighborhood graph on each agent, leading to computational inefficiencies and scalability issues.

Significant advancements in distributed PGO (DPGO) began with Choudhary et al.’s [4] distributed Gauss–Seidel (DGS) approach. This method transforms the PGO problem into two linear problems, which are then distributedly solved using the Gauss–Seidel technique [25]. However, this method struggles with unfavorable convergence at high noise levels and necessitates synchronous operation. Later, the ASAPP method, based on distributed gradient descent, was proposed by Tian et al. [26], functioning as a distributed and asynchronous version of Riemann gradient descent [27]. This work was notable for being the first to incorporate communication considerations, introducing an asynchronous DPGO approach. Tian et al.’s [28] subsequent work introduced the DC2-PGO method using the distributed Riemann-staircase, a certifiably correct DPGO approach. These DPGO methods have been incorporated into various practical SLAM systems, such as DOOR-SLAM [3] that utilizes DGS, and Kimera-multi [2], [29] which employs both ASAPP and DC2-PGO.

2) Distributed Bundle Adjustment: BA [30], a core component in structure-from-motion (SfM) and sparse SLAM, has evolved with significant advancements in distributed processing. Eriksson et al. [15] pioneered a consensus-based distributed BA method, partitioning camera poses to enhance processing in a manner akin to the ADMM, though it requires extensive communication for landmark data. Addressing this challenge, Zhang et al. [14] developed a “camera consensus” method, effectively reducing communication needs and improving convergence. Karrer and Chli [31] investigated a loosely coupled approach for fusing VIO with relative measurements in distributed VIO, utilizing ARock [16], an asynchronous distributed optimization algorithm. However, their focus on predominantly two-robot scenarios highlights the necessity for more versatile solutions. In addition, the use of multistate constraint Kalman filter (MSCKF) for distributed BA, as studied in [32], [33], and [34], although promising, may suffer from accuracy loss due to the single linearization of measurements.

Building on these insights, our approach, inspired by [14], adopts a strategy of dividing landmarks into disjoint sets for an ADMM-based collaborative VIO in a distributed setting, aiming to enhance both accuracy and efficiency in multirobot SLAM applications. Our method stands out as the first to tightly coupled distributed VIO in contrast to existing ADMM-based distributed BA approaches. Unlike the SfM methods [14], [15], which do not prioritize real-time performance, our method is designed for real-time operation. Compared to approach [31], our method is applicable to multirobot scenarios, not just limited to two UAVs. A unique feature of $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ is the directly and tightly integration of both visual and IMU measurements, a capability absent in previous works. This enhancement maximizes the utility of visual-inertial inputs to improve accuracy, making our method highly suitable for aerial swarm applications.

B. Current CSLAM Systems

In this section, we review existing CSLAM systems, highlighting their strengths and limitations. DPGO-based systems, such as [2] and [3], are known for their global consistency and distributed capabilities, yet they often lack in high relative localization accuracy. Systems employing BA on landmarks, including [31], [32], [33], [34], offer multicamera state estimation but typically fall short in global consistency, which limits their broader application.

In addition, methods incorporating relative measurements like UWB ranging or visual detection are gaining attention. For example, the authors in [11], [35], and [36] applied UWBodometry fusion for relative state estimation but face challenges in achieving global consistency. Ziegler et al. [36] used ARock for back-end optimization to scale up UWB-odometry fusion, while Nguyen et al. [37] integrated UWB anchors for enhanced global consistency, albeit with reliance on ground infrastructure. On the other hand, Xu et al. [1] utilized a map-based localization approach for achieving global consistency, though UWB systems often encounter issues like obscuration and radio interference in real-world environments.

Furthermore, visual-detection-based approaches, such as [1], [11], and [38] face limitations due to reliance on pretrained models, particularly when UAVs are at a distance.

Contrastingly, $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ not only ensures global consistency but also high relative localization accuracy at close ranges. Its distributed architecture is particularly suited for scalable swarm applications. Diverging from approaches like in [2], [5], [9], [32], [33], [35], and [36] that depend on ground truth poses for initialization, $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ enhances system flexibility by allowing runtime merging of different robots’ reference systems and sparse maps.

Fig. 2. Architecture of $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}.$ The $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ is independently running on each UAV. The data will be first processed by front end and then sent to back end for state estimation. Results can be utilized for dense mapping, planning and control.

Unlike the distributed algorithms typically limited to dataset validation or simulation [2], [14], [15], [32], [33], our approach has undergone extensive testing both through in-the-loop flight experiments. $D^2\mathbf{S}\mathbf{I}$ AM demonstrates promising performance in robotic systems’ perception-control loops, showcasing its practical usability and effectiveness.

III. PRELIMINARY

This article adopts the symbol system from [1], details of which are not repeated here. We consider an aerial swarm consisting of N UAVs, each with a unique ID, $i\in \mathcal{D}$ , where $\mathcal{D}=\{1,2,\dots ,N\}$ represents the set of all UAVs in the swarm. By default, the swarm’s global reference system is established based on the initial position of UAV k, unless stated otherwise. $D^2\mathbf{S}\mathbf{I}$ AM operates independently on each UAV, with a focus on its implementation on the ith UAV unless specified. Details on establishing a unified reference system among the UAVs will be provided in Section IV-C.

A. State Estimation Problem ofCSLAM on Aerial Swarm

The state estimation problem in an aerial swarm is defined as follows: For each UAV $k\in \mathcal{D}$ , estimate the 6-degrees-offreedom (DoF) pose $v_k{\mathbf{T}}_i^t$ of every other $\mathrm{U}\mathrm{A}\mathrm{V}i\in {\mathcal{D}}_a^k$ at time t in UAV k’s local frame, where ${\mathcal{D}}_a^k$ represents the set of UAVs in communication with UAV k. This problem is divided into two key parts for UAV k as follows.

1) Estimation of UAV k’s own ego-motion state ${\widehat{\mathbf{T}}}_k^t$ in its local frame.

2) Estimation of the state $v_k{\widehat{\mathbf{T}}}_i^t$ of any other UAV i.

In SLAM research [1], [39], [40], [41], global consistency is understood as the ability to bound the absolute trajectory error (ATE) [42], ensuring that state estimation errors do not progressively increase with the robot’s movement. In this article, it means that the estimated poses $v_k{\widehat{\mathbf{T}}}_i^t$ remain accurate and do not suffer from drift as the robot moves.

Fig. 3. Visual-inertial UWB fusion [11] and Omni-Swarm [1] are decentralized, and asynchronous Distributed ADMM [43] is distributed. D²SLAM is both decentralized and distributed.

B. Decentralized versus Distributed

Decentralized and distributed systems, often used interchangeably, have distinct meanings. Decentralized systems operate without a central node, with algorithms running independently across multiple nodes. In contrast, distributed systems, as defined in [44], spread computation or resources like storage across multiple nodes, aiming for nonredundant processing. However, decentralized systems can involve significant redundant computation, while distributed systems may still rely on a central node for coordination. The relationship between decentralization and distribution is illustrated in Fig. 3. For instance, Omni-swarm [11] is decentralized but not distributed, lacking a central node but involving redundant optimization across the swarm, leading to computational inefficiency. Conversely, asynchronous distributed ADMM [43] employs a master–worker structure that distributes computation across multiple nodes, yet relies on a central node.

This article aims to develop a fully distributed and decentralized $(D^2)$ CSLAM algorithm, $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ . This approach ensures robustness against single node failures and communication losses, enabling the swarm to dynamically form subswarms or integrate into a larger swarm. Moreover, $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ efficiently utilizes the computational resources of each UAV, enhancing overall system performance.

IV. SYSTEM OVERVIEW

A. D²SLAM System Architecture

Fig. 2 depicts the architecture of $D^{2}SLAM$ . On each UAV, the front-end processes visual inputs for key frame extraction, sparse feature tracking, and loop closure detection. These processed data are then forwarded to the back end for two types of state estimations: 1) collaborative visual odometry (near-field state estimation) and 2) PGO (far-field state estimation), handled by $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ and $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ , respectively. The resulting estimations support further processes like dense map building, planning, and control.

B. Communication Modes

D²SLAM employs a combination of three distinct communication modes to enhance system robustness and minimize communication overhead as follows.

1) Discover Mode: Activated when UAVs’ relative states are unknown, this mode involves broadcasting complete keyframes (Section V-A) and states for both near (Section VI) and far-field (Section VII) state estimation. This facilitates rapid initialization of relative state estimation and supports map merging (Section IV-C) for integrating UAVs’ reference systems.

2) Near Mode: When other UAVs are predicted to be in proximity (within 3 to 5 m), based on current state estimates, $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ broadcasts complete keyframes and states necessary for near and far-field state estimation. The proximity prediction relies on state estimation from both $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ and $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$

3) Far Mode: Used when UAVs are distant from each other, this mode transmits compact keyframes (Section V-A) and performs distributed loop closure detection (Section V-E). It broadcasts states for far-field state estimation. In far mode, near-field state estimation simplifies to single-robot VIO.

Fig. 4(a) depicts $D^2\mathrm{S}\mathrm{L}\mathrm{A}{\mathrm{M}}^{\mathrm{\prime }}\mathrm{s}$ communication mode transitions. After initialization, near-field state estimation is limited to near mode to save bandwidth, while far-field state estimation operates in all modes for global consistency. Details on data transmission per mode are provided in later sections.

C. Multirobot Map Merging

In D²SLAM, each robot starts with its own local reference frame ${\mathbf{F}}_i$ at boot time. When map-merging occurs, typically during encounters with other UAVs or revisiting areas covered by others, the UAVs align their coordinate systems to form a unified reference system. This alignment, triggered by relative measurements, integrates the coordinate systems without exchanging landmarks, allowing each UAV to maintain its individual sparse map. Specifically, the system adopts the reference frame of the UAV with the smaller ID, and the larger ID UAV’s states in $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ and $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ are converted to this unified system.

(a)

(b)
Fig. 4. (a) State machine governing $D^2\mathrm{S}\mathrm{l}$ LAM’s communication modes begins in discover mode. In this initial phase, each UAV broadcasts its complete keyframe to support rapid initialization and transmits data essential for $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}.$ Once UAVs have successfully initialized their relative states, $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ transitions to either near or far mode, depending on their estimated relative positions. The system can fluidly alternate between near and far modes in response to changes in UAV positioning. Importantly, if a new UAV enters the system, typically seen during the initialization phase, $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ reverts to discover mode to incorporate this new member. (b) Map merging occurs during the discover mode, as shown in the flow charts. If near or far-field state estimation identifies other UAVs, the UAV with the higher index transfers its map state to the UAV with the lower index.

Fig. 4(b) illustrates the map-merging process in $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}.$ UAVs continuously transmit heartbeat packets for detection by others, facilitating their discovery. During the discover mode, $D^2\mathbf{S}\mathbf{I}$ LAM broadcasts information until the swarm’s UAVs are unified under a single coordinate system. After completing map-merging, subsequent state estimations proceed without this process, unless new UAVs join the swarm.

D. Operating Conditions of $D^{2}SLAM$

D²SLAM adapts to environmental and communication constraints by transitioning to single-robot SLAM or VIO when necessary. This ensures aerial swarm stability and enables shortterm formation flight using only VIO, as shown in [10]. Detailed discussions on these limitations follow.

1) Communication: In $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ , a loss of communication triggers a transition to single-robot SLAM, referred to as the near communication mode. Near-field state estimation requires extensive data exchange and real-time performance, hence a robust, low-latency network is essential. Conversely, far-field state estimation can function effectively with lower bandwidth and some latency.

2) Environments: In open environments with limited environmental features, such as grasslands or rough walls, sparse visual SLAM faces challenges in feature matching for relative localization and loop closure detection. In response to these limitations, our system is designed to downgrade to single-robot VIO, ensuring flight safety under such conditions.

Fig. 5. Front end of $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ processes visual data in several stages. Initially, the data undergoes reprojection, followed by extraction of global descriptors and features. Subsequently, it is used for feature tracking, multi-UAV feature matching, and loop closure detection. The final results are then fused in the back end.

3) Field ofView: Accurate relative localization in $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ is contingent on FoV overlap and a sufficient number ofcommon features. Yaw alignment is crucial for FoV-limited cameras, but this requirement is eliminated with omnidirectional cameras. The local precision of $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ helps offset brief losses of common features, ensuring reliable relative state estimation. When UAVs lose shared vision with others for an extended period, D²VINS degrades to single-robot VIO. We believe that the need for accurate relative localization lessens when UAVs are either out of sight for extended periods or too distant to share features.

V. FRONT END

D²SLAM features a versatile and efficient front end that supports various camera configurations, including omnidirectional and stereo cameras.

Structure of Front-End: Fig. 5 illustrates the components of our front end, which include: 1) preprocessing fisheye images from omnidirectional cameras; 2) general preprocessing for extracting landmarks and global descriptors; 3) feature tracking for the local UAV; 4) sparse feature matching across multiple UAVs; 5) distributed loop closure detection.

Landmarks & Features: Similar to other sparse visual SLAM systems, D²SLAM utilizes landmark points to model the external environment. It treats a set of consistently tracked or matched 2-D feature points across images as measurements of identical landmarks. Subsequent sections will detail the methods for tracking and matching landmark-related features, both by individual UAVs and collaboratively among multiple UAVs.

A. Visual Data Preprocessing

In this work, as depicted in Fig. 7, we employ a quad fisheye camera system as an omnidirectional camera. This configuration, compared to dual fisheye systems [1], [45], offers more effective FoV usage and generates superior quality depth information. Its advantages have led to its adoption in both SLAM research [46] and advanced commercial UAV products, such as the DJI Mavic 3.³

Fisheye cameras produce significantly distorted images, challenging direct application of conventional vision techniques, as shown in Fig. 6(a). Following [47], we first reproject these images into a cylindrical projection [see Fig. 6(b)] before processing. Plaut et al. [47] demonstrated that applying convolutional neural networks (CNNs) trained on public datasets to these reprojected images yields effective results.

In $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}.$ , to conserve communication bandwidth, complete keyframe information, such as SuperPoint features and global descriptors for each camera view, is broadcasted to other UAVs only in discover or near communication modes. On the other hand, the compact keyframes, include only the NetVLAD descriptors, are used in far mode to save bandwidth.

B. Descriptor Extraction & Dimension Reduction

We employ MobileNetVLAD [48] to construct a global descriptor for each camera view, and SuperPoint [49] for landmark extraction, coupled with PCA for dimension reduction of the descriptors. These techniques, validated in our previous work [1], are not elaborated here for brevity.

C. Sparse Feature Tracking

Sparse visual SLAM typically employs two types of feature tracking: 1) Lucas–Kanade (LK) method, as in VINS-Mono [12], offering robustness in feature-poor environments like grass, and 2) descriptor-based matching, notably in ORB-SLAM [50], with CNN-based approaches excelling in handling large parallax for multi-UAV matching and loop closure detection, though less effective in feature-sparse areas. Given the importance of both multi-UAV feature matching and robustness in our work, we adopt a hybrid front end to meet these requirements, illustrated in Fig. 6(b).

Each camera view in $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ tracks up to $N_{\mathrm{m}\mathrm{a}\mathrm{x}}$ (typically 100–200) landmarks. Initially, we extract $N_{{\mathrm{m}\mathrm{a}\mathrm{x}}_{sp}}(50–150)$ SuperPoint [49] sparse features and their descriptors, assuming $N_{\mathrm{s}\mathrm{p}}$ successful extractions. SuperPoint is preferred for its robustness over ORB features [50]. These landmarks are tracked interframe using a kNN matcher with ratio test [51]. In addition, Shi-Tomasi corners are detected to fill the remaining quota up to $N_{\mathrm{m}\mathrm{a}\mathrm{x}}$ and tracked with the LK method. SuperPoint features are utilized for loop closure detection and multi-UAV matching, while LK landmarks aid ego-motion estimation. In feature-scarce environments, if no SuperPoint features are detected $(N_{\mathrm{s}\mathrm{p}}=$ 0), the system defaults to single-robot VIO. Similiar to [12], keyframe determination is based on parallax and the count of new landmarks after feature tracking.

Fig. 6. Fisheye Image Demonstration. (a) Original fisheye images. (b) Reprojected fisheye images featuring tracking. Blue points represent SuperPoint features, while orange points indicate LK features, and green arrows depict the inter-frame movements of these features.

Fig. 7. Omnidirectional Camera Configuration: The transparent areas represent the Field of View (FoV) of each camera. These cameras boast a lateral FoV of approximately 200° and feature an overlap of 110° between adjacent cameras.

Upon completing per-frame tracking, multiview feature tracking is applied across different cameras on the same UAV. For stereo inputs, matching SuperPoint features and LK optical flow between left and right cameras is straightforward. In contrast, with quad fisheye cameras, as illustrated in Fig. 7, each camera shares landmarks only within the half-plane of its neighboring camera. Therefore, feature matching for SuperPoint features is limited to these corresponding half-planes. Landmarks’ positions are predicted based on the cameras’ internal and external parameters, serving as initial values for solving the LK optical flow.

D. Multi-UAV Sparse Feature Matching

When a new keyframe is received, the system first searches the sliding window for the most recently matching keyframe, determined by their global descriptors’ inner product surpassing the threshold ${\tau }_{mg}$ (default 0.8). Following a successful match, kNN feature matching with ratio test is performed on the Superpoint features of these keyframes to establish feature correspondences. Subsequently, once a match is confirmed, we combine the corresponding landmark points of each matched feature pair into an inter-UAV landmark.

Fig. 8. Demonstration of distributed loop closure detection: UAVs transmit compact keyframes, which include NetVLAD global descriptors, across the swarm. Upon receiving this data from UAV i, UAV j initiates loop closure detection. In case of a successful match, UAV j sends a complete keyframe containing landmark information back to UAV i, enabling the extraction of the final relative pose.

E. Distributed Loop Closure Detection

Following [1], we utilize Faiss [52] for whole image searching, kNN for feature matching, Perspective-n-Point (PnP) for relative pose extraction, and implement outlier rejection. Subsequently, loop edges are processed by the back end for PGO. As illustrated in Fig. 8, $D^2\mathbf{S}\mathbf{I}$ AM adopts distributed loop closure detection [2], [3] to minimize communication bandwidth in far mode, and direct loop clousure detection [1] in near and discover mode.

Unlike [1], in $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ , we use 3-D positions of Superpoint landmarks, estimated by $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ from historical frames, along with 2-D observations from the current frame for PnP computation. In addition, with quad fisheye cameras as input, solving a multicamera PnP problem becomes necessary for relative pose extraction and geometric verification. In practice, we initially apply the UPnP RANSAC algorithm [53] for pose estimation and outlier rejection. Subsequent inlier results are then used in a BA problem for multicamera PnP [54], enhancing pose accuracy.

VI. NEAR-FIELD STATE ESTIMATION: D²VINS

A. Problem Formulation

To facilitate near-field state estimation in $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}.$ we introduce $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ , a collaborative VIO. Designed for real-time, accurate 6-DoF odometry estimation of both the local UAV and nearby UAVs, $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ operates on a graph optimization framework with a sliding window, akin to [12]. Its state estimation problem is modeled using a factor graph [19], depicted in Fig. 9. State estimation within the sliding window is achieved by maximizing the a posteriori inference of this factor graph through distributed nonlinear least squares optimization. The state of the collaborative VIO problem is defined as

Fig. 9. Demonstration of the factor graph in $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ . The states within the sliding window, comprising pose, velocity, and IMU bias, are interconnected by various factors including IMU, visual, and prior factors.

where $v_k{\tilde{\mathbf{T}}}_i^{t_j}$ is the estimated pose of UAV i at time $t_j$ in the local frame of UAV k, m is the length of the sliding window and n is the number of the UAVs in the swarm, $\mathbf { b } _ { i } ^ { t _ { j } } = \left[ ^ { v _ { k } } \mathbf { v } _ { i } ^ { t _ { j } } , b _ { a } ^ { t _ { j } } , b _ { g } ^ { t _ { j } } \right]$isthevelocity,accelerationbiasandangularvelocitybiasoftheIMUofUAViattime$t _ { j }$,and$L$isthetotalnumberofthelandmark.Inthisarticle,weusetheinversedepthparametrizationforlandmarks[55],where$l _ { j }$representstheinverseofthedistancefromlandmark$j$tothekeyframeitisassociatedwith.Forconvenience,wedenotethesetofallposesinXas$\tau$ . The optimization problemfor collaborative VIO is formulated as

where $\rho (·)$ is the Huber norm [56], $\Vert (·){\Vert }_{\Sigma }$ is the Mahalanobis norm, B is the set of IMU factors, r_B is the residual of the IMU measurement factor, L is the set of visual measurements, and ${\mathbf{r}}_{\mathcal{L}}$ is the visual measurement residual, ${\mathbf{r}}_B$ and ${\mathbf{r}}_{\mathcal{L}}$ are defined in [12]; $\Vert {\mathbf{r}}_p-{\mathbf{H}}_p\mathbf{\chi }{\Vert }^2$ is the prior factor generated from previous marginalization, which will be detailed in Section VI-D.

B. Decentralized and Distributed Optimization of Collaborative Visual-Inertial-Odometry Problem

1) Alternating Direction Method of Multipliers (ADMM) Algorithm: In this section, the target of decentralized and distributed optimization is to solve the problem

using the decentralized (and distributed) ADMM method [13], [14], where ${\mathbf{x}}_i$ is the local state for UAV i and z is the global state, Problem (3) can be jointly solved by iteratively performing the following update on each UAV i:

Here, in each iteration, each UAV solves Problem (4) and updates its states by averaging the results obtained from all UAVs.

2) Problem Splitting: As outlined in Section II-A2, effectively partitioning the problem and state is crucial in ADMM optimization. In ${\overline{D}}^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ , we split the landmarks $\mathcal{L}$ into disjoint sets ${\mathcal{L}}_1,{\mathcal{L}}_2,···{\mathcal{L}}_N$ , where ${\mathcal{L}}_i$ represents the set allocated to UAV i. This partitioning approach and the landmark distribution policy are further elaborated in Section VI-C. In addition, we denote the set of poses observing landmarks in ${\mathcal{L}}_i$ as ${\tau }_i.$ . Consequently, we define the local state ${\mathcal{X}}_i$ of UAV i as

where ${\mathbf{b}}_i^{t_0},{\mathbf{b}}_i^{t_1}···{\mathbf{b}}_i^{t_{m-1}}$ are the state of the velocity and IMU bias of UAV i, they are only essential for local optimization. Due to the presence of the hybrid tracking feature method (Section $\mathrm{V}-\mathrm{C}),X_i$ necessarily contains all $i{\mathrm{}}^{\circ }\mathrm{s}$ pose in X . This ensures that once all remote UAV are disconnected, $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ can still estimate the ego-motion, i.e., degrade to a single-robot VIO.

3) Distributed Optimization for $D^{2}VINS;$ Rewritten (2) to the form of decentralized optimization (3), we have

Algorithm 1: Distributed Optimization for Collaborative
VIO on UAV i.
Input: n UAVs, m landmarks, iter iterations, $\rho$ and
${\rho }_T.$
Output: n UAVs' local states ${\mathcal{X}}_i$
1 Initialize ${\mathcal{X}}_i$ and X, ${\mathcal{T}}_i,{\mathcal{L}}_i.$
2 for $t=1$ to iter do
3 Solve the local optimization problem (9) with ${\mathcal{X}}_i$
4 Broadcast the poses ${\tau }_i$ in local state ${\mathcal{X}}_i$ to all UAVs
5 Update ${\mathcal{X}}_i$ with by averaging poses among swarm
with Eq. (11).
6 Update the states for auxiliary variables $\{{\tilde{\mathbf{P}}}_j^k...\}$
using Eq. (12).

where $B_i$ is the set of the IMU factors of UAV $i,{\mathbf{P}}_j$ is the jth pose in ${\mathcal{T}}_i,{\mathbf{E}}_i$ project the global state X to local state ${\mathcal{X}}_i$ $f_{{\mathrm{c}\mathrm{v}\mathrm{i}\mathrm{o}}_i}({\mathcal{X}}_i)$ is the subproblem of the global optimization Problem (2) on UAV i.

Following (4)–(6), the iteration update for the collaborative VIO problem (8) is

where $\log (·)$ is the logarithm map of the Lie group SE(3) [57], $({\tilde{\mathbf{P}}}_j^k{)}^t$ is the auxiliary variable, which will be detailed in Section VI-B4. $h({\mathcal{X}}_i)$ is the consensus item to ensure the local state ${\mathcal{X}}_i$ is consistent with the global state $\mathcal{X}.$ Inside $h({\mathcal{X}}_i)$ the first item $\begin{array} { r } { \sum _ { P \in \mathcal { T } _ { i } } | \log ( ( \mathbf { P } _ { i } ^ { t } ) ^ { - 1 } \mathbf { P } _ { i } ^ { k } ) + ( \tilde { \mathbf { P } _ { i } ^ { k } } ) ^ { t } | _ { \Sigma _ { \rho _ { T } } } } \end{array}$istoensuretheposesareconsistentamongUAVs,andtheseconditem$\textstyle \rho _ { \mathcal { L } } \sum _ { l _ { i } \in \mathcal { L } _ { \rangle } } | l _ { i } - l _ { i } ^ { t } | $ is to ensure the convergence of the distributed optimization, as proved in [14]. The full algorithm of the distributed optimization for collaborative VIO is shown in Algorithm 1.

Subsequent experiments, as detailed in Section IX-B, confirm ADMM’s suitability for D²VINS. Importantly, each UAV independently computes (11). Due to differing neighbor relationships, UAV i focuses only on averaging poses in ${\mathcal{X}}_i$ . While this involves some computational redundancy, its impact on the overall distributed computing performance is minimal. In addition, data sharing in Algorithm 1 is limited to UAVs within the near-field state estimation scope.

4) Optimization on Manifold: One key problem in SLAM is the optimization on the manifold. In Algorithm 1, Problem (9) is solved by the Riemannian trust region (RTR) method [27] to ensure the pose state is on SE(3) group, i.e., ${\mathbf{P}}_i\in \mathbf{S}\mathbf{E}(3)\forall {\mathbf{P}}_i$ ∈ ${\mathcal{T}}_i.$ In addition, the auxiliary states $\{{\tilde{\mathbf{P}}}_j^k...\}$ are defined on se(3). We use the logarithm map [57] to transform the error $(\dot{{\mathbf{P}}_j^{t+1}}{)}^{-1}({\mathbf{P}}_j^k{)}^t$ defined on SE(3) into se(3). The quaternion averaging in (11) is solved by quaternion averaging algorithm [58], which is well known in the aerospace field.

Our modified ADMM algorithm in Algorithm 1 allows states to be defined on SE(3), enabling us to leverage a wide range of foundational tools from VINS research, such as the RTR method (adopted in [12]) and existing marginalization techniques (Section VI-D). This approach also helps avoid deviations from the linearization point due to incorrect initialization. In contrast, for pose estimation problem, ARock shall use variables on se3 (will be detailed in Section VII). When using ARock on Problem 8, if the initialization of VIO is suboptimal—which is unavoidable in practice—the results on se3 may deviate significantly from the linearization point, potentially compromising the accuracy. We believe above factors positively influence the convergence of our algorithm compared to the ARock approach, as verified by our early-stage tests. What is more, ADMM’s robustness in complex, large-scale BA scenarios is well established [14], [15], leading us to select this approach instead of using ARock.

C. Landmark Management

It is essential to partition landmarks to disjoint sets for each UAV (detailed in Section II-A2), cap the number of landmarks to balance real-time performance demands, while ensuring sufficient measurements for observability in challenging conditions. Moreover, filtering out outlier measurements increases the system’s robustness and reduces the likelihood of false matches. These requirements of landmark partitioning can be formulated as follows, for agent k:

(17)

(18)

where $f_s({\mathcal{L}}_k)$ is the weighted number of measurements of ${\mathcal{L}}_k$ $m_l({\mathcal{L}}_k)$ and $m_r({\mathcal{L}}_k)$ represent the number of measurements of ${\mathcal{L}}_k$ from local keyframes (keyframes generated by UAV k itself) and remote keyframes, respectively. $m({\mathcal{L}}_k,{\mathcal{K}}_i^t)$ is the number of measurement of ${\mathcal{L}}_k$ and keyframe ${\mathcal{K}}_j^t$ , and $f_c({\mathcal{L}}_k)$ is its lower bound. ${\mathtt{\Psi }}_{{\mathrm{L}}_k}{f}_c({\mathcal{L}}_k)$ aims to maximize this lower bound. $o({\mathcal{L}}_k)$ is the number of outlier measurements of ${\mathcal{L}}_k$ . The weights $w_l,w_r$ , and $w_o$ dictate the significance of respective measurements. The number of landmarks in ${\mathcal{L}}_k$ is capped by ${\tau }_l,$ , while the maximum number of measurements is restricted by ${\tau }_m.{\kappa }_j^t$ refers to a frame within the sliding window. $\mathcal{L}{\mathcal{D}}_k$ represents the landmarks first discovered by UAV k, so the sets $\{\mathcal{L}{\mathcal{D}}_1,\mathcal{L}{\mathcal{D}}_2,\dots ,\mathcal{L}{\mathcal{D}}_n\}$ partition landmarks $\mathcal{L}$ into n disjoint groups and the condition ${\mathcal{L}}_k\subseteq \mathcal{L}{\mathcal{D}}_k$ ensures that $\mathcal{L}$ is divided into disjoint sets in Problem (13). $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ adopts an “observed-first” principle for this partitioning [in (18)], which primarily benefits from single-round communication during complete keyframe broadcasts. While alternative strategies like graph partitioning techniques [59] could potentially enhance optimization, they may introduce extra communication overhead. Therefore, such methods are not implemented in $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$

Algorithm 2: Multirobot Landmark Selection.
Input: Current UAV $k,$ available landmarks $\mathcal{L},$ set of
available keyframes $\{\mathcal{F}\}$ , a dictionary O
representing the observed landmarks of each
frame, with the relationship $O[\mathcal{F}]\to {\mathcal{L}}_f$
Output: Set of selected landmarks ${\mathcal{L}}_k$
1 ${\mathcal{N}}_l\leftarrow \varnothing$
2 ${\mathcal{L}}_k\varnothing$
3 $O_a\leftarrow O$
4 foreach $\mathcal{F}\in \{\mathcal{F}\}$ do
5 $\lfloor {\mathcal{N}}_l[\mathcal{F}]\leftarrow \dot{0}$
6 while True do
7 if $empty({\mathcal{N}}_l)$ then
8 break
9 $\mathcal{F}$ arg min $.\mathcal{F}\in \{\mathcal{F}\}\sqrt{{}_l}[\mathcal{F}]$
10 if $empty({\mathcal{O}}_a[\mathcal{F}])$ then
11 ${\mathcal{N}}_l\leftarrow {\mathcal{N}}_l⧵\{\mathcal{F}\}$
12 continue
13 $l\leftarrow \mathrm{\backslash arg}{max}_{l\in {\mathcal{O}}_a[\mathcal{F}]}$ cost(k)
14 foreach track ∈ l.tracks do
15 $\underset{―}{{\mathcal{N}}_l[track.\mathcal{F}]}{\mathcal{N}}_l[track.\mathcal{F}]+1$
16 ${\mathcal{L}}_k\leftarrow {\mathcal{L}}_k\cup \{l\}$
17 foreach $\mathcal{F}\in \{\mathcal{F}\}$ do
18 $\begin{array} { r } { \lfloor \mathcal { O } _ { a } [ \mathcal { F } ]  \dot { \mathcal { O } } _ { a } [ \mathcal { F } ] \setminus \{ l \} } \end{array}$
19 if $|{\mathcal{L}}_k|>{\tau }_{l}or{m}_l({\mathcal{L}}_k)+m_r({\mathcal{L}}_k)>{\tau }_m$ then
20 break

Problem (13) is an extended coverage problem, which involves multiobjective optimization. To solve this, we introduce a novel multirobot landmark selection (MLS) algorithm, detailed in Algorithm 2. Within this algorithm, we use the following cost function to approximate (14):

where $m_l(l)$ and $m_r(l)$ represent the number of measurements of landmark l from local and remote keyframes, respectively, and $o(l)$ is the count of outlier measurements for l.

To select landmarks, our algorithm initially sets up dictionary ${\mathcal{N}}_l$ to track each landmark’s current measurement count and dictionary ${\mathcal{O}}_a$ to record unselected landmarks observed by each frame. Each iteration begins by identifying the keyframe with the fewest measurements in the sliding window (line 9), aiming to approximate (15). It then selects a landmark l observed by this keyframe that maximizes (19). Subsequently, the algorithm updates ${\mathcal{N}}_l$ , incrementing $l{\mathrm{}}^{\circ }\mathrm{s}$ measurement count for each corresponding keyframe in l tracks. The selected landmark l is added to ${\mathcal{L}}_k$ , and ${\mathcal{O}}_a$ is updated to exclude l from each keyframe’s observed landmarks. The process repeats until the number of selected landmarks surpasses ${\tau }_l.$ , the count of selected measurements reaches ${\tau }_m$ , or no more measurements are available. The resulting set of selected landmarks is ${\mathcal{L}}_k$

Given the occasional scarcity of common landmarks among robots, landmarks may be reused across different robots in $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ , a situation that arises when (18) is not fully met. This reuse increases the computational burden and can slightly compromise accuracy, yet it is vital for the system’s robustness. Our experiments have shown that the system can become unstable or even diverge when measurements per frame are few, necessitating a compromise in Algorithm 2 for stability. In practice, we set $w_l=1$ and $w_r=2$ to encourage the addition of more cross-robot feature points and $w_o=2$ to reduce outliers, identified by large reprojection errors prior to optimization.

D. Sliding Window & Marginalization

1) Sliding Window: In D²VINS, each UAV independently manages its keyframes within a sliding window, akin to the approach in [12]. Upon adding a new frame, if it is not a keyframe, the second newest frame is discarded; otherwise, the oldest keyframe is removed. Following each update of the sliding window, the latest information is shared with other UAVs in the swarm.

2) Management of Remote Keyframes: Successful multirobot sparse feature matching, as detailed in Section V-D, results in the inclusion of the remote frame’s pose $v_{{}^k}{\widehat{\mathbf{T}}}_i^t$ into the local state vector ${\mathcal{X}}_i$ for optimization in Problem (8). Upon receiving updated sliding window data from other UAVs, $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ removes any remote keyframes from UAV k that are no longer within its local sliding window. This removal, akin to local keyframe deletion, leads to marginalization, which will be discussed further.

3) Marginalization: When discarding old keyframes, each subproblem $f_{{\mathrm{c}\mathrm{v}\mathrm{i}\mathrm{o}}_i}(\mathcal{X}i)$ ofProblem (8) is linearized, and a new set of priors ${ ( \mathbf { r } _ { p _ { 0 } } , \mathbf { H } _ { p _ { 0 } } ) , ( \mathbf { r } _ { p _ { 1 } } , \mathbf { H } _ { p _ { 1 } } ) \cdot \cdot \cdot ( \mathbf { r } _ { p _ { N - 1 } } , \mathbf { H } _ { p _ { N - 1 } } ) }$ is computed using the Schur complement to exclude old states. This process, known as marginalization [12], [60], treats each subproblem independently and is therefore executed distributively. Since Problem (8) involves states averaged across UAVs (11), the marginalization process maintains consistency across UAVs, meaning states are linearized at similar or identical points on different UAVs.

E. Initialization

D²VINS initialization involves two phases: 1) local keyframe initialization and 2) remote UAV initialization. For local keyframes, utilizing stereo or multicamera setups, we employ triangulation for landmark position initialization where measurements have a sufficient baseline (typically 0.05 cm) from multiple cameras or due to motion. In addition, IMU predictions initialize poses of subsequent local UAV Keyframes. To enhance stability, priors are added to the unobservable components $(x,y,z,yaw)$ of the first keyframe. In multi-UAV scenarios, PnP RANSAC (or UPnP) is used to initialize the pose of remote keyframes when their coordinate system references differ from the local UAV.

F. Implementation

$D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ solves Problem (9) using Ceres-solver [61], applying a Dogleg strategy [62] and a dense Schur solver. For enforcing pose manifold constraints, we utilize the Local $\mathrm{P}\mathrm{a}-$ rameterization feature in Ceres, essentially implementing the RTR method [27].

Algorithm 1, due to (11), operates synchronously, requiring UAVs to wait for others’ results each iteration, which can be less efficient in poor communication environments. However, we have found that relaxing the synchronization condition of (11)—using the most recently received state averages without waiting—also produces results close to the synchronous approach. We label this method as asynchronous $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ , and a comparative analysis with its synchronous counterpart will be presented in Section IX-B.

VII. FAR-FIELD STATE ESTIMATION: D²PGO

Far-field estimation in $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ encompasses both global trajectory consistency $(D^2\mathrm{P}\mathrm{G}\mathrm{O})$ and real-time global odometry. We model the global trajectory estimation using a pose graph approach [63], where keyframes are graph nodes and edges represent relative poses between them. This pose graph is formulated as a factor graph, treating measurements as Gaussiandistributed, thus constituting the classic PGO problem [19]. $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ employs a two-stage optimization process to decentralize and distribute the PGO across multiple UAVs. The PGO problem is

where $\mathcal{E}$ is the set of all edge, including loop closure edge (generated by loop closure detection) and ego-motion edge (generated by $D ^ { 2 } \dot { \mathbf { V } } \mathbf { I N S } ) , \mathbf { z } _ { \mathbf { p } } { } _ { i j } ^ { t _ { 0 } t _ { 1 } } , \mathbf { z } _ { \mathbf { R } } { } _ { i j } ^ { t _ { 0 } t _ { 1 } }$aretherotationpartandtranslationpartoftherelativeposeoftheedge$( i _ { t _ { 0 } } , j _ { t _ { 1 } } )$respectively.${ \boldsymbol { v } } _ { k } \mathbf { R } _ { i } ^ { t }$istherotationpartof$v _ { k } \hat { \mathbf { T } } _ { i } ^ { t }$,and$\boldsymbol { v } _ { k } _ { \mathbf { p } _ { i } ^ { t } }$isthetranslationpartof$\boldsymbol { v } _ { k } \hat { \mathbf T } _ { i } ^ { t }$ , x is the full state of the pose graph

where $N$ is the number of UAVs, M is the number of keyframes of each UAV.

By reformulating the PGO problem (20) into a distributed framework (3), the equation becomes

where $x_i$ is the state of the UAV $i,{\mathbf{E}}_{i_t}^{\mathbf{R}}\mathbf{z},{\mathbf{E}}_{i_t}^p$ project global state z to local states, ${\mathcal{E}}^i$ is the set of edges of the UAV i.

$D^2\mathrm{P}\mathrm{G}{\mathrm{O}}^{\circ }\mathrm{s}$ initial stage employs rotation initialization for the pose graph to circumvent local minima. The second stage focuses on refining the PGO. Beyond optimization, an outlier rejection module is implemented to filter anomalies in the pose graph problem (20). The final step involves merging the PGO results with the VIO results to establish each ${\mathrm{U}\mathrm{A}\mathrm{V}}_{\mathrm{\Delta }}$ globally consistent real-time odometry

where $v_k{\widehat{\mathbf{T}}}_i^t$ denotes the latest keyframe pose of UAV i in $D^2\mathrm{P}\mathrm{G}\mathrm{O},{\tilde{\mathbf{T}}}_i^t$ represents the odometry estimation from $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ for this keyframe, and ${\tilde{\mathbf{T}}}_i^{t_1}$ is the real-time odometry estimation with $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$

A. ARock for Distributed Optimization

To solve the PGO, we employ ARock [16], an asynchronous distributed optimization algorithm. ARock iteratively updates the distributed optimization problem (3), as described in [16]

where $\mathcal{R}(i)$ represents the set of neighbors for agent $i,(·{)}^t$ denotes the state in the tth iteration, and ${\eta }_k$ and γ are two parameters. Dual variables $z_{ij,i}$ and $z_{ij,j}$ for agents i and j are updated independently by each, and $z_{ij,i}=z_{ji,i,}{z}_{ij,j}=z_{ji,j}.$ We modify the update rule by substituting dual variables with $y_{ij,i}=-z_{ij,i/\gamma }$ and $y_{ij,j}=-z_{ij,j}/\gamma$ to obtain

The complete ARock algorithm for decentralized optimization is outlined in Algorithm 3. Unlike the synchronous Algorithm 1, ARock operates asynchronously, utilizing the latest received dual variables from remote agents without the need for synchronous execution as per (26) and (27), making it less sensitive to communication delays. In addition, ARock is proven to have linear convergence [16].

B. Asynchronous Distributed Rotation Initialization

Nonlinearities in the pose graph problem primarily arise from rotations. With proper rotation initialization, the problem closely resembles a linear least squares issue, simplifying its solution. This technique of rotation initialization is known to effectively avoid local minima in PGO [64]. The goal of this initialization is to find an exact or approximate solution for the rotational component of Problem (20), a task akin to solving the rotation

Algorithm 3: ARock for Distributed Optimization.
Input: n UAVs, m landmarks, iter iterations.
1 Initialize $y_{ij,i}^0,y_{ij,j}^0$ for all $i,j$ to 0.
2 for $t=1$ to iter do
3 Solve the local optimization problem (26)
4 Update the dual states $y_{ri,i}^t$ with (27).
5 Broadcast the dual state $y_{ri,i}^t$ to all UAVs
6 if Convergence then
7 Break

averaging problem [65]

Chordal relaxation [66] is an effective algorithm for solving the rotation initialization problem. It starts by relaxing the SO(3) constraint in (28)

where ${\mathcal{R}}_i$ denotes all rotations in ${\mathbf{x}}_i,$ and ${\bar{\mathbf{R}}}_i^t$ is a $3\times 3$ matrix representing the rotation of UAV i at time t. The term ${\Vert }^{v_k}({\bar{\mathbf{R}}}_i^t{)}_3-{\mathbf{v}}_i{\Vert }_{{\Sigma }_a}$ is the vertical prior, added because the rollpitch angle from VIO is observable and can be used as a prior, enhancing initialization [64]. Here, $v_k({\bar{\mathbf{R}}}_i^t{)}_3$ is the third row of ${\bar{\mathbf{R}}}_i^t,{\mathbf{v}}_i=({\tilde{\mathbf{R}}}_i^t{)}^T\mathbf{g}$ with $g=[0,0,1{]}^T$ , and ${\tilde{\mathbf{R}}}_i^t$ is the odometry output rotation of UAV i at time t. Problem (29) is a linear least squares problem that can be efficiently solved with a linear solver. Then, we can recover the rotation matrix by

where $\Vert ·{\Vert }_F$ is the Frobenius norm. Problem (30) has a closedform solution with singular value decomposition (SVD) [65]

where ${\mathit{v}}_k{\breve{\mathbf{R}}}_i^t\in SO(3)$ is the initial result of $v_k{\widehat{\mathbf{R}}}_i^t$ , and $\mathbf{S}\mathbf{D}{\mathbf{V}}^{\mathbf{T}}$ is the SVD decomposition of ${\bar{\mathbf{R}}}_i^t$

In $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ , we solve Problem (29) distributively, redefining it as

Algorithm 4: Asynchronous Distributed Rotation Initializa
tion for UAV i.
Input: ε for termination, max_iter iterations,
min_iter to avoid early exiting
1 Initialize $y_{ij,i}^0,y_{ij,j}^0$ for all $i,j$ to 0.
2 for t = 1 to max iter do
3 Solve the local optimization problem with $f_i$
defined from (32)
4 Update the dual states $y_{ir,i}^t$ with Eq. (27).
5 Broadcast the dual state $y_{ir,i}^t$ to neighbors
6 if $\begin{array} { r } { \frac { 1 } { | \mathcal { R } _ { i } | } \sum _ { \overline { { \mathbf { R } } } \in \mathcal { R } _ { i } } | ( \overline { { \mathbf { R } } } ) ^ { t } - ( \overline { { \mathbf { R } } } ) ^ { t - 1 } | _ { F } / | ( \overline { { \mathbf { R } } } ) ^ { t } | _ { F } < \epsilon } \end{array}$
and iter > min iter then
7 break
8 for $\bar{R}\in {\mathcal{R}}_i$ do
9 Recover the rotation $v_k{\widehat{\mathbf{R}}}_i^t$ with (31)

Algorithm 5: Asynchronous Distributed Pose Graph Opti
mization for UAV i.
1 while Not shutdown do
2 if New data received then
3 Adding new states or cost functions to
optimizer.
4 Solve the local optimization problem (26) with $f_i$
defined from (33)
5 Update the dual states $y_{ir,i}^t$ with (27).
6 Broadcast the dual state $y_{ir,i}^t$ to neighbors
7 Recover the poses with (34)
8 Sleep for a while

Utilizing Algorithm 3, we address problem (32) through our asynchronous distributed rotation initialization algorithm, detailed in Algorithm 4. The local optimization problem within Algorithm 4 [converting (32) into (26)] is a linear least squares problem, efficiently solvable via Cholesky factorization [67]. Iteration termination occurs when changes in the optimized state become negligible, as indicated in Algorithm 4 line 6, with $(\bar{\mathbf{R}}{)}^t$ representing the post-t-iteration state. In addition, to avoid premature termination (e.g., due to initial data absence), a minimum number of iterations is also set.

C. Asynchronous Distributed Pose Graph Optimization

After completing the initialization, we build the perturbation problem of (22) with the initialization rotation

\begin{array} { r l } & { \displaystyle \operatorname* { m i n } _ { { \bf x } _ { i } ^ { ( 1 ) } } \sum _ { i = 0 } ^ { N - 1 } f _ { { \bf p } _ { 0 } ^ { ( 2 ) } } ^ { * } ( { \bf x } _ { i } ^ { * } ) } \\ & { \displaystyle \int _ { { \bf p } _ { 0 } \bf { s } _ { i } } ^ { * } ( { \bf x } _ { i } ^ { * } ) = \sum _ { ( i _ { 0 } , j _ { i _ { 1 } } ) \in \mathcal { E } ^ { I } } \| ^ { n _ { k } } \hat { \bf p } _ { j } ^ { t _ { 1 } } - { \boldsymbol \nu } _ { k } ^ { n _ { k } } \hat { \bf p } _ { i } ^ { t _ { 0 } } - } \\ & { \displaystyle \mathbb { E } ( { \bf \mu } ^ { v _ { k } } \tilde { \bf R } _ { i } ^ { t _ { 0 } } , { \boldsymbol \nu } ^ { u } \delta \theta _ { i } ^ { t } ) { \bf z } _ { { \bf p } _ { i } - j _ { i } } ^ { v _ { i } \in \mathcal { I } _ { 1 } } \| _ { { { \bf D } _ { p } } } ^ { 2 } + } \\ & { \displaystyle \left\| \mathbb { E } ( { \bf \mu } ^ { v _ { k } } \tilde { \bf R } _ { j } ^ { t _ { 1 } } , { \boldsymbol \nu } ^ { u } \delta \theta _ { j } ^ { t _ { 1 } } ) - \mathbb { E } \| ^ { ( v _ { k } } \tilde { \bf R } _ { i } ^ { t _ { 0 } } , { \boldsymbol \nu } ^ { v _ { k } } \delta \theta _ { i } ^ { t _ { 1 } } ) { \bf z } _ { { \bf R } _ { i } - j _ { j } } ^ { t _ { 0 } \to \bar { t } _ { 1 } } \right\| _ { { { \bf D } _ { R } } } ^ { 2 } } \\ &  \displaystyle \quad \times { \bf { I } } ^ { v _ { k } } \delta \theta _ { i } ^ { t } = { \bf E } _ { { \bf { k } } _ { i } ^ { n } } { \bf { ' } } ^ { * } { \boldsymbol \sigma } ^  \end{array}

where $({}^{v_k}{\widehat{\mathbf{p}}}_i^{t_0},{}^{v_k}\delta {\theta }_i^{t_0})$ is the perturbation state of $v_k{\mathbf{\Delta }\mathbf{T}}_i^{t_0}$ , where ${\mathit{v}}_k{\widehat{\mathbf{p}}}_i^{t_0}$ is the position of ${v_k}_{{\mathbf{T}}_i^{t_0}}$ and $v_k\delta {\theta }_i^{t_0}\in$ so3 is the perturbation state of the rotation. ${\mathit{x}}_i^{*}$ is the the perturbation state of ${\mathbf{x}}^i$ and ${\mathbf{z}}^{*}$ is the global perturbation state, $\mathbb { E } ( { \mathbf { R } } , \delta { \boldsymbol { \theta } } ) = { \mathbf { R } } E x p ( \delta { \boldsymbol { \theta } } _ { i } ^ { t _ { 0 } } )$ retracts so3 vector to SO(3) at R [27], where Exp(·) maps so3 to SO(3) [68]. Contrasting with the two-step PGO approach in [4, eq. (10)], our Problem (33) is nonlinear and circumvents the accuracy issues associated with the linearization used in [4].

Our algorithm employs Algorithm 3 to address (33), ensuring asynchronicity. Transforming $f_{{\mathrm{p}\mathrm{g}\mathrm{o}}_i}^{*}({\mathbf{x}}_i^{*})$ per (26) yields a nonlinear least squares problem, solvable via the Levenberg– Marquardt algorithm [69]. Convergence is determined by observing changes in the cost function of $f_{{\mathrm{p}\mathrm{g}\mathrm{o}}_i}^{*}({\mathbf{x}}_i^{*})$ . As in $\mathrm{A}\mathrm{l}-$ gorithm 4, a minimum iteration count is set to avoid premature termination. After convergence, we recover the global pose with

Our asynchronous distributed PGO (ARockPGO) is outlined in Algorithm 5. Unlike Algorithm 4, ARockPGO operates continuously throughout UAV flight, updating at a fixed frequency (typically 1 Hz) and incrementally integrating new information. This approach balances convergence speed against communication load.

D. Convergence

ARock demonstrates strong convergence performance, suggesting it can potentially reach optimality in the initial stage of linear least squares problems. The effectiveness of rotation initialization in preventing convergence to incorrect local optima [64] enhances $D^2{\mathrm{P}\mathrm{G}\mathrm{O}}^{\circ }\mathrm{s}$ ability to achieve or closely approximate the global optimum.

E. Outlier Rejection

Currently, the mainstream outlier rejection methods for multirobot PGO include pairwise consistent measurement set maximization (PCM) [1], [3], [70] and graduated nonconvexity (GNC) [71]. PCM, independent ofback-end optimization, is efficient but less effective with large pose graphs. Conversely, GNC is integrated into back-end optimization, offering higher efficiency but operating synchronously. In $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ , we adopt a distributed version of PCM as used in [1] and [3], mitigating PCM’s efficiency issues at scale. We believe this approach provides adequate performance for PGO that is not real-time critical.

F. Implementation

The initial stage of $D^2\mathrm{P}\mathrm{G}\mathrm{O}$ is designed to ensure accurate trajectory estimation even in highly noisy environments. However, in real-world applications, this stage is often unnecessary, as VIO’s yaw measurements are typically precise, allowing for its omission in practice.

$D^2\mathrm{P}\mathrm{G}\mathrm{O}$ is implemented in C++, utilizing Eigen [72]’s LLT algorithm for the linear least squares calculation in rotation initialization. For the second stage, we employ the Levenberg– Marquardt algorithm from Ceres-solver [61].

Fig. 10. Our aerial platform equipped with quad fisheye cameras, an onboard computer, an all-in-one flight controller, an additional Intel RealSense d435i camera, and a 6-s 18650 battery.

VIII. SYSTEM SETUP

Our experimental aerial swarm is based on custom-modified aerial robots, as depicted in Fig. 10. Each UAV is a modified 3.5-inch cinewhoop FPV UAV, equipped with a quartet of fisheye cameras providing 360° horizontal visibility. They are powered by NVIDIA Xavier NX onboard computers, boasting robust GPU capabilities. To minimize takeoff weight, we use a lightweight, integrated flight controller with four electronic speed controls and customized PX4 [74] firmware. The UAV’s takeoff weight is approximately 623 g with a 22.8v 1550-mah battery, offering a maximum endurance of about 10 min, which suffices for our tests. Optionally, an Intel RealSense d435i camera can be mounted on the UAV.

$D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ is implemented in C++ and utilizes ONNX Runtime [75] for CNN inference. We activate TensorRT for enhanced performance, employing the int8 mode on the onboard computer to optimize speed. In addition, we develop TaichiSLAM, a GPU-accelerated mapping module, serving as the dense mapping back end for later experiments. TaichiSLAM, crafted in the high-performance Taichi language [76], integrates various algorithms like OctoMap [77], TSDF & ESDF [78], and submap fusion [21] to create globally consistent maps. However, as dense mapping algorithms are not the primary focus of $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ , they are not elaborated in detail.

IX. EXPERIMENTS

A. Datasets & Evaluation Setup

In the evaluation presented in this section, we test $D^2\mathrm{S}\mathrm{L}\mathrm{A}\mathrm{M}$ using various publicly available and customized datasets. These datasets use either stereo cameras or quad cameras as input and partly used a motion capture system to capture ground truth data, as shown in Table I. To simulate the scenario of multiple robots operating simultaneously, we align multiple segments of data into a single multirobot dataset.

Details on the evaluation metrics are available in [1]. Notably, the term relative error (RE) in this context refers to the accuracy of relative state estimation among UAVs. In the following tables, position measurements are in meters and rotation in degrees.

In real-time scenarios, we do not require distributed optimization to fully converge, similar to some single-robot SLAM systems like VINS-Mono [12], which typically limits backend optimization to just eight iterations. Synchronous $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ performs four iterations per solution of (9)–(12), while asynchronous $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ requires only a single run of these equations per optimization. Given that $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ continuously integrates new data and commences optimization from the previous results, with most of the states from the last optimization still included in ${\mathcal{X}}_i,(9)-(12)$ effectively experience multiple iterations over time in the asynchronous $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ framework. $D^2\mathrm{P}\mathrm{G}{\mathrm{O}}^{\circ }\mathrm{s}$ handling in real-time systems is similar to asynchronous $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$

TABLE I
DATASETS USED FOR VALIDATION

Numbers added as suffix to the datasets correspond to the number of robots involved.

TABLE II
STATISTICAL COMPARISON OF $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ AGAINST VINS-MONO AND D²VINS (SINGLE) ON THE TUM VI ROOM DATASET, WHERE ATE AND RE ARE DEFINED AS [1]

These datasets exhibit good common FoV during the initial 30 s, hence the results are separately presented for this duration, marked as (30 s) The overall best results in ATE and RE are in bold

B. Evaluation of $D^{2}VINS$

Our initial tests focused on $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ to assess its near-field state estimation performance. These tests did not rely on ground truth for initializing $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ . We also compared $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ with its single-robot variant, referred to as $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ (single), and with the state-of-the-art VIO approach, VINS-Mono [12]. For alignment, VINS-Mono and $D^2\mathrm{V}\mathrm{I}\mathrm{N}\mathrm{S}$ (single) are synchronized using the initial pose from ground truth, mirroring the known UAV departure points setup used in [10].