1. 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

2. Map representations 2/72 Point cloud Elevation map Octree (occupancy grid) Multi-level Surface map from Wurm et al., OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation for Robotic Systems, 2010

3. Occupancy grids 3/72 An occupancy grid can represent any point cloud with arbitrary precision [Moravec and Elfes 1985]

4. Occupancy grids 4/72 An occupancy grid can represent any point cloud with arbitrary precision [Moravec and Elfes 1985] Occupied

5. Occupancy grids 5/72 An occupancy grid can represent any point cloud with arbitrary precision [Moravec and Elfes 1985] Occupied Free

6. Occupancy grids 6/72 An occupancy grid can represent any point cloud with arbitrary precision [Moravec and Elfes 1985] Occ Free OccFree OccFree OccFree

7. Occupancy grids 7/72 Leads naturally to a hierarchical representation: QuadTree (2D) … and an efficient encoding Occupied Free Occupied Occ Free OccFree OccFree OccFree O FOOO O O F OO F F FF O F F 1 1110 1101100001010000 1 = Occupied 0 = Free For continuous surface S (3D), occupancy grid is discretization of characteristic function  S : R 3  {0,1} requires ~2 bits per sample point [Kaufman et al. 1993; Botsch et al. 2002]

8. Representing “I don’t know” 8/72 Occupancy grids can explicitly represent unknown areas (which sensor has not yet seen) Occ Unk FreeOcc FreeOccFree OccFree sensor coverage Unk

9. Recall 9/72 QuadTree (2D) representation: encoding: Occupied Free Occupied Occ Free OccFree OccFree OccFree O FOOO O O F OO F F FF O F F 1 1110 1101100001010000 1 = Occupied 0 = Free

10. Encoding “I don’t know” 10/72 With 3 states, we need 2 bits per node/cell … but this is inefficient: Occupied Unk Free Occupied Occ Unk FreeOccUnk FreeOccFree OccFree 10 10001001 101001100000000001100110 00 = Unknown 01 = Free 10 = Occupied 11 = (wasted) O FOUO O O F OU U F O F F

11. Encoding “I don’t know” 11/72 Make representation symmetric: O means confidently completely occupied F means confidently completely free U means unconfident P means parent (mixed) … which is more efficient: Occupied Unk Free Occupied Occ Unk FreeOccUnk FreeOccFree OccFree 11 11001101 1010011001100110 00 = Unknown 01 = Free 10 = Occupied 11 = Parent P FPUP O O F OU U F O F F

12. Octrees (3D) In 3D, we use an octree: 12/72 unknown occupied free : 00 = U : 01 = F : 10 = O : 11 = P

13. OctoMap OctoMap is a probabilistic octree Each node stores likelihood (log odds) of being occupied 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 13/72 [Wurm et al. 2010] cell ndata up to time t OctoMap, http://octomap.sourceforge.net/ http://octomap.sourceforge.net/

14. OctoMap Thus, OctoMap combines –probabilistic occupancy mapping –discretely-labeled octrees Automatically prune and regenerate nodes as needed Whenever map needs to be stored, force binarization on all nodes using “maximum likelihood” Result: OctoMap is an efficient, updatable, multi-resolution representation that explicitly captures unknown regions 14/72 [Wurm et al. 2010] OctoMap, http://octomap.sourceforge.net/ http://octomap.sourceforge.net/

15. 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) 15/72 How to correct or avoid drift?

16. Solution #1: Assume trajectory known Original Octomap assumed prior knowledge of entire sensor trajectory  not on-line 16/72 [Wurm et al. 2010] (44 x 18 x 3 m)

17. Solution #2: Use Manhattan world assumption Manhattan world assumes all planes are axis-aligned (parallel or perpendicular):  not on-line 17/72 [Furukawa et al. 2009] estimated walls (suffer from drift) actual walls actual robot position estimated robot position Rotational drift leads to severe error: 1 degree angle error yields 2 m position error (after 100 m) But Manhattan  zero rotational drift!

18. Implementation 18/72 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 robot trajectorywall

19. Robot pose found on-line using PoseSLAM from incremental Smoothing and Mapping (iSAM) Factor graph (undirected bipartite) Compute MAP estimate over all measurements Computing robot pose 19/72 relative position from robot odometry (O i ) and visual registration (V i ) global orientation from Manhattan (W i ) robot pose GTSAM, https://collab.cc.gatech.edu/borg/gtsam/ https://collab.cc.gatech.edu/borg/gtsam/

20. Laboratory with adjoining rooms: RO RO+VR RO+M RO+VR+M Results 20/72 (10.6 x 20.6 m)

21. Long corridor (loop closure not possible) RO+VR RO+VR+M Results 21/72 (23.9 x 47.8 m)

22. Results 22/72 (52.6 x 53.2 m) Large building (no loop closure) RO+VR

23. Results 23/72 (52.6 x 53.2 m) Large building (no loop closure) RO+VR+M

24. (approximate 2D Euclidean position error at intersections) Quantitative measurements 24/72

25. Size of OctoMaps Octree reduces storage requirements by 3 orders of magnitude: 25/72

26. 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 on- line, large-scale mapping 26/72

27. Thanks! 27/72