efficient scene simulation for robust monte carlo ... · efficient scene simulation for robust...
TRANSCRIPT
Efficient Scene Simulation for Robust Monte
Carlo Localization Using an RGB-D Camera
Maurice Fallon, Hordur Johannsson
and John Leonard
RGB-D Vision SLAM
Kinect has fueled new interest in Vision SLAM:
• 3D maps [Henry et al. ISER 2010]
• Kinect Fusion [Newcombe et al. ISMAR 2011]
Full model requires:
• Views of every room, every direction, less than 5m
range
• Storage requirements for a building 100s of GBs
• Mesh in video: 300MB
Background
-Visual SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Raphael Favier TuE
Henry et al. ISER 2010
Newcombe et al. ISMAR 2011
Extreme 3D motion. Low lighting
Cheap sensors. Dynamic environments
vLocalization
The Case for Visual Localization
Current
vSLAM
capabilities
Other Approaches to Localization
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Convert to simulated ‘LIDAR’ scan:
• Use with LIDAR MCL, limited to 2D with wheel odometry
3D Visual Feature Registration:
• Requires full SfM solution
• Many failure modes
• Potentially more accurate
• Complementary to our
research
H. Johannsson
Simple Input Map
Convert 3D vSLAM output to
Planar Map Representation
• Large indoor planes don’t
often change
• Can simulate views not seen
during SLAM
Scales well:
• Low maintenance
• Supports loop-closures
• Sparse, small file size:
• 2MB for 9 floor MIT Stata
Center
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
convert points to planes
Maintaining Particle Diversity
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Localization must degrade smoothly:
• Model is incomplete or has changed
• When sensor imagery is blurred or uninformative
• People moving, occluding the sensor
Efficient belief propagation required:
• 1000s of particles @ 10-20Hz
Particle Filter Overview
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Propagation
Likelihood
Particle Propagation – Visual Odometry
State Vector:
Propagate using FOVIS:
• Fast Odometry for Vision1
• 0.08m/s mean velocity error
When VO Fails:
• Add extra noise, drift
• Future: IMU integration
[1] A Huang, A Bachrach, P Henry, M Krainin, D Maturana, D Fox, N Roy. Visual
Odometry and Mapping for Autonomous Flight Using an RGB-D Camera, ISRR 2011
http://code.google.com/p/fovis/
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Particle Filter Overview
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Propagation
Likelihood
Likelihood Function – Dense Depth
Kinect/RGB-D:
• Dense disparity information
• Uncertainty increases quickly
with range
Our approach:
• Efficient depth image simulation
using GPU
• Naturally captures uncertainty
• Uses long range measurement
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
20m
away
Example Depth Image Simulation
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Likelihood Function Evaluation
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Likelihood for pixel i (evaluated on disparity):
Parallel Depth Image Simulation
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
• Virtual camera array created using particle poses
• Each scene simultaneously rendered
• Likelihood function evaluated in parallel on GPU using GLSL
• Final likelihoods transferred back to CPU for reweighting and resampling.
Measurements decimated to 20x15
Human Portable
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Extensive Testing
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
1.2m/s, 0.48m 1.05m/s, 0.66m 0.47m/s, 0.3m
Paper results out of date
Significant optimization since
2D works with 10s particles
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Fast, Cheap and In Control
x4 realtime
Summary
Contribution:
• Efficient simulation of 1000s of model views
• Robust Localization using only RGB-D
• Extensive testing
• Including closed loop
Open Source Code:
• FOVIS VO Library
• Integrated within Point Cloud Library:
• pcl::simulation
Future Work:
• IMU
• Color [Mason et al, ICRA 2011]
• Bag of Words Visual Recovery
• Barometry
• Integration with Lifelong vSLAM
Background
-Vision SLAM
-Input Map
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Bayesian Filters
Grid Based Methods
• Cannot scale to high dimensionality
Kalman Filter
• Optimal linear systems in Gaussian noise
• Extended Kalman Filter
• Non-linear using local linearization (may be incorrect)
• Data must still be Gaussian
State Estimation
-Traditional
-Stochastic
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Particle Filter – Overview 2
Implementation Issues:
• Degeneracy
• High dimensionality
• Importance Sampling
M. Isard Microsoft Research
State Estimation
-Traditional
-Stochastic
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Robust Visual Localization
• 2D Laser Localization:
– Wheel Odometry required
– Flat-world assumptions
• Aim: Flexible, mobile, 3D with recovery mechanisms
• Motivating Applications:
State Estimation
-Traditional
-Stochastic
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Monte Carlo Integration
Bayesian numerical integration technique:
• State approximated as a cloud of discrete samples
• Samples weighted according to their likelihoods
• Parameters of the distribution can be estimated from a large set
of samples
State Estimation
-Traditional
-Stochastic
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
A simplistic solution: simulate a laser
State Estimation
-Traditional
-Stochastic
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
• Retain Laser MCL paradigm
[Dellaert, ICRA 1999]:
• Extract non-floor portion of
measurements
• Discard longer ranges (>5m)
• 2D Flatworld approach
• Requires wheel odometry
• Localize versus LIDAR maps or
floor plans
• How to deal with non-Gaussian
noise?
Input: Low-Fi Planar Map
State Estimation
-Traditional
-Stochastic
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
Performance Metrics
State Estimation
-Traditional
-Stochastic
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work
• Error can be significantly reduced if ICP
used on most likely particle
Summary
Summary:
• Efficient simulation of 1000s of
model views
• Robust Localization using only
RGB-D
• Extensive testing
• including closed loop
Open Source Code:
• FoVis Visual Odometry Library
• Simulation Library within Point
Cloud Library: pcl::simulation
Alternative Sources of Information:
• IMU
• Color
• Bag of Words Visual Recovery
• Barometry
State Estimation
-Traditional
-Stochastic
Kinect Localization:
-Visual Odometry
-Depth Simulation
Results:
-Multi-robot demos
-Future work