Joydeep Biswas,
Manuela Veloso
joydeepb@ri.cmu.edu, mmv@cs.cmu.edu
 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, joydeepb@ri.cmu.edu
 Manuela Veloso, mmv@cs.cmu.edu
28