Category Archives: Mobile Robot Slam

Interesting summary of SLAM and its computational cost approaches

Joan Vallvé, Joan Solà, Juan Andrade-Cetto, Pose-graph SLAM sparsification using factor descent. Robotics and Autonomous Systems, Volume 119, 2019, Pages 108-118, DOI: 10.1016/j.robot.2019.06.004.

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\u2019s 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\u2013Liebler divergence between the two distributions. Using simple topologies such as Chow\u2013Liu 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.

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.

SLAM based on submap joining that achieves linear cost through a novel choice of the reference frame of each submap, and an interesting related works on map joining, i.e., considering submaps as observations

Liang Zhao, Shoudong Huang, Gamini Dissanayake, Linear SLAM: Linearising the SLAM problems using submap joining, Automatica, Volume 100, 2019, Pages 231-246, DOI: 10.1016/j.automatica.2018.10.037.

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.

A nice review of visual SLAM with deep learning, and its evolution from non-learning visual SLAM

Ruihao Li, Sen Wang, DongBing Gu, Ongoing Evolution of Visual SLAM from Geometry to Deep Learning: Challenges and Opportunities, Cognitive Computation, December 2018, Volume 10, Issue 6, pp 875–889, DOI: 10.1007/s12559-018-9591-8.

Visual simultaneous localization and mapping (SLAM) has been investigated in the robotics community for decades. Significant progress and achievements on visual SLAM have been made, with geometric model-based techniques becoming increasingly mature and accurate. However, they tend to be fragile under challenging environments. Recently, there is a trend to develop data-driven approaches, e.g., deep learning, for visual SLAM problems with more robust performance. This paper aims to witness the ongoing evolution of visual SLAM techniques from geometric model-based to data-driven approaches by providing a comprehensive technical review. Our contribution is not only just a compilation of state-of-the-art end-to-end deep learning SLAM work, but also an insight into the underlying mechanism of deep learning SLAM. For such a purpose, we provide a concise overview of geometric model-based approaches first. Next, we identify visual depth estimation using deep learning is a starting point of the evolution. It is from depth estimation that ego-motion or pose estimation techniques using deep learning flourish rapidly. In addition, we strive to link semantic segmentation using deep learning with emergent semantic SLAM techniques to shed light on simultaneous estimation of ego-motion and high-level understanding. Finally, we visualize some further opportunities in this research direction.

Interesting close-loop detection for robot SLAM that only uses odometry and topology

Rohou, S., Franek, P., Aubry, C., & Jaulin, L. , Proving the existence of loops in robot trajectories, The International Journal of Robotics Research, DOI: 10.1177/0278364918808367.

In this paper we present a reliable method to verify the existence of loops along the uncertain trajectory of a robot, based on proprioceptive measurements only, within a bounded-error context. The loop closure detection is one of the key points in simultaneous localization and mapping (SLAM) methods, especially in homogeneous environments with difficult scenes recognitions. The proposed approach is generic and could be coupled with conventional SLAM algorithms to reliably reduce their computing burden, thus improving the localization and mapping processes in the most challenging environments such as unexplored underwater extents. To prove that a robot performed a loop whatever the uncertainties in its evolution, we employ the notion of topological degree that originates in the field of differential topology. We show that a verification tool based on the topological degree is an optimal method for proving robot loops. This is demonstrated both on datasets from real missions involving autonomous underwater vehicles and by a mathematical discussion.

SLAM as a sampling problem, with some references to the signal sampling state-of-the-art

Golnoosh Elhami, et. al Sampling at Unknown Locations: Uniqueness and Reconstruction Under Constraints, IEEE Transactions on Signal Processing, Vol 66 no. 22, DOI: 10.1109/TSP.2018.2872019.

Traditional sampling results assume that the sample locations are known. Motivated by simultaneous localization and mapping (SLAM) and structure from motion (SfM), we investigate sampling at unknown locations. Without further constraints, the problem is often hopeless. For example, we recently showed that, for polynomial and bandlimited signals, it is possible to find two signals, arbitrarily far from each other, that fit the measurements. However, we also showed that this can be overcome by adding constraints to the sample positions. In this paper, we show that these constraints lead to a uniform sampling of a composite of functions. Furthermore, the formulation retains the key aspects of the SLAM and SfM problems, whilst providing uniqueness, in many cases. We demonstrate this by studying two simple examples of constrained sampling at unknown locations. In the first, we consider sampling a periodic bandlimited signal composite with an unknown linear function. We derive the sampling requirements for uniqueness and present an algorithm that recovers both the bandlimited signal and the linear warping. Furthermore, we prove that, when the requirements for uniqueness are not met, the cases of multiple solutions have measure zero. For our second example, we consider polynomials sampled such that the sampling positions are constrained by a rational function. We previously proved that, if a specific sampling requirement is met, uniqueness is achieved. In addition, we present an alternate minimization scheme for solving the resulting non-convex optimization problem. Finally, fully reproducible simulation results are provided to support our theoretical analysis.

Interesting study of the number of optimal points in SLAM, considering it as a non-linear, non-convex optimization problem

Heng Wang, Shoudong Huang, Guanghong Yang, Gamini Dissanayake, Comparison of two different objective functions in 2D point feature SLAM, Automatica,
Volume 97, 2018, Pages 172-181, DOI: 10.1016/j.automatica.2018.08.009.

This paper compares two different objective functions in 2D point feature Simultaneous Localization and Mapping (SLAM). It is shown that the objective function can have a significant impact on the convergence of the iterative optimization techniques used in SLAM. When Frobenius norm is adopted for the error term of the orientation part of odometry, the SLAM problem has much better convergence properties, as compared with that using the angle difference as the error term. For one-step case, we have proved that there is one and only one minimum to the SLAM problem, and strong duality always holds. For two-step case, strong duality always holds except when three very special conditions hold simultaneously (which happens with probability zero), thus the global optimal solution to primal SLAM problem can be obtained by solving the corresponding Lagrangian dual problem in most cases. Further, for arbitrary m-step cases, we also show using examples that much better convergence results can be obtained. Simulation examples are given to demonstrate the different convergence properties using two different objective functions.

A novel hybridization of semantic and topological maps applied to mapping and localization in outdoors

Fernando Bernuy, Javier Ruiz-del-Solar, Topological Semantic Mapping and Localization in Urban Road Scenarios, Journal of Intelligent & Robotic Systems, September 2018, Volume 92, Issue 1, pp 19–32, DOI: 10.1007/s10846-017-0744-x.

Autonomous vehicle self-localization must be robust to environment changes, such as dynamic objects, variable illumination, and atmospheric conditions. Topological maps provide a concise representation of the world by only keeping information about relevant places, being robust to environment changes. On the other hand, semantic maps correspond to a high level representation of the environment that includes labels associated with relevant objects and places. Hence, the use of a topological map based on semantic information represents a robust and efficient solution for large-scale outdoor scenes for autonomous vehicles and Advanced Driver Assistance Systems (ADAS). In this work, a novel topological semantic mapping and localization methodology for large-scale outdoor scenarios for autonomous driving and ADAS applications is presented. The methodology uses: (i) a deep neural network for obtaining semantic observations of the environment, (ii) a Topological Semantic Map (TSM) for storing selected semantic observations, and (iii) a topological localization algorithm which uses a Particle Filter for obtaining the vehicle’s pose in the TSM. The proposed methodology was tested on a real driving scenario, where a True Estimate Rate of the vehicle’s pose of 96.9% and a Mean Position Accuracy of 7.7[m] were obtained. These results are much better than the ones obtained by other two methods used for comparative purposes. Experiments also show that the method is able to obtain the pose of the vehicle when its initial pose is unknown.

A nice hybridization of RBPF, high-frequency scan matching and topological maps to perform SLAM, with an also nice state-of-the-art

Aristeidis G. Thallas, Emmanouil G. Tsardoulias, Loukas Petrou, Topological Based Scan Matching – Odometry Posterior Sampling in RBPF Under Kinematic Model Failures, Journal of Intelligent & Robotic Systems, September 2018, Volume 91, Issue 3–4, pp 543–568, DOI: 10.1007/s10846-017-0730-3.

Rao-Blackwellized Particle Filters (RBPF) have been utilized to provide a solution to the SLAM problem. One of the main factors that cause RBPF failure is the potential particle impoverishment. Another popular approach to the SLAM problem are Scan Matching methods, whose good results require environments with lots of information, however fail in the lack thereof. To face these issues, in the current work techniques are presented to combine Rao-Blackwellized particle filters with a scan matching algorithm (CRSM SLAM). The particle filter maintains the correct hypothesis in environments lacking features and CRSM is employed in feature-rich environments while simultaneously reduces the particle filter dispersion. Since CRSM’s good performance is based on its high iteration frequency, a multi-threaded combination is presented which allows CRSM to operate while RBPF updates its particles. Additionally, a novel method utilizing topological information is proposed, in order to reduce the number of particle filter resamplings. Finally, we present methods to address anomalous situations where scan matching can not be performed and the vehicle displays behaviors not modeled by the kinematic model, causing the whole method to collapse. Numerous experiments are conducted to support the aforementioned methods’ advantages.