Geometry of 3D Environments and Sum of Squares Polynomials

Amir Ali Ahmadi1 Georgina Hall1 Ameesh Makadia2 Vikas Sindhwani3

Fig. 1: Sublevel sets of sos-convex polynomials of increasing degree (left); sublevel sets of sos polynomials of increasingnonconvexity (middle); growth and shrinkage of an sos-body with sublevel sets (right)

Abstract— Motivated by applications in robotics and com-puter vision, we study problems related to spatial reasoningof a 3D environment using sublevel sets of polynomials. Theseinclude: tightly containing a cloud of points (e.g., representingan obstacle) with convex or nearly-convex basic semialgebraicsets, computation of Euclidean distances between two suchsets, separation of two convex basic semalgebraic sets thatoverlap, and tight containment of the union of several basicsemialgebraic sets with a single convex one. We use algebraictechniques from sum of squares optimization that reduce allthese tasks to semidefinite programs of small size and presentnumerical experiments in realistic scenarios.

I. INTRODUCTIONA central problem in robotics, computer graphics, virtual

and augmented reality (VR/AR), and many applicationsinvolving complex physics simulations is the accurate, real-time determination of proximity relationships between three-dimensional objects [4] situated in a cluttered environment.In robot navigation and manipulation tasks, path plannersneed to compute a dynamically feasible trajectory connect-ing an initial state to a goal configuration while avoidingobstacles in the environment. In VR/AR applications, ahuman immersed in a virtual world may wish to touchcomputer generated objects, that must respond to contactsin physically realistic ways. Likewise, when collisions aredetected, 3D gaming engines and physics simulators (e.g., formolecular dynamics) need to activate appropriate directionalforces on interacting entities. All of these applications requiregeometric notions of separation and penetration between rep-resentations of three-dimensional objects to be continuouslymonitored.

A rich class of computational geometry problems arisesin this context, when 3D objects are outer approximated by

1Dept. of Operations Research and Financial Engineering,Princeton University, Princeton NJ, USA. Partially supported by aGoogle Faculty Research Award. a_a_a@princeton.edu,gh4@princeton.edu

2Google, New York, USA makadia@google.com2Google Brain, New York, USA sindhwani@google.com

convex bounding volumes. Due to convexity, the Euclideandistance between such bounding volumes can be computedvery precisely, providing a reliable certificate of safety forthe objects they enclose. This can also be effective fornonconvex objects which can be tightly covered by a finiteunion of convex shapes, e.g., using convex decompositionmethods [10], [13]—the distance to such an object can becomputed by taking the minimum of distances to its convexcomponents. When objects overlap, quantitative measures ofdegree of penetration are needed in order to optimally resolvecollisions, e.g., by a gradient-based trajectory optimizer.Multiple such measures have been proposed in the literature.The penetration depth is the minimum magnitude translationthat brings the overlapping objects out of collision. Thegrowth distance [15] is a related concept: it is the minimumshrinkage of the two bodies required to reduce volumepenetration down to merely surface touching.

A. Contributions and organization of the paper

In this paper, we propose to represent the geometry of agiven 3D environment comprising multiple static or dynamicrigid bodies using sublevel sets of polynomials. The paper isorganized as follows: In Section II, we provide an overviewof the algebraic concepts of sum of squares (sos) andsum of squares-convex (sos-convex) polynomials as well astheir relation to semidefinite programming and polynomialoptimization. In Section III, we consider the problem ofcontaining a cloud of 3D points with tight-fitting convexor nearly convex sublevel sets of polynomials. In particular,we propose a new volume minimization heuristic for thesesublevel sets which empirically results in tighter fittingpolynomials than previous proposals [12], [8]. The extent ofconvexity imposed on these sublevel set bounding volumescan be explicitly tuned using sum of squares optimizationtechniques. If convexity is imposed, we refer to them assos-convex bodies; if it is not, we term them simply assos-bodies. (See Section II for a more formal definition.)

The bounding volumes obtained are highly compact, andadapt to the shape of the data in more flexible ways thancanned convex primitives typically used in standard boundingvolume hierarchies. Their construction involves small-scalesemidefinite programs (SDPs) that can fit, in an offlinepreprocessing phase, 3D meshes with tens of thousands ofdata points in a few seconds. In Section IV, we definenotions related to measuring separation or penetration of twoof these polynomial sublevel sets and show how they canbe efficiently computed using sum of squares optimization.These include Euclidean distance and growth distance [15]computations. In Section V, we study the problem of con-taining (potentially nonconvex) polynomial sublevel sets (asopposed to points as in Section III) within one convexpolynomial sublevel set. We end in Section VI with somefuture directions.

B. Preview of some experiments

Figure 1 gives a preview of some of the methods developedin this paper using as an example a 3D chair point cloud.On the left, we enclose the chair within the 1-sublevelset of three sos-convex polynomials with increasing degree(2, 4 and 6) leading to correspondingly tighter fits. Themiddle plot presents the 1-sublevel set of three degree-6sos polynomials with increasing nonconvexity showing howtighter representations can be obtained by relaxing convexity.The right plot shows the 2, 1 and 0.75 sublevel sets of asingle degree-6 sos polynomial; the 1-sublevel set coloredgreen encloses the chair, while greater or lower values ofthe level set define grown and shrunk versions of the object.The computation of Euclidean distances, and sublevel-basedmeasures of separation and penetration between such bodiesis a tiny convex optimization problem that can be solved ina matter of milliseconds.

II. SUM OF SQUARES AND SOS-CONVEXITY

In this section, we briefly review the notions of sum ofsquares polynomials, sum of squares-convexity, and polyno-mial optimization which will all be central to the geometricproblems we discuss later. We refer the reader to the recentmonograph [7] for a more detailed overview of the subject.

Throughout, we will denote the set of n × n symmetricmatrices by Sn×n and the set of degree-2d polynomialswith real coefficients by R2d[x]. We say that a polynomialp(x1, . . . , xn) ∈ R2d[x] is nonnegative if p(x1, . . . , xn) ≥0,∀x ∈ Rn. In many applications (including polynomialoptimization that we will cover later), one would like toconstrain certain coefficients of a polynomial so as to makeit nonnegative. Unfortunately, even testing whether a givenpolynomial (of degree 2d ≥ 4) is nonnegative is NP-hard.As a consequence, we would like to replace the intractablecondition that p be nonnegative by a sufficient conditionfor it that is more tractable. One such condition is forthe polynomial to have a sum of squares decomposition.We say that a polynomial p is a sum of squares (sos) ifthere exist polynomials qi such that p =

∑i q

2i . From this

definition, it is clear that any sos polynomial is nonnegative,

though not all nonnegative polynomials are sos; see, e.g.,[18],[9] for some counterexamples. Furthermore, requiringthat a polynomial p be sos is a computationally tractablecondition as a consequence of the following characterization:A polynomial p of degree 2d is sos if and only if thereexists a positive semidefinite matrix Q such that p(x) =z(x)TQz(x), where z(x) is the vector of all monomialsof degree up to d [16]. The matrix Q is sometimes calledthe Gram matrix of the sos decomposition and is of size(n+dd

)×(n+dd

). (Throughout the paper, we let N : =

(n+dd

).)

The task of finding a positive semidefinite matrix Q thatmakes the coefficients of p all equal to the coefficients ofz(x)TQz(x) is a semidefinite programming problem, whichcan be solved in polynomial time to arbitrary accuracy [20].

The concept of sum of squares can also be used to definea sufficient condition for convexity of polynomials knownas sos-convexity. We say that a polynomial p is sos-convexif the polynomial yT∇2p(x)y in 2n variables x and y isa sum of squares. Here, ∇2p(x) denotes the Hessian of p,which is a symmetric matrix with polynomial entries. Fora polynomial of degree 2d in n variables, one can checkthat the dimension of the Gram matrix associated to thesos-convexity condition is N : = n ·

(n+d−1d−1

). It follows

from the second order characterization of convexity that anysos-convex polynomial is convex, as yT∇2p(x)y being sosimplies that ∇2p(x) � 0, ∀x. The converse however is nottrue, though convex but not sos-convex polynomials are hardto find in practice; see [2]. Through its link to sum of squares,it is easy to see that testing whether a given polynomial issos-convex is a semidefinite program. By contrast, testingwhether a polynomial of degree 2d ≥ 4 is convex is NP-hard [1].

A polynomial optimization problem is a problem of theform

minx∈K

p(x), (1)

where the objective p is a (multivariate) polynomial and thefeasible set K is a basic semialgebraic set; i.e., a set definedby polynomial inequalities:

K := {x | gi(x) ≥ 0, i = 1, . . . ,m}.

It is straightforward to see that problem (1) can beequivalently formulated as that of finding the largest constantγ such that p(x) − γ ≥ 0,∀x ∈ K. Under mild conditions(specifically, under the assumption that K is Archimedean[9]), the condition p(x) − γ > 0,∀x ∈ K is equivalent tothe existence of sos polynomials σi(x) such that p(x)−γ =σ0(x) +

∑mi=1 σi(x)gi(x) [3]. Hence, problem (1) becomes

max γ

s.t. p(x)− γ = σ0 +

m∑i=1

σi(x)gi(x), (2)

σi sos, i = 0, . . . ,m.

For any fixed upper bound on the degrees of σi this is asemidefinite programming problem which produces a lower

bound on the optimal value of (1). As the degrees of σiincrease, these lower bounds are guaranteed to converge tothe true optimal value of (1). (Note that we are makingno convexity assumptions about the polynomial optimizationproblem and yet solving it globally through a sequence ofsemidefinite programs.)

III. 3D POINT CLOUD CONTAINMENT

Throughout this section, we are interested in finding abody of minimum volume, parametrized as the 1-sublevelset of a polynomial of degree 2d, which encloses a set ofgiven points {x1, . . . , xm} in Rn.

A. Convex sublevel sets

We focus first on finding a convex bounding volume.Convexity is a common constraint in the bounding volumeliterature and it makes certain tasks (e.g., distance computa-tion among the different bodies) simpler. In order to make aset of the form {x ∈ R3| p(x) ≤ 1} convex, we will requirethe polynomial p to be convex. (Note that this is a sufficientbut not necessary condition.) Furthermore, to have a tractableformulation, we will replace the convexity condition with ansos-convexity condition as described previously. Even afterthese relaxations, the problem of minimizing the volume ofour sublevel sets remains a difficult one. The remainder ofthis section discusses several heuristics for this task.

1) The Hessian-based approach: In [12], Magnani et al.propose the following heuristic to minimize the volume ofthe 1-sublevel set of an sos-convex polynomial

minp∈R2d[x],H∈SN×N

− log det(H)

s.t.p sos,

yT∇2p(x)y = w(x, y)THw(x, y), H � 0, (3)p(xi) ≤ 1, i = 1, . . . ,m,

where w(x, y) is a vector of monomials in x and y, of degree1 in y and d − 1 in x. This problem outputs a polynomialp whose 1-sublevel set corresponds to the bounding volumethat we are interested in. A few remarks on this formulationare in order:• The last constraint simply ensures that all the data points

are within the 1-sublevel set of p as required.• The second constraint imposes that p be sos-convex.

The matrix H is the Gram matrix associated with thesos condition on yT∇2p(x)y.

• The first constraint requires that the polynomial p weare looking for be sos. This is a necessary conditionfor boundedness of (3) when p is parametrized withaffine terms. To see this, note that for any given pos-itive semidefinite matrix Q, one can always pick thecoefficients of the affine terms in such a way that theconstraint p(xi) ≤ 1 for i = 1, . . . ,m be trivially satis-fied. Likewise one can pick the remaining coefficientsof p in such a way that the sos-convexity condition besatisfied. The restriction to sos polynomials, however,

can be done without loss of generality. Indeed, supposethat the minimum volume sublevel set was given by{x | p(x) ≤ 1} where p is an sos-convex polynomial.As p is convex and nonaffine, ∃γ ≥ 0 such thatp(x)+γ ≥ 0 for all x. Define now q(x) := p(x)+γ

1+γ . Wehave that {x | p(x) ≤ 1} = {x | q(x) ≤ 1}, but here,q is sos as it is sos-convex and nonnegative [5, Lemma8].

The objective function of the above formulation is moti-vated in part by the degree 2d = 2 case. Indeed, when 2d =2, the sublevel sets of convex polynomials are ellipsoids ofthe form {x | xTPx ≤ 1} and their volume is given by43π ·

√det(P−1). Hence, by minimizing − log det(P ), we

would exactly minimize volume. Furthermore, the matrixP associated with the form xTPx is none other than theHessian of the form (up to a multiplicative constant).

A related minimum volume heuristic that we will alsoexperiment with corresponds to the following problem:

minp∈R2d[x],H∈SN×N ,V ∈SN×N

trace(V )

s.t.p sos,

yT∇2p(x)y = w(x, y)THw(x, y), H � 0, (4)p(xi) ≤ 1, i = 1, . . . ,m,[V II H

]� 0.

The last constraint can be rewritten using the Schur comple-ment as V � H−1. As a consequence, this trace formulationminimizes the sum of the inverse of the eigenvalues of Hwhereas the log det formulation described in (3) minimizesthe product of the inverse of the eigenvalues.

2) Our approach: We propose here yet another heuristicfor obtaining a tight-fitting convex body containing pointsin Rn. Empirically, we validate that it tends to consis-tently return convex bodies of smaller volume than theones obtained with the methods described above. It alsogenerates a relatively smaller convex optimization problem.Our formulation is as follows:

minp∈R2d[x],P∈SN×N

− log det(P )

s.t.

p(x) = z(x)TPz(x), P � 0,

p sos-convex, (5)p(xi) ≤ 1, i = 1, . . . ,m.

One can also obtain a trace formulation of this problem byreplacing the log det objective by a trace one as it was done

to go from (3) to (4):

minp∈R2d[x],P∈SN×N ,V ∈SN×N

trace(V )

s.t.

p(x) = z(x)TPz(x), P � 0,

p sos-convex, (6)p(xi) ≤ 1, i = 1, . . . ,m,[V II P

]� 0.

Note that the main difference between (3) and (5) lies inthe Gram matrix chosen for the objective function. In (3),the Gram matrix comes from the sos-convexity constraint,whereas in (5), the Gram matrix is generated by the sosconstraint.

In the case where the polynomial is quadratic and convex,we saw that the formulation (3) is exact as it finds the min-imum volume ellipsoid containing the points. It so happensthat the formulation given in (5) is also exact in the quadraticcase, and, in fact, both formulations return the same optimalellipsoid. As a consequence, the formulation given in (5) canalso be viewed as a natural extension of the quadratic case.

To provide more intuition as to why this formulationperforms well, we interpret the 1-sublevel set

S := {x | p(x) ≤ 1}

as the projection of some set whose volume is being mini-mized. More precisely, as p(x) = z(x)TPz(x), then the setS can be written as

S = {x ∈ Rn | ∃v ∈ RN−n s.t. (x, v) ∈ T}

where

T = {(x, v) ∈ RN | (x, v)TP (x, v) ≤ 1, z(x) = (x, v)}.

Note that the function z takes as input x, i.e., the first ncomponents of (x, v), and maps it to the monomial vectorz(x). In other words, z defines a set of polynomial equalitiesin (x, v) (of the type, e.g., v1 = x21). In this way, it is easyto see that T is not an ellipsoid itself though it is containedin the ellipsoid {(x, v) | (x, v)TP (x, v) ≤ 1}. When weminimize log detP , we are in fact minimizing the volumeof this latter ellipsoid. Hence, we are indirectly minimizingthe volume of T . As S is a projection of T , this is a heuristicfor minimizing the volume of S.

B. Relaxing convexity

Though containing a set of points with a convex sublevelset has its advantages, it is sometimes necessary to havea tighter fit than the one provided by a convex body,particularly if the object of interest is highly nonconvex. Oneway of handling such scenarios is via convex decompositionmethods [10], [13], which would enable us to represent theobject as a tight union of sos-convex bodies. Alternatively,one can aim for problem formulations where convexityof the sublevel sets is not imposed. In the remainder ofthis subsection, we first review a recent approach from the

literature to do this and then present our own approachwhich allows for controlling the level of nonconvexity ofthe sublevel set.

1) The inverse moment approach: In very recent work [8],Lasserre and Pauwels propose an approach for contain-ing a cloud of points with sublevel sets of polynomials(with no convexity constraint). Given a set of data pointsx1, . . . , xm ∈ Rn, it is observed in that paper that thesublevel sets of the degree 2d sos polynomial

pµ,d(x) := z(x)TMd(µ(x1, . . . , xm))−1z(x), (7)

tend to take the shape of the data accurately. Here, z(x)is the vector of all monomials of degree up to d andMd(µ(x1, . . . , xm)) is the moment matrix of degree d asso-ciated with the empirical measure µ := 1

m

∑mi=1 δxi

definedover the data. This is an

(n+dd

)×(n+dd

)symmetric positive

semidefinite matrix which can be cheaply constructed fromthe data x1, . . . , xm ∈ Rn (see [8] for details). One very nicefeature of this method is that to construct the polynomial pµ,din (7) one only needs to invert a matrix (as opposed to solvinga semidefinite program as our approach would require) aftera single pass over the point cloud. The approach howeverdoes not a priori provide a particular sublevel set of pµ,d thatis guaranteed to contain all data points. Hence, once pµ,d isconstructed, one could slowly increase the value of a scalarγ and check whether the γ-sublevel set of pµ,d contains allpoints.

2) Our approach and controlling convexity: An advantageof our proposed formulation (5) is that one can easily dropthe sos-convexity assumption in the constraints and therebyobtain a sublevel set which is not necessarily convex. Theproblem then becomes:

minp∈R2d[x],P∈SN×N

− log det(P )

s.t.

p = z(x)TPz(x), P � 0 (8)p(xi) ≤ 1, i = 1, . . . ,m.

This is not an option for formulation (3) as the Gram matrixassociated to the sos-convexity constraint intervenes in theobjective.

Note that in neither this formulation nor the inverse mo-ment approach of Lasserre and Pauwels, does the optimizerhave control over the shape of the sublevel sets produced,which may be convex or far from convex. For some ap-plications, it is useful to control in some way the degreeof convexity of the sublevel sets obtained by introducing aparameter which when increased or decreased would makethe sets more or less convex. This is what our followingproposed optimization problem does via the parameter c,

which corresponds in some sense to a measure of convexity:

minp∈R2d[x],P∈SN×N

− log det(P )

s.t.

p = z(x)TPz(x), P � 0 (9)

p(x)− c(∑i

x2i )d sos-convex.

p(xi) ≤ 1, i = 1, . . . ,m.

Note that when c = 0, the problem we are solving corre-sponds exactly to (8) and the sublevel set obtained is convex.When c < 0, we allow for nonconvexity of the sublevel sets.As we increase c however, we obtain sublevel sets which getprogressively more and more convex.

C. Bounding volume numerical experiments

Figure 1 (left) shows the 1-sublevel sets of sos-convexbodies with degrees 2, 4 and 6. A degree 6 polynomialgives a much tighter fit than an ellipsoid (degree 2). In themiddle figure, we freeze the degree to be 6 and reduce theconvexity parameter c in the relaxed convexity formulationof equation (9); the 1-sublevel sets of the resulting sospolynomials with c = 0,−10,−100 are shown. It can beseen that the sublevel sets gradually bend to better adapt tothe shape of the object. The right figure shows the 2, 1, 0.75sublevel sets of degree 6 polynomial with c = −10: theshape is retained as the body is expanded or contracted.

In Table 1, we provide a comparison of various boundingvolumes on Princeton Shape Benchmark datasets [19]1. Itcan be seen that sos-convex bodies with higher degreepolynomials provide much tighter fits than spheres or axis-aligned bounding boxes (AABB) in general. The proposedminimum volume heuristic of our formulation in (5) or (6)works better than that proposed in [12] (see (3) or (4)).In both formulations, typically, the log-determinant criteriaoutperforms the trace criteria. The convex hull is the tightestpossible convex body. However, for smooth objects like thevase, the number of extreme vertices describing the convexhull can be a substantial fraction of the original number ofpoints in the point cloud. (The number of vertices of theconvex hull is written in parentheses in the Convex-Hullrow of the table.) When convexity is relaxed, a degree 6sos polynomial compactly described by just 84 coefficientsgives a tighter fit than the convex hull. For the same degree,solutions to our formulation (9) with a negative value of coutperform the inverse moment construction of [8].

The bounding volume construction time is shown in Fig-ure 3 for sos-convex chair models. In comparison to thevolume heuristics of [12], our heuristic runs significantlyfaster as soon as degree exceeds 6 since our formulation leadsto smaller SDPs. Our implementation uses YALMIP [11]2

with the splitting conic solver (SCS) [14]3 as its backendSDP solver (run for 2500 iterations). Note that the inverse

1http://shape.cs.princeton.edu/benchmark/2http://users.isy.liu.se/johanl/yalmip/3https://github.com/cvxgrp/scs

moment approach of [8] is the fastest as it does not involveany optimization and makes just one pass over the pointcloud. However, this approach is not guaranteed to returna convex body, and for nonconvex bodies, tighter fittingpolynomials can be estimated using log-determinant or traceoptimization as described in (9).

2 3 4 5 6 7 8

DEGREE

0

20

40

60

80

100

120

140

160

180

200

CO

NS

TR

UC

TIO

N T

IME

(S

EC

S)

inv-mom [Lasserre and Pauwels, 2016]

logdet(P -1)[proposed in this paper]

trace(P -1) [proposed in this paper]

logdet(H -1) [Magnani et al, 2005]

trace(H -1)

Fig. 3: Bounding volume construction times

IV. MEASURES OF SEPARATION AND PENETRATION

A. Euclidean Distance

In this section, we are interested in computing the Eu-clidean distance between two sets

S1: = {x ∈ Rn | g1(x) ≤ 1, . . . , gm ≤ 1},

andS2: = {x ∈ Rn | h1(x) ≤ 1, . . . , hr ≤ 1},

where g1, . . . , gm and h1, . . . , hr are all sos-convex. Thisproblem can formally be written as follows

minx∈S1,y∈S2

||x− y||22. (10)

This is a constrained convex optimization problem whichcan be solved using generic techniques such as interiorpoint methods. It also turns out that viewed as a polynomialoptimization problem, this problem can be solved exactlyusing semidefinite programming. This is a corollary of thefollowing more general result due to Lasserre [6] to whichour problem conforms.

Theorem 4.1 ([6]): Consider the polynomial optimizationproblem

minxp0(x)

s.t. p1(x) ≤ 0, . . . , ps(x) ≤ 0

where p0, . . . , ps are all sos-convex. Then the optimal valueof this problem is the same as the optimal value of thefollowing SDP:

maxγ∈R,λ∈Rs,σ0∈R2d[x]

γ

p0(x)− γ = σ0(x) +

s∑i=1

λipi(x),

σ0(x) sos.

Object → Human Chair Hand Vase OctopusBounding Body ↓ id:#vertices 10:9508 101:8499 181:7242 361:14859 121:5944

Convex-Hull 0.29 (364) 0.66 (320) 0.36 (652) 0.91 (1443) 0.5 (414)Sphere 3.74 3.73 3.84 3.91 4.1AABB 0.59 1.0 0.81 1.73 1.28

sos-convex (2) logdet 0.58 1.79 0.82 1.16 1.30trace 0.97 1.80 1.40 1.2 1.76

sos-convex (4)

logdet(H−1) 0.57 1.55 0.69 1.13 1.04trace(H−1) 0.56 2.16 1.28 1.09 3.13logdet(P−1) 0.44 1.19 0.53 1.05 0.86trace(P−1) 0.57 1.25 0.92 1.09 1.02

sos-convex (6)

logdet(H−1) 0.57 1.27 0.58 1.09 0.93trace(H−1) 0.56 1.30 0.57 1.09 0.87logdet(P−1) 0.41 1.02 0.45 0.99 0.74trace(P−1) 0.45 1.21 0.48 1.03 0.79

Inverse-Moment (2) 4.02 1.42 2.14 1.36 1.74Inverse-Moment (4) 1.53 0.95 0.90 1.25 0.75Inverse-Moment (6) 0.48 0.54 0.58 1.10 0.57

sos (d=4, c=-10) logdet(P−1) 0.38 0.72 0.42 1.05 0.63trace(P−1) 0.51 0.78 0.48 1.11 0.71

sos (d=6, c=-10) logdet(P−1) 0.35 0.49 0.34 0.92 0.41trace(P−1) 0.37 0.56 0.39 0.99 0.54

sos (d=4, c=-100) logdet(P−1) 0.36 0.64 0.39 1.05 0.46trace(P−1) 0.42 0.74 0.46 1.10 0.54

sos (d=6, c=-100) logdet(P−1) 0.21 0.21 0.26 0.82 0.28trace(P−1) 0.22 0.30 0.29 0.85 0.37

TABLE I: Comparison of various bounding volume techniques

In this situation, the first level of the sos hierarchy describedat the end of Section II is exact. Even in the case wherethe sets S1 and S2 are not convex (which would mean inparticular that their defining polynomials gi and hi are not allconvex), we can obtain increasingly accurate lower boundsconverging to the exact Euclidean distance between the sets.This is done by applying higher levels of the sos hierarchy.The points achieving the minimum distance between degree6 sos-convex minimum volume (log-det) bodies enclosinghuman and chair 3D point clouds are shown below.

The distance query time, reported in the table below,ranges from around 80 milliseconds to 340 millisecondsseconds as the degree is increased from 2 to 8, usingMATLAB’s fmincon active-set solver. We believe that theexecution time can be improved by an order of magnitudewith more efficient polynomial representations, warm startsfor repeated queries and reduced convergence tolerance forlower-precision results.

degree 2 4 6 8time (secs) 0.08 0.083 0.13 0.34

B. Penetration measures for overlapping bodies

As another application of sos-convex polynomial opti-mization problems, we discuss a problem relevant to collisionavoidance. Here, we assume that our two bodies S1, S2are of the form S1 := {x | p1(x) ≤ 1} and S2 :={x | p2(x) ≤ 1} where p1, p2 are sos-convex. As shown inFigure 1 (right), by varying the sublevel value, we can growor shrink the sos representation of an object. The followingconvex optimization problem, with optimal value denoted byd(p1||p2), provides a measure of separation or penetrationbetween the two bodies:

d(p1||p2) = min p1(x)

s.t. p2(x) ≤ 1. (11)

Note that the measure is asymmetric, i.e., d(p1||p2) 6=d(p2||p1). It is clear that

p2(x) ≤ 1⇒ p1(x) ≥ d(p1||p2).

In other words, the sets {x | p2(x) ≤ 1} and {x | p1(x) ≤d(p1||p2)} do not overlap. As a consequence, the optimalvalue of (11) gives us a measure of how much we need toshrink the level set defined by p1 to eventually move out ofcontact of the set S2 assuming that the “seed point”, i.e., theminimum of p1, is outside S2. It is clear that,• if d(p1||p2) > 1, the bounding volumes are separated.• if d(p1||p2) = 1, the bounding volumes touch.• if d(p1||p2) < 1, the bounding volumes overlap.These measures are closely related to the notion of growthmodels and growth distances [15]. Note that as a conse-quence of Theorem 4.1, the optimal solution d(p1||p2) to (11)can be computed exactly using semidefinite programming,or using a generic convex optimizer. The two leftmost

-10 -5 0 5 10Translation of the Chair (Left to Right)

0

5

10

15

20

25

30

Gro

wth

Dis

tan

ce

p1:chair, p2:humanp1:human, p2:chair

-10 -5 0 5 10Translation of the Chair (Left to Right)

0

0.05

0.1

0.15

0.2

0.25

Gro

wth

Dis

tance C

om

puta

tion T

ime (

secs)

p1:chair, p2:humanp1:human, p2:chair

Fig. 4: Growth distances for separated (left) or overlapping (second-left) sos-convex bodies; growth distance as a functionof the position of the chair (second-right); time taken to solve (11) with warm-start (right)

subfigures of Figure 4 show a chair and a human boundedby 1-sublevel sets of degree 6 sos-convex polynomials (ingreen). In both cases, we compute d(p1||p2) and d(p2||p1)and plot the corresponding minimizers. In the first subfigure,the level set of the chair needs to grow in order to touch thehuman and vice-versa, certifying separation. In the secondsubfigure, we translate the chair across the volume occupiedby the human so that they overlap. In this case, the level setsneed to contract. In the third subfigure, we plot the optimalvalue of the problem in (11) as the chair is translated fromleft to right, showing how the growth distances dip uponpenetration and rise upon separation. The final subfigureshows the time taken to solve (11) when warm started fromthe previous solution. The time taken is of the order of 150milliseconds without warm starts to 10 milliseconds withwarm starts.

C. Separation and penetration under rigid body motion

Suppose {x | p(x) ≤ 1} is a minimum-volume sos-convexbody enclosing a rigid 3D object. If the object is rotated byR ∈ SO(3) and translated by t ∈ R3, then the polynomialp′(x) = p(RTx−RT t) encloses the transformed object. Thisis because, if p(x) ≤ 1, then p′(Rx+ t) ≤ 1. For continuousmotion, the optimization for Euclidean distance or sublevel-based separation/penetration distances can be warm startedfrom the previous solution. The computation of the gradientof these measures using parametric convex optimization, andexploring the potential of this idea for motion planning is leftfor future work.

V. CONTAINMENT OF POLYNOMIAL SUBLEVEL SETS

In this section, we show how the sum of squares machinerycan be used in a straightforward manner to contain polyno-mial sublevel sets (as opposed to point clouds) with a convexpolynomial level set. More specifically, we are interested inthe following problem: Given a basic semialgebraic set

S := {x ∈ Rn| g1(x) ≤ 1, . . . , gm(x) ≤ 1}, (12)

find a convex polynomial p of degree 2d such that

S ⊆ {x ∈ Rn| p(x) ≤ 1}. (13)

Moreover, we typically want the unit sublevel set of p to havesmall volume. Note that if we could address this question,

then we could also handle a scenario where the unit sublevelset of p is required to contain the union of several basicsemialgebraic sets (simply by containing each set separately).For the 3D geometric problems under our consideration, wehave two applications of this task in mind:• Convexification: In some scenarios, one may have a

nonconvex outer approximation of an obstacle (e.g.,obtained by the computationally inexpensive inversemoment approach of Lasserre and Pauwels as describedin Section III-B) and be interested in containing it witha convex set. This would e.g. make the problem ofcomputing distances among obstacles more tractable;cf. Section IV.

• Grouping multiple obstacles: For various navigationaltasks involving autonomous agents, one may want tohave a mapping of the obstacles in the environment invarying levels of resolution. A relevant problem here istherefore to group obstacles. In our setting, this wouldlead to the problem of containing several polynomialsublevel sets with one.

In order to solve the problem laid out above, we propose thefollowing sos program:

minp∈R2d[x],τi∈R2d[x],P∈SN×N

− log det(P )

s.t.

p(x) = z(x)TPz(x), P � 0,

p(x) sos-convex, (14)

1− p(x)−m∑i=1

τi(x)(1− gi(x)) sos, (15)

τi(x) sos, i = 1, . . . ,m. (16)

It is straightforward to see that constraints (15) and (16)imply (and algebraically certify) the required set containmentcriterion in (13). As usual, the constraint in (14) ensuresconvexity of the unit sublevel set of p. The objective functionattempts to minimize the volume of this set. A naturalchoice for the degree 2d of the polynomials τi is 2d =2d−mini deg(gi), though better results can be obtained byincreasing this parameter.Example. In Figure 5, we have drawn in black three randomellipsoids and a degree-4 convex polynomial sublevel set (in

yellow) containing the ellipsoids. This degree-4 polynomialwas the output of the optimization problem described abovewhere the sos multipliers τi(x) were chosen to have degree2.

Fig. 5: Containment of 3 ellipsoids using a sublevel set of aconvex degree-4 polynomial

We end by noting that the formulation proposed here isbacked up theoretically by the following converse result.Theorem 5.1: Suppose the set S in (12) is Archimedean4 andthat S ⊂ {x ∈ Rn| p(x) ≤ 1}. Then there exists an integerd and sum of squares polynomials τ1, . . . , τm of degree atmost d such that

1− p(x)−m∑i=1

τi(x)(1− gi(x)) (17)

is a sum of squares.Proof: The proof follows from a standard application

of Putinar’s Positivstellensatz [17] and is omitted.

VI. CONCLUSION

In this paper we have shown that sos polynomials offerviable bounding volume alternatives for 3D environments.The experiments on realistic 3D datasets indicate that thesepolynomials satisfy the core practical functions of boundingvolumes (e.g., fast construction, tightness of fit, real-timeproximity evaluations). We have also proposed new formu-lations that improve upon previous ones (e.g., in the sensethat they provide tighter fitting bounding volumes and cancontrol the extent of nonconvexity of these volumes).Our results open multiple application areas for future work.• Given the efficiency of our distance calculations (see Sec-

tion IV), it is natural to investigate how sos polynomialswould perform in the real-time 3D motion planning set-ting. Critical objectives here would be handling dynamicupdates, robustness of bounding volume estimation withnoisy point clouds, and understanding the performance ofhierarchical bounding volumes.

• Employing sos polynomial bounding volumes for controlof articulated objects (e.g., human motion) would providea formulation for avoiding self-intersection, but also in-troduce the challenge of handling dynamic shape surfacedeformation.

4See, e.g., [9] for a formal definition. This is a mild assumption thatcan always be met, e.g., if the set S is compact and the radius of a ballcontaining it is known.

• Bounding volumes are ubiquitous in rendering applica-tions, e.g., object culling and ray tracing. When consid-ering sos polynomial bounding volumes it is not difficultto see how these ray-surface intersection operations couldbe framed as convex optimization problems similar to thedistance calculations in sec IV. It would be interesting toexplore how such techniques would perform when inte-grated within GPU-optimized rendering and game engineframeworks.

ACKNOWLEDGEMENTS

We thank Erwin Coumans, Mrinal Kalakrishnan and VincentVanhoucke for several technically insightful discussions andguidance.

REFERENCES

[1] A. A. Ahmadi, A. Olshevsky, P. A. Parrilo, and J. N. Tsitsiklis. Np-hardness of deciding convexity of quartic polynomials and relatedproblems. Mathematical Programming, 137(1-2):453–476, 2013.

[2] A. A. Ahmadi and P. A. Parrilo. A complete characterization of the gapbetween convexity and sos-convexity. SIAM Journal on Optimization,23(2):811–833, 2013.

[3] E. De Klerk and M. Laurent. On the lasserre hierarchy of semidefiniteprogramming relaxations of convex polynomial optimization prob-lems. SIAM Journal on Optimization, 21(3):824–832, 2011.

[4] C. Ericson. Real-time Collision Detection. Morgan Kaufmann Seriesin Interactive 3-D Technology, 2004.

[5] J. W. Helton and J. Nie. Semidefinite representation of convex sets.Mathematical Programming, 122(1):21–64, 2010.

[6] J. B. Lasserre. Convexity in semialgebraic geometry and polynomialoptimization. SIAM Journal on Optimization, 19(4):1995–2014, 2009.

[7] J. B. Lasserre. Introduction to Polynomial and Semi-Algebraic Opti-mization. Cambridge University Press, 2015.

[8] J. B. Lasserre and E. Pauwels. Sorting out typicality with the inversemoment matrix SOS polynomial. arXiv preprint arXiv:1606.03858,2016.

[9] M. Laurent. Sums of squares, moment matrices and optimization overpolynomials. In Emerging applications of algebraic geometry, pages157–270. Springer, 2009.

[10] J.-M. Lien and N. M. Amato. Approximate convex decomposition.Proc. ACM Symp. Comput. Geom., June 2004.

[11] J. Lofberg. Yalmip : A toolbox for modeling and optimization inmatlab. In Proceedings of the CACSD Conference, Taiwan, 2004.

[12] A. Magnani, S. Lall, and S. Boyd. Tractable fitting with convexpolynomials via sum-of-squares. IEEE Conference on Decision andControl and European Control Conference, 2005.

[13] K. Mammou and F. Ghorbel. A simple and efficient approach for3d mesh approximate convex decomposition. IEEE InternationalConference on Image Processing, 2009.

[14] B. O’Donoghue, E. Chu, N. Parikh, and S. Boyd. Conic optimizationvia operator splitting and homogeneous self-dual embedding. Journalof Optimization Theory and Applications, 169(3), 2016.

[15] C. J. Ong and E. G. Gilbert. Growth distances: New measures ofobject separation and penetration. IEEE Transactions on Roboticsand Automation, December 1996.

[16] P. A. Parrilo. Structured semidefinite programs and semialgebraic ge-ometry methods in robustness and optimization. PhD thesis, CaliforniaInstitute of Technology, 2000.

[17] M. Putinar. Positive polynomials on compact semi-algebraic sets.Indiana University Mathematics Journal, 42(3):969–984, 1993.

[18] B. Reznick. Some concrete aspects of hilbert’s 17th problem. Con-temporary Mathematics, 253:251–272, 2000.

[19] P. Shilane, P. Min, M. Kazhdan, and T. Funkhouser. The princetonshape benchmark. In Shape modeling applications, 2004. Proceedings,pages 167–178. IEEE, 2004.

[20] L. Vandenberghe and S. Boyd. Semidefinite programming. SIAMreview, 38(1):49–95, 1996.