Tag Archives: Graph-based Slam

A new pose-graph optimization algorithm for SLAM and other problems whose, through a formulation as global optimization in SE(3), results are certifiable and more robust than standard approaches, and a curious relation between this problem and the clock synchronization problem

Rosen, D. M., Carlone, L., Bandeira, A. S., & Leonard, J. J., SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group, The International Journal of Robotics Research, 38(2–3), 95–125, 2019 DOI: 10.1177/0278364918784361.

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 x1,…,xn∈SE(d) given noisy measurements of a subset of their pairwise relative transforms x−1ixj. 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.

Interesting mathematical study of the properties of graphs for graph-based SLAM and other graph-based estimation problems

Khosoussi, K., Giamou, M., Sukhatme, G. S., Huang, S., Dissanayake, G., & How, J. P., Reliable Graphs for SLAM, The International Journal of Robotics Research, 2019, DOI: 10.1177/0278364918823086.

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.

Interesting implementation of visual graph SLAM in C++ for educational purposes

Dominik Schlegel, Mirco Colosi, Giorgio Grisetti, ProSLAM: Graph SLAM from a Programmer’s Perspective/strong>, arXiv:1709.04377.

In this paper we present ProSLAM, a lightweight stereo visual SLAM system designed with simplicity in mind. Our work stems from the experience gathered by the authors while teaching SLAM to students and aims at providing a highly modular system that can be easily implemented and understood. Rather than focusing on the well known mathematical aspects of Stereo Visual SLAM, in this work we highlight the data structures and the algorithmic aspects that one needs to tackle during the design of such a system. We implemented ProSLAM using the C++ programming language in combination with a minimal set of well known used external libraries. In addition to an open source implementation, we provide several code snippets that address the core aspects of our approach directly in this paper. The results of a thorough validation performed on standard benchmark datasets show that our approach achieves accuracy comparable to state of the art methods, while requiring substantially less computational resources.

Insights into the sparsity of graph-SLAM (i.e., in the smoothing / optimization approach to SLAM) and a good formalization of the problem

K. Khosoussi, S. Huang and G. Dissanayake, “A Sparse Separable SLAM Back-End,” in IEEE Transactions on Robotics, vol. 32, no. 6, pp. 1536-1549, Dec. 2016. DOI: 10.1109/TRO.2016.2609394.

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.

An excellent survey of metrical SLAM (and of map representations and other issues related to SLAM) as of 2016

C. Cadena et al., “Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age,” in IEEE Transactions on Robotics, vol. 32, no. 6, pp. 1309-1332, Dec. 2016. DOI: 10.1109/TRO.2016.2624754.

Simultaneous localization and mapping (SLAM) consists in the concurrent construction of a model of the environment (the map), and the estimation of the state of the robot moving within it. The SLAM community has made astonishing progress over the last 30 years, enabling large-scale real-world applications and witnessing a steady transition of this technology to industry. We survey the current state of SLAM and consider future directions. We start by presenting what is now the de-facto standard formulation for SLAM. We then review related work, covering a broad set of topics including robustness and scalability in long-term mapping, metric and semantic representations for mapping, theoretical performance guarantees, active SLAM and exploration, and other new frontiers. This paper simultaneously serves as a position paper and tutorial to those who are users of SLAM. By looking at the published research with a critical eye, we delineate open challenges and new research issues, that still deserve careful scientific investigation. The paper also contains the authors’ take on two questions that often animate discussions during robotics conferences: Do robots need SLAM? and Is SLAM solved?

Dealing with multiple hypothesis in Graph-SLAM through multigraphs (as in multi-hierarchical graphs)

Max Pfingsthorn and Andreas Birk, Generalized graph SLAM: Solving local and global ambiguities through multimodal and hyperedge constraints, The International Journal of Robotics Research May 2016 35: 601-630, DOI: 10.1177/0278364915585395.

Research in Graph-based Simultaneous Localization and Mapping has experienced a recent trend towards robust methods. These methods take the combinatorial aspect of data association into account by allowing decisions of the graph topology to be made during optimization. The Generalized Graph Simultaneous Localization and Mapping framework presented in this work can represent ambiguous data on both local and global scales, i.e. it can handle multiple mutually exclusive choices in registration results and potentially erroneous loop closures. This is achieved by augmenting previous work on multimodal distributions with an extended graph structure using hyperedges to encode ambiguous loop closures. The novel representation combines both hyperedges and multimodal Mixture of Gaussian constraints to represent all sources of ambiguity in Simultaneous Localization and Mapping. Furthermore, a discrete optimization stage is introduced between the Simultaneous Localization and Mapping frontend and backend to handle these ambiguities in a unified way utilizing the novel representation of Generalized Graph Simultaneous Localization and Mapping, providing a general approach to handle all forms of outliers. The novel Generalized Prefilter method optimizes among all local and global choices and generates a traditional unimodal unambiguous pose graph for subsequent continuous optimization in the backend. Systematic experiments on synthetic datasets show that the novel representation of the Generalized Graph Simultaneous Localization and Mapping framework with the Generalized Prefilter method, is significantly more robust and faster than other robust state-of-the-art methods. In addition, two experiments with real data are presented to corroborate the results observed with synthetic data. Different general strategies to construct problems from real data, utilizing the full representational power of the Generalized Graph Simultaneous Localization and Mapping framework are also illustrated in these experiments.

Implementation of spatial relations in graph-SLAM through quaternions instead of homogeneous matrices

Jiantong Cheng, Jonghyuk Kim, Zhenyu Jiang, Wanfang Che, Dual quaternion-based graphical SLAM, Robotics and Autonomous Systems, Volume 77, March 2016, Pages 15-24, ISSN 0921-8890, DOI: 10.1016/j.robot.2015.12.001.

This paper presents a new parameterization approach for the graph-based SLAM problem and reveals the differences of two popular over-parameterized ways in the optimization procedure. In the SALM problem, constraints or relative transformations between any two poses are generally separated into translations plus 3D rotations, which are then described in a homogeneous transformation matrix (HTM) to simplify computational operations. This however introduces added complexities in frequent conversions between the HTM and state variables, due to their different representations. This new approach, unit dual quaternion (UDQ), describes a spatial transformation as a screw with only 8 elements. We show that state variables can be directly represented by UDQs, and how their relative transformations can be written with the UDQ product, without the trivial computations of HTM. Then, we explore the performances of the unit quaternion and the axis–angle representations in the graph-based SLAM problem, which have been successfully applied to over parameterize perturbations under the assumption of small errors. Based on public synthetic and real-world datasets in 2D and 3D environments, experimental results show that the proposed approach reduces greatly the computational complexity while obtaining the same optimization accuracies as the HTM-based algorithm, and the axis–angle representation is superior to be the quaternion in the case of poor initial estimations.

A very interesting review of current approaches to SLAM based on smoothing (i.e., graph optimization) and in clustering the map into submaps

Jiantong Cheng, Jonghyuk Kim, Jinliang Shao, Weihua Zhang, Robust linear pose graph-based SLAM, Robotics and Autonomous Systems, Volume 72, October 2015, Pages 71-82, ISSN 0921-8890, DOI: 10.1016/j.robot.2015.04.010.

This paper addresses a robust and efficient solution to eliminate false loop-closures in a pose-graph linear SLAM problem. Linear SLAM was recently demonstrated based on submap joining techniques in which a nonlinear coordinate transformation was performed separately out of the optimization loop, resulting in a convex optimization problem. This however introduces added complexities in dealing with false loop-closures, which mostly stems from two factors: (a) the limited local observations in map-joining stages and (b) the non block-diagonal nature of the information matrix of each submap. To address these problems, we propose a Robust Linear SLAM by (a) developing a delayed optimization for outlier candidates and (b) utilizing a Schur complement to efficiently eliminate corrupted information block. Based on this new strategy, we prove that the spread of outlier information does not compromise the optimization performance of inliers and can be fully filtered out from the corrupted information matrix. Experimental results based on public synthetic and real-world datasets in 2D and 3D environments show that this robust approach can cope with the incorrect loop-closures robustly and effectively.

A nice SLAM approach based on hybrid Normal Distribution Transform (NDT) + occupancy grid maps intended for long term operation in dynamic environments

Erik Einhorn, Horst-Michael Gross, Generic NDT mapping in dynamic environments and its application for lifelong SLAM, Robotics and Autonomous Systems, Volume 69, July 2015, Pages 28-39, ISSN 0921-8890, DOI: 10.1016/j.robot.2014.08.008.

In this paper, we present a new, generic approach for Simultaneous Localization and Mapping (SLAM). First of all, we propose an abstraction of the underlying sensor data using Normal Distribution Transform (NDT) maps that are suitable for making our approach independent from the used sensor and the dimension of the generated maps. We present several modifications for the original NDT mapping to handle free-space measurements explicitly. We additionally describe a method to detect and handle dynamic objects such as moving persons. This enables the usage of the proposed approach in highly dynamic environments. In the second part of this paper we describe our graph-based SLAM approach that is designed for lifelong usage. Therefore, the memory and computational complexity is limited by pruning the pose graph in an appropriate way.

Good related work of graph-based SLAM algorithms that employ some reduction technique on the graph to improve long-term operation, and proposal of a new method of reduction

Carlevaris-Bianco, N.; Kaess, M.; Eustice, R.M., Generic Node Removal for Factor-Graph SLAM, Robotics, IEEE Transactions on , vol.30, no.6, pp.1371,1385, Dec. 2014. DOI: 10.1109/TRO.2014.2347571

This paper reports on a generic factor-based method for node removal in factor-graph simultaneous localization and mapping (SLAM), which we call generic linear constraints (GLCs). The need for a generic node removal tool is motivated by long-term SLAM applications, whereby nodes are removed in order to control the computational cost of graph optimization. GLC is able to produce a new set of linearized factors over the elimination clique that can represent either the true marginalization (i.e., dense GLC) or a sparse approximation of the true marginalization using a Chow-Liu tree (i.e., sparse GLC). The proposed algorithm improves upon commonly used methods in two key ways: First, it is not limited to graphs with strictly full-state relative-pose factors and works equally well with other low-rank factors, such as those produced by monocular vision. Second, the new factors are produced in such a way that accounts for measurement correlation, which is a problem encountered in other methods that rely strictly upon pairwise measurement composition. We evaluate the proposed method over multiple real-world SLAM graphs and show that it outperforms other recently proposed methods in terms of Kullback–Leibler divergence. Additionally, we experimentally demonstrate that the proposed GLC method provides a principled and flexible tool to control the computational complexity of long-term graph SLAM, with results shown for ${34.9}, {rm {h}}$ of real-world indoor–outdoor data covering ${147.4}{hbox{ km}}$ collected over $27$ mapping sessions spanning a period of $15$ months.