Accurate estimation of the environment structure simultaneously with the robot pose is a key capability of autonomous robotic vehicles. Classical simultaneous localization and mapping (SLAM) algorithms rely on the static world assumption to formulate the estimation problem, however, the real world has a significant amount of dynamics that can be exploited for a more accurate localization and versatile representation of the environment. In this paper we propose a technique to integrate the motion of dynamic objects into the SLAM estimation problem, without the necessity of estimating the pose or the geometry of the objects. To this end, we introduce a novel representation of the pose change of rigid bodies in motion and show the benefits of integrating such information when performing SLAM in dynamic environments. Our experiments show consistent improvement in robot localization and mapping accuracy when using a simple constant motion assumption, even for objects whose motion slightly violates this assumption.
In this paper we will describe a representation for spatial relationships which makes explicit their inherent uncertainty. We will show ways to manipulate them to obtain estimates of relationships and associated uncertainties not explicitly given, and show how decisions to sense or act can be made a priori based on those estimates. We will show how new constraint information, usually obtained by measurement, can be used to update the world model of relationships consistently, and in some situations, optimally. The framework we describe relies only on well-known state estimation methods. Many popular problems in robotics and computer vision including various types of simultaneous localization and mapping (SLAM) or bundle adjustment (BA) can be phrased as least squares optimization of an error function that can be represented by a graph. This paper describes the general structure of such problems and presents g2o, an open-source C++ framework for optimizing graph-based nonlinear error functions. Our system has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code. The current implementation provides solutions to several variants of SLAM and BA. We provide evaluations on a wide range of real-world and simulated datasets. The results demonstrate that while being general g2o offers a performance comparable to implementations of state-of-the-art approaches for the specific problems. The paper analyzes the properties of the full covariance simultaneous localization and map building problem (SLAM). We prove that, even for the special case of a stationary vehicle (with no process noise) which uses a range-bearing sensor and has non-zero angular uncertainty, the full covariance SLAM algorithm always yields an inconsistent map. We also show, through simulations, that these conclusions appear to extend to a moving vehicle with process noise. However, these inconsistencies only become apparent after several hundred beacon updates.   Being able to build a map of the environment and to simultaneously localize within this map is an essential skill for mobile robots navigating in unknown environments in absence of external referencing systems such as GPS. This so-called simultaneous localization and mapping (SLAM) problem has been one of the most popular research topics in mobile robotics for the last two decades and efficient approaches for solving this task have been proposed. One intuitive way of formulating SLAM is to use a graph whose nodes correspond to the poses of the robot at different points in time and whose edges represent constraints between the poses. The latter are obtained from observations of the environment or from movement actions carried out by the robot. Once such a graph is constructed, the map can be computed by finding the spatial configuration of the nodes that is mostly consistent with the measurements modeled by the edges. In this paper, we provide an introductory description to the graph-based SLAM problem. Furthermore, we discuss a state-of-the-art solution that is based on least-squares error minimization and exploits the structure of the SLAM problems during optimization. The goal of this tutorial is to enable the reader to implement the proposed methods from scratch.   This article presents an algorithm for autonomous map building and maintenance for a mobile robot. We believe that mobile robot navigation can be treated as a problem of tracking ge ometric features that occur naturally in the environment. We represent each feature in the map by a location estimate (the feature state vector) and two distinct measures of uncertainty: a covariance matrix to represent uncertainty in feature loca tion, and a credibility measure to represent our belief in the validity of the feature. During each position update cycle, pre dicted measurements are generated for each geometric feature in the map and compared with actual sensor observations. Suc cessful matches cause a feature's credibility to be increased. Unpredicted observations are used to initialize new geometric features, while unobserved predictions result in a geometric feature's credibility being decreased. We describe experimental results obtained with the algorithm that demonstrate successful map building using real son...