Fast Sampling Plane Filtering and Indoor Mobile Robots
Joydeep Biswas, Manuela [email protected], [email protected]
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
Scene Inliers Sampled points(valid depth)
Sampling Efficiency(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
Tests run on a single core of 3.06GHz Intel Core i7 950 CPU
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
1. Associate points pi with line li, given robot pose xr
(Analytic Ray-casting)2. Compute offset of point from
line, di
3. Compute likelihood of having observed point pi from line li
4. 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 pointsFor 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
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:1. Merged centroid is given by:
2. S2m is computed from pm
3. Merged scatter matrix is given by:
4. New convex hull is found as the convex hull of the convex hulls of individual polygons
23
Correspondence Matching
1. Colour index polygons2. Render scene of polygons on GPU using
OpenGL3. Inspect colour of pixels in rendered image to
find matching polygons4. 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
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
Tests run on a single core of 3.06GHz Intel Core i7 950 CPU
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, [email protected] Manuela Veloso, [email protected]