accurate on-line 3d occupancy grids using manhattan world constraints
DESCRIPTION
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. Elevation map. Point cloud. - PowerPoint PPT PresentationTRANSCRIPT
Accurate On-Line 3D Occupancy GridsUsing Manhattan World Constraints
Brian Peasley and Stan BirchfieldDept. of Electrical and Computer Engineering
Clemson University
Alex Cunningham and Frank DellaertCollege of Computing
Georgia Tech
Map representations
2/72
Point cloud Elevation map
Octree(occupancy grid)Multi-level
Surface map
from Wurm et al., OctoMap: A Probabilistic, Flexible, and Compact3D Map Representation for Robotic Systems, 2010
Occupancy grids
3/72
An occupancy grid can represent any point cloud with arbitrary precision
[Moravec and Elfes 1985]
Occupancy grids
4/72
An occupancy grid can represent any point cloud with arbitrary precision
[Moravec and Elfes 1985]
Occupied
Occupancy grids
5/72
An occupancy grid can represent any point cloud with arbitrary precision
[Moravec and Elfes 1985]
Occupied
Occupied Occupied
Free
Occupancy grids
6/72
An occupancy grid can represent any point cloud with arbitrary precision
[Moravec and Elfes 1985]
Occ Occ Occ Free
Free Occ Free Free
Free Occ Free Free
Free Occ Free Free
Occupancy grids
7/72
Leads naturally to a hierarchical representation: QuadTree (2D)
… and an efficient encoding
Occupied
Occupied Occupied
Free
Occupied
Occ Occ Occ Free
Free Occ Free Free
Free Occ Free Free
Free Occ Free Free
O
FO O O
O O F O O F F F F O F O F F F F
1 1110 1101100001010000 1 = Occupied0 = 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]
Representing “I don’t know”
8/72
Occupancy grids can explicitly represent unknown areas (which sensor has not yet seen)
Occ Occ Unk
Free Occ
Free Occ Free Free
Free Occ Free Free
sensor coverage
Unk
Unk Unk
Recall
9/72
QuadTree (2D) representation:
encoding:Occupied
Occupied Occupied
Free
Occupied
Occ Occ Occ Free
Free Occ Free Free
Free Occ Free Free
Free Occ Free Free
O
FO O O
O O F O O F F F F O F O F F F F
1 1110 11011000010100001 = Occupied0 = Free
Encoding “I don’t know”
10/72
With 3 states, we need 2 bits per node/cell
… but this is inefficient:Occupied
Occupied Unk
Free
Occupied
Occ Occ Unk Unk
Free Occ Unk Unk
Free Occ Free Free
Free Occ Free Free
10 10001001 10100110000000000110011000 = Unknown01 = Free10 = Occupied11 = (wasted)
O
FO U O
O O F O U U U U F O F O F F F F
Encoding “I don’t know”
11/72
Make representation symmetric: O means confidently completely occupied F means confidently completely free U means unconfident P means parent (mixed)
… which is more efficient:Occupied
Occupied Unk
Free
Occupied
Occ Occ Unk Unk
Free Occ Unk Unk
Free Occ Free Free
Free Occ Free Free
11 11001101 101001100110011000 = Unknown01 = Free10 = Occupied11 = Parent
P
FP U P
O O F O U U U U F O F O F F F F
Octrees (3D)
In 3D, we use an octree:
12/72
unknown
occupied
free
: 00 = U: 01 = F: 10 = O: 11 = P
OctoMap• OctoMap is a probabilistic octree• Each node stores likelihood (log odds) of being occupied
• 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
13/72
[Wurm et al. 2010]
cell n data up to time t
OctoMap, http://octomap.sourceforge.net/
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)
15/72
How to correct or avoid drift?
Solution #1: Assume trajectory known
• Original Octomap assumed prior knowledge of entire sensor trajectory
not on-line
16/72
[Wurm et al. 2010]
(44 x 18 x 3 m)
Solution #2: Use Manhattan world assumption
• Manhattan world assumes all planes are axis-aligned (parallel or perpendicular):
not on-line
17/72
[Furukawa et al. 2009]
estimated walls (suffer from drift)
actual walls
actual robot position
estimated robot position
• Rotational drift leads to severe error: 1 degree angle error yields 2 m position error (after 100 m)
But Manhattan zero rotational drift!
Implementation
18/72
• 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
robot trajectorywall
• Robot pose found on-line using PoseSLAM from incremental Smoothing and Mapping (iSAM)
• Factor graph (undirected bipartite)• Compute MAP estimate over all measurements
Computing robot pose
19/72
relative positionfrom robot odometry (Oi)
and visual registration (Vi)
global orientationfrom Manhattan (Wi)
robot pose
GTSAM, https://collab.cc.gatech.edu/borg/gtsam/
• Laboratory with adjoining rooms:
RO RO+VR RO+M RO+VR+M
Results
20/72(10.6 x 20.6 m)
• Long corridor (loop closure not possible)
RO+VR RO+VR+M
Results
21/72(23.9 x 47.8 m)
Results
22/72(52.6 x 53.2 m)
• Large building (no loop closure)
RO+VR
Results
23/72(52.6 x 53.2 m)
• Large building (no loop closure)
RO+VR+M
(approximate 2D Euclidean position error at intersections)
Quantitative measurements
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 on-line, large-scale mapping
26/72
Thanks!
27/72