Joydeep Biswas, Manuela Veloso

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

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

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

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

Estimate Plane parameters (normal, offset) 6

Compute Window size ~ World plane size at mean depth 7

Sample l -3 additional points within window 8

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

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

SceneInliersSampled points (valid depth) Sampling Efficiency (total) FSPF Run Time (ms) (per frame) % % % % % Tests run on a single core of 3.06GHz Intel Core i7 950 CPU 11

 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

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

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

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

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 Robot pose hypotheses in orange, projected plane filtered points in red

 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

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

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

 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

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

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

Sample rendered scene in hallway: 24

25

ScenePolygons per frame Merged PolygonsFSPF + Polygon Merge time (ms) Tests run on a single core of 3.06GHz Intel Core i7 950 CPU 26

 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

 Open Source Code (and test Data) of FSPF and Kinect Localization will be available by August 2011   Contacts:  Joydeep Biswas,  Manuela Veloso, 28