Fast Sampling Plane Filtering and Indoor Mobile Robots

1. Joydeep Biswas, Manuela Veloso joydeepb@ri.cmu.edu, mmv@cs.cmu.edu Fast Sampling Plane Filtering and Indoor Mobile Robots

2. Motivation • 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

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

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

5. Fast Sampling Plane Filtering Sample point p1, then p2 and p3 within distance η of p1

6. Fast Sampling Plane Filtering Estimate Plane parameters (normal, offset)

7. Fast Sampling Plane Filtering Compute Window size ~ World plane size at mean depth

8. Fast Sampling Plane Filtering Sample l -3 additional points within window

9. Fast Sampling Plane Filtering If fraction of inliers > f , store all inliers + normals

10. Fast Sampling Plane Filtering Do nmax times, or until num inlier points > kmax

11. FSPF - Computational Requirements Tests run on a single core of 3.06GHz Intel Core i7 950 CPU 1 2 3 4 5

12. FSPF For Mobile Indoor Robot Localization • 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

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

14. Sensor Model / Observation Function Analytic Ray Casting to associate planes from map with observed points pi given robot pose xr

15. Sensor Model / Observation Function • Associate points pi with line li, given robot pose xr(Analytic Ray-casting) • Compute offset of point from line, di • Compute likelihood of having observed point pi from line li • Combine likelihoods of points, using geometric mean to discount for inter-dependence

16. Localization: MCL - CGR 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

17. Results (Video) Robot pose hypotheses in orange, projected plane filtered points in red

18. Kinect Localization Results • 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!

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

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

21. Polygon Update • Key idea: Decompose Scatter matrix S into two components S = S1 + nS2 where S1 depends only on the absolute location of the points (not relative to the centroid), and S1 depends only on the centroid:

22. Polygon Update Polygons are thus merged as follows: • Merged centroid is given by: • S2m is computed from pm • Merged scatter matrix is given by: • New convex hull is found as the convex hull of the convex hulls of individual polygons

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

24. Polygon / Point to plane matching Sample rendered scene in hallway:

25. Experimental Results

26. Computational Requirements Tests run on a single core of 3.06GHz Intel Core i7 950 CPU 1 2 3 4 5

27. Summary • 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!

28. Questions? • 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