Category Archives: Mobile Robot Slam

A possibly interesting paper on the estimation and adaptation of EKF-SLAM to actual models of the system and the noise that I have been unable to read due to its painful syntax

Yingzhong Tian, Heru Suwoyo, Wenbin Wang, Dziki Mbemba, Long Li, An AEKF-SLAM Algorithm with Recursive Noise Statistic Based on MLE and EM, Journal of Intelligent & Robotic Systems (2020) 97:339–355, DOI: 10.1007/s10846-019-01044-8.

Extended Kalman Filter (EKF) has been popularly utilized for solving Simultaneous Localization and Mapping (SLAM)
problem. Essentially, it requires the accurate system model and known noise statistic. Nevertheless, this condition can
be satisfied in simulation case. Hence, EKF has to be enhanced when it is applied in the real-application. Mainly, this
improvement is known as adaptive-based approach. In many different cases, it is indicated by some manners of estimating
for either part or full noise statistic. This paper present a proposed method based on the adaptive-based solution used for
improving classical EKF namely An Adaptive Extended Kalman Filter. Initially, the classical EKF was improved based on
Maximum Likelihood Estimation (MLE) and Expectation-Maximization (EM) Creation. It aims to equips the conventional
EKF with ability of approximating noise statistic and its covariance matrices recursively. Moreover, EKF was modified and
improved to tune the estimated values given by MLE and EM creation. Besides that, the recursive noise statistic estimators
were also estimated based on the unbiased estimation. Although it results high quality solution but it is followed with some
risks of non-positive definite matrices of the process and measurement noise statistic covariances. Thus, an addition of
Innovation Covariance Estimation (ICE) was also utilized to depress this possibilities. The proposed method is applied for
solving SLAM problem of autonomous wheeled mobile robot. Henceforth, it is termed as AEKF-SLAM Algorithm. In order
to validate the effectiveness of proposed method, some different SLAM-Based algorithm were compared and analyzed.
The different simulation has been showing that the proposed method has better stability and accuracy compared to the
conventional filter in term of Root Mean Square Error (RMSE) of Estimated Map Coordinate (EMC) and Estimated Path
Coordinate (EPC).

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.