Efficient Proximity Search for 3-D Cuboids Jie Gao1 1
⋆
and Rakesh Gupta2
Department of Computer Science, Stanford University, Stanford, CA 94305, USA
[email protected] 2 Honda Research Institute USA, Inc. 800 California Street, Suite 300, Mountain View, CA 94041, USA
[email protected]
Abstract. In this paper, we give the definition for the voronoi diagram and its dual graph – Delaunay triangulation for 3D cuboids. We prove properties of the 3D Delaunay triangulation, and provide algorithms to construct and update the Delaunay triangulation. The Delaunay triangulation data structure is used to perform proximity searches for both static and kinetic cases. We describe experimental results that show how the Delaunay triangulation is used on a mobile robot to model, understand and reason about the spatial information of the environment.
1
Introduction
In this paper, we describe the Delaunay triangulation spatial data structure for 3D cuboids for a mobile robot, that allows it to do simple reasoning such as spatial relationship between two objects, or finding objects in the neighborhood of a given object. Specifically, given a query volume of an arbitrary shape, the goal is to find all the cuboids that intersect with the query volume. When the query volume varies with time, we want to keep track of all the intersected cuboids. We address the static and kinetic proximity search problem among a set of n cuboids in 3D. Information about the environment is obtained and accumulated as the robot explores the world. Our mobile robot needs to build a 3D spatial model according to the partial information obtained in sequential frames, and update the model as more information comes in. Moreover, we need to support efficient queries in the egocentic frame of reference, where the environment is modeled relative to the robot. Typical approaches for proximity search group objects by proximity into hierarchies, and store them in another data structure such as a kd-tree or R-tree [13]. These conventional data structures do not work well for our purpose. First, kd-tree or quad-tree, do not support efficient sequential insertion and deletion. R-tree may generate very unbalanced tree structure when the data comes in sequentially. Second, kd-tree and R-tree support only standard query ⋆
This work was done when the author was working at Honda Research Institute USA, Inc.
volumes, like boxes in 3D and rectangles in 2D. For our purpose, the query volume can be what the robot is currently viewing or has seen before, which might be a cone or some other flexible volume. In addition, proximity search in kd-tree or R-tree is not fast enough when the number of objects is large [4, 1]. The Voronoi diagram and its dual Delaunay triangulation [2, 6, 4] provide a natural framework for reasoning about objects in space. Voronoi model maps well to natural language, and how humans judge and express measurement, and is able to provide an answer without referring to metrical details. Delaunay triangulation representation can be built efficiently using a randomized incremental algorithm. This representation is easy to update as more information about objects comes in. Another nice property of Delaunay triangulation is that it allows efficient proximity searches, especially when the robot is moving around. The Voronoi diagram [2] of 3D points have been studied extensively. Proximity search for points can be solved in O(log n + k) time, where k is the number of points inside the query volume and n is the number of points. For convex polygons in the plane, McAllister et. al. defined a compact piecewise-linear Voronoi diagram [12]. Govrilova studied the generalized Voronoi diagram on weighted sites under Euclidean, power, Manhattan and supremum metrics in 2 and higher dimensions [8]. She also applied the generalized Voronoi diagram on collision detection and proximity search. In this paper we extend the Voronoi diagram and Delaunay triangulation to 3D cuboids. We gives definitions of Voronoi diagram and Delaunay triangulation for 3D convex objects in section 2. We also prove the local property of Delaunay triangulation. Section 3 shows how proximity queries can be performed using Delaunay triangulation, for both static and kinetic cases. We implemented the algorithm in the scene understanding for mobile robots. Details of the construction of the Delaunay triangulation is discussed in section 4. Section 5 uses experiments to show how the Delaunay triangulation helps the mobile robot in spatial reasoning about the scene.
2
Definition
We represent objects using polytopes. Suppose we have a set of disjoint polytopes S = (P1 , P2 , ...Pn ). A distance function δ between two points can be extended to a point a and a polytope P in R3 by setting δ(a, P ) = minq∈P δ(a, q). For any point a outside a polytope P , there is a unique point p on P realizing the distance δ(a, P ). We say line segment ap is a spoke and p is the attachment point. Obviously δ(a, P ) = δ(a, p). For two disjoint polytopes P1 and P2 , the bisector π(P1 , P2 ) between them is defined to be the locus of points that are equidistant from them. Three polytopes are called collinear if there is a plane tangent to them simultaneously. Four polytopes are called cocircular if there is a ball tangent to them simultaneously.
2.1
Voronoi Diagram
The generalized Voronoi diagram V (S) for polytopes is defined similarly as for points. We partition the free space into a number of Voronoi cells according to the nearest object. All the points in one Voronoi cell have the same nearest object. We denote the Voronoi cell for polytope Pi as Vi . A Voronoi vertex of four polytopes P1 , P2 , P3 , P4 , is a point v that is equidistant to all of them. There is a sphere centered at v that is tangent to all the four polytopes called the Voronoi sphere. The Voronoi sphere of a valid Voronoi vertex is free of any objects. In another point of view, v is also the intersection point of bisectors π(Pi , Pj ), i 6= j, 1 ≤ i, j ≤ 4. The Voronoi vertex defined by four points uniquely exists. The Voronoi cells for points are convex polytopes. However, for four polytopes, there could be zero or more than one Voronoi vertices, the Voronoi cells for polytopes may not be as regular as convex polytopes. We will talk about the Voronoi vertex computation in later sections. We say that a set X is star-shaped with respect to a convex polytope P , if P ⊆ X and every spoke spoke(a, P ) is contained in X, with a ∈ X. Lemma 1. The Voronoi cell V is star-shaped with respect to P . Proof. Firstly P ⊆ V . We only need to prove that if the nearest neighbor of a point a is P , then for any point q on the spoke spoke(a, P ), its nearest neighbor is P , too. We draw a sphere Sa centered at a, then Sa is free of any objects and Sa is tangent with the object P , with the tangent point p. Draw a sphere Sq centered at q with radius qp, see Figure 1. Sq is fully contained in Sp , so it is also outside any other objects. Then the nearest object of q must be P .
P p
V
Sq
q a
Sa
Fig. 1. The Voronoi cell is star-shaped
2.2
Delaunay Triangulation
The Delaunay triangulation T (S) is the dual graph of the Voronoi diagram V (S). For each pair of adjacent Voronoi cells, we connect the corresponding centroids of the cuboids and get the Delaunay triangulation. For any four polytopes P1 , P2 , P3 , P4 , if there is a corresponding Voronoi vertex defined by them, then the Voronoi cells Vi and Vj (i 6= j) must be adjacent. We define a junction tetrahedron corresponding to each Voronoi vertex. The vertices of the junction tetrahedron are the tangent points of each cuboid with the Voronoi sphere. It is easy to see that the circumsphere of the junction tetrahedron is the Voronoi sphere. So the junction tetrahedron is outside any of the objects. Two Voronoi vertices are adjacent if they are incident to three common objects. We also define the corresponding junction tetrahedrons as adjacent. We define the neighboring polytopes for a junction tetrahedron to be the polytopes incident to it or to one of its adjacent junction tetrahedrons. The Delaunay triangulation can be built from the Voronoi diagram, by connecting two objects if there exists a Voronoi vertex incident to both of them. The worse-case complexity of the Delaunay triangulation for 3D cuboids can be Θ(n2 ). But for all practical purposes, three-dimensional Delaunay triangulation is observed to have linear complexity [7]. It is well known that the Delaunay triangulation for points has a local property. Specifically, if the circumcircle of a triangle doesn’t contain any of its neighbors, we define this triangle to be locally Delaunay. The local property says that the triangulation is proved to be a Delaunay triangulation if all the triangles are locally Delaunay. It is also extendable to our case. We say that one junction tetrahedron is locally Delaunay if its circumsphere doesn’t intersect with any of its neighboring polytopes. Clearly, if the triangulation is a Delaunay triangulation, all the junction tetrahedrons are Delaunay tetrahedrons. We will show that vice versa is also true. This property can be used in the incremental construction and dynamic update of the Delaunay triangulation. To prove the local property, we first state some properties of the local Delaunay triangulation. Define fe the sector of the sphere chopped off by the face f of a junction tetrahedron T . Lemma 2. If the junction tetrahedrons of the triangulation have the local Delaunay property, f1 is a face of a tetrahedron T1 , f2 is a face of a tetrahrdron T2 , T1 T and T2 are adjacent, f1 and f2 are incident to the same three objects, then fe1 fe2 are free of any objects. Proof. T By contradiction, if there is a point p inside some object P , and p is inside fe1 fe2 . So there must exist a tetrahedron of P, P1 , P2 , P3 , with P1 , P2 , P3 as the tangent objects of both f1 and f2 . This contradicts with the local Delaunay property. Theorem 1. If all the junction tetrahedrons are locally Delaunay, then the triangulation T (S) is a Delaunay triangulation.
Proof. We prove by contradiction. The idea in this proof comes from the local property of the Delaunay triangulation for 2D polygons [10]. Assume that T (S) is not a Delaunay triangulation. Then one of the Voronoi vertices is not valid, which means there exists a point p interior to one of the polytopes in S such that p is inside a Voronoi sphere S1 . Suppose the tetrahedron corresponding to the Voronoi sphere is T1 . By definition of the junction tetrahedron, T1 is outside of any polytopes. For a face f1 of T1 , denote by fe1 the sector of the sphere chopped off by the face f1 , denote by c1 the intersection (a disk) of the sphere S1 and the plane defined by f1 . Since p ∈ S1 and p is not in T1 , then p ∈ fe1 for some face f1 of T1 . We define the solid angle from p to the disk c1 to be θ(p, c1 ). Among all the junction tetrahedrons whose circumspheres contain p, let T1 be the one with the maximum θ(p, c1 ) and let f1 be the face of T1 so that p ∈ fe1 , see Figure 2 (P3 appears twice in the figure because we can not draw it since this situation won’t happen.).
P1 S1 T1
f2
p S2
P4
f1 P3 P2
T2 P4′
P3
Fig. 2. Proof of the local property
Suppose the objects that are tangent with the face f1 are P1 , P2 , P3 . Consider the tetrahedron T2 that shares the same three objects with face f1 . Denote the corresponding face incident to P1 , P2 , P3 as f2 . Claim that there exists a T2 such that p is inside its circumsphere S2 . This comes from the local Delaunay property, i.e., p is not inside any of the neighboring polytopes of T1 . If all the circumspheres of T1 ’s neighboring junction tetrahedrons don’t contain p, then either fe1 is free of any objects, which contradicts with the assumption that p is inside fe1 , or there exists a junction tetrahedron defined for P, P1 , P2 , P3 which contradicts the local Delaunay property. From lemma 2, we know that p is not inside fe2 . Suppose the intersection of S2 (S1 ) with the plane defined by f2 is c2 (c′2 ). Clearly c′2 is inside c2 . So the
solid angle θ(p, c2 ) > θ(p, c′2 ) > θ(p, c1 ), which contradicts with the maximality of θ(p, c1 ).
3
Proximity Query in Delaunay Triangulation
The Delaunay triangulation can be used to perform efficient proximity queries. Specifically, given a query volume, we want to find all the objects intersecting with the query volume. Furthermore, we can keep track of the objects inside the query volume as the query volume is moving continuously. We explain the two cases in the following part. 3.1
Static Proximity Query
Delaunay triangulation can be used in the proximity search. Intuitively, in the Delaunay triangulation, two objects are connected if they are near each other in some direction. To find all the objects intersecting a query volume, we start from a initial object, which is known to be inside the query volume. Then, we check its neighbors in the Delaunay triangulation. Specifically, we keep a suspect list, which contains the objects that might be inside the query volume. The list is initialized to contain the initial seed. Each time we pop out one object, if it is inside the area, we put all its unchecked neighbors into the suspect list and continue the process until the list is empty. It is easy to prove that the method gives the correct result. If one object P intersecting the query volume isn’t in the output, then P is not put in the suspect list, which implies that all P ’s neighbors are not in the list. So P is in a different connected component with the initial object, which contradicts with the connectivity of the Delaunay triangulation. In the algorithm we assume that an object inside the query volume is known. In practice, the initial value can be obtained efficiently. For example, a moving robot subtracts information from the environment and build up the Delaunay triangulation incrementally. A typical task for the robot is to track a set of objects visible to the robot. So we can easily make use of the locality information and the initial value can always be obtained from the result in the last time step. The running time of the algorithm is O(f (n) + k), where f (n) is the time used to find the initial seed, and k is the number of objects intersecting with the query volume. 3.2
Kinetic Proximity Query
In the example above, the proximity query is done around a moving object, say, a robot. Changes to the proximity search result only happen in discrete time, when an object enters or leaves the query volume. This fits perfectly into the Kinetic Data Structure(KDS for short) framework [3] proposed by Bash et. al. In a KDS, we assume the robot follows a posted flight plan, though it can change it at any moment. The data structure will detect the change and update accordingly. The correctness of the structure is certified by a set of conditions, i.e., certificates.
Each certificate has a failure time, which is inserted as events in a event queue. Only when a certificate fails, the KDS certification repair mechanism is invoked to repair the certificate set and the result as well. For a survey of KDS on various problems, see [9]. The kinetic proximity search maintains a set of certificates. The query volume divides R3 into 2 point sets, the inside and the outside. An object is called inside (outside) object if it is completely inside (outside) the query volume. An object is called an overlapping object if it intersects both inside and outside sets. We say an object is on the boundary, if it is an overlapping object, or has a neighbor in the Delaunay triangulation which is not of the same type with itself. Intuitively the set of objects inside the query volume will not change until one boundary object moves in or out of the query volume. Instead of computing the visible objects of the robot from scratch for every time step, we can instead maintain the set of visible objects as well as the boundary objects and update them whenever a boundary object moves in or out. Specifically, we have: – An overlapping object P moves completely inside or outside. Its neighbors need to be updated. Some of them might be changed from boundary objects to non-boundary objects. When P moves totally outside, the query result needs to be changed as well. – A boundary and non-overlapping object P becomes an overlapping object. Similarly, its neighbors need to be changed to boundary objects. P is inserted into the query result.
(i)
(ii)
Fig. 3. Illustration of the maintenance of the objects inside the query volume. Boundary objects are shown as solid dots.
This method can be extended to the moving cuboids as well. Basically in addition to maintaining the boundary objects, we also need to maintain the Delaunay triangulation when the cuboids are moving around. We add more events to the system that certify the validity of a Voronoi vertex, in a standard KDS approach [9]. When an event fails, i.e., a Voronoi vertex becomes invalid, the Delaunay triangulation need to be updated. The update to the Delaunay triangulation involves only local flipping operation. So the update cost is O(1) for
each event. One problem here is that computing when there exists a ball that touches five cuboids doesn’t have closed form solution. So finding the failure time of a Voronoi vertex event can only be done numerically. We will talk more about this in the next section.
4
Computing the Delaunay Triangulation
Construction of the Delaunay triangulation for cuboids is much harder than the case of points, especially because the Voronoi sphere has no closed-form solution. Finding the Voronoi vertex is equivalent to finding a sphere that is tangent to four objects at the same time. We reduce this problem to an optimization problem and use the FSQP optimization algorithm [11]. For details, suppose the four objects are P1 , P2 , P3 , P4 respectively, the center of the sphere is X = (x1 , x2 , x3 ), the radius of the sphere is r. Then the objective function we want to minimize is P4 (δ(X, Pi ) − r)2 . The optimization problem needs an initial value, which is i=1 obtained by computing the circumsphere defined by four points on each object. For the cube case, we choose the centers of the four objects. Considering that there might exist more than 1 Voronoi vertices for 4 objects and the FSQP optimization algorithm depends on the intial value, we have an alternate strategy if the optimation fails to find a Voronoi vertex. We retry with a different initial value, say the circumsphere defined by four points, with the four points being any of the vertices of the four cuboids. Experiments show that the method is very fast so that we can assume the computation takes O(1) time. This method is extendable to convex objects, except that the computation of the distance from a point to a convex polytopes with m faces, costs O(log m) [5]. To compute the shortest distance from a point p to an arbitrary convex polytope P , we can compute the distance from p to a simplex whose vertices are vertices of the convex polytope. This value is a upper bound on the shortest distance since the simplex is entirely inside the polytope. The value decreases monotonically to the minimum when we refine the simplex. Combined with a hill climbing method, we can get the shortest distance in O(log m), where m is the number of faces of the convex polytope. A well-known algorithm to construct the Delaunay triangulation for points is the randomized incremental algorithm. This algorithm can also be extended to 3D polytopes. The objects are inserted randomly and the Delaunay triangulation is updated upon insertion of new objects. By similar computation as in [4], the expected running time of this algorithm is O(n log n), where n is the number of objects, by assuming that finding the Voronoi vertex of four cuboids takes O(1). Upon deletion of one object P , we first delete all the Voronoi vertices incident to the object. The original Delaunay triangulation is left with a hole, with P ’s neighbors as the boundary. The hole is then filled by re-triangulating the boundary objects. Similarly if the position or the shape of a object P is changed. We only need to check the Voronoi vertices incident to P . If any of the Voronoi vertices are not valid, then they are deleted and the hole is filled by re-triangulating the boundary objects.
5
Experiments
We build a spatial data structure following the top-down approach. Object information is extracted from a stereo camera when the robot is exploring the environment. Information about objects and humans extracted from the scene is put in the spatial data structure. We extract object location and size information and organize them in 3D Delaunay triangulation that allows efficient proximity queries. We do not compute the Voronoi cells but only the Delaunay edges which are sufficient to determine proximity relationships. We build a 3D spatial model according to the partial information obtained in sequential frames, and update the Delaunay triangulation as more information comes in. The Delaunay triangulation is used to compute a number of linguistic concepts of space such as near, behind, between, inside, and among. Figure 4 (i) shows a query for objects on the table. To determine objects that satisfy the query, we’ll only need to search in the local neighborhood of the object using the Delaunay triangulation data structure. This kind of query will be very efficient even if there are hundreds of objects in the scene. An example with more objects is shown in figure 4 (ii). Here the objects intersecting the view frustum of the robot are shown as solid. The time taken for both queries is essentially same and independent of total number of objects in the environment.
(i)
(ii)
Fig. 4. (i) The objects returned by the query on the table are shown solid. Delaunay edges are shown going from centroid of the cuboids. (ii) The objects inside the view frustum of the robot are shown solid.
6
Summary
In this paper we describe work towards spatial scene understanding for a mobile robot. We have provided proofs for the local property of Delaunay triangulation. We have shown how proximity queries can be performed using Delaunay triangulation. We propose a method for building a world model using sequential data
from a mobile stereo camera. Objects are modeled as 3D cuboids and organized in a Voronoi data structure, which supports efficient proximity searches. There are multiple advantages of this representation. First, the data can be incremental and we do not need to see full object in a view to model its spatial properties. Second, using this data structure allows efficient spatial queries in the robot egocentric frame of reference. We can efficiently answer queries about relationship among objects in the scene or objects in the proximity of a given object. Such a world model with a shared spatial context allows natural interaction between the human and mobile robot. The human can then effectively instruct the robot on where to go and what to look for.
References 1. P. Agarwal, M. de Berg, J. Gudmundsson, M. Hammar, and H. Haverkort. Boxtrees and R-trees with near-optimal query time. In Proceedings of the 17th ACM Symposium on Computational Geometry (SoCG 01’), pages 124–133, June 2001. 2. F. Aurenhammer. Voronoi diagrams: A survey of a fundamental geometric data structure. ACM Comput. Surv., 23(3):345–405, Sept. 1991. 3. J. Basch, L. J. Guibas, and J. Hershberger. Data structures for mobile data. J. Alg., 31(1):1–28, 1999. 4. M. de Berg, M. van Kreveld, M. Overmars, and O. Schwarzkopf. Computational Geometry: Algorithms and Applications. Springer-Verlag, Berlin, 1997. 5. D. W. J. E. G. Gilbert and S. S. Keerthi. A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE Trans. Robotics and Automation, 4(2):193–203, April 1988. 6. G. Edwards and B. Moulin. Toward the simulation of spatial mental images using the voronoi model. Representation and Processing of Spatial Expressions, pages 163–184, 1997. 7. J. Erickson. Dense point sets have sparse delaunay triangulations. In Proceedings of the 13th Annual ACM-SIAM Symposium on Discrete Algorithms, pages 125–134, 2002. 8. M. Gavrilova. Proximity and Applications in General Metrics. PhD thesis, Dept. of Computer Science, The University of Calgary, 1998. 9. L. J. Guibas. Kinetic data structures — a state of the art report. In P. K. Agarwal, L. E. Kavraki, and M. Mason, editors, Proc. Workshop Algorithmic Found. Robot., pages 191–209. A. K. Peters, Wellesley, MA, 1998. 10. L. J. Guibas, J. Snoeyink, and L. Zhang. Compact voronoi diagrams for moving convex polygons. In 7th Scandinavian Worshop on Algorithm Theory, 2000. 11. C. Lawrence, J. L. Zhou, and A. L. Tits. User’s guide for cfsqp version 2.5: A c code for solving (large scale) constrained nonlinear (minimax) optimization problems, generating iterates satisfying all inequality constraints. Technical Report Institute for Systems Research TR-94-16R1, University of Maryland, College Park, 1997. 12. M. McAllister, D. G. Kirkpatrick, and J. Snoeyink. A compact piecewise-linear voronoi diagram for convex sites in the plane. In IEEE Symposium on Foundations of Computer Science, pages 573–582, 1993. 13. O. Procopiuc. Data structures for spatial database systems.
