Efficient Nearest Neighbor Searching for Motion Planning

1
Generating Uniform Incremental Grids on SO(3)
Using the Hopf Fibration
Anna Yershova1, Steven M. LaValle2,and Julie C.
Mitchell3 1Dept. of Computer Science, Duke
University 2Dept. of Computer Science, University
of Illinois at Urbana-Champaign 3Dept. of
Mathematics, University of Wisconsin December 8,
2008
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
2
Introduction
Presentation Overview

• Introduction
• Motivation
• Problem Formulation
• Properties and Representations of the space of
  rotations, SO(3)
• Literature Overview
• Method Presentation
• Conclusions and Discussion

Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
3
Introduction
Motivation
Sampling SO(3) Occurs in

• Automotive Assembly
• Computational Chemistryand Biology
• Manipulation Planning
• Medical applications
• Computer Graphics(motions for digital actors)
• Autonomous vehicles andspacecrafts

Courtesy of Kineo CAM
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
4
Introduction
Motivation
Our Main Motivation Motion Planning
The graph over C-space should capture the path
connectivity of the space
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
5
Introduction
Motivation
Our Main Motivation Motion Planning

• The quality of the undelying samples affect the
  quality of the graph
• SO(3) is often the C-space

Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
6
Introduction
Problem Formulation
Problem Formulation
Desirable properties of samples over the SO(3)

• uniform
• deterministic
• incremental
• grid structure

Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
7
Introduction
Problem Formulation
Problem Formulation
Desirable properties of samples over the SO(3)

• uniform
• deterministic
• incremental
• grid structure

Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
8
Introduction
Problem Formulation
Problem Formulation
Desirable properties of samples over the SO(3)

• uniform
• deterministic
• incremental
• grid structure

Deterministic The uniformity measures can be
deterministically computed Reason resolution
completeness
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
9
Introduction
Problem Formulation
Problem Formulation
Desirable properties of samples over the SO(3)

• uniform
• deterministic
• incremental
• grid structure

Incremental The uniformity measures are
optimized with every new point Reason it is
unknown how many points are needed to solve the
problem in advance
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
10
Introduction
Problem Formulation
Problem Formulation
Desirable properties of samples over the SO(3)

• uniform
• deterministic
• incremental
• grid structure

Grid Reason Trivializes nearest neighbor
computations
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
11
SO(3) Properties
SO(3) Topology, Manifold Structure

• SO(3) is a Lie group
• SO(3) is diffeomorphic to S3 with antipodal
  points identified
• Haar measure on SO(3) corresponds to the surface
  measure on S3
• SO(3) has a fiber bundle structure
• Fibers represent SO(3) as a product of S1 and S2.
  Locally it is a Cartesian product
• Remark sampling on spheres and SO(3) are related

Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
12
SO(3) Properties
SO(3) Parameterizations and Coordinates

• Euler angles
• Axis angle representation (topology)
• Spherical coordinates (topology, Haar measure)
• Quaternions (topology, Haar measure, group
  operation)
• Hopf coordinates (topology, Haar measure, Hopf
  bundle)

Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
13
Literature Overview
Literature overview
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
14
Literature Overview
Euclidean Spaces, 0,1d
uniform deterministic incremental - grid
structure
uniform deterministic incremental - grid
structure
uniform - deterministic incremental - grid
structure
Halton points
Hammersley points
Random sequence
uniform deterministic - incremental grid
structure
uniform deterministic - incremental grid
structure
Sukharev grid
A lattice
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
15
Literature Overview
Euclidean Spaces, 0,1d

• Layered Sukharev Grid Sequence
• Lindemann, LaValle 2003

uniform deterministic incremental grid
structure
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
16
Literature Overview
Spheres, Sd, and SO(3)

• Random sequences
• subgroup method for random sequences SO(3)
• almost optimal discrepancy random sequences for
  spheres
• Beck, 84 Diaconis, Shahshahani 87 Wagner,
  93 Bourgain, Linderstrauss 93
• Deterministic point sets
• optimal discrepancy point sets for Sd, SO(3)
• uniform deterministic point sets for SO(3)
• Lubotzky, Phillips, Sarnak 86 Mitchell 07
• No deterministic sequences to our knowledge

uniform - deterministic incremental - grid
structure
uniform deterministic - incremental - grid
structure
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
17
Literature Overview
Our previous approach Spheres
uniform deterministic incremental grid
structure

• Make a Layered Sukharev Grid sequence inside each
  face
• Define the ordering across faces
• Combine these two into a sequence on the cube
• Project the faces of the cube outwards to form
  spherical tiling
• Use barycentric coordinates to define the
  sequence on the sphere
• Yershova, LaValle, ICRA 2004

Ordering on faces Ordering inside faces
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
18
Literature Overview
Our previous approach Cartesian Products

• Make grid cells inside X and Y
• Naturally extend the grid structure to X ? Y
• Define the cell ordering and the ordering inside
  each cell

Y
X
Lindemann, Yershova, LaValle, WAFR 2004
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
19
Method Presentation
Our approach SO(3)

• Hopf coordinates preserve the fiber bundle
  structure of R P3
• Locally, R P3 is a product of S1 and S2

Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
20
Method Presentation
Our approach SO(3)

• The method for Cartesian products can then be
  extended to R P3
• Need two grids, for S1 and S2

Grid on S2
Grid on S1
Healpix, Gorski,05
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
21
Method Presentation
Our approach SO(3)

• The method for Cartesian products can then be
  extended to R P3
• Need two grids, for S1 and S2

Grid on S1
Grid on S2
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
22
Method Presentation
Our approach SO(3)

• The method for Cartesian products can then be
  extended to R P3
• Need two grids, for S1 and S2
• Ordering on cells, ordering on 0,13

uniform deterministic incremental grid
structure
Grid on S1
Grid on S2
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
23
Method Presentation
Propositions

• The dispersion of the sequence T on SO(3) at the
  resolution level l is
• in which is the dispersion of the
  sequence over S2.
• Note The best bound so far to our knowledge.
• The sequence T has the following properties
• The position of the i-th sample in the sequence T
  can be generated in O(log i) time.
• For any i-th sample any of the 2d nearest grid
  neighbors from the same layer can be found in
  O((log i)/d) time.

Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
24
Method Presentation
Illustration on Motion Planning

• Configuration space SO(3)

(a)
(b)
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
25
Conclusions
Conclusions
1. We have designed a sequence of samples over
the SO(3) which are

• uniform
• deterministic
• incremental
• grid structure

2. Main point Hopf coordinates naturally
preserve the grid structure on SO(3). (Subgroup
aglorithm by Shoemake implicitly utilizes them)
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)
26
Conclusions
Conclusions
Thank you!
Anna Yershova, et. al.
Uniform Incremental Grids on SO(3)