Report

Joydeep Biswas, Manuela Veloso [email protected], [email protected] 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 Down – Sampling Look for geometric features : planes Hough Transform Region Growing [Poppinga et al, IROS 2008] RANSAC based filtering : Fast Sampling Plane Filtering 3 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 nmax times, or until num inlier points > kmax 10 Tests run on a single core of 3.06GHz Intel Core i7 950 CPU Scene Inliers Sampled points Sampling Efficiency (valid depth) (total) FSPF Run Time (ms) (per frame) 1 2002 5434 36.84 % 1.86 2 2004 8620 24.27 % 1.39 3 2006 8620 23.28 % 1.66 4 2001 6861 19.17 % 1.66 5 2001 11009 18.18 % 2.06 1 2 3 4 5 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 pi given robot pose xr 14 1. 2. 3. 4. 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 interdependence 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 Robot pose hypotheses in orange, projected plane filtered points in red 17 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 = 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: 21 Polygons are thus merged as follows: 1. Merged centroid is given by: S2m is computed from pm 3. Merged scatter matrix is given by: 2. 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 Tests run on a single core of 3.06GHz Intel Core i7 950 CPU Scene Polygons per frame Merged Polygons FSPF + Polygon Merge time (ms) 1 71 14 2.23 2 70 14 2.51 3 81 5 2.37 4 67 7 2.61 5 82 3 2.44 1 2 3 4 5 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 www.cs.cmu.edu/~coral/projects/localization Contacts: Joydeep Biswas, [email protected] Manuela Veloso, [email protected] 28