Feature based graph SLAM with high level representation using rectangles

  • Published on

  • View

  • Download


<ul><li><p>Robotics and Autonomous Systems 63 (2015) 8088Contents lists available at ScienceDirect</p><p>Robotics and Autonomous Systems</p><p>journal homepage: www.elsevier.com/locate/robot</p><p>Feature based graph SLAM with high level representation usingrectanglesPaloma de la Puente ,1, Diego Rodriguez-LosadaETSI Industriales, Universidad Politecnica de Madrid, c/Jose Gutierrez Abascal, 2, 28006 Madrid, Spain</p><p>h i g h l i g h t s</p><p> A new method for building maps of rectangles in mobile robotics is proposed. A generic feature based graph SLAM framework for different types of features and extraction methods is used. Incorrect structure constraints can be removed. Rectangles can be detected from segments obtained from laser data even in conditions of only partial visibility. Properties such as parallelism and orthogonality can be preserved, and the resulting maps are compact and convenient for human interpretation.</p><p>a r t i c l e i n f o</p><p>Article history:Received 26 January 2014Received in revised form22 August 2014Accepted 8 September 2014Available online 16 September 2014</p><p>Keywords:Mobile roboticsFeature based SLAMEnvironment modeling</p><p>a b s t r a c t</p><p>In mobile robotics, feature based maps are very popular for the representation of the environment. Someof the main advantages of these maps are final compactness and expressivity, aspects that make storageeasier and simplify higher level reasoning. Most existing approaches, however, stick to low level featuressuch as points, segments and sometimes circles, corners or splines. This paper presents the incorporationof rectangles as higher level features in a feature based graph Simultaneous Localization and Mapping(SLAM) framework for the consideration of the structure of the environment in the mapping process.</p><p> 2014 Elsevier B.V. All rights reserved.1. Introduction</p><p>Whenwehumans think about or describe indoor environments,we often employ a representation based on concepts such asregions, rooms and corridors [1,2]. Instead of relying on purelymetric maps, for some applications it would be desirable thattopological and semantic levels could be added to the maps builtand used by mobile robots, so that interaction with humans couldbe more natural and efficient.</p><p>In general, sensor data processing is hard andmost existing ap-proaches do not deal with the perception problem for higher levelfeature extraction. As stated by Blanco et al. [3], SLAM remains alargely unsolved problem in relation to high level representationsand long termoperation in large-scale environments, even though</p><p> Correspondence to: Automation and Control Institute, Vienna University ofTechnology, Austria.</p><p>E-mail addresses: pdelapuente@acin.tuwien.ac.at, paloma.delapuente@upm.es(P. de la Puente), diego.rlosada@upm.es (D. Rodriguez-Losada).1 Currently at: Automation and Control Institute, Vienna University of Technol-</p><p>ogy.</p><p>http://dx.doi.org/10.1016/j.robot.2014.09.0060921-8890/ 2014 Elsevier B.V. All rights reserved.there have been important improvements in the state of the art [4].We believe that employing rectangles for feature based mappingin most common structured environments can constitute a com-pact and intuitive representation method that may be an impor-tant step towardsmore advanced place recognition and navigationtechniques. Our framework provides a flexible way to incorporatevirtual features and constraints into a graph based formulation ofthe Simultaneous Localization andMapping (SLAM) problem, witha hierarchical representation of structure. Detection of structureand inference are performed iteratively, following an ExpectationMaximization (EM) algorithm [4,5].</p><p>This paper mainly focuses on the representation and detec-tion of virtual rectangles for 2D SLAMwithin the above mentionedframework, but other features have been successfully used aswell (and the structure detection algorithms are very easily inter-changeable). In the work presented here, rectangles are detectedfrom segments extracted from laser sensor data.</p><p>The organization of the paper is as follows. Section 2 studiesprevious work related to this research and Section 3 describesour feature based graph SLAM framework for structured environ-ments. Section 4 presents the parametrization that we use for therectangles and the formulation of constraints, while Section 5 fo-cuses on the rectangle detection algorithm and on how existing</p><p>http://dx.doi.org/10.1016/j.robot.2014.09.006http://www.elsevier.com/locate/robothttp://www.elsevier.com/locate/robothttp://crossmark.crossref.org/dialog/?doi=10.1016/j.robot.2014.09.006&amp;domain=pdfmailto:pdelapuente@acin.tuwien.ac.atmailto:paloma.delapuente@upm.esmailto:diego.rlosada@upm.eshttp://dx.doi.org/10.1016/j.robot.2014.09.006</p></li><li><p>P. de la Puente, D. Rodriguez-Losada / Robotics and Autonomous Systems 63 (2015) 8088 81rectangles are extended. Section 6 contains experiments and re-sults and finally, Section 7 summarizes our conclusions and futureworking lines.</p><p>2. Related work</p><p>The feature based approach to the SLAM problem is very pop-ular in mobile robotics. Many existing solutions employ esti-mation tools based on the Extended Kalman Filter (EKF), whichprovides analytical estimates of the full Gaussian posterior [6,7].Other important algorithms are based on particle filters [8], in-formation filters [911] and least-square error minimization tech-niques [12,13]. This approach is known as graph based SLAM orgraph SLAM. A related approach (with some particularities) is em-ployed by the computer vision community, where the term BundleAdjustment (BA) is employed [14]. The standard graph SLAM algo-rithm includes robot poses and observed features as nodes of thegraph, but most existing approaches marginalize out the featuresand deal only with robot poses [1517]. We recently proposed fea-ture based graph SLAM [4,5], based on themarginalization of robotposes instead of the marginalization of features, in order to exploitthe knowledge available due to the possible existence of structurein the environment.</p><p>The simplest and most common feature is the point [1820].While point based representations usually disregard less informa-tion than other feature based approaches, they also require longercomputation times and present more difficulties for the data as-sociation process. Furthermore, they do not provide any furtherinformation to make scene interpretation simpler. Another geo-metric element that is fairly easy to extract by means of segmen-tation and yet allows for more compactness and expressivity isthe line or the line segment [21]; many existing SLAM solutionsemploy this geometric primitive for the representation of the en-vironment [9,22]. The presence of circular features in indoor envi-ronments is not that common, but in certain environments it canbe very useful too [23], as circles are usually quite distinctive. Ina more generic approach, splines can be used to model differentfeatures in the environment [24]. The advantages of feature basedSLAM are especially relevant in 3D mapping, where each scan isa point cloud containing a large number of points and memoryrequirements can become problematic, as suggested for examplein several previous works [2527]. Some important frameworksfor the representation of features in feature based SLAM are theSPmap [28], Square Root SAM [29] and iSAM [30]. TheM-Space [31]feature representation for SLAM is also very interesting.</p><p>Recently, several works have addressed the problem of consid-ering prior information about the structure of the environment inSLAM,which can lead tomuchmoremeaningful and accuratemod-els [3235,5,36]. Apparently, however, there is not much previouswork based on the assumption that rooms and corridors in moststructured environments can be modeled by rectangles.</p><p>This hypothesis has been considered for segmentation [37], butin this case the representation is grid based. Very recently, anattractivemethod for the offline extraction of rectangular roomsbyapplying Bayesian reasoning to input occupancy gridmaps has alsohighlighted the advantages of obtaining abstract floor plans [38].</p><p>Regarding representation, relevant work in this area presentedthe idea that rooms and corridors in indoor environments can bemodeled by four lines forming a rectangle, with the goal of keepinga minimalistic representation [39]. However, in this approach allfour walls of a room are parametrized separately, and it was notedthat some parts of the maps built when performing SLAM wereslightly bent or misaligned. Another important step towards moreabstract spatial models was based on the extraction of a boundingrectangle after applying a room segmentation method [40]. Thisapproach allowed the height and width of a rectangular roomto be determined in a way largely invariant to the arrangementof internal furniture. The segmentation and parametrization ofthe regions were applied on top of a fuzzy occupancy grid map.The whole method works as a virtual sensor for the detection ofrectangular rooms from range data.</p><p>Several other algorithms for rectangle detection exist [4144].Nevertheless, they are designed for different applications and arenot suitable for the particular characteristics of the problem athandhere (e.g. there can be occlusions and rectangles only partiallyobserved, rectangles are obtained from segments, segments maybe close to each other . . . ).</p><p>A very interesting SLAM algorithm for the integration of metric,topological and semantic information into the map introduced arepresentation idea based on regions defined by up to three rectan-gles lined up along an axis [45]. The proposed shape is very expres-sive and does not require toomany parameters, while the problemof partial visibility was also taken into account. In this work, thesekinds of features were used within a particle filter implementa-tion. A virtual sensor was simulated for high level feature extrac-tion, which makes this method not directly applicable to existingdatasets and robots. Once a map of these regions is created, it canbe used for advanced navigation [46].</p><p>3. Feature based graph SLAM in structured environments</p><p>Our graph notation is similar to those used by Olson et al. [15]and Grisetti et al. [16]:</p><p> x is the state vector representing the nodes a function f (xi, xj) = fij(x) represents the constraint equationsbetween nodes i and jwith expected values uij and variances Pij</p><p> the error value at a given estimate for xi and xj is defined aseij(x) = fij(x) uij.</p><p>The cost, or negative log probability of the node positions, invectorial form, is given by:</p><p> ln p(x) eT (x)P1e(x). (1)</p><p>In our case, we propose to distinguish between two differenttypes of constraints:</p><p> we denote the error values due to structural constraints im-posed between features i and j by esij and the corresponding co-variance by Psij</p><p> the rest of the constraints come from the measurements. Thecorresponding error values will be denoted ezij , with covariancePzij .</p><p>Structure constraints are detected from the state of the nodesand a graphminimization is performed iteratively, following an EMbased approach described in detail in [4,5]. Summarizing, at eachiteration the most probable state is obtained as:</p><p>x argminx</p><p>eTz (x)Pz</p><p>1ez(x) + eTs (x)P</p><p>s1es(x)</p><p>, (2)</p><p>where the structure constraints correspond to the most probablestructure inferred at the previous step and with (P s)</p><p>1 formed bythe information matrices of the structure constraints scaled ac-cording to their corresponding weights indicating their reliability.</p><p>Both terms can be grouped into</p><p>eTa (x)PA1ea(x), (3)</p><p>with ea containing the error values from all the constraints (struc-tural and non-structural), and PA1 integrated by the properlyscaled information matrices of all the constraints. The back-endminimization can be carried out in a variety of ways. We followthe approach used by Olson et al. [15] and Grisetti et al. [16] andemploy the CSparse [47] library for the minimization.</p></li><li><p>82 P. de la Puente, D. Rodriguez-Losada / Robotics and Autonomous Systems 63 (2015) 8088Table 1Some examples of possible configurations for the features.</p><p>No. Level 0 Level 1 Level 2 Level 3 Level 4</p><p>1 Points Lines Orientations 2 Points Circles Circle centers Lines Orientations3 Segments Orientations 4 Segments Rectangles Orientations 5 Circles Circle centers Lines Orientations </p><p>Note: Items on level 0 are obtained directly from the laser measurements.Fig. 1. Imposing constraints from the segments to the rectangles. Left: beforeminimization. Right: after minimization.</p><p>Fig. 2. Adding orthogonality constraints for the rectangles. Left: before minimiza-tion. Right: after minimization.</p><p>When inferring the most probable structure by means of struc-ture detection algorithms, features in a given level allow to cre-ate the next one, so that new structure features and constraintsmay be discovered from previously found structure. This hier-archical approach simplifies the detection processes, providesflexibility to the framework and allows for the detection and elim-ination of improbable structural constraints. The elimination of in-correct structure is studied anddiscussed elsewhere [4,5]. Our testsshowed that it can be useful in order to remove constraints thatmay increase the final error, especially in environments that areonly similar to ideal geometries.</p><p>Table 1 shows some possible configurations for the arrange-ment of the features. This paper focuses on example No. 4, witha low level of segments extracted from sensor data by means ofsegmentation, a subsequent level of rectangles detected from thesegments and a higher level of commonorientations for the rectan-gles. This representation is compatible with the existence of someirregularities such as columns in a wall or similar situations be-cause even if not contemplated at the rectangles level, they can stillbe present at the lower level containing the extracted segments.</p><p>Fig. 1 shows a simple example of the correction when usingrectangles and Fig. 2 shows the result when incorporating orien-tation constraints. The example employs simulated segments andgiven rectangles (a priori known structure, no detection), with allfour sides present and quite large noise of 15 in the initial seg-ments orientations so that the correction is more noticeable.</p><p>4. Parametrization of rectangles and formulation of con-straints</p><p>In structured indoor environments, walls are usually parallel orperpendicular to each other. Our previouswork [4,5] shows how toutilize this knowledge within the presented framework, by meansof virtual orientations to which segments are constrained. VirtualFig. 3. Rectangle representation.</p><p>orientations are obtained by fitting the segments orientationsdistribution to a Gaussian mixture model. Constraints betweenpairs of orientations can be added too.</p><p>Now we propose to represent indoor spaces by rectangularfeatures. A rectangle r can be defined by five parameters: thecenter pose parameters rx, ry, r , the length rl and thewidth rw . Weconsider that the orientation is given by the longest dimension; thefour side...</p></li></ul>


View more >