Geometric Representations & Collision Detection Kris Hauser I400/B659: Intelligent Robotics Spring 2014

3D models in robotics Design Simulation Robot collision detection (i.e. prediction) Proximity calculation Map building Object recognition Grasp planning Etc..

Common Representations Primitives Raw data Point clouds Depth image Polygon soup Surfaces Polygon mesh Parametric curves Subdivision surfaces Implicit surface Volumes Voxels Distance transforms Quad/octree

Aspects to consider How accurately does the geometry need to be represented? What operations need to be performed on the geometry? (Recognition? Matching? Simulation? Collision detection? Visualization? Distance computation?) How quickly? How accurately? Storage and transmission limitations? How easily / reliably / accurately / quickly can the source data (e.g., raw sensor data, CAD models) be converted to the desired format?

Visualization Primitives Raw data Point clouds Depth image Polygon soup Surfaces Polygon mesh Parametric curves Subdivision surfaces Implicit surface Volumes Voxels Distance transforms Quad/octree Easy to convert to polygons Convert to polygons (somewhat expensive) Use specialized rasterization techniques

Easy Conversions From primitives/surfaces to: Polygon soups : discretization Point clouds: sampling Depth images: rasterization From volumes to surfaces: Simple: output a box for each occupied cell (coarse) Marching cubes: walk along the volume until a surface is hit, output a piece of the surface in the volume (accurate)

Harder Conversions From point clouds / soups to surfaces / volumes: a challenge Fill cells that contain points (coarse) Implicit function fitting Silhouette carving Lots of other methods… no “perfect” way to deal with missing data From surfaces to volumes : Fill cells that contain surface (coarse) Euclidean distance transform: wavefront propagation from surface cells (closed surfaces, accurate)

Resolution issues Accuracy, space, & computation complexity determined by resolution h Suppose object size W Point clouds, polygon meshes: O((W/h) 2 ) space Voxels: O((W/h) 3 ) space Downsampling Mesh simplification

Geometric queries Collision detection: When A and B are at hypothetical poses T A and T B, would they collide? Distance calculation When A and B are at hypothetical poses T A and T B, how far are they apart? Time-of-first contact When A and B move along paths T A (t) and T B (t), at what time do they first collide?

Geometric primitives Points, Rays, Lines, Segments, Triangles, Spheres, Ellipses, Boxes Collision detection, distance detection fast operations (O(1)) Ex: segment – sphere collision detection / distance calculation r

Managing large scenes O(n 2 ) pairs of objects – how to check collisions?

Collision Detection Methods Many different methods In particular: Grid method: good for many simple moving objects of about the same size (e.g., many moving discs with similar radii) Closest-feature tracking: good for moving polyhedral objects Bounding Volume Hierarchy (BVH) method: good for few moving objects with complex and diverse geometry

Grid Method  Subdivide space into a regular grid cubic of square bins  Index each object in a bin d

Grid Method d Running time is proportional to number of moving objects Useful also to compute pairs of objects within some distance (vision, sound, …)

Closest-Feature Tracking (M. Lin and J. Canny. A Fast Algorithm for Incremental Distance Calculation. Proc. IEEE Int. Conf. on Robotics and Automation, 1991)  The closest pair of features (vertex, edge, face) between two polyhedral objects are computed at the start configurations of the objects  During motion, at each small increment of the motion, they are updated  Efficiency derives from two observations:  The pair of closest features changes relatively infrequently  When it changes the new closest features will usually be on a boundary of the previous closest features

Closest-Feature Test for Vertex- Vertex Vertex

Application: Detecting Self-Collision in Humanoid Robots (J. Kuffner et al. Self-Collision and Prevention for Humanoid Robots. Proc. IEEE Int. Conf. on Robotics and Automation, 2002)

BVH with spheres: S. Quinlan. Efficient Distance Computation Between Non-Convex Objects. Proc. IEEE Int. Conf. on Robotics and Automation, BVH with Oriented Bounding Boxes: S. Gottschalk, M. Lin, and D. Manocha. OBB-Tree: A Hierarchical Structure for Rapid Interference Detection. Proc. ACM SIGGRAPH '96, Combination of BVH and feature-tracking: S.A. Ehmann and M.C. Lin. Accurate and Fast Proximity Queries Between Polyhedra Using Convex Surface Decomposition. Proc Eurographics, Vol. 20, No. 3, pp , Adaptive bisection in dynamic collision checking: F. Schwarzer, M. Saha, J.C. Latombe. Adaptive Dynamic Collision Checking for Single and Multiple Articulated Robots in Complex Environments, manuscript, Bounding Volume Hierarchy Method

 Enclose objects into bounding volumes (spheres or boxes)  Check the bounding volumes first  Decompose an object into two Bounding Volume Hierarchy Method

 Enclose objects into bounding volumes (spheres or boxes)  Check the bounding volumes first  Decompose an object into two  Proceed hierarchically Bounding Volume Hierarchy Method

 Enclose objects into bounding volumes (spheres or boxes)  Check the bounding volumes first  Decompose an object into two  Proceed hierarchically Bounding Volume Hierarchy Method

BVH is pre-computed for each object Bounding Volume Hierarchy Method

BVH in 3D

Collision Detection Two objects described by their precomputed BVHs A B C D EF G A B C D EF G

Collision Detection A Search tree A A pruning

Collision Detection A CCBCBBCBCB Search tree A A A B C D EF G

Collision Detection CCBCBBCBCB A Search tree pruning A B C D EF G

If two leaves of the BVH’s overlap (here, G and D) check their content for collision Collision Detection CCBCBBCBCB A Search tree GEGEGDGDFEFEFDFD A B C D EF G G D

Variant A CCBCBBCBCB Search tree A A A B C D EF G A CACABABA

Collision Detection Pruning discards subsets of the two objects that are separated by the BVs Each path is followed until pruning or until two leaves overlap When two leaves overlap, their contents are tested for overlap

Search Strategy and Heuristics  If there is no collision, all paths must eventually be followed down to pruning or a leaf node  But if there is collision, it is desirable to detect it as quickly as possible  Greedy best-first search strategy with f(N) = d/(r X +r Y ) [Expand the node XY with largest relative overlap (most likely to contain a collision)] rXrX rYrY d X Y

Recursive (Depth-First) Collision Detection Algorithm Test(A,B) 1.If A and B do not overlap, then return 1 2.If A and B are both leaves, then return 0 if their contents overlap and 1 otherwise 3.Switch A and B if A is a leaf, or if B is bigger and not a leaf 4.Set A 1 and A 2 to be A’s children 5.If Test(A 1,B) = 1 then return Test(A 2,B) else return 0

Performance Several thousand collision checks per second for 2 three- dimensional objects each described by 500,000 triangles, on a 1-GHz PC

Distance Computation M > M, prune

Greedy Distance Computation Greedy-Distance(A,B,M) 1.If min-dist(A,B) > M, then return M 2.If A and B are both leaves, then return distance between their contents 3.Switch A and B if A is a leaf, or if B is bigger and not a leaf 4.Set A 1 and A 2 to be A’s children 5.M  min(max-dist(A 1,B), max-dist(A 2,B), M) 6.d 1  Greedy-Distance(A 1,B,M) 7.d 2  Greedy-Distance(A 2,B,M) 8.Return Min(d 1,d 2 ) M (upper bound on distance) is initialized to infinity

Approximate Distance Approx-Greedy-Distance(A,B,M,  ) 1.If (1+  )min-dist(A,B) > M, then return M 2.If A and B are both leaves, then return distance between their contents 3.Switch A and B if A is a leaf, or if B is bigger and not a leaf 4.Set A 1 and A 2 to be A’s children 5.M  min(max-dist(A 1,B), max-dist(A 2,B), M) 6.d 1  Approx-Greedy-Distance(A 1,B,M,  ) 7.d 2  Approx-Greedy-Distance(A 2,B,M,  ) 8.Return Min(d 1,d 2 ) M (upper bound on distance) is initialized to infinity

Desirable Properties of BVs and BVHs BVs: Tightness Efficient testing Invariance BVH:  Separation  Balanced tree ?

Spheres Invariant Efficient to test But tight?

Axis-Aligned Bounding Box (AABB)

 Not invariant  Efficient to test  Not tight

Oriented Bounding Box (OBB)

 Invariant  Less efficient to test  Tight Oriented Bounding Box (OBB)

Comparison of BVs SphereAABBOBB Tightness---+ Testing++o Invarianceyesnoyes No type of BV is optimal for all situations

Desirable Properties of BVs and BVHs BVs: Tightness Efficient testing Invariance BVH:  Separation  Balanced tree ?

Desirable Properties of BVs and BVHs BVs: Tightness Efficient testing Invariance BVH:  Separation  Balanced tree

Construction of a BVH Top-down construction At each step, create the two children of a BV Example: For OBB, split longest side at midpoint

Computation of an OBB [Gottschalk, Lin, and Manocha, 96]  N points a i = (x i, y i, z i ) T, i = 1,…, N  SVD of A = (a 1 a 2... a N )  A = UDV T where  D = diag(s 1,s 2,s 3 ) such that s 1  s 2  s 3  0  U is a 3x3 rotation matrix that defines the principal axes of variance of the a i ’s  OBB’s directions  The OBB is defined by max and min coordinates of the a i ’s along these directions  Possible improvements: use vertices of convex hull of the a i ’s or dense uniform sampling of convex hull x y X Y rotation described by matrix U