Report

Accurate On-Line 3D Occupancy Grids Using Manhattan World Constraints Brian Peasley and Stan Birchfield Dept. of Electrical and Computer Engineering Clemson University Alex Cunningham and Frank Dellaert College of Computing Georgia Tech Map representations Point cloud Elevation map Multi-level Surface map Octree (occupancy grid) from Wurm et al., OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation for Robotic Systems, 2010 2/72 Occupancy grids [Moravec and Elfes 1985] An occupancy grid can represent any point cloud with arbitrary precision 3/72 Occupancy grids [Moravec and Elfes 1985] Occupied An occupancy grid can represent any point cloud with arbitrary precision 4/72 Occupancy grids [Moravec and Elfes 1985] Occupied Occupied Occupied Free An occupancy grid can represent any point cloud with arbitrary precision 5/72 Occupancy grids [Moravec and Elfes 1985] Occ Occ Occ Free Free Occ Free Free Free Occ Free Free Free Occ Free Free An occupancy grid can represent any point cloud with arbitrary precision 6/72 Occupancy grids Leads naturally to a hierarchical representation: QuadTree (2D) O Occupied O O O F OOFO OFFF FOFO FFFF Occupied Occupied … and an efficient encoding Occupied Occ Free Occ Occ Free Free Occ Free Free Free Occ Free Free Free Occ Free Free 1 1110 1101100001010000 1 = Occupied 0 = Free For continuous surface S (3D), • occupancy grid is discretization of characteristic function cS: R3 {0,1} • requires ~2 bits per sample point [Kaufman et al. 1993; Botsch et al. 2002] 7/72 Representing “I don’t know” Occupancy grids can explicitly represent unknown areas (which sensor has not yet seen) sensor coverage Occ Occ Unk Unk Free Occ Unk Unk Free Occ Free Free Free Occ Free Free 8/72 Recall QuadTree (2D) representation: O Occupied O Occupied O O F Occupied OOFO OFFF FOFO FFFF Occupied Free encoding: 1 1110 1101100001010000 Occ Occ Occ Free Free Occ Free Free 1 = Occupied 0 = Free Free Occ Free Free Free Occ Free Free 9/72 Encoding “I don’t know” With 3 states, we need 2 bits per node/cell O Occupied O Occupied U O F Unk OOFO UUUU FOFO FFFF Occupied Free Occ Occ Unk Unk Free Occ Unk Unk … but this is inefficient: 10 10001001 101001100000000001100110 Free Occ Free Free Free Occ Free Free 00 = Unknown 01 = Free 10 = Occupied 11 = (wasted) 10/72 Encoding “I don’t know” Make representation symmetric: O means confidently completely occupied F means confidently completely free U means unconfident P means parent (mixed) P Occupied P Occupied U P F Unk OOFO UUUU FOFO FFFF Occupied Free Occ Occ Unk Unk Free Occ Unk Unk … which is more efficient: 11 11001101 1010011001100110 Free Occ Free Free Free Occ Free Free 00 = Unknown 01 = Free 10 = Occupied 11 = Parent 11/72 Octrees (3D) In 3D, we use an octree: : 00 = U : 01 = F : 10 = O : 11 = P unknown free occupied 12/72 OctoMap [Wurm et al. 2010] • OctoMap is a probabilistic octree • Each node stores likelihood (log odds) of being occupied cell n data up to time t • Clamping update policy saturates leaf nodes to either minimum or maximum value [Yguel et al. 2007] avoids overconfidence • Automatic: – If all children are stable (saturated), then prune them – If future measurements contradict state, children are regenerated • Whenever map needs to be stored, force binarization OctoMap, http://octomap.sourceforge.net/ 13/72 Problem • Occupancy grids – discretize space lose information – assume fixed coordinate system cannot be corrected • SLAM (simultaneous localization and mapping) – estimate of sensor pose is constantly adjusted (especially during loop closure) How to correct or avoid drift? 15/72 Solution #1: Assume trajectory known [Wurm et al. 2010] • Original Octomap assumed prior knowledge of entire sensor trajectory (44 x 18 x 3 m) not on-line 16/72 Solution #2: Use Manhattan world assumption [Furukawa et al. 2009] • Manhattan world assumes all planes are axis-aligned (parallel or perpendicular): estimated robot position actual robot position actual walls estimated walls (suffer from drift) • Rotational drift leads to severe error: 1 degree angle error yields 2 m position error (after 100 m) But Manhattan zero rotational drift! 17/72 Implementation • Forward-facing Kinect sensor on Pioneer mobile base • Manually drove robot • To enforce Manhattan world constraint, – detect dominant vertical plane(s) (apply RANSAC to horizontal scan) – classify based on normal (just 2 categories) – only 1 plane needed • Visual registration using SURF point correspondences wall robot trajectory 18/72 Computing robot pose • Robot pose found on-line using PoseSLAM from incremental Smoothing and Mapping (iSAM) • Factor graph (undirected bipartite) • Compute MAP estimate over all measurements global orientation from Manhattan (Wi) robot pose relative position from robot odometry (Oi) and visual registration (Vi) GTSAM, https://collab.cc.gatech.edu/borg/gtsam/ 19/72 Results • Laboratory with adjoining rooms: RO RO+VR RO+M (10.6 x 20.6 m) RO+VR+M 20/72 Results • Long corridor (loop closure not possible) RO+VR RO+VR+M (23.9 x 47.8 m) 21/72 Results • Large building (no loop closure) RO+VR (52.6 x 53.2 m) 22/72 Results • Large building (no loop closure) RO+VR+M (52.6 x 53.2 m) 23/72 Quantitative measurements (approximate 2D Euclidean position error at intersections) 24/72 Size of OctoMaps • Octree reduces storage requirements by 3 orders of magnitude: 25/72 Conclusion • On-line system for accurate 3D map generation of indoor rectilinear buildings – Manhattan world assumption with inference on factor graph to estimate robot position and orientation – Zero rotational drift – Stores in OctoMap (efficient, hierarchical) • Allows occupancy grids to be used for online, large-scale mapping 26/72 Thanks! 27/72