Accurate On-Line 3D Occupancy Grids
Using Manhattan World Constraints
Brian Peasley and Stan Birchfield
Dept. of Electrical and Computer Engineering
Clemson University
Alex Cunningham and Frank Dellaert
College of Computing
Georgia Tech
Map representations
Point cloud
Elevation map
Multi-level
Surface map
Octree
(occupancy grid)
from Wurm et al., OctoMap: A Probabilistic, Flexible, and Compact
3D Map Representation for Robotic Systems, 2010
2/72
Occupancy grids
[Moravec and Elfes 1985]
An occupancy grid can represent any point cloud with arbitrary precision
3/72
Occupancy grids
[Moravec and Elfes 1985]
Occupied
An occupancy grid can represent any point cloud with arbitrary precision
4/72
Occupancy grids
[Moravec and Elfes 1985]
Occupied
Occupied
Occupied
Free
An occupancy grid can represent any point cloud with arbitrary precision
5/72
Occupancy grids
[Moravec and Elfes 1985]
Occ
Occ
Occ
Free
Free
Occ
Free
Free
Free
Occ
Free
Free
Free
Occ
Free
Free
An occupancy grid can represent any point cloud with arbitrary precision
6/72
Occupancy grids
Leads naturally to a
hierarchical representation: QuadTree (2D)
O
Occupied
O
O
O
F
OOFO OFFF FOFO FFFF
Occupied
Occupied
… and an efficient encoding
Occupied
Occ
Free
Occ Occ Free
Free Occ Free Free
Free Occ Free Free
Free Occ Free Free
1 1110 1101100001010000
1 = Occupied
0 = Free
For continuous surface S (3D),
•
occupancy grid is discretization of
characteristic function cS: R3  {0,1}
• requires ~2 bits per sample point
[Kaufman et al. 1993; Botsch et al. 2002]
7/72
Representing “I don’t know”
Occupancy grids can explicitly represent unknown areas
(which sensor has not yet seen)
sensor
coverage
Occ
Occ
Unk
Unk
Free
Occ
Unk
Unk
Free
Occ
Free
Free
Free
Occ
Free
Free
8/72
Recall
QuadTree (2D) representation:
O
Occupied
O
Occupied
O
O
F
Occupied
OOFO OFFF FOFO FFFF
Occupied
Free
encoding:
1 1110 1101100001010000
Occ
Occ Occ Free
Free Occ Free Free
1 = Occupied
0 = Free
Free Occ Free Free
Free Occ Free Free
9/72
Encoding “I don’t know”
With 3 states, we need 2 bits per node/cell
O
Occupied
O
Occupied
U
O
F
Unk
OOFO UUUU FOFO FFFF
Occupied
Free
Occ
Occ Unk
Unk
Free Occ Unk
Unk
… but this is inefficient:
10 10001001 101001100000000001100110
Free Occ Free Free
Free Occ Free Free
00 = Unknown
01 = Free
10 = Occupied
11 = (wasted)
10/72
Encoding “I don’t know”
Make representation symmetric:
O means confidently completely occupied
F means confidently completely free
U means unconfident
P means parent (mixed)
P
Occupied
P
Occupied
U
P
F
Unk
OOFO UUUU FOFO FFFF
Occupied
Free
Occ
Occ Unk
Unk
Free Occ Unk
Unk
… which is more efficient:
11 11001101 1010011001100110
Free Occ Free Free
Free Occ Free Free
00 = Unknown
01 = Free
10 = Occupied
11 = Parent
11/72
Octrees (3D)
In 3D, we use an octree:
: 00 = U
: 01 = F
: 10 = O
: 11 = P
unknown
free
occupied
12/72
OctoMap
[Wurm et al. 2010]
• OctoMap is a probabilistic octree
• Each node stores likelihood (log odds) of being occupied
cell n
data up to time t
• Clamping update policy saturates leaf nodes to either
minimum or maximum value [Yguel et al. 2007]
 avoids overconfidence
• Automatic:
– If all children are stable (saturated), then prune them
– If future measurements contradict state, children are regenerated
• Whenever map needs to be stored, force binarization
OctoMap, http://octomap.sourceforge.net/
13/72
Problem
• Occupancy grids
– discretize space
 lose information
– assume fixed coordinate system
 cannot be corrected
• SLAM (simultaneous localization and mapping)
– estimate of sensor pose is constantly adjusted
(especially during loop closure)
How to correct or avoid drift?
15/72
Solution #1:
Assume trajectory known
[Wurm et al. 2010]
• Original Octomap assumed prior knowledge
of entire sensor trajectory
(44 x 18 x 3 m)
 not on-line
16/72
Solution #2:
Use Manhattan world assumption
[Furukawa et al. 2009]
• Manhattan world assumes all planes are axis-aligned
(parallel or perpendicular):
estimated robot
position
actual robot
position
actual walls
estimated walls
(suffer from drift)
• Rotational drift leads to severe error:
1 degree angle error yields 2 m position error (after 100 m)
But Manhattan  zero rotational drift!
17/72
Implementation
• Forward-facing Kinect sensor on Pioneer mobile base
• Manually drove robot
• To enforce Manhattan world constraint,
– detect dominant vertical plane(s)
(apply RANSAC to horizontal scan)
– classify based on normal
(just 2 categories)
– only 1 plane needed
• Visual registration
using SURF point
correspondences
wall
robot trajectory
18/72
Computing robot pose
• Robot pose found on-line using PoseSLAM from
incremental Smoothing and Mapping (iSAM)
• Factor graph (undirected bipartite)
• Compute MAP estimate over all measurements
global orientation
from Manhattan (Wi)
robot pose
relative position
from robot odometry (Oi)
and visual registration (Vi)
GTSAM, https://collab.cc.gatech.edu/borg/gtsam/
19/72
Results
• Laboratory with adjoining rooms:
RO
RO+VR
RO+M
(10.6 x 20.6 m)
RO+VR+M
20/72
Results
• Long corridor (loop closure not possible)
RO+VR
RO+VR+M
(23.9 x 47.8 m)
21/72
Results
• Large building (no loop closure)
RO+VR
(52.6 x 53.2 m)
22/72
Results
• Large building (no loop closure)
RO+VR+M
(52.6 x 53.2 m)
23/72
Quantitative measurements
(approximate 2D Euclidean
position error at intersections)
24/72
Size of OctoMaps
• Octree reduces storage requirements
by 3 orders of magnitude:
25/72
Conclusion
• On-line system for accurate 3D map
generation of indoor rectilinear buildings
– Manhattan world assumption with inference on
factor graph to estimate robot position and
orientation
– Zero rotational drift
– Stores in OctoMap (efficient, hierarchical)
• Allows occupancy grids to be used for online, large-scale mapping
26/72
Thanks!
27/72