diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index cd4ee9961..b007a07b2 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -27,6 +27,7 @@ import trimesh from scenic.core.distributions import ( + FunctionDistribution, MultiplexerDistribution, RandomControlFlowError, Samplable, @@ -1255,67 +1256,125 @@ def boundingBox(self): @cached_property def inradius(self): """A lower bound on the inradius of this object""" - # First check if all needed variables are defined. If so, we can - # compute the inradius exactly. - width, length, height = self.width, self.length, self.height - shape = self.shape - if not any(needsSampling(val) for val in (width, length, height, shape)): - shapeRegion = MeshVolumeRegion( - mesh=shape.mesh, dimensions=(width, length, height) - ) - return shapeRegion.inradius - # If we havea uniform distribution over shapes and a supportInterval for each dimension, - # we can compute a supportInterval for this object's inradius + # Define a helper function that computes the support of the inradius, + # given the sub supports. + def inradiusSupport(width_s, length_s, height_s, shape_s): + # Unpack the dimension supports (and ignore the shape support) + min_width, max_width = width_s + min_length, max_length = length_s + min_height, max_height = height_s + + if None in [ + min_width, + max_width, + min_length, + max_length, + min_height, + max_height, + ]: + # Can't get a bound on one or more dimensions, abort + return None, None + + min_bounds = np.array([min_width, min_length, min_height]) + max_bounds = np.array([max_width, max_length, max_height]) + + # Extract a list of possible shapes + if isinstance(self.shape, Shape): + shapes = [self.shape] + elif isinstance(self.shape, MultiplexerDistribution) and all( + isinstance(opt, Shape) for opt in self.shape.options + ): + shapes = self.shape.options + else: + # Something we don't recognize, abort + return None, None - # Define helper class - class InradiusHelper: - def __init__(self, support): - self.support = support + # Get the inradius for each shape with the min and max bounds + min_distances = [ + MeshVolumeRegion(mesh=shape.mesh, dimensions=min_bounds).inradius + for shape in shapes + ] + max_distances = [ + MeshVolumeRegion(mesh=shape.mesh, dimensions=max_bounds).inradius + for shape in shapes + ] - def supportInterval(self): - return self.support + distance_range = (min(min_distances), max(max_distances)) - # Extract bounds on all dimensions - min_width, max_width = supportInterval(width) - min_length, max_length = supportInterval(length) - min_height, max_height = supportInterval(height) + return distance_range - if None in [min_width, max_width, min_length, max_length, min_height, max_height]: - # Can't get a bound on one or more dimensions, abort - return 0 + # Define a helper function that computes the actual inradius + @distributionFunction(support=inradiusSupport) + def inradiusActual(width, length, height, shape): + return MeshVolumeRegion( + mesh=shape.mesh, dimensions=(width, length, height) + ).inradius - min_bounds = np.array([min_width, min_length, min_height]) - max_bounds = np.array([max_width, max_length, max_height]) + # Return the inradius (possibly a distribution) with proper support information + return inradiusActual(self.width, self.length, self.height, self.shape) - # Extract a list of possible shapes - if isinstance(shape, Shape): - shapes = [shape] - elif isinstance(shape, MultiplexerDistribution): - if all(isinstance(opt, Shape) for opt in shape.options): - shapes = shape.options + @cached_property + def planarInradius(self): + """A lower bound on the planar inradius of this object. + + This is defined as the inradius of the polygon of the occupiedSpace + of this object projected into the XY plane, assuming that pitch and + roll are both 0. + """ + + # Define a helper function that computes the support of the inradius, + # given the sub supports. + def planarInradiusSupport(width_s, length_s, shape_s): + # Unpack the dimension supports (and ignore the shape support) + min_width, max_width = width_s + min_length, max_length = length_s + + if None in [min_width, max_width, min_length, max_length]: + # Can't get a bound on one or more dimensions, abort + return None, None + + min_bounds = np.array([min_width, min_length, 1]) + max_bounds = np.array([max_width, max_length, 1]) + + # Extract a list of possible shapes + if isinstance(self.shape, Shape): + shapes = [self.shape] + elif isinstance(self.shape, MultiplexerDistribution) and all( + isinstance(opt, Shape) for opt in self.shape.options + ): + shapes = self.shape.options else: # Something we don't recognize, abort - return 0 + return None, None + + # Get the inradius of the projected for each shape with the min and max bounds + min_distances = [ + MeshVolumeRegion( + mesh=shape.mesh, dimensions=min_bounds + ).boundingPolygon.inradius + for shape in shapes + ] + max_distances = [ + MeshVolumeRegion( + mesh=shape.mesh, dimensions=max_bounds + ).boundingPolygon.inradius + for shape in shapes + ] - # Check that all possible shapes contain the origin - if not all(shape.containsCenter for shape in shapes): - # One or more shapes has inradius 0 - return 0 + distance_range = (min(min_distances), max(max_distances)) - # Get the inradius for each shape with the min and max bounds - min_distances = [ - MeshVolumeRegion(mesh=shape.mesh, dimensions=min_bounds).inradius - for shape in shapes - ] - max_distances = [ - MeshVolumeRegion(mesh=shape.mesh, dimensions=max_bounds).inradius - for shape in shapes - ] + return distance_range - distance_range = (min(min_distances), max(max_distances)) + # Define a helper function that computes the actual planarInradius + @distributionFunction(support=planarInradiusSupport) + def planarInradiusActual(width, length, shape): + return MeshVolumeRegion( + mesh=shape.mesh, dimensions=(width, length, 1) + ).boundingPolygon.inradius - return InradiusHelper(support=distance_range) + # Return the planar inradius (possibly a distribution) with proper support information + return planarInradiusActual(self.width, self.length, self.shape) @cached_property def surface(self): diff --git a/src/scenic/core/pruning.py b/src/scenic/core/pruning.py index 00d28d486..60f7a93c6 100644 --- a/src/scenic/core/pruning.py +++ b/src/scenic/core/pruning.py @@ -5,11 +5,14 @@ """ import builtins +import collections import math import time +import numpy import shapely.geometry import shapely.geos +from trimesh.transformations import translation_matrix from scenic.core.distributions import ( AttributeDistribution, @@ -17,6 +20,7 @@ MethodDistribution, OperatorDistribution, Samplable, + dependencies, needsSampling, supportInterval, underlyingFunction, @@ -25,10 +29,17 @@ from scenic.core.geometry import hypot, normalizeAngle, plotPolygon, polygonUnion from scenic.core.object_types import Object, Point import scenic.core.regions as regions -from scenic.core.regions import EmptyRegion, MeshSurfaceRegion, MeshVolumeRegion +from scenic.core.regions import ( + EmptyRegion, + MeshSurfaceRegion, + MeshVolumeRegion, + PolygonalRegion, + VoxelRegion, +) from scenic.core.type_support import TypecheckedDistribution from scenic.core.vectors import ( PolygonalVectorField, + Vector, VectorField, VectorMethodDistribution, VectorOperatorDistribution, @@ -36,9 +47,11 @@ from scenic.core.workspaces import Workspace from scenic.syntax.relations import DistanceRelation, RelativeHeadingRelation -### Utilities +### Constants +PRUNING_PITCH = 0.01 +### Utilities def currentPropValue(obj, prop): """Get the current value of an object's property, taking into account prior pruning.""" value = getattr(obj, prop) @@ -62,8 +75,7 @@ def isFunctionCall(thing, function): def matchInRegion(position): """Match uniform samples from a `Region` - Returns the Region, if any, and a lower and upper bound - on the distance the object will be placed along with any + Returns the Region, if any, along with any offset that should be added to the base. """ # Case 1: Position is simply a point in a region @@ -71,7 +83,7 @@ def matchInRegion(position): reg = position.region if isinstance(reg, Workspace): reg = reg.region - return reg, 0, 0, None + return reg, None # Case 2: Position is a point in a region with a vector offset. if isinstance(position, VectorOperatorDistribution) and position.operator in ( @@ -80,15 +92,14 @@ def matchInRegion(position): ): if isinstance(position.object, regions.PointInRegionDistribution): reg = position.object.region + if isinstance(reg, Workspace): + reg = reg.region assert len(position.operands) == 1 offset = position.operands[0] - # TODO: Proper vector supportInterval calculations. Right now this gives us None - # if value is not exact - lower, upper = supportInterval(offset.norm()) - return reg, lower, upper, offset + return reg, offset - return None, 0, 0, None + return None, None def matchPolygonalField(heading, position): @@ -155,6 +166,7 @@ def prune(scenario, verbosity=1): pruneContainment(scenario, verbosity) pruneRelativeHeading(scenario, verbosity) + pruneVisibility(scenario, verbosity) if verbosity >= 1: totalTime = time.time() - startTime @@ -162,8 +174,6 @@ def prune(scenario, verbosity=1): ## Pruning based on containment - - def pruneContainment(scenario, verbosity): """Prune based on the requirement that individual Objects fit within their container. @@ -174,7 +184,7 @@ def pruneContainment(scenario, verbosity): """ for obj in scenario.objects: # Extract the base region and container region, while doing minor checks. - base, _, maxDistance, offset = matchInRegion(obj.position) + base, offset = matchInRegion(obj.position) if base is None or needsSampling(base): continue @@ -190,19 +200,70 @@ def pruneContainment(scenario, verbosity): if isinstance(container, regions.EmptyRegion): raise InvalidScenarioError(f"Object {obj} contained in empty region") - # Erode the container region if possible. - minRadius, _ = supportInterval(obj.inradius) - + # Compute the maximum distance the object can be from the sampled point + if offset is not None: + # TODO: Support interval doesn't really work here for random values. + if isinstance(base, PolygonalRegion): + # Special handling for 2D regions that ignores vertical component of offset + offset_2d = Vector(offset.x, offset.y, 0) + _, maxDistance = supportInterval(offset_2d.norm()) + else: + _, maxDistance = supportInterval(offset.norm()) + else: + maxDistance = 0 + + # Compute the minimum radius of the object, with respect to the + # bounded dimensions of the container. if ( - hasattr(container, "buffer") - and maxDistance is not None + isinstance(base, PolygonalRegion) + and supportInterval(obj.pitch) == (0, 0) + and supportInterval(obj.roll) == (0, 0) + ): + # Special handling for 2D regions with no pitch or roll, + # using planar inradius instead. + minRadius, _ = supportInterval(obj.planarInradius) + else: + # For most regions, use full object inradius. + minRadius, _ = supportInterval(obj.inradius) + + # Erode the container if possible and productive + if ( + maxDistance is not None and minRadius is not None + and (maxErosion := minRadius - maxDistance) > 0 ): - maxErosion = minRadius - maxDistance - if maxErosion > 0: + if hasattr(container, "buffer"): + # We can do an exact erosion container = container.buffer(-maxErosion) + elif isinstance(container, MeshVolumeRegion): + # We can attempt to erode a voxel approximation of the MeshVolumeRegion. + # Compute a voxel overapproximation of the mesh. Technically this is not + # an overapproximation, but one dilation with a rank 3 structuring unit + # with connectivity 3 is. To simplify, we just erode one less time than + # needed. + target_pitch = PRUNING_PITCH * max(container.mesh.extents) + voxelized_container = container.voxelized(target_pitch, lazy=True) + + # Erode the voxel region. Erosion is done with a rank 3 structuring unit with + # connectivity 3 (a 3x3x3 cube of voxels). Each erosion pass can erode by at + # most math.hypot([pitch]*3). Therefore we can safely make at most + # floor(maxErosion/math.hypot([pitch]*3)) passes without eroding more + # than maxErosion. We also subtract 1 iteration for the reasons above. + iterations = ( + math.floor(maxErosion / math.hypot(*([target_pitch] * 3))) - 1 + ) - # Restrict the base region to the container, unless + if iterations > 0: + eroded_container = voxelized_container.dilation( + iterations=-iterations + ) + + # Now check if this erosion is useful, i.e. do we have less volume to sample from. + # If so, replace the original container. + if eroded_container.size < container.size: + container = eroded_container + + # Restrict the base region to the possibly eroded container, unless # they're the same in which case we're done if base is container: continue @@ -215,30 +276,28 @@ def pruneContainment(scenario, verbosity): if isinstance(base, MeshVolumeRegion) and isinstance(newBase, MeshSurfaceRegion): continue + # Check newBase properties if isinstance(newBase, EmptyRegion): raise InvalidScenarioError(f"Object {obj} does not fit in container") - if verbosity >= 1: - if ( - base.dimensionality is None - or newBase.dimensionality is None - or base.dimensionality != newBase.dimensionality - ): + percentage_pruned = percentagePruned(base, newBase) + + if percentage_pruned is None: + if verbosity >= 1: print( f" Region containment constraint pruning attempted but could not compute percentage for {base} and {newBase}." ) - elif base.dimensionality == newBase.dimensionality: - ratio = newBase.size / base.size - percent = max(0, 100 * (1.0 - ratio)) - - if percent <= 0.001: - # We didn't really prune anything, don't bother setting new position - continue + else: + if percentage_pruned <= 0.001: + # We didn't really prune anything, don't bother setting new position + continue + if verbosity >= 1: print( - f" Region containment constraint pruned {percent:.1f}% of space." + f" Region containment constraint pruned {percentage_pruned:.1f}% of space." ) + # Condition object to pruned position newPos = regions.Region.uniformPointIn(newBase) if offset is not None: @@ -248,8 +307,6 @@ def pruneContainment(scenario, verbosity): ## Pruning based on orientation - - def pruneRelativeHeading(scenario, verbosity): """Prune based on requirements bounding the relative heading of an Object. @@ -279,7 +336,7 @@ def pruneRelativeHeading(scenario, verbosity): # Check for relative heading relations among such objects for obj, (field, offsetL, offsetR) in fields.items(): position = currentPropValue(obj, "position") - base, _, _, offset = matchInRegion(position) + base, offset = matchInRegion(position) # obj must be positioned uniformly in a Region if base is None or needsSampling(base): @@ -333,6 +390,102 @@ def pruneRelativeHeading(scenario, verbosity): obj.position.conditionTo(newPos) +# Pruning based on visibility +def pruneVisibility(scenario, verbosity): + ego = scenario.egoObject + + for obj in scenario.objects: + # Extract the base region if it exists + position = currentPropValue(obj, "position") + base, offset = matchInRegion(position) + + if base is None or needsSampling(base): + continue + + newBase = base + + # Define a helper function to buffer an oberver's visibleRegion, resulting + # in a region that contains all points that could feasibly be the position + # of obj, if it is visible from the observer. + def bufferHelper(viewRegion): + # Compute a voxel overapproximation of the mesh. Technically this is not + # an overapproximation, but one dilation with a rank 3 structuring unit + # with connectivity 3 is. To simplify, we just dilate one additional time. + target_pitch = PRUNING_PITCH * max(viewRegion.mesh.extents) + voxelized_vr = viewRegion.voxelized(target_pitch, lazy=True) + + # Dilate the voxel region. Dilation is done with a rank 3 structuring unit with + # connectivity 3 (a 3x3x3 cube of voxels). Each dilation pass must dilate by at + # least pitch. Therefore we must make at least ceiling((radius/2)/pitch) passes + # to ensure we have dilated by the half the circumradius of the object. We also + # add 1 iteration for the reasons above. + iterations = math.ceil((obj.radius / 2) / target_pitch) + 1 + dilated_vr = voxelized_vr.dilation(iterations=iterations) + + return dilated_vr + + # Prune based off visibility/non-visibility requirements + if obj.requireVisible: + # We can restrict the base region to the buffered visible region + # of the ego. + if ( + base is not ego.visibleRegion + and not needsSampling(ego.visibleRegion) + and not checkCyclical(base, ego.visibleRegion) + ): + if verbosity >= 1: + print( + f" Pruning restricted base region of {obj} to visible region of ego." + ) + newBase = newBase.intersect(bufferHelper(ego.visibleRegion)) + + if obj._observingEntity: + # We can restrict the base region to the buffered visible region + # of the observing entity. Only do this if the visible + # region is fixed, to avoid creating it at every timestep. + if ( + base is not obj._observingEntity.visibleRegion + and not needsSampling(obj._observingEntity.visibleRegion) + and not checkCyclical(base, obj._observingEntity.visibleRegion) + ): + if verbosity >= 1: + print( + f" Pruning restricted base region of {obj} to visible region of {obj._observingEntity}." + ) + newBase = newBase.intersect( + bufferHelper(obj._observingEntity.visibleRegion) + ) + + # Check newBase properties + if isinstance(newBase, EmptyRegion): + raise InvalidScenarioError( + f"Object {obj} can not satisfy visibility/non-visibility constraints." + ) + + percentage_pruned = percentagePruned(base, newBase) + + if percentage_pruned is None: + if verbosity >= 1: + print( + f" Visibility pruning attempted but could not compute percentage for {base} and {newBase}." + ) + else: + if percentage_pruned <= 0.001: + # We didn't really prune anything, don't bother setting new position + continue + + if verbosity >= 1: + print(f" Visibility pruning pruned {percentage_pruned:.1f}% of space.") + + # Condition object to pruned position + newPos = regions.Region.uniformPointIn(newBase) + + if offset is not None: + newPos += offset + + obj.position.conditionTo(newPos) + + def maxDistanceBetween(scenario, obj, target): """Upper bound the distance between the given Objects.""" # visDist is initialized to infinity. Then we can use @@ -428,3 +581,56 @@ def relativeHeadingRange( tPoints.extend((math.pi, -math.pi)) rhs = [tp - p for tp in tPoints for p in points] # TODO improve return min(rhs), max(rhs) + + +def percentagePruned(base, newBase): + if ( + base.dimensionality + and newBase.dimensionality + and base.dimensionality == newBase.dimensionality + ): + ratio = newBase.size / base.size + percent = max(0, 100 * (1.0 - ratio)) + return percent + + return None + + +def checkCyclical(A, B): + """Check for a potential circular dependency + + Returns True if the scenario would have a circular dependency + if A depended on B. + """ + state = collections.defaultdict(lambda: 0) + + def dfs(target): + # Check if the target is already completed/in-process + if state[target] == 2: + return False + elif state[target] == 1: + return True + + # Set to in-process + state[target] = 1 + + # Recurse on children + deps = conditionedDeps(target) + + for child in deps: + if child is A: + return True + + if dfs(child): + return True + + # Set to completed + state[target] = 2 + + return False + + return dfs(B) + + +def conditionedDeps(samp): + return list(dependencies(samp._conditioned if isinstance(samp, Samplable) else samp)) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 10bc40383..a4aa2e266 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -21,7 +21,8 @@ import shapely.ops import trimesh from trimesh.transformations import ( - concatenate_matrices, + compose_matrix, + identity_matrix, quaternion_matrix, translation_matrix, ) @@ -987,9 +988,9 @@ def isConvex(self): @property def AABB(self): return ( - tuple(self.mesh.bounds[0]), - tuple(self.mesh.bounds[1]), - tuple(self.mesh.bounds[2]), + tuple(self.mesh.bounds[:, 0]), + tuple(self.mesh.bounds[:, 1]), + tuple(self.mesh.bounds[:, 2]), ) @cached_property @@ -1735,11 +1736,12 @@ def distanceTo(self, point): return abs(dist) @cached_property + @distributionFunction def inradius(self): center_point = self.mesh.bounding_box.center_mass pq = trimesh.proximity.ProximityQuery(self.mesh) - region_distance = abs(pq.signed_distance([center_point])[0]) + region_distance = pq.signed_distance([center_point])[0] if region_distance < 0: return 0 @@ -1755,6 +1757,10 @@ def size(self): return self.mesh.mass / self.mesh.density ## Utility Methods ## + def voxelized(self, pitch, lazy=False): + """Returns a VoxelRegion representing a filled voxelization of this mesh""" + return VoxelRegion(voxelGrid=self.mesh.voxelized(pitch).fill(), lazy=lazy) + @cached_method def getSurfaceRegion(self): """Return a region equivalent to this one, except as a MeshSurfaceRegion""" @@ -2055,6 +2061,134 @@ def evaluateInner(self, context): ) +class VoxelRegion(Region): + """Region represented by a voxel grid in 3D space. + + Args: + voxelGrid: The Trimesh voxelGrid to be used. + orientation: An optional vector field describing the preferred orientation at every point in + the region. + name: An optional name to help with debugging. + lazy: Whether or not to be lazy about pre-computing internal values. Set this to True if this + VoxelRegion is unlikely to be used outside of an intermediate step in compiling/pruning. + """ + + def __init__(self, voxelGrid, orientation=None, name=None, lazy=False): + # Initialize superclass + super().__init__(name, orientation=orientation) + + # Check that the encoding isn't empty. In that case, raise an error. + if voxelGrid.encoding.is_empty: + raise ValueError("Tried to create an empty VoxelRegion.") + + # Store voxel grid and extract points and scale + self.voxelGrid = trimesh.voxel.VoxelGrid( + voxelGrid.encoding, transform=voxelGrid.transform.copy() + ) + self.voxel_points = self.voxelGrid.points + self.scale = self.voxelGrid.scale + + @cached_property + def kdTree(self): + return scipy.spatial.KDTree(self.voxel_points) + + def containsPoint(self, point): + point = toVector(point) + + # Find closest voxel point + _, index = self.kdTree.query(point) + closest_point = self.voxel_points[index] + + # Check voxel containment + voxel_low = closest_point - self.scale / 2 + voxel_high = closest_point + self.scale / 2 + + return numpy.all(voxel_low <= point) & numpy.all(point <= voxel_high) + + def containsObject(self, obj): + raise NotImplementedError + + def containsRegionInner(self, reg): + raise NotImplementedError + + def distanceTo(self, point): + raise NotImplementedError + + def projectVector(self, point, onDirection): + raise NotImplementedError + + def uniformPointInner(self): + # First generate a point uniformly in a box with dimensions + # equal to scale, centered at the origin. + base_pt = numpy.random.random_sample(3) - 0.5 + scaled_pt = base_pt * self.scale + + # Pick a random voxel point and add it to base_pt. + voxel_base = self.voxel_points[random.randrange(len(self.voxel_points))] + offset_pt = voxel_base + scaled_pt + + return Vector(*offset_pt) + + def dilation(self, iterations, structure=None): + """Returns a dilated/eroded version of this VoxelRegion. + + Args: + iterations: How many times repeat the dilation/erosion. A positive + number indicates a dilation and a negative number indicates an + erosion. + structure: The structure to use. If none is provided, a rank 3 + structuring unit with connectivity 3 is used. + """ + # Parse parameters + if iterations == 0: + return self + + if iterations > 0: + morphology_func = scipy.ndimage.binary_dilation + else: + morphology_func = scipy.ndimage.binary_erosion + + iterations = abs(iterations) + + if structure == None: + structure = scipy.ndimage.generate_binary_structure(3, 3) + + # Compute a dilated/eroded encoding + new_encoding = trimesh.voxel.encoding.DenseEncoding( + morphology_func( + trimesh.voxel.morphology._dense(self.voxelGrid.encoding, rank=3), + structure=structure, + iterations=iterations, + ) + ) + + # Check if the encoding is empty, in which case we should return the empty region. + if new_encoding.is_empty: + return nowhere + + # Otherwise, return a VoxelRegion representing the eroded region. + new_voxel_grid = trimesh.voxel.VoxelGrid( + new_encoding, transform=self.voxelGrid.transform + ) + return VoxelRegion(voxelGrid=new_voxel_grid) + + @property + def AABB(self): + return ( + tuple(self.voxelGrid.bounds[:, 0]), + tuple(self.voxelGrid.bounds[:, 1]), + tuple(self.voxelGrid.bounds[:, 2]), + ) + + @property + def size(self): + return self.voxelGrid.volume + + @property + def dimensionality(self): + return 3 + + class PolygonalFootprintRegion(Region): """Region that contains all points in a polygonal footprint, regardless of their z value. @@ -2680,6 +2814,19 @@ def distanceTo(self, point): dist2D = shapely.distance(self.polygons, makeShapelyPoint(point)) return math.hypot(dist2D, point[2] - self.z) + @cached_property + @distributionFunction + def inradius(self): + minx, miny, maxx, maxy = self.polygons.bounds + center = makeShapelyPoint(((minx + maxx) / 2, (maxy + miny) / 2)) + + # Check if center is contained + if not self.polygons.contains(center): + return 0 + + # Return the distance to the nearest boundary + return shapely.distance(self.polygons.boundary, center) + def projectVector(self, point, onDirection): raise NotImplementedError( f'{type(self).__name__} does not yet support projection using "on"' diff --git a/tests/core/test_regions.py b/tests/core/test_regions.py index 33dc99495..af4154d69 100644 --- a/tests/core/test_regions.py +++ b/tests/core/test_regions.py @@ -521,6 +521,76 @@ def test_pointset_region(): assert ps.AABB == ((1, 5), (2, 6), (0, 5)) +def test_voxel_region(): + encoding = [ + [[0, 0, 0], [0, 1, 0], [0, 0, 0]], + [[0, 1, 1], [0, 1, 0], [1, 1, 0]], + [[0, 0, 0], [0, 1, 0], [0, 0, 0]], + ] + + vg1 = trimesh.voxel.VoxelGrid(encoding=numpy.asarray(encoding)) + + centering_matrix = translation_matrix((vg1.scale - vg1.extents) / 2) + vg1.apply_transform(centering_matrix) + + scale = vg1.extents / numpy.array((3, 3, 3)) + scale_matrix = numpy.eye(4) + scale_matrix[:3, :3] /= scale + vg1.apply_transform(scale_matrix) + + position_matrix = translation_matrix((4, 5, 6)) + vg1.apply_transform(position_matrix) + + vr1 = VoxelRegion(vg1) + + assert vr1.containsPoint((4, 5, 6)) + assert vr1.containsPoint((4, 6, 5)) + assert vr1.containsPoint((4, 4, 7)) + assert not vr1.containsPoint((4, 6, 7)) + assert not vr1.containsPoint((4, 4, 5)) + assert not vr1.containsPoint((100, 100, 100)) + + for _ in range(100): + sampled_pt = vr1.uniformPointInner() + assert vr1.containsPoint(sampled_pt) + + assert vr1.AABB == ((2.5, 5.5), (3.5, 6.5), (4.5, 7.5)) + + vg2 = trimesh.voxel.VoxelGrid(encoding=numpy.asarray(encoding)) + + centering_matrix = translation_matrix((vg2.scale - vg2.extents) / 2) + vg2.apply_transform(centering_matrix) + + scale = vg2.extents / numpy.array((5, 5, 3)) + scale_matrix = numpy.eye(4) + scale_matrix[:3, :3] /= scale + vg2.apply_transform(scale_matrix) + + vr2 = VoxelRegion(vg2) + + assert vr2.size == pytest.approx((7 / 27) * 5 * 5 * 3) + assert vr2.dimensionality == 3 + + +def test_mesh_voxelization(getAssetPath): + plane_region = MeshVolumeRegion.fromFile(getAssetPath("meshes/classic_plane.obj.bz2")) + vr = plane_region.voxelized(max(plane_region.mesh.extents) / 100) + + for sampled_pt in trimesh.sample.volume_mesh(plane_region.mesh, 100): + assert vr.containsPoint(sampled_pt) + + for _ in range(100): + sampled_pt = vr.uniformPointInner() + assert vr.containsPoint(sampled_pt) + + +def test_empty_erosion(): + box_region = BoxRegion(position=(0, 0, 0), dimensions=(1, 1, 1)) + vr = box_region.voxelized(pitch=0.1) + erosion = vr.dilation(iterations=-6) + assert isinstance(erosion, EmptyRegion) + + # ViewRegion tests H_ANGLES = [0.1, 45, 90, 135, 179.9, 180, 180.1, 225, 270, 315, 359.9, 360] diff --git a/tests/syntax/test_properties.py b/tests/syntax/test_properties.py index f73663034..1b8f29888 100644 --- a/tests/syntax/test_properties.py +++ b/tests/syntax/test_properties.py @@ -1,8 +1,9 @@ import numpy as np import pytest +from scenic.core.distributions import Distribution, supportInterval from scenic.core.errors import SpecifierError -from tests.utils import compileScenic, sampleEgoFrom +from tests.utils import compileScenic, sampleEgo, sampleEgoFrom def test_position_wrong_type(): @@ -128,3 +129,156 @@ def test_backRight(): def test_heading_set_directly(): with pytest.raises(SpecifierError): compileScenic("ego = new Object with heading 4") + + +def test_object_inradius(): + # Statically Sized Cube Example + scenario = compileScenic( + """ + ego = new Object with width 3, with length 3, with height 3, + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg) + """ + ) + ego = sampleEgo(scenario) + assert scenario.objects[0].inradius == 1.5 + assert supportInterval(scenario.objects[0].inradius) == (1.5, 1.5) + assert ego.inradius == 1.5 + + # Randomly Sized Cube Example + scenario = compileScenic( + """ + ego = new Object with width Range(1, 3), + with length Range(1, 3), with height Range(1, 3), + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg) + """ + ) + ego = sampleEgo(scenario) + assert isinstance(scenario.objects[0].inradius, Distribution) + assert supportInterval(scenario.objects[0].inradius) == (0.5, 1.5) + assert ego.inradius == pytest.approx(min(ego.width, ego.length, ego.height) / 2) + + # Hollow Static Object Example + scenario = compileScenic( + """ + import trimesh + hollow_mesh = trimesh.creation.box((1,1,1)).difference( + trimesh.creation.box((0.5,0.5,0.5))) + ego = new Object with width 3, with length 3, with height 3, + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg), + with shape MeshShape(hollow_mesh) + """ + ) + ego = sampleEgo(scenario) + assert scenario.objects[0].inradius == 0 + assert supportInterval(scenario.objects[0].inradius) == (0, 0) + assert ego.inradius == 0 + + # Hollow Random Object Example + scenario = compileScenic( + """ + import trimesh + hollow_mesh = trimesh.creation.box((1,1,1)).difference( + trimesh.creation.box((0.5,0.5,0.5))) + ego = new Object with width Range(1, 3), + with length Range(1, 3), with height Range(1, 3), + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg), + with shape MeshShape(hollow_mesh) + """ + ) + ego = sampleEgo(scenario) + assert supportInterval(scenario.objects[0].inradius) == (0, 0) + assert ego.inradius == 0 + + # Random Shape Example + scenario = compileScenic( + """ + import trimesh + annulus_shape = MeshShape(trimesh.creation.annulus(0.5,1,1)) + ego = new Object with width Range(1, 3), + with length Range(1, 3), with height Range(1, 3), + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg), + with shape Uniform(BoxShape(), annulus_shape) + """ + ) + ego = sampleEgo(scenario) + assert isinstance(scenario.objects[0].inradius, Distribution) + assert supportInterval(scenario.objects[0].inradius) == (0, 1.5) + + +def test_object_planarInradius(): + # Statically Sized Cube Example + scenario = compileScenic( + """ + ego = new Object with width 3, with length 3, with height 0.5, + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg) + """ + ) + ego = sampleEgo(scenario) + assert scenario.objects[0].planarInradius == 1.5 + assert supportInterval(scenario.objects[0].planarInradius) == (1.5, 1.5) + assert ego.planarInradius == 1.5 + + # Randomly Sized Cube Example + scenario = compileScenic( + """ + ego = new Object with width Range(1, 3), + with length Range(1, 3), with height Range(0.25, 0.5), + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg) + """ + ) + ego = sampleEgo(scenario) + assert isinstance(scenario.objects[0].planarInradius, Distribution) + assert supportInterval(scenario.objects[0].planarInradius) == (0.5, 1.5) + assert ego.planarInradius == pytest.approx(min(ego.width, ego.length) / 2) + + # Hollow Static Object Example + scenario = compileScenic( + """ + import trimesh + hollow_mesh = trimesh.creation.box((1,1,1)).difference( + trimesh.creation.box((0.5,0.5,0.5))) + ego = new Object with width 3, with length 3, with height 0.5, + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg), + with shape MeshShape(hollow_mesh) + """ + ) + ego = sampleEgo(scenario) + assert scenario.objects[0].planarInradius == pytest.approx(1.5) + assert supportInterval(scenario.objects[0].planarInradius) == pytest.approx( + (1.5, 1.5) + ) + assert ego.planarInradius == pytest.approx(1.5) + + # Hollow Random Object Example + scenario = compileScenic( + """ + import trimesh + hollow_mesh = trimesh.creation.box((1,1,1)).difference( + trimesh.creation.box((0.5,0.5,0.5))) + ego = new Object with width Range(1, 3), + with length Range(1, 3), with height Range(0.25, 0.5), + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg), + with shape MeshShape(hollow_mesh) + """ + ) + ego = sampleEgo(scenario) + assert isinstance(scenario.objects[0].planarInradius, Distribution) + assert supportInterval(scenario.objects[0].planarInradius) == pytest.approx( + (0.5, 1.5) + ) + assert ego.planarInradius == pytest.approx(min(ego.width, ego.length) / 2) + + # Random Shape Example + scenario = compileScenic( + """ + import trimesh + annulus_shape = MeshShape(trimesh.creation.annulus(0.5,1,1)) + ego = new Object with width Range(1, 3), + with length Range(1, 3), with height Range(1, 3), + facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg), + with shape Uniform(BoxShape(), annulus_shape) + """ + ) + ego = sampleEgo(scenario) + assert isinstance(scenario.objects[0].planarInradius, Distribution) + assert supportInterval(scenario.objects[0].planarInradius) == (0, 1.5) diff --git a/tests/syntax/test_pruning.py b/tests/syntax/test_pruning.py index ed4e54b0b..215b03a83 100644 --- a/tests/syntax/test_pruning.py +++ b/tests/syntax/test_pruning.py @@ -4,7 +4,9 @@ import pytest from scenic.core.errors import InconsistentScenarioError -from tests.utils import compileScenic, sampleEgo +from scenic.core.pruning import checkCyclical +from scenic.core.vectors import Vector +from tests.utils import compileScenic, sampleEgo, sampleParamP def test_containment_in(): @@ -21,6 +23,57 @@ def test_containment_in(): assert any(0.5 <= x <= 0.7 or 1.3 <= x <= 1.5 for x in xs) +def test_containment_2d_region(): + """Test pruning based on object containment in a 2D region. + + Specifically tests that vertical portions of baseOffset are not added + to maxDistance, and that if objects are known to be flat in the plane, + their height is not considered as part of the minRadius. + """ + # Tests the effect of the vertical portion of baseOffset in a 2D region. + scenario = compileScenic( + """ + workspace = Workspace(PolygonalRegion([0@0, 2@0, 2@2, 0@2])) + ego = new Object on workspace + """ + ) + # Sampling should only require 1 iteration after pruning + xs = [sampleEgo(scenario).position.x for i in range(60)] + assert all(0.5 <= x <= 1.5 for x in xs) + assert any(0.5 <= x <= 0.7 or 1.3 <= x <= 1.5 for x in xs) + + # Test height's effect in a 2D region. + scenario = compileScenic( + """ + workspace = Workspace(PolygonalRegion([0@0, 2@0, 2@2, 0@2])) + ego = new Object in workspace, with height 0.1 + """ + ) + # Sampling should only require 1 iteration after pruning + xs = [sampleEgo(scenario).position.x for i in range(60)] + assert all(0.5 <= x <= 1.5 for x in xs) + assert any(0.5 <= x <= 0.7 or 1.3 <= x <= 1.5 for x in xs) + + # Test both combined, in a slightly more complicated case. + # Specifically, there is a non vertical component to baseOffset + # that should be accounted for and the height is random. + scenario = compileScenic( + """ + class TestObject: + baseOffset: (0.1, 0, self.height/2) + + workspace = Workspace(PolygonalRegion([0@0, 2@0, 2@2, 0@2])) + ego = new TestObject on workspace, with height Range(0.1,0.5) + """ + ) + # Sampling should fail ~30.56% of the time, so + # 34 rejections are allowed to get the failure probability + # to ~1e-18. + xs = [sampleEgo(scenario, maxIterations=34).position.x for i in range(60)] + assert all(0.5 <= x <= 1.5 for x in xs) + assert any(0.5 <= x <= 0.7 or 1.3 <= x <= 1.5 for x in xs) + + def test_containment_in_polyline(): """As above, but when the object is placed on a polyline.""" scenario = compileScenic( @@ -106,3 +159,88 @@ def test_relative_heading_inconsistent(): require abs(relative heading of other) < -1 """ ) + + +def test_visibility_pruning(): + """Test visibility pruning in general. + + The following scenarios are equivalent except for how they specify that foo + must be visible from ego. The size of the workspace and the visibleDistance + of ego are chosen such that without pruning the chance of sampling a valid + scene over 100 tries is 1-(1-Decimal(3.14)/Decimal(1e10**2))**100 = ~1e-18. + Assuming the approximately buffered volume of the viewRegion has a 50% chance of + rejecting (i.e. it is twice as large as the true buffered viewRegion, which testing + indicates in this case has about a 10% increase in volume for this case), the chance + of not finding a sample in 100 iterations is 1e-31. + + We also want to confirm that we aren't pruning too much, i.e. placing the position + in the viewRegion instead of at any point where the object intersects the view region. + Because of this, we want to see at least one sample where the position is outside + the viewRegion but the object intersects the viewRegion. The chance of this happening + per sample is 1 - (1 / 1.1)**3 = ~25%, so by repeating the process 30 times we have + a 1e-19 chance of not getting a single point in this zone. + """ + # requireVisible + scenario = compileScenic( + """ + workspace = Workspace(RectangularRegion(0@0, 0, 1e10, 1e10)) + ego = new Object at (0,0,0), with visibleDistance 1 + foo = new Object in workspace, with requireVisible True, + with shape SpheroidShape(dimensions=(0.2,0.2,0.2)) + param p = foo.position + """ + ) + positions = [sampleParamP(scenario, maxIterations=100) for i in range(30)] + assert all(pos.distanceTo(Vector(0, 0, 0)) <= 1.1 for pos in positions) + assert any(pos.distanceTo(Vector(0, 0, 0)) >= 1 for pos in positions) + + # visible + scenario = compileScenic( + """ + workspace = Workspace(RectangularRegion(0@0, 0, 1e10, 1e10)) + ego = new Object at (0,0,0), with visibleDistance 1 + foo = new Object in workspace, visible, + with shape SpheroidShape(dimensions=(0.2,0.2,0.2)) + param p = foo.position + """ + ) + positions = [sampleParamP(scenario, maxIterations=100) for i in range(30)] + assert all(pos.distanceTo(Vector(0, 0, 0)) <= 1.1 for pos in positions) + assert any(pos.distanceTo(Vector(0, 0, 0)) >= 1 for pos in positions) + + +def test_visibility_pruning_cyclical(): + """A case where a cyclical dependency could be introduced if pruning is not done carefully. + + NOTE: We don't currently prune this case so this test is a sentinel for future behavior. + """ + scenario = compileScenic( + """ + workspace = Workspace(PolygonalRegion([0@0, 100@0, 100@100, 0@100])) + foo = new Object with requireVisible True, in workspace + ego = new Object visible from foo, in workspace + """ + ) + + sampleEgo(scenario, maxIterations=100) + + +def test_checkCyclical(): + scenario = compileScenic( + """ + workspace = Workspace(PolygonalRegion([0@0, 100@0, 100@100, 0@100])) + foo = new Object in workspace + ego = new Object in workspace + """ + ) + assert not checkCyclical(scenario.objects[1].position, scenario.objects[0].position) + + scenario = compileScenic( + """ + workspace = Workspace(PolygonalRegion([0@0, 100@0, 100@100, 0@100])) + foo = new Object with requireVisible True, in workspace + ego = new Object visible from foo + """ + ) + + assert checkCyclical(scenario.objects[1].position, scenario.objects[0].visibleRegion)