Fast Sampling Plane Filtering and Indoor Mobile Robots

Joydeep Biswas, Manuela Velosojoydeepb@ri.cmu.edu, mmv@cs.cmu.edu

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