1. Joydeep Biswas, Manuela Veloso joydeepb@ri.cmu.edu, mmv@cs.cmu.edu

2.  Goal: Indoor Mobile Robot Localization & Mapping  Challenges / Constraints: ▪ Clutter ▪ Volume of data : 9.2 M points/sec! ▪ Real time processing (640x480 @ 30fps) ▪ Limited computation power on robot 2

3. 3  Down – Sampling  Look for geometric features : planes  Hough Transform  Region Growing [Poppinga et al, IROS 2008]  RANSAC based filtering : Fast Sampling Plane Filtering

4. The Problem: (Efficiently) Estimate points P and normals R belonging to planes given depth image image 4

5. Sample point p1, then p2 and p3 within distance η of p1 5

6. Estimate Plane parameters (normal, offset) 6

7. Compute Window size ~ World plane size at mean depth 7

8. Sample l -3 additional points within window 8

9. If fraction of inliers > f, store all inliers + normals 9

10. Do n max times, or until num inlier points > k max 10

11. SceneInliersSampled points (valid depth) Sampling Efficiency (total) FSPF Run Time (ms) (per frame) 12002543436.84 %1.86 22004862024.27 %1.39 32006862023.28 %1.66 42001686119.17 %1.66 520011100918.18 %2.06 1234512345 Tests run on a single core of 3.06GHz Intel Core i7 950 CPU 11

12.  Use Existing 2D Vector Map  Planes generated by extruding lines  Correspond FSPF inlier points to planes (Ignore non-vertical planes)  Use Corrective Gradient Refinement (CGR) [Biswas and Veloso, 2011] MCL for localization 12

13.  Map - List of Geometric Features (Lines)  Use Available Architectural Plans 13

14. Analytic Ray Casting to associate planes from map with observed points p i given robot pose x r 14

15. 1. Associate points p i with line l i, given robot pose x r (Analytic Ray-casting) 2. Compute offset of point from line, d i 3. Compute likelihood of having observed point p i from line l i 4. Combine likelihoods of points, using geometric mean to discount for inter- dependence 15

16. Monte Carlo Localization, with Corrective Gradient Refinement [Biswas et al, To appear in IROS’11] Key Idea: Use state space gradients of observation likelihood to refine proposal distributions (rotation + translation) Efficiently computed analytically due to vector nature of map 16

17. 17 Robot pose hypotheses in orange, projected plane filtered points in red

18.  Mean accuracy: 20cm, 0.5°  Error along halls for lack of features  Robust recovery using CGR  Localizes Cobot (our indoor mobile robot) on multiple floors: 21km Since March ‘11 and counting! 18

19. Sub-problems required for 3D Plane-SLAM:  Plane filtering, polygon construction  Correspondence matching  Pose update  Polygon update 19

20. Used to construct a convex polygon for each neighborhood of plane filtered points For each local neighborhood of “inlier” points : 1. Compute Centroid and Scatter Matrix: 2. Plane Normal and basis vectors found by eigenvector decomposition of S 3. Construct convex hull using Graham scan over 2D projected points 20

21.  Key idea: Decompose Scatter matrix S into two components S = S 1 + nS 2 where S 1 depends only on the absolute location of the points (not relative to the centroid), and S 1 depends only on the centroid: 21

22. Polygons are thus merged as follows: 1. Merged centroid is given by: 2. S 2 m is computed from p m 3. Merged scatter matrix is given by: 4. New convex hull is found as the convex hull of the convex hulls of individual polygons 22

23. 1. Colour index polygons 2. Render scene of polygons on GPU using OpenGL 3. Inspect colour of pixels in rendered image to find matching polygons 4. Runs at > 4000fps (nVidia GTX 460), hence faster than real time! 23

24. Sample rendered scene in hallway: 24

25. 25

26. ScenePolygons per frame Merged PolygonsFSPF + Polygon Merge time (ms) 171142.23 270142.51 38152.37 46772.61 58232.44 1234512345 Tests run on a single core of 3.06GHz Intel Core i7 950 CPU 26

27.  Fast Sampling Plane Filtering  3-DOF Indoor Mobile robot localization using plane filtered points and vector map  Polygon estimation and update from plane filtered point cloud  Point to plane correspondence matching  All algorithms run faster than real time at full resolution and frame rates! 27

28.  Open Source Code (and test Data) of FSPF and Kinect Localization will be available by August 2011  www.cs.cmu.edu/~coral/projects/localization  Contacts:  Joydeep Biswas, joydeepb@ri.cmu.edu  Manuela Veloso, mmv@cs.cmu.edu 28