Pose Estimation of Large-

Scale Objects with 3D

Sensors
Robots@Home

Aljosa Osep
Edgar Taktakidze

Bonn, 9.3.2011
Objective

Goal
Tracking of pose of large-scale objects (furniture)

Teach-in
Manual segmentation of objects in 3D scan

Pose tracking
Kinect depth image
Fast scan-matching with ICP
Approach

Extract objects (models) from scene scans

Initial alignment using FPFH to roughly localize

Incrementally register model in scene using ICP

Difference between two alignments tells us about robot's

new pose
Manual Object Segmentation

Take a scan with Kinect, object should be in the scene

Create image mask using RGB image from Kinect
Extract object, masked pixels belong to object
Remove outliers
Initial approach to register with ICP

Should be FAST
Steps
Downsample model and scene
Find correspondences
SVD
Transform model
Problem
Model get's stuck in local minima
Problems with ICP

Works well, when model and object are close enough

When model is too far away from original object, it is very

hard to determine correct correspondences

ICP works on point-level, it doesn't consider any surface

properties

Need better method for good initial guess

Should consider surface properties

Point Feature Histogram Approach

Pose and rotation invariant local features of model

Computed with combining geometrical relations between
point's k-nearest neighbours
FPFH for realtime applications

Image credits: Radu Bogdan Rusu

Fast Point Feature Histogram Approach

O(n*k^2) vs. O(n*k)

Simplified point feature histogram (SPFH)
Computing only relationships for each query point
Redetermine k-neighbors for each point
Use the resulting SPFH values for final histogram:

Image credits: Radu Bogdan Rusu

Sample Consensus Initial Alignment

SAC-IA algorithm for alignment

Randomly select s-sample points in model
For each point, find similar points in scene based on
histogram, randomly select one
Compute rigid transformation based on their
correspondences, calculate error
If error is small enough, add rigid transformation to final
transformation
Initial Alignment with FPFH and Priors

Calculate point clouds and histograms on a few different

resolutions using octree
Try with different resolutions at each step of SAC-IA
algorithm

Image credits: TU Wien, nVidia

Initial Alignment with FPFH and Priors

Prior knowledge
Objects shouldn't be turned upside down (rotation error
metric for pitch and roll)
We assume single object corresponding to model to be
in a scene (otherwise we would be aligning model with
nonsense)
Initial Alignment with FPFH and Priors
Optimizations

ICP
Downsampling for both ICP and FPFH registration
Cut scene around initial guess by FPFH registration
under some threshold

SAC-IA
Pre-calculation of model's features and points at different
resolutions
Results

Tool for capturing point clouds and corresponding RGB

image
Tool for model extraction from scene cloud based on
model's mask
IncrementalRegistration Class
ICP_registration method
FPFH_registration method
FPFH_feature_calculate method
Methods for plotting (histograms, connections, clouds)
Modified SAC-IA algorithm
Uses randomly different model and scene resolutions in
each iteration
Incorporates priors in error metric
Results

Initial registration using FPFH Transform provides good

initial guess
For further registration, we use ICP (speed)
Results
Demonstration
Limitations

Manual object segmentation

If object disappears from scene, we have a problem
Robot shouldn't move too fast for ICP (can be solved with
one FPFH registration step when ICP error too big)
Even FPFH can fail, if model is too similar to something else
(tables like to be walls, for example :))
Future work

Segment models from scans automatically

Incorporate even more priors for FPFH registration
Transform scene scans (remove floor, for example)
Thank you for your attention.