Pose-graph Simultaneous Localization And Mapping Research Articles

Cramér–Rao Bounds and Optimal Design Metrics for Pose-Graph SLAM

Two-dimensional (2-D)/3-D pose-graph simultaneous localization and mapping (SLAM) is a problem of estimating a set of poses based on noisy measurements of relative rotations and translations. This article focuses on the relation between the graphical structure of pose-graph SLAM and Fisher information matrix (FIM), Cramer–Rao lower bounds (CRLB), and its optimal design metrics (T-optimality and D-optimality). As a main contribution, based on the assumption of isotropic Langevin noise for rotation and block-isotropic Gaussian noise for translation, the FIM and CRLB are derived and shown to be closely related to the graph structure, in particular, the weighted Laplacian matrix. We also prove that total node degree and weighted number of spanning trees, as two graph connectivity metrics, are, respectively, closely related to the trace and determinant of the FIM. The discussions show that, compared with the D-optimality metric, the T-optimality metric is more easily computed but less effective. We also present upper and lower bounds for the D-optimality metric, which can be efficiently computed and are almost independent of the estimation results. The results are verified with several well-known datasets, such as Intel, KITTI, sphere, and so on.

Acoustic Camera-Based Pose Graph SLAM for Dense 3-D Mapping in Underwater Environments

In this article, a novel dense underwater 3-D mapping paradigm based on pose graph simultaneous localization and mapping (SLAM) using an acoustic camera mounted on a rotator is proposed. The demands of underwater tasks, such as unmanned construction using robots, are growing rapidly. In recent years, the acoustic camera, which is a state-of-the-art forward-looking imaging sonar, has been gradually applied in underwater exploration. However, distinctive imaging principles make it difficult to gain an intuitive perception of an underwater environment. In this study, an acoustic camera with a rotator was used for dense 3-D mapping of the underwater environment. The proposed method first applies a 3-D occupancy mapping framework based on the acoustic camera rotating around the acoustic axis using a rotator at a stationary position to generate 3-D local maps. Then, scan matching of adjacent local maps is implemented to calculate odometry without involving internal sensors, and an approximate dense global map is built in real time. Finally, based on a graph optimization scheme, offline refinement is performed to generate a final dense global map. Our experimental results demonstrate that our 3-D mapping framework for an acoustic camera can achieve dense 3-D mapping of underwater environments robustly and accurately.

Range-Aided Pose-Graph-Based SLAM: Applications of Deployable Ranging Beacons for Unknown Environment Exploration

Simultaneous Localization and Mapping (SLAM) is a critical part of robotic exploration in unknown environments. SLAM over large scales typically presents challenges with limiting drift and requires loop closures to correct accumulated errors. However, real-time loop closure detection can be limited by high computational cost and susceptibility to outliers, especially in perceptually challenging, unknown, and large-scale environments. One solution to these challenges is to deploy ranging beacons from a robot and to utilize them for unambiguous place recognition. In this letter, we develop an architecture to take advantage of deployable ranging beacons in pose-graph SLAM. First, we highlight the inherent ambiguity in range measurements for sparsely deployed ranging beacons by analyzing the challenges in applying conventional methods of processing range measurements for localization. To both handle this ambiguity and address the challenges of loop closure for large-scale, perceptually challenging environments, we propose range-aided loop closure (RA-LC), an algorithm that combines beacon-based place recognition with geometric loop closures. We implement RA-LC in our hardware system and demonstrate the efficiency of RA-LC in field tests. Our results show that RA-LC can achieve equivalent accuracy of geometric-only lidar-based loop closure with significantly lower computational cost (95% reduction) and eight-times fewer outliers.

Broadcast Your Weaknesses: Cooperative Active Pose-Graph SLAM for Multiple Robots

In this letter, we propose a low-cost, high-efficiency framework for cooperative active pose-graph simultaneous localization and mapping (SLAM) for multiple robots in three-dimensional (3D) environments based on graph topology. Based on the selection of weak connections in pose graphs, this method aims to find the best trajectories for optimal information exchange to repair these weaknesses opportunistically when robots move near them. Based on tree-connectivity, which is greatly related to the D-optimality metric of the Fisher information matrix (FIM), we explore the relationship between measurement (edge) selection and pose-measurement (node-edge) selection, which often occurs in active SLAM, in terms of information increment. The measurement selection problem is formulated as a submodular optimization problem and solved by an exhaustive method using rank-1 updates. We decide which robot takes the selected measurements through a bidding framework where each robot computes its predicted cost. Finally, based on a novel continuous trajectory optimization method, these additional measurements collected by the winning robot are sent to the requesting robot to strengthen its pose graph. In simulations and experiments, we validate our approach by comparing against existing methods. Further, we demonstrate online communication based on offline planning results using two unmanned aerial vehicles (UAVs).

Active SLAM for Autonomous Underwater Exploration

Exploration of a complex underwater environment without an a priori map is beyond the state of the art for autonomous underwater vehicles (AUVs). Despite several efforts regarding simultaneous localization and mapping (SLAM) and view planning, there is no exploration framework, tailored to underwater vehicles, that faces exploration combining mapping, active localization, and view planning in a unified way. We propose an exploration framework, based on an active SLAM strategy, that combines three main elements: a view planner, an iterative closest point algorithm (ICP)-based pose-graph SLAM algorithm, and an action selection mechanism that makes use of the joint map and state entropy reduction. To demonstrate the benefits of the active SLAM strategy, several tests were conducted with the Girona 500 AUV, both in simulation and in the real world. The article shows how the proposed framework makes it possible to plan exploratory trajectories that keep the vehicle’s uncertainty bounded; thus, creating more consistent maps.

Pose-graph SLAM sparsification using factor descent

Since state of the art simultaneous localization and mapping (SLAM) algorithms are not constant time, it is often necessary to reduce the problem size while keeping as much of the original graph’s information content. In graph SLAM, the problem is reduced by removing nodes and rearranging factors. This is normally faced locally: after selecting a node to be removed, its Markov blanket sub-graph is isolated, the node is marginalized and its dense result is sparsified. The aim of sparsification is to compute an approximation of the dense and non-relinearizable result of node marginalization with a new set of factors. Sparsification consists on two processes: building the topology of new factors, and finding the optimal parameters that best approximate the original dense distribution. This best approximation can be obtained through minimization of the Kullback–Liebler divergence between the two distributions. Using simple topologies such as Chow–Liu trees, there is a closed form for the optimal solution. However, a tree is oftentimes too sparse and produces bad distribution approximations. On the contrary, more populated topologies require nonlinear iterative optimization. In the present paper, the particularities of pose-graph SLAM are exploited for designing new informative topologies and for applying the novel factor descent iterative optimization method for sparsification. Several experiments are provided comparing the proposed topology methods and factor descent optimization with state-of-the-art methods in synthetic and real datasets with regards to approximation accuracy and computational cost.

Robust GICP-Based 3D LiDAR SLAM for Underground Mining Environment.

Unmanned mining is one of the most effective methods to solve mine safety and low efficiency. However, it is the key to accurate localization and mapping for underground mining environment. A novel graph simultaneous localization and mapping (SLAM) optimization method is proposed, which is based on Generalized Iterative Closest Point (GICP) three-dimensional (3D) point cloud registration between consecutive frames, between consecutive key frames and between loop frames, and is constrained by roadway plane and loop. GICP-based 3D point cloud registration between consecutive frames and consecutive key frames is first combined to optimize laser odometer constraints without other sensors such as inertial measurement unit (IMU). According to the characteristics of the roadway, the innovative extraction of the roadway plane as the node constraint of pose graph SLAM, in addition to automatic removing the noise point cloud to further improve the consistency of the underground roadway map. A lightweight and efficient loop detection and optimization based on rules and GICP is designed. Finally, the proposed method was evaluated in four scenes (such as the underground mine laboratory), and compared with the existing 3D laser SLAM method (such as Lidar Odometry and Mapping (LOAM)). The results show that the algorithm could realize low drift localization and point cloud map construction. This method provides technical support for localization and navigation of underground mining environment.

Reliable Graphs for SLAM

Estimation-over-graphs (EoG) is a class of estimation problems that admit a natural graphical representation. Several key problems in robotics and sensor networks, including sensor network localization, synchronization over a group, and simultaneous localization and mapping (SLAM) fall into this category. We pursue two main goals in this work. First, we aim to characterize the impact of the graphical structure of SLAM and related problems on estimation reliability. We draw connections between several notions of graph connectivity and various properties of the underlying estimation problem. In particular, we establish results on the impact of the weighted number of spanning trees on the D-optimality criterion in 2D SLAM. These results enable agents to evaluate estimation reliability based only on the graphical representation of the EoG problem. We then use our findings and study the problem of designing sparse SLAM problems that lead to reliable maximum likelihood estimates through the synthesis of sparse graphs with the maximum weighted tree connectivity. Characterizing graphs with the maximum number of spanning trees is an open problem in general. To tackle this problem, we establish several new theoretical results, including the monotone log-submodularity of the weighted number of spanning trees. We exploit these structures and design a complementary greedy–convex pair of efficient approximation algorithms with provable guarantees. The proposed synthesis framework is applied to various forms of the measurement selection problem in resource-constrained SLAM. Our algorithms and theoretical findings are validated using random graphs, existing and new synthetic SLAM benchmarks, and publicly available real pose-graph SLAM datasets.

Linear SLAM: Linearising the SLAM problems using submap joining

The main contribution of this paper is a new submap joining based approach for solving large-scale Simultaneous Localization and Mapping (SLAM) problems. Each local submap is independently built using the local information through solving a small-scale SLAM; the joining of submaps mainly involves solving linear least squares and performing nonlinear coordinate transformations. Through approximating the local submap information as the state estimate and its corresponding information matrix, judiciously selecting the submap coordinate frames, and approximating the joining of a large number of submaps by joining only two maps at a time, either sequentially or in a more efficient Divide and Conquer manner, the nonlinear optimization process involved in most of the existing submap joining approaches is avoided. Thus the proposed submap joining algorithm does not require initial guess or iterations since linear least squares problems have closed-form solutions. The proposed Linear SLAM technique is applicable to feature-based SLAM, pose graph SLAM and D-SLAM, in both two and three dimensions, and does not require any assumption on the character of the covariance matrices. Simulations and experiments are performed to evaluate the proposed Linear SLAM algorithm. Results using publicly available datasets in 2D and 3D show that Linear SLAM produces results that are very close to the best solutions that can be obtained using full nonlinear optimization algorithm started from an accurate initial guess. The C/C++ and MATLAB source codes of Linear SLAM are available on OpenSLAM.

SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group

Many important geometric estimation problems naturally take the form of synchronization over the special Euclidean group: estimate the values of a set of unknown group elements given noisy measurements of a subset of their pairwise relative transforms . Examples of this class include the foundational problems of pose-graph simultaneous localization and mapping (SLAM) (in robotics), camera motion estimation (in computer vision), and sensor network localization (in distributed sensing), among others. This inference problem is typically formulated as a non-convex maximum-likelihood estimation that is computationally hard to solve in general. Nevertheless, in this paper we present an algorithm that is able to efficiently recover certifiably globally optimal solutions of the special Euclidean synchronization problem in a non-adversarial noise regime. The crux of our approach is the development of a semidefinite relaxation of the maximum-likelihood estimation (MLE) whose minimizer provides an exact maximum-likelihood estimate so long as the magnitude of the noise corrupting the available measurements falls below a certain critical threshold; furthermore, whenever exactness obtains, it is possible to verify this fact a posteriori, thereby certifying the optimality of the recovered estimate. We develop a specialized optimization scheme for solving large-scale instances of this semidefinite relaxation by exploiting its low-rank, geometric, and graph-theoretic structure to reduce it to an equivalent optimization problem defined on a low-dimensional Riemannian manifold, and then design a Riemannian truncated-Newton trust-region method to solve this reduction efficiently. Finally, we combine this fast optimization approach with a simple rounding procedure to produce our algorithm, SE-Sync. Experimental evaluation on a variety of simulated and real-world pose-graph SLAM datasets shows that SE-Sync is capable of recovering certifiably globally optimal solutions when the available measurements are corrupted by noise up to an order of magnitude greater than that typically encountered in robotics and computer vision applications, and does so significantly faster than the Gauss–Newton-based approach that forms the basis of current state-of-the-art techniques.

Exactly sparse delayed state filter on Lie groups for long-term pose graph SLAM

In this paper we propose a simultaneous localization and mapping (SLAM) back-end solution called the exactly sparse delayed state filter on Lie groups (LG-ESDSF). We derive LG-ESDSF and demonstrate that it retains all the good characteristics of the classic Euclidean ESDSF, the main advantage being the exact sparsity of the information matrix. The key advantage of LG-ESDSF in comparison with the classic ESDSF lies in the ability to respect the state space geometry by negotiating uncertainties and employing filtering equations directly on Lie groups. We also exploit the special structure of the information matrix in order to allow long-term operation while the robot is moving repeatedly through the same environment. To prove the effectiveness of the proposed SLAM solution, we conducted extensive experiments on two different publicly available datasets, namely the KITTI and EuRoC datasets, using two front-ends: one based on the stereo camera and the other on the 3D LIDAR. We compare LG-ESDSF with the general graph optimization framework ( g 2 o ) when coupled with the same front-ends. Similarly to g 2 o the proposed LG-ESDSF is front-end agnostic and the comparison demonstrates that our solution can match the accuracy of g 2 o , while maintaining faster computation times. Furthermore, the proposed back-end coupled with the stereo camera front-end forms a complete visual SLAM solution dubbed LG-SLAM. Finally, we evaluated LG-SLAM using the online KITTI protocol and at the time of writing it achieved the second best result among the stereo odometry solutions and the best result among the tested SLAM algorithms.

SOFT‐SLAM: Computationally efficient stereo visual simultaneous localization and mapping for autonomous unmanned aerial vehicles

AbstractAutonomous navigation of unmanned aerial vehicles (UAVs) in GPS‐denied environments is a challenging problem, especially for small‐scale UAVs characterized by a small payload and limited battery autonomy. A possible solution to the aforementioned problem is vision‐based simultaneous localization and mapping (SLAM), since cameras, due to their dimensions, low weight, availability, and large information bandwidth, circumvent all the constraints of UAVs. In this paper, we propose a stereo vision SLAM yielding very accurate localization and a dense map of the environment developed with the aim to compete in the European Robotics Challenges (EuRoC) targeting airborne inspection of industrial facilities with small‐scale UAVs. The proposed approach consists of a novel stereo odometry algorithm relying on feature tracking (SOFT), which currently ranks first among all stereo methods on the KITTI dataset. Relying on SOFT for pose estimation, we build a feature‐based pose graph SLAM solution, which we dub SOFT‐SLAM. SOFT‐SLAM has a completely separate odometry and mapping threads supporting large loop‐closing and global consistency. It also achieves a constant‐time execution rate of 20 Hz with deterministic results using only two threads of an onboard computer used in the challenge. The UAV running our SLAM algorithm obtained the highest localization score in the EuRoC Challenge 3, Stage IIa–Benchmarking, Task 2. Furthermore, we also present an exhaustive evaluation of SOFT‐SLAM on two popular public datasets, and we compare it to other state‐of‐the‐art approaches, namely ORB‐SLAM2 and LSD‐SLAM. The results show that SOFT‐SLAM obtains better localization accuracy on the majority of datasets sequences, while also having a lower runtime.

A Sparse Separable SLAM Back-End

We propose a scalable algorithm to take advantage of the separable structure of simultaneous localization and mapping (SLAM). Separability is an overlooked structure of SLAM that distinguishes it from a generic nonlinear least-squares problem. The standard relative-pose and relative-position measurement models in SLAM are affine with respect to robot and features’ positions. Therefore, given an estimate for robot orientation, the conditionally optimal estimate for the rest of the state variables can be easily computed by solving a sparse linear least-squares problem. We propose an algorithm to exploit this intrinsic property of SLAM by stripping the problem down to its nonlinear core, while maintaining its natural sparsity. Our algorithm can be used in conjunction with any Newton-based solver and is applicable to 2-D/3-D pose-graph and feature-based SLAM. Our results suggest that iteratively solving the nonlinear core of SLAM leads to a fast and reliable convergence as compared to the state-of-the-art sparse back-ends.

Magnetic field constraints and sequence-based matching for indoor pose graph SLAM

The objective of pose graph optimization is to estimate the robot trajectory from the constraints of relative pose measurements. Since the magnetic field in indoor environments is stable in the temporal domain and sufficiently varying in the spatial domain, we can exploit these characteristics to generate the constraints of the pose graph. In this paper we provide a method of solving a simultaneous localization and mapping (SLAM) problem by employing pose graph optimization and indoor magnetic measurements. Specifically, different types of constraints for local heading correction and global loop closing, respectively, are designed. For the loop closing constraints in particular, we first examine spatial similarity of the indoor magnetic field and verify that the use of measurement sequences rather than a single measurement mitigates the ambiguity of the magnetic measurements. A loop closing algorithm is then proposed based on the sequence of magnetic measurement and applied to the pose graph optimization. Experimental results show that the proposed SLAM system with only wheel encoders and a single magnetometer obtains comparable results with a reference-level SLAM system in terms of robot trajectory, thereby validating the feasibility of applying magnetic constraints to indoor pose graph SLAM.

Underground localization using dual magnetic field sequence measurement and pose graph SLAM for directional drilling

With the development of unconventional gas, the technology of directional drilling has become more advanced. Underground localization is the key technique of directional drilling for real-time path following and system control. However, there are problems such as vibration, disconnection with external infrastructure, and magnetic field distortion. Conventional methods cannot solve these problems in real time or in various environments. In this paper, a novel underground localization algorithm using a re-measurement of the sequence of the magnetic field and pose graph SLAM (simultaneous localization and mapping) is introduced. The proposed algorithm exploits the property of the drilling system that the body passes through the previous pass. By comparing the recorded measurement from one magnetic sensor and the current re-measurement from another magnetic sensor, the proposed algorithm predicts the pose of the drilling system. The performance of the algorithm is validated through simulations and experiments.