Tag Archives: Graph-based Slam

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.