├── .gitignore ├── LICENSE.txt ├── README.md ├── pyproject.toml ├── src └── pytorch_volumetric │ ├── __init__.py │ ├── chamfer.py │ ├── model_to_sdf.py │ ├── sdf.py │ ├── visualization.py │ ├── volume.py │ └── voxel.py └── tests ├── YcbPowerDrill ├── collision_vhacd.mtl ├── collision_vhacd.obj ├── model.urdf ├── texture_map.png ├── textured_simple_reoriented.mtl └── textured_simple_reoriented.obj ├── offset_wrench.urdf ├── offset_wrench_nogrip.obj ├── probe.obj ├── pv_sdf_debug ├── box_template.obj ├── scene_mesh_gt.obj ├── scene_mesh_overlap.mtl ├── scene_mesh_overlap.obj ├── scene_mesh_separated.mtl ├── scene_mesh_separated.obj ├── scene_mesh_wrong.obj └── test_export_composed_sdf.py ├── test_chamfer.py ├── test_model_to_sdf.py ├── test_sdf.py └── test_voxel_sdf.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | temp* 3 | dist 4 | *.egg-info 5 | *.pyc 6 | *.pkl 7 | *.mp4 8 | tests/intermediate_meshes -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2023 University of Michigan ARM Lab 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of 4 | this software and associated documentation files (the "Software"), to deal in 5 | the Software without restriction, including without limitation the rights to 6 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 7 | of the Software, and to permit persons to whom the Software is furnished to do 8 | so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Pytorch Volumetric 2 | 3 | - signed distance field (SDF) pytorch implementation with parallelized query for value and gradients 4 | - voxel grids with automatic expanding range 5 | - unidirectional chamfer distance (points to mesh) 6 | - robot model to SDF with parallelized query over robot configurations and points 7 | 8 | SDF slice animated over a KUKA robot 9 | 10 | ![pv_sdf_slice](https://github.com/user-attachments/assets/d9081706-6463-4716-82ec-b278969d24f4) 11 | 12 | 13 | ## Installation 14 | 15 | ```shell 16 | pip install pytorch-volumetric 17 | ``` 18 | 19 | For development, clone repository somewhere, then `pip3 install -e .` to install in editable mode. 20 | For testing, run `pytest` in the root directory. 21 | 22 | ## Usage 23 | 24 | See `tests` for code samples; some are also shown here 25 | 26 | ### SDF from mesh 27 | 28 | ```python 29 | import pytorch_volumetric as pv 30 | 31 | # supposing we have an object mesh (most formats supported) - from https://github.com/eleramp/pybullet-object-models 32 | obj = pv.MeshObjectFactory("YcbPowerDrill/textured_simple_reoriented.obj") 33 | sdf = pv.MeshSDF(obj) 34 | ``` 35 | 36 | An `open3d` mesh can be provided via the `mesh=` argument to `MeshObjectFactory`. When doing so, transform parameters 37 | such as scale are ignored. 38 | 39 | ### Cached SDF 40 | 41 | ```python 42 | import pytorch_volumetric as pv 43 | 44 | obj = pv.MeshObjectFactory("YcbPowerDrill/textured_simple_reoriented.obj") 45 | sdf = pv.MeshSDF(obj) 46 | # caching the SDF via a voxel grid to accelerate queries 47 | cached_sdf = pv.CachedSDF('drill', resolution=0.01, range_per_dim=obj.bounding_box(padding=0.1), gt_sdf=sdf) 48 | ``` 49 | 50 | By default, query points outside the cache will be compared against the object bounding box. 51 | To instead use the ground truth SDF, pass `out_of_bounds_strategy=pv.OutOfBoundsStrategy.LOOKUP_GT_SDF` to 52 | the constructor. 53 | 54 | Note that the bounding box comparison will always under-approximate the SDF value, but empirically it is sufficient 55 | for most applications when querying out of bound points. It is **dramatically faster** than using the ground truth SDF. 56 | 57 | ### Composed SDF 58 | Multiple SDFs can be composed together to form an SDF that is convenient to query. This may be because your scene 59 | is composed of multiple objects and you have them as separate meshes. Note: the objects should not be overlapping or 60 | share faces, otherwise there will be artifacts in the SDF query in determining interior-ness. 61 | 62 | ```python 63 | import pytorch_volumetric as pv 64 | import pytorch_kinematics as pk 65 | 66 | obj = pv.MeshObjectFactory("YcbPowerDrill/textured_simple_reoriented.obj") 67 | 68 | # 2 drills in the world 69 | sdf1 = pv.MeshSDF(obj) 70 | sdf2 = pv.MeshSDF(obj) 71 | # need to specify the transform of each SDF frame 72 | tsf1 = pk.Translate(0.1, 0, 0) 73 | tsf2 = pk.Translate(-0.2, 0, 0.2) 74 | sdf = pv.ComposedSDF([sdf1, sdf2], tsf1.stack(tsf2)) 75 | ``` 76 | 77 | ### SDF value and gradient queries 78 | 79 | Suppose we have an `ObjectFrameSDF` (such as created from above) 80 | 81 | ```python 82 | import numpy as np 83 | import pytorch_volumetric as pv 84 | 85 | # get points in a grid in the object frame 86 | query_range = np.array([ 87 | [-1, 0.5], 88 | [-0.5, 0.5], 89 | [-0.2, 0.8], 90 | ]) 91 | 92 | coords, pts = pv.get_coordinates_and_points_in_grid(0.01, query_range) 93 | # N x 3 points 94 | # we can also query with batched points B x N x 3, B can be any number of batch dimensions 95 | sdf_val, sdf_grad = sdf(pts) 96 | # sdf_val is N, or B x N, the SDF value in meters 97 | # sdf_grad is N x 3 or B x N x 3, the normalized SDF gradient (points along steepest increase in SDF) 98 | ``` 99 | 100 | ### Plotting SDF Slice 101 | 102 | ```python 103 | import pytorch_volumetric as pv 104 | import numpy as np 105 | 106 | # supposing we have an object mesh (most formats supported) - from https://github.com/eleramp/pybullet-object-models 107 | obj = pv.MeshObjectFactory("YcbPowerDrill/textured_simple_reoriented.obj") 108 | sdf = pv.MeshSDF(obj) 109 | # need a dimension with no range to slice; here it's y 110 | query_range = np.array([ 111 | [-0.15, 0.2], 112 | [0, 0], 113 | [-0.1, 0.2], 114 | ]) 115 | pv.draw_sdf_slice(sdf, query_range) 116 | ``` 117 | 118 | ![drill SDF](https://i.imgur.com/TFaGmx6.png) 119 | 120 | ### Robot Model to SDF 121 | 122 | For many applications such as collision checking, it is useful to have the 123 | SDF of a multi-link robot in certain configurations. 124 | First, we create the robot model (loaded from URDF, SDF, MJCF, ...) with 125 | [pytorch kinematics](https://github.com/UM-ARM-Lab/pytorch_kinematics). 126 | For example, we will be using the KUKA 7 DOF arm model from pybullet data 127 | 128 | ```python 129 | import os 130 | import torch 131 | import pybullet_data 132 | import pytorch_kinematics as pk 133 | import pytorch_volumetric as pv 134 | 135 | urdf = "kuka_iiwa/model.urdf" 136 | search_path = pybullet_data.getDataPath() 137 | full_urdf = os.path.join(search_path, urdf) 138 | chain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), "lbr_iiwa_link_7") 139 | d = "cuda" if torch.cuda.is_available() else "cpu" 140 | 141 | chain = chain.to(device=d) 142 | # paths to the link meshes are specified with their relative path inside the URDF 143 | # we need to give them the path prefix as we need their absolute path to load 144 | s = pv.RobotSDF(chain, path_prefix=os.path.join(search_path, "kuka_iiwa")) 145 | ``` 146 | 147 | By default, each link will have a `MeshSDF`. To instead use `CachedSDF` for faster queries 148 | 149 | ```python 150 | s = pv.RobotSDF(chain, path_prefix=os.path.join(search_path, "kuka_iiwa"), 151 | link_sdf_cls=pv.cache_link_sdf_factory(resolution=0.02, padding=1.0, device=d)) 152 | ``` 153 | 154 | Which when the `y=0.02` SDF slice is visualized: 155 | ![sdf slice](https://i.imgur.com/Putw72A.png) 156 | 157 | With surface points corresponding to: 158 | ![wireframe](https://i.imgur.com/L3atG9h.png) 159 | ![solid](https://i.imgur.com/XiAks7a.png) 160 | 161 | Queries on this SDF is dependent on the joint configurations (by default all zero). 162 | **Queries are batched across configurations and query points**. For example, we have a batch of 163 | joint configurations to query 164 | 165 | ```python 166 | th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0], device=d) 167 | N = 200 168 | th_perturbation = torch.randn(N - 1, 7, device=d) * 0.1 169 | # N x 7 joint values 170 | th = torch.cat((th.view(1, -1), th_perturbation + th)) 171 | ``` 172 | 173 | And also a batch of points to query (same points for each configuration): 174 | 175 | ```python 176 | y = 0.02 177 | query_range = np.array([ 178 | [-1, 0.5], 179 | [y, y], 180 | [-0.2, 0.8], 181 | ]) 182 | # M x 3 points 183 | coords, pts = pv.get_coordinates_and_points_in_grid(0.01, query_range, device=s.device) 184 | ``` 185 | 186 | We set the batch of joint configurations and query: 187 | 188 | ```python 189 | s.set_joint_configuration(th) 190 | # N x M SDF value 191 | # N x M x 3 SDF gradient 192 | sdf_val, sdf_grad = s(pts) 193 | ``` 194 | 195 | Queries are reasonably quick. For the 7 DOF Kuka arm (8 links), using `CachedSDF` on a RTX 2080 Ti, 196 | and using CUDA, we get 197 | 198 | ```shell 199 | N=20, M=15251, elapsed: 37.688577ms time per config and point: 0.000124ms 200 | N=200, M=15251, elapsed: elapsed: 128.645445ms time per config and point: 0.000042ms 201 | ``` 202 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "pytorch_volumetric" 3 | version = "0.5.2" 4 | description = "Volumetric structures such as voxels and SDFs implemented in pytorch" 5 | readme = "README.md" # Optional 6 | 7 | # Specify which Python versions you support. In contrast to the 8 | # 'Programming Language' classifiers above, 'pip install' will check this 9 | # and refuse to install the project if the version does not match. See 10 | # https://packaging.python.org/guides/distributing-packages-using-setuptools/#python-requires 11 | requires-python = ">=3.6" 12 | 13 | # This is either text indicating the license for the distribution, or a file 14 | # that contains the license 15 | # https://packaging.python.org/en/latest/specifications/core-metadata/#license 16 | license = { file = "LICENSE.txt" } 17 | 18 | # This field adds keywords for your project which will appear on the 19 | # project page. What does your project relate to? 20 | # 21 | # Note that this is a list of additional keywords, separated 22 | # by commas, to be used to assist searching for the distribution in a 23 | # larger catalog. 24 | keywords = ["robotics", "sdf", "voxels", "pytorch"] # Optional 25 | authors = [ 26 | { name = "Sheng Zhong", email = "zhsh@umich.edu" } # Optional 27 | ] 28 | maintainers = [ 29 | { name = "Sheng Zhong", email = "zhsh@umich.edu" } # Optional 30 | ] 31 | 32 | # Classifiers help users find your project by categorizing it. 33 | # 34 | # For a list of valid classifiers, see https://pypi.org/classifiers/ 35 | classifiers = [# Optional 36 | "Development Status :: 4 - Beta", 37 | # Indicate who your project is intended for 38 | "Intended Audience :: Developers", 39 | # Pick your license as you wish 40 | "License :: OSI Approved :: MIT License", 41 | # Specify the Python versions you support here. In particular, ensure 42 | # that you indicate you support Python 3. These classifiers are *not* 43 | # checked by "pip install". See instead "python_requires" below. 44 | "Programming Language :: Python :: 3", 45 | "Programming Language :: Python :: 3 :: Only", 46 | ] 47 | 48 | # This field lists other packages that your project depends on to run. 49 | # Any package you put here will be installed by pip when your project is 50 | # installed, so they must be valid existing projects. 51 | # 52 | # For an analysis of this field vs pip's requirements files see: 53 | # https://packaging.python.org/discussions/install-requires-vs-requirements/ 54 | dependencies = [# Optional 55 | 'torch', 56 | 'numpy', 57 | 'open3d', 58 | 'arm-pytorch-utilities>=0.4', 59 | 'multidim-indexing', 60 | 'pytorch-kinematics>=0.5.6', 61 | ] 62 | 63 | # List additional groups of dependencies here (e.g. development 64 | # dependencies). Users will be able to install these using the "extras" 65 | # syntax, for example: 66 | # 67 | # $ pip install sampleproject[dev] 68 | # 69 | # Similar to `dependencies` above, these must be valid existing 70 | # projects. 71 | [project.optional-dependencies] # Optional 72 | test = ["pytest", "pybullet"] 73 | 74 | # List URLs that are relevant to your project 75 | # 76 | # This field corresponds to the "Project-URL" and "Home-Page" metadata fields: 77 | # https://packaging.python.org/specifications/core-metadata/#project-url-multiple-use 78 | # https://packaging.python.org/specifications/core-metadata/#home-page-optional 79 | # 80 | # Examples listed include a pattern for specifying where the package tracks 81 | # issues, where the source is hosted, where to say thanks to the package 82 | # maintainers, and where to support the project financially. The key is 83 | # what's used to render the link text on PyPI. 84 | [project.urls] # Optional 85 | "Homepage" = "https://github.com/UM-ARM-Lab/pytorch_volumetric" 86 | "Bug Reports" = "https://github.com/UM-ARM-Lab/pytorch_volumetric/issues" 87 | "Source" = "https://github.com/UM-ARM-Lab/pytorch_volumetric" 88 | 89 | # The following would provide a command line executable called `sample` 90 | # which executes the function `main` from this package when invoked. 91 | #[project.scripts] # Optional 92 | #sample = "sample:main" 93 | 94 | # This is configuration specific to the `setuptools` build backend. 95 | # If you are using a different build backend, you will need to change this. 96 | [tool.setuptools] 97 | # If there are data files included in your packages that need to be 98 | # installed, specify them here. 99 | 100 | [build-system] 101 | # These are the assumed default build requirements from pip: 102 | # https://pip.pypa.io/en/stable/reference/pip/#pep-517-and-518-support 103 | requires = ["setuptools>=43.0.0", "wheel"] 104 | build-backend = "setuptools.build_meta" -------------------------------------------------------------------------------- /src/pytorch_volumetric/__init__.py: -------------------------------------------------------------------------------- 1 | from pytorch_volumetric.chamfer import batch_chamfer_dist, PlausibleDiversity, pairwise_distance, \ 2 | pairwise_distance_chamfer 3 | from pytorch_volumetric.sdf import sample_mesh_points, ObjectFrameSDF, MeshSDF, CachedSDF, ComposedSDF, SDFQuery, \ 4 | ObjectFactory, MeshObjectFactory, OutOfBoundsStrategy, SphereSDF 5 | from pytorch_volumetric.voxel import Voxels, VoxelGrid, VoxelSet, ExpandingVoxelGrid, get_divisible_range_by_resolution, \ 6 | get_coordinates_and_points_in_grid, voxel_down_sample 7 | from pytorch_volumetric.model_to_sdf import RobotSDF, cache_link_sdf_factory, aabb_to_ordered_end_points 8 | from pytorch_volumetric.visualization import draw_sdf_slice, get_transformed_meshes 9 | from pytorch_volumetric.volume import is_inside 10 | -------------------------------------------------------------------------------- /src/pytorch_volumetric/chamfer.py: -------------------------------------------------------------------------------- 1 | import time 2 | from typing import NamedTuple 3 | 4 | import torch 5 | import pytorch_kinematics as pk 6 | from pytorch_kinematics import transforms as tf 7 | from pytorch_kinematics.transforms.rotation_conversions import matrix_to_pos_rot 8 | 9 | from pytorch_volumetric.sdf import ObjectFactory, sample_mesh_points, ObjectFrameSDF 10 | 11 | 12 | def pairwise_distance(world_to_link_tfs: pk.Transform3d): 13 | m = world_to_link_tfs.get_matrix() 14 | t = m[:, :3, 3] 15 | r = pk.rotation_conversions.matrix_to_rotation_6d(m[:, :3, :3]) 16 | cont_rep = torch.cat((t, r), dim=1) 17 | return torch.cdist(cont_rep, cont_rep) 18 | 19 | 20 | def pairwise_distance_chamfer(A_link_to_world_tfs: pk.Transform3d, B_world_to_link_tfs: pk.Transform3d = None, 21 | obj_factory: ObjectFactory = None, 22 | obj_sdf: ObjectFrameSDF = None, 23 | model_points_eval: torch.tensor = None, vis=None, scale=1000): 24 | """ 25 | Compute the pairwise chamfer distance between a set of world to link transforms of an object. 26 | :param A_link_to_world_tfs: B x 4 x 4 link frame to world frame transforms 27 | :param B_world_to_link_tfs: P x 4 x 4 world frame to link frame transforms, if None, then the inverse of A will be used 28 | :param obj_factory: object factory to evaluate against 29 | :param obj_sdf: object sdf to evaluate against (accelerates the computation at the cost of accuracy) 30 | :param model_points_eval: points to evaluate the chamfer distance on; if None, sample from the object 31 | :param vis: visualizer 32 | :param scale: units with respect to the position units; e.g. if the position units are in meters, then the scale=1 33 | :return: B x B chamfer distance matrix in squared distance units, averaged across the model_points_eval 34 | """ 35 | if model_points_eval is None: 36 | model_points_eval, _, _ = sample_mesh_points(obj_factory, num_points=500, name=obj_factory.name, 37 | device=A_link_to_world_tfs.device) 38 | 39 | # effectively can apply one transform then take the inverse using the other one; if they are the same, then 40 | # we should end up in the base frame if that T == Tp 41 | # want pairwise matrix multiplication |T| x |Tp| x 4 x 4 T[0]@Tp[0], T[0]@Tp[1] 42 | T = A_link_to_world_tfs.get_matrix() 43 | if B_world_to_link_tfs is None: 44 | # this is fastor than inverting the 4x4 since we can exploit orthogonality 45 | T_inv = A_link_to_world_tfs.inverse().get_matrix() 46 | else: 47 | T_inv = B_world_to_link_tfs.get_matrix() 48 | Iapprox = torch.einsum("bij,pjk->bpik", T_inv, T) 49 | 50 | B = len(T) 51 | P = len(T_inv) 52 | # the einsum does the multiplication below and is about twice as fast 53 | # Iapprox = T.inverse().view(-1, 1, 4, 4) @ T.view(1, -1, 4, 4) 54 | 55 | errors_per_batch = batch_chamfer_dist(Iapprox.reshape(B * P, 4, 4), model_points_eval, 56 | obj_factory=obj_factory, obj_sdf=obj_sdf, 57 | viewing_delay=0, vis=vis, scale=scale) 58 | errors_per_batch = errors_per_batch.view(B, P) 59 | return errors_per_batch 60 | 61 | 62 | def batch_chamfer_dist(world_to_object: torch.tensor, model_points_world_frame_eval: torch.tensor, 63 | obj_factory: ObjectFactory = None, obj_sdf: ObjectFrameSDF = None, viewing_delay=0, scale=1000., 64 | print_err=False, vis=None): 65 | """ 66 | Compute batched unidirectional chamfer distance between the observed world frame surface points and the 67 | surface points of the object transformed by a set of 4x4 rigid transform matrices (from the object frame). 68 | :param world_to_object: B x 4 x 4 transformation matrices from world to object frame 69 | :param model_points_world_frame_eval: N x 3 points to evaluate the chamfer distance on 70 | :param obj_factory: object to evaluate against 71 | :param obj_sdf: sdf of the object to evaluate against (potentially much faster than obj_sdf, but less accurate) 72 | :param viewing_delay: if a visualizer is given, sleep between plotting successive elements 73 | :param scale: units with respect to the position units; e.g. if the position units are in meters, then the scale 74 | being 1000 will convert the distance to mm 75 | :param print_err: whether to visualize and print the chamfer error 76 | :param vis: optional visualizer 77 | :return: B chamfer error for each transform, averaged across the N points to evaluate against 78 | """ 79 | B = world_to_object.shape[0] 80 | eval_num_points = model_points_world_frame_eval.shape[0] 81 | world_to_link = tf.Transform3d(matrix=world_to_object) 82 | model_points_object_frame_eval = world_to_link.transform_points(model_points_world_frame_eval) 83 | 84 | if obj_sdf is not None: 85 | d, _ = obj_sdf(model_points_object_frame_eval) 86 | elif obj_factory is not None: 87 | res = obj_factory.object_frame_closest_point(model_points_object_frame_eval) 88 | d = res.distance 89 | else: 90 | raise ValueError("Either obj_sdf or obj_factory must be given") 91 | # convert to scale such as mm**2 92 | chamfer_distance = (scale * d) ** 2 93 | # average across the evaluation points 94 | errors_per_batch = chamfer_distance.mean(dim=-1) 95 | 96 | if vis is not None and obj_factory is not None: 97 | link_to_world = world_to_link.inverse() 98 | res = obj_factory.object_frame_closest_point(model_points_object_frame_eval) 99 | closest_pt_world_frame = link_to_world.transform_points(res.closest) 100 | m = link_to_world.get_matrix() 101 | for b in range(B): 102 | pos, rot = matrix_to_pos_rot(m[b]) 103 | obj_factory.draw_mesh(vis, "chamfer evaluation", (pos, rot), rgba=(0, 0.1, 0.8, 0.1), 104 | object_id=vis.USE_DEFAULT_ID_FOR_NAME) 105 | # vis.draw_point("avgerr", [0, 0, 0], (1, 0, 0), label=f"avgerr: {round(errors_per_batch[b].item())}") 106 | 107 | if print_err: 108 | for i in range(eval_num_points): 109 | query = model_points_world_frame_eval[i].cpu() 110 | closest = closest_pt_world_frame[b, i].cpu() 111 | vis.draw_point(f"query.{i}", query, (0, 1, 0)) 112 | vis.draw_point(f"closest.{i}", closest, (0, 1, 1)) 113 | vis.draw_2d_line(f"qc.{i}", query, closest - query, (0, 1, 0), scale=1) 114 | 115 | time.sleep(viewing_delay) 116 | 117 | # move somewhere far away 118 | obj_factory.draw_mesh(vis, "chamfer evaluation", ([0, 0, 100], [0, 0, 0, 1]), rgba=(0, 0.2, 0.8, 0.2), 119 | object_id=vis.USE_DEFAULT_ID_FOR_NAME) 120 | return errors_per_batch 121 | 122 | 123 | class PlausibleDiversityReturn(NamedTuple): 124 | plausibility: torch.tensor 125 | coverage: torch.tensor 126 | most_plausible_per_estimated: torch.tensor 127 | most_covered_per_plausible: torch.tensor 128 | 129 | 130 | class PlausibleDiversity: 131 | """ 132 | Compute the plausibility and coverage of an estimated set of transforms against a set of plausible transforms. 133 | Each transform is a 4x4 homogeneous matrix. The return are in units of squared coordinates (for example if points 134 | are given with their coordinates in mm, then the return is in mm^2). In some sense it is a set divergence. 135 | """ 136 | 137 | def __init__(self, obj_factory: ObjectFactory, model_points_eval: torch.tensor = None, num_model_points_eval=500, 138 | obj_sdf: ObjectFrameSDF = None): 139 | self.obj_factory = obj_factory 140 | self.obj_sdf = obj_sdf 141 | # if model points are not given, sample them from the object 142 | if model_points_eval is None: 143 | model_points_eval, _, _ = sample_mesh_points(obj_factory, num_points=num_model_points_eval, 144 | name=obj_factory.name) 145 | self.model_points_eval = model_points_eval 146 | 147 | def __call__(self, T_est_inv, T_p, bidirectional=False, scale=1000.): 148 | """ 149 | Compute the plausibility and coverage of an estimated set of transforms against a set of plausible transforms. 150 | 151 | :param T_est_inv: The inverse of the estimated transforms, in the form of Bx4x4 homogeneous matrices. 152 | :param T_p: The plausible transforms, in the form of Px4x4 homogeneous matrices. 153 | :return: plausibility score, coverage score, most_plausible_per_estimated, most_covered_per_plausible 154 | """ 155 | errors_per_batch = self.compute_tf_pairwise_error_per_batch(T_est_inv, T_p, scale=scale) 156 | ret = self.do_evaluate_plausible_diversity_on_pairwise_chamfer_dist(errors_per_batch) 157 | if bidirectional: 158 | errors_per_batch_rev = self.compute_tf_pairwise_error_per_batch(T_p, T_est_inv, scale=scale) 159 | ret2 = self.do_evaluate_plausible_diversity_on_pairwise_chamfer_dist(errors_per_batch_rev) 160 | # the plausibility and coverage are flipped when we reverse the transforms 161 | ret = PlausibleDiversityReturn( 162 | plausibility=(ret.plausibility + ret2.coverage) / 2, 163 | coverage=(ret.coverage + ret2.plausibility) / 2, 164 | most_plausible_per_estimated=ret.most_plausible_per_estimated, 165 | most_covered_per_plausible=ret.most_covered_per_plausible, 166 | ) 167 | return ret 168 | 169 | def compute_tf_pairwise_error_per_batch(self, T_est_inv, T_p, scale=1000.): 170 | # effectively can apply one transform then take the inverse using the other one; if they are the same, then 171 | # we should end up in the base frame if that T == Tp 172 | # want pairwise matrix multiplication |T| x |Tp| x 4 x 4 T[0]@Tp[0], T[0]@Tp[1] 173 | Iapprox = torch.einsum("bij,pjk->bpik", T_est_inv, T_p) 174 | # the einsum does the multiplication below and is about twice as fast 175 | # Iapprox = T_est_inv.view(-1, 1, 4, 4) @ T_p.view(1, -1, 4, 4) 176 | 177 | B, P = Iapprox.shape[:2] 178 | self.model_points_eval = self.model_points_eval.to(device=Iapprox.device, dtype=Iapprox.dtype) 179 | errors_per_batch = batch_chamfer_dist(Iapprox.reshape(B * P, 4, 4), self.model_points_eval, 180 | self.obj_factory, obj_sdf=self.obj_sdf, viewing_delay=0, vis=None, 181 | scale=scale) 182 | errors_per_batch = errors_per_batch.view(B, P) 183 | return errors_per_batch 184 | 185 | @staticmethod 186 | def do_evaluate_plausible_diversity_on_pairwise_chamfer_dist(errors_per_batch): 187 | B, P = errors_per_batch.shape 188 | 189 | best_per_sampled = errors_per_batch.min(dim=1) 190 | best_per_plausible = errors_per_batch.min(dim=0) 191 | 192 | bp_plausibility = best_per_sampled.values.sum() / B 193 | bp_coverage = best_per_plausible.values.sum() / P 194 | 195 | return PlausibleDiversityReturn(bp_plausibility, bp_coverage, best_per_sampled, best_per_plausible) 196 | -------------------------------------------------------------------------------- /src/pytorch_volumetric/model_to_sdf.py: -------------------------------------------------------------------------------- 1 | import typing 2 | 3 | import numpy as np 4 | import torch 5 | import pytorch_kinematics as pk 6 | from pytorch_volumetric import sdf 7 | import logging 8 | 9 | logger = logging.getLogger(__file__) 10 | 11 | 12 | class RobotSDF(sdf.ObjectFrameSDF): 13 | """Create an SDF for a robot model described by a pytorch_kinematics Chain. 14 | The SDF is conditioned on a joint configuration which must be set.""" 15 | 16 | def __init__(self, chain: pk.Chain, default_joint_config=None, path_prefix='', 17 | link_sdf_cls: typing.Callable[[sdf.ObjectFactory], sdf.ObjectFrameSDF] = sdf.MeshSDF): 18 | """ 19 | 20 | :param chain: Robot description; each link should be a mesh type - non-mesh geometries are ignored 21 | :param default_joint_config: values for each joint of the robot by default; None results in all zeros 22 | :param path_prefix: path to search for referenced meshes inside the robot description (e.g. URDF) which may use 23 | relative paths. This given path is prefixed onto those relative paths in order to find the meshes. 24 | :param link_sdf_cls: Factory of each link's SDFs; **kwargs are forwarded to this factory 25 | :param kwargs: Keyword arguments fed to link_sdf_cls 26 | """ 27 | self.chain = chain 28 | self.dtype = self.chain.dtype 29 | self.device = self.chain.device 30 | self.q = None 31 | self.object_to_link_frames: typing.Optional[pk.Transform3d] = None 32 | self.joint_names = self.chain.get_joint_parameter_names() 33 | self.frame_names = self.chain.get_frame_names(exclude_fixed=False) 34 | self.sdf: typing.Optional[sdf.ComposedSDF] = None 35 | self.sdf_to_link_name = [] 36 | self.configuration_batch = None 37 | 38 | sdfs = [] 39 | offsets = [] 40 | # get the link meshes from the frames and create meshes 41 | for frame_name in self.frame_names: 42 | frame = self.chain.find_frame(frame_name) 43 | # TODO create SDF for non-mesh primitives 44 | # TODO consider the visual offset transform 45 | for link_vis in frame.link.visuals: 46 | if link_vis.geom_type == "mesh": 47 | logger.info(f"{frame.link.name} offset {link_vis.offset}") 48 | link_obj = sdf.MeshObjectFactory(link_vis.geom_param[0], 49 | scale=link_vis.geom_param[1], 50 | path_prefix=path_prefix) 51 | link_sdf = link_sdf_cls(link_obj) 52 | self.sdf_to_link_name.append(frame.link.name) 53 | sdfs.append(link_sdf) 54 | offsets.append(link_vis.offset) 55 | else: 56 | logger.warning(f"Cannot handle non-mesh link visual type {link_vis} for {frame.link.name}") 57 | 58 | self.offset_transforms = offsets[0].stack(*offsets[1:]).to(device=self.device, dtype=self.dtype) 59 | self.sdf = sdf.ComposedSDF(sdfs, self.object_to_link_frames) 60 | self.set_joint_configuration(default_joint_config) 61 | 62 | def surface_bounding_box(self, **kwargs): 63 | return self.sdf.surface_bounding_box(**kwargs) 64 | 65 | def link_bounding_boxes(self): 66 | """ 67 | Get the bounding box of each link in the robot's frame under the current configuration. 68 | Note that the bounding box is not necessarily axis-aligned, so the returned bounding box is not just 69 | the min and max of the points. 70 | :return: [A x] [B x] 8 x 3 points of the bounding box for each link in the robot's frame 71 | """ 72 | tfs = self.sdf.obj_frame_to_link_frame.inverse() 73 | bbs = [] 74 | for i in range(len(self.sdf.sdfs)): 75 | sdf = self.sdf.sdfs[i] 76 | bb = aabb_to_ordered_end_points(sdf.surface_bounding_box(padding=0)) 77 | bb = tfs.transform_points(torch.tensor(bb, device=tfs.device, dtype=tfs.dtype))[ 78 | self.sdf.ith_transform_slice(i)] 79 | bbs.append(bb) 80 | return torch.stack(bbs).squeeze() 81 | 82 | def set_joint_configuration(self, joint_config=None): 83 | """ 84 | Set the joint configuration of the robot 85 | :param joint_config: [A x] M optionally arbitrarily batched joint configurations. There are M joints; A can be 86 | any number of batched dimensions. 87 | :return: 88 | """ 89 | M = len(self.joint_names) 90 | if joint_config is None: 91 | joint_config = torch.zeros(M, device=self.device, dtype=self.dtype) 92 | # Transform3D only works with 1 batch dimension, so we need to manually flatten any additional ones 93 | # save the batch dimensions for when retrieving points 94 | if len(joint_config.shape) > 1: 95 | self.configuration_batch = joint_config.shape[:-1] 96 | joint_config = joint_config.reshape(-1, M) 97 | else: 98 | self.configuration_batch = None 99 | tf = self.chain.forward_kinematics(joint_config, end_only=False) 100 | tsfs = [] 101 | for link_name in self.sdf_to_link_name: 102 | tsfs.append(tf[link_name].get_matrix()) 103 | # make offset transforms have compatible batch dimensions 104 | offset_tsf = self.offset_transforms.inverse() 105 | if self.configuration_batch is not None: 106 | # must be of shape (num_links, *self.configuration_batch, 4, 4) before flattening 107 | expand_dims = (None,) * len(self.configuration_batch) 108 | offset_tsf_mat = offset_tsf.get_matrix()[(slice(None),) + expand_dims] 109 | offset_tsf_mat = offset_tsf_mat.repeat(1, *self.configuration_batch, 1, 1) 110 | offset_tsf = pk.Transform3d(matrix=offset_tsf_mat.reshape(-1, 4, 4)) 111 | 112 | tsfs = torch.cat(tsfs) 113 | self.object_to_link_frames = offset_tsf.compose(pk.Transform3d(matrix=tsfs).inverse()) 114 | if self.sdf is not None: 115 | self.sdf.set_transforms(self.object_to_link_frames, batch_dim=self.configuration_batch) 116 | 117 | def __call__(self, points_in_object_frame): 118 | """ 119 | Query for SDF value and SDF gradients for points in the robot's frame 120 | :param points_in_object_frame: [B x] N x 3 optionally arbitrarily batched points in the robot frame; B can be 121 | any number of batch dimensions. 122 | :return: [A x] [B x] N SDF value, and [A x] [B x] N x 3 SDF gradient. A are the configurations' arbitrary 123 | number of batch dimensions. 124 | """ 125 | return self.sdf(points_in_object_frame) 126 | 127 | 128 | def cache_link_sdf_factory(resolution=0.01, padding=0.1, **kwargs): 129 | def create_sdf(obj_factory: sdf.ObjectFactory): 130 | gt_sdf = sdf.MeshSDF(obj_factory) 131 | return sdf.CachedSDF(obj_factory.name, resolution, obj_factory.bounding_box(padding=padding), gt_sdf, **kwargs) 132 | 133 | return create_sdf 134 | 135 | 136 | def aabb_to_ordered_end_points(aabb, arrange_in_sequential_order=False): 137 | aabbMin = aabb[:, 0] 138 | aabbMax = aabb[:, 1] 139 | if arrange_in_sequential_order: 140 | arr = [ 141 | [aabbMin[0], aabbMin[1], aabbMin[2]], 142 | [aabbMax[0], aabbMin[1], aabbMin[2]], 143 | [aabbMax[0], aabbMax[1], aabbMin[2]], 144 | [aabbMin[0], aabbMax[1], aabbMin[2]], 145 | [aabbMin[0], aabbMin[1], aabbMin[2]], 146 | [aabbMin[0], aabbMin[1], aabbMax[2]], 147 | [aabbMax[0], aabbMin[1], aabbMax[2]], 148 | [aabbMax[0], aabbMin[1], aabbMin[2]], 149 | [aabbMax[0], aabbMin[1], aabbMax[2]], 150 | [aabbMax[0], aabbMax[1], aabbMax[2]], 151 | [aabbMax[0], aabbMax[1], aabbMin[2]], 152 | [aabbMax[0], aabbMax[1], aabbMax[2]], 153 | [aabbMin[0], aabbMax[1], aabbMax[2]], 154 | [aabbMin[0], aabbMax[1], aabbMin[2]], 155 | [aabbMin[0], aabbMax[1], aabbMax[2]], 156 | [aabbMin[0], aabbMin[1], aabbMax[2]], 157 | ] 158 | else: 159 | arr = [ 160 | [aabbMin[0], aabbMin[1], aabbMin[2]], 161 | [aabbMax[0], aabbMin[1], aabbMin[2]], 162 | [aabbMin[0], aabbMax[1], aabbMin[2]], 163 | [aabbMin[0], aabbMin[1], aabbMax[2]], 164 | [aabbMin[0], aabbMax[1], aabbMax[2]], 165 | [aabbMax[0], aabbMin[1], aabbMax[2]], 166 | [aabbMax[0], aabbMax[1], aabbMin[2]], 167 | [aabbMax[0], aabbMax[1], aabbMax[2]] 168 | ] 169 | if torch.is_tensor(aabb): 170 | return torch.tensor(arr, device=aabb.device, dtype=aabb.dtype) 171 | return np.array(arr) 172 | -------------------------------------------------------------------------------- /src/pytorch_volumetric/sdf.py: -------------------------------------------------------------------------------- 1 | import abc 2 | import enum 3 | import math 4 | import os 5 | import typing 6 | from typing import Any, NamedTuple, Union 7 | 8 | import numpy as np 9 | import open3d as o3d 10 | 11 | import torch 12 | from torch.autograd import Function 13 | from arm_pytorch_utilities import tensor_utils, rand 14 | from multidim_indexing import torch_view 15 | from functools import partial 16 | 17 | from pytorch_volumetric.voxel import VoxelGrid, get_divisible_range_by_resolution, get_coordinates_and_points_in_grid 18 | import pytorch_kinematics as pk 19 | import logging 20 | 21 | logger = logging.getLogger(__name__) 22 | 23 | 24 | class SDFQuery(NamedTuple): 25 | closest: torch.Tensor 26 | distance: torch.Tensor 27 | gradient: torch.Tensor 28 | normal: Union[torch.Tensor, None] 29 | 30 | 31 | class ObjectFactory(abc.ABC): 32 | def __init__(self, name='', scale=1.0, vis_frame_pos=(0, 0, 0), vis_frame_rot=(0, 0, 0, 1), 33 | plausible_suboptimality=0.001, mesh=None, **kwargs): 34 | """ 35 | :param name: path to the mesh obj if loading from file 36 | :param scale: scaling factor for the mesh 37 | :param vis_frame_pos: position of the mesh in the object frame 38 | :param vis_frame_rot: quaternion rotation of the mesh in the object frame 39 | :param plausible_suboptimality: how much error to tolerate in the SDF 40 | :param mesh: open3d mesh object; can be provided instead of the path to the mesh; however, 41 | giving this directly means scale, vis_frame_pos, and vis_frame_rot are ignored 42 | """ 43 | self.name = name 44 | self.scale = scale if scale is not None else 1.0 45 | # frame from model's base frame to the simulation's use of the model 46 | self.vis_frame_pos = vis_frame_pos 47 | self.vis_frame_rot = vis_frame_rot 48 | self.other_load_kwargs = kwargs 49 | self.plausible_suboptimality = plausible_suboptimality 50 | 51 | # use external mesh library to compute closest point for non-convex meshes 52 | self._mesh = mesh 53 | self._mesht = None 54 | self._raycasting_scene = None 55 | self._face_normals = None 56 | self.precompute_sdf() 57 | 58 | def __reduce__(self): 59 | return partial(self.__class__, scale=self.scale, vis_frame_pos=self.vis_frame_pos, 60 | vis_frame_rot=self.vis_frame_rot, 61 | plausible_suboptimality=self.plausible_suboptimality, **self.other_load_kwargs), \ 62 | (self.name,) 63 | 64 | @abc.abstractmethod 65 | def make_collision_obj(self, z, rgba=None): 66 | """Create collision object of fixed and position along x-y; returns the object ID and bounding box""" 67 | 68 | @abc.abstractmethod 69 | def get_mesh_resource_filename(self): 70 | """Return the path to the mesh resource file (.obj, .stl, ...)""" 71 | 72 | def get_mesh_high_poly_resource_filename(self): 73 | """Return the path to the high poly mesh resource file""" 74 | return self.get_mesh_resource_filename() 75 | 76 | def draw_mesh(self, dd, name, pose, rgba, object_id=None): 77 | frame_pos = np.array(self.vis_frame_pos) * self.scale 78 | return dd.draw_mesh(name, self.get_mesh_resource_filename(), pose, scale=self.scale, rgba=rgba, 79 | object_id=object_id, vis_frame_pos=frame_pos, vis_frame_rot=self.vis_frame_rot) 80 | 81 | def bounding_box(self, padding=0., padding_ratio=0): 82 | aabb = self._mesh.get_axis_aligned_bounding_box() 83 | world_min = aabb.get_min_bound() 84 | world_max = aabb.get_max_bound() 85 | # already scaled, but we add a little padding 86 | ranges = np.array(list(zip(world_min, world_max))) 87 | extents = ranges[:, 1] - ranges[:, 0] 88 | ranges[:, 0] -= padding + padding_ratio * extents 89 | ranges[:, 1] += padding + padding_ratio * extents 90 | return ranges 91 | 92 | def center(self): 93 | """Get center of mass assuming uniform density. Return is in object frame""" 94 | if self._mesh is None: 95 | self.precompute_sdf() 96 | return self._mesh.get_center() 97 | 98 | def precompute_sdf(self): 99 | if self._mesh is None: 100 | full_path = self.get_mesh_high_poly_resource_filename() 101 | full_path = os.path.expanduser(full_path) 102 | if not os.path.exists(full_path): 103 | raise RuntimeError(f"Expected mesh file does not exist: {full_path}") 104 | self._mesh = o3d.io.read_triangle_mesh(full_path) 105 | # scale mesh 106 | scale_transform = np.eye(4) 107 | np.fill_diagonal(scale_transform[:3, :3], self.scale) 108 | self._mesh.transform(scale_transform) 109 | 110 | # convert from mesh object frame to simulator object frame 111 | x, y, z, w = self.vis_frame_rot 112 | self._mesh = self._mesh.rotate(o3d.geometry.get_rotation_matrix_from_quaternion((w, x, y, z)), 113 | center=[0, 0, 0]) 114 | self._mesh = self._mesh.translate(np.array(self.vis_frame_pos) * self.scale) 115 | 116 | if self._mesht is None: 117 | self._mesht = o3d.t.geometry.TriangleMesh.from_legacy(self._mesh) 118 | self._raycasting_scene = o3d.t.geometry.RaycastingScene() 119 | _ = self._raycasting_scene.add_triangles(self._mesht) 120 | self._mesh.compute_triangle_normals() 121 | self._face_normals = np.asarray(self._mesh.triangle_normals) 122 | 123 | @tensor_utils.handle_batch_input(n=2) 124 | def _do_object_frame_closest_point(self, points_in_object_frame, compute_normal=False): 125 | 126 | if torch.is_tensor(points_in_object_frame): 127 | dtype = points_in_object_frame.dtype 128 | device = points_in_object_frame.device 129 | points_in_object_frame = points_in_object_frame.detach().cpu().numpy() 130 | else: 131 | dtype = torch.float 132 | device = "cpu" 133 | points_in_object_frame = points_in_object_frame.astype(np.float32) 134 | 135 | closest = self._raycasting_scene.compute_closest_points(points_in_object_frame) 136 | closest_points = closest['points'] 137 | face_ids = closest['primitive_ids'] 138 | pts = closest_points.numpy() 139 | # negative SDF gradient outside the object and positive SDF gradient inside the object 140 | gradient = pts - points_in_object_frame 141 | 142 | distance = np.linalg.norm(gradient, axis=-1) 143 | # normalize gradients 144 | has_direction = distance > 0 145 | gradient[has_direction] = gradient[has_direction] / distance[has_direction, None] 146 | 147 | # ensure ray destination is outside the object 148 | ray_destination = np.repeat(self.bounding_box(padding=1.0)[None, :, 1], points_in_object_frame.shape[0], axis=0) 149 | # add noise to ray destination, this helps reduce artifacts in the sdf 150 | ray_destination = ray_destination + 1e-4 * np.random.randn(*points_in_object_frame.shape) 151 | ray_destination = ray_destination.astype(np.float32) 152 | # check if point is inside the object 153 | rays = np.concatenate([points_in_object_frame, ray_destination], axis=-1) 154 | intersection_counts = self._raycasting_scene.count_intersections(rays).numpy() 155 | is_inside = intersection_counts % 2 == 1 156 | distance[is_inside] = distance[is_inside] * -1 157 | # fix gradient direction to point away from surface outside 158 | gradient[~is_inside] = gradient[~is_inside] * -1 159 | 160 | # for any points very close to the surface, it is better to use the surface normal as the gradient 161 | # this is because the closest point on the surface may be noisy when close by 162 | # e.g. if you are actually on the surface, the closest surface point is itself so you get no gradient info 163 | on_surface = np.abs(distance) < 1e-3 164 | surface_normals = self._face_normals[face_ids.numpy()[on_surface]] 165 | gradient[on_surface] = surface_normals 166 | 167 | pts, distance, gradient = tensor_utils.ensure_tensor(device, dtype, pts, distance, gradient) 168 | 169 | normals = None 170 | if compute_normal: 171 | normals = self._face_normals[face_ids.numpy()] 172 | normals = torch.tensor(normals, device=device, dtype=dtype) 173 | return pts, distance, gradient, normals 174 | 175 | def object_frame_closest_point(self, points_in_object_frame, compute_normal=False) -> SDFQuery: 176 | """ 177 | Assumes the input is in the simulator object frame and will return outputs 178 | also in the simulator object frame. Note that the simulator object frame and the mesh object frame may be 179 | different 180 | 181 | :param points_in_object_frame: N x 3 points in the object frame 182 | (can have arbitrary batch dimensions in front of N) 183 | :param compute_normal: bool: whether to compute surface normal at the closest point or not 184 | :return: dict(closest: N x 3, distance: N, gradient: N x 3, normal: N x 3) 185 | the closest points on the surface, their corresponding signed distance to the query point, the negative SDF 186 | gradient at the query point if the query point is outside, otherwise it's the positive SDF gradient 187 | (points from the query point to the closest point), and the surface normal at the closest point 188 | """ 189 | 190 | return SDFQuery(*self._do_object_frame_closest_point(points_in_object_frame, compute_normal=compute_normal)) 191 | 192 | 193 | class MeshObjectFactory(ObjectFactory): 194 | def __init__(self, mesh_name='', path_prefix='', **kwargs): 195 | self.path_prefix = path_prefix 196 | # whether to strip the package:// prefix from the mesh name, for example if we are loading a mesh manually 197 | # with a path prefix 198 | self.strip_package_prefix = path_prefix != '' 199 | # specify ranges=None to infer the range from the object's bounding box 200 | super(MeshObjectFactory, self).__init__(mesh_name, **kwargs) 201 | 202 | def __reduce__(self): 203 | return partial(self.__class__, path_prefix=self.path_prefix, scale=self.scale, vis_frame_pos=self.vis_frame_pos, 204 | vis_frame_rot=self.vis_frame_rot, 205 | plausible_suboptimality=self.plausible_suboptimality, **self.other_load_kwargs), \ 206 | (self.name,) 207 | 208 | def make_collision_obj(self, z, rgba=None): 209 | return None, None 210 | 211 | def get_mesh_resource_filename(self): 212 | mesh_path = self.name 213 | if self.strip_package_prefix: 214 | mesh_path = mesh_path.replace("package://", "") 215 | return os.path.join(self.path_prefix, mesh_path) 216 | 217 | 218 | class ObjectFrameSDF(Function): 219 | 220 | @abc.abstractmethod 221 | def __call__(self, points_in_object_frame): 222 | """ 223 | Evaluate the signed distance function at given points in the object frame 224 | :param points_in_object_frame: B x N x d d-dimensional points (2 or 3) of B batches; located in object frame 225 | :return: tuple of B x N signed distance from closest object surface in m and B x N x d SDF gradient pointing 226 | towards higher SDF values (away from surface when outside the object and towards the surface when inside) 227 | """ 228 | 229 | @staticmethod 230 | def backward(ctx: Any, *grad_outputs: Any) -> Any: 231 | # only have d(sdf_vals)/d(points_in_object_frame) = sdf_grad, others should be None 232 | sdf_grad, = ctx.saved_tensors 233 | dsdf_vals_dpoints_in_object_frame = grad_outputs[0].unsqueeze(-1) * sdf_grad 234 | outputs = [None for _ in range(ctx.num_inputs)] 235 | outputs[0] = dsdf_vals_dpoints_in_object_frame 236 | return tuple(outputs) 237 | 238 | @abc.abstractmethod 239 | def surface_bounding_box(self, padding=0., padding_ratio=0.): 240 | """ 241 | Get the bounding box for the 0-level set in the form of a sequence of (min,max) coordinates 242 | :param padding: amount to inflate the min and max from the actual bounding box 243 | :param padding_ratio: ratio of the extent of that dimension to use for padding; added on top of absolute padding 244 | :return: (min,max) for each dimension 245 | """ 246 | 247 | def outside_surface(self, points_in_object_frame, surface_level=0): 248 | """ 249 | Check if query points are outside the surface level set; separate from querying the values since some 250 | implementations may have a more efficient way of computing this 251 | :param points_in_object_frame: 252 | :param surface_level: The level set value for separating points 253 | :return: B x N bool 254 | """ 255 | sdf_values, _ = self.__call__(points_in_object_frame) 256 | outside = sdf_values > surface_level 257 | return outside 258 | 259 | def get_voxel_view(self, voxels: VoxelGrid = None, dtype=torch.float, device='cpu') -> torch_view.TorchMultidimView: 260 | """ 261 | Get a voxel view of a part of the SDF 262 | :param voxels: the voxel over which to evaluate the SDF; if left as none, take the default range which is 263 | implementation dependent 264 | :param dtype: torch type of the default voxel grid (can be safely omitted if voxels is supplied) 265 | :param device: torch device of the default voxel grid (can be safely omitted if voxels is supplied) 266 | :return: 267 | """ 268 | if voxels is None: 269 | voxels = VoxelGrid(0.01, self.surface_bounding_box(padding=0.1).cpu().numpy(), dtype=dtype, device=device) 270 | 271 | pts = voxels.get_voxel_center_points() 272 | sdf_val, sdf_grad = self.__call__(pts.unsqueeze(0)) 273 | cached_underlying_sdf = sdf_val.reshape([len(coord) for coord in voxels.coords]) 274 | 275 | return torch_view.TorchMultidimView(cached_underlying_sdf, voxels.range_per_dim, invalid_value=self.__call__) 276 | 277 | def get_filtered_points(self, unary_filter, voxels: VoxelGrid = None, dtype=torch.float, 278 | device='cpu') -> torch.tensor: 279 | """ 280 | Get a N x d sequence of points extracted from a voxel grid such that their SDF values satisfy a given 281 | unary filter (on their SDF value) 282 | :param unary_filter: filter on the SDF value of each point, evaluating to true results in accepting that point 283 | :param voxels: voxel grid over which to evaluate each point (there can be infinitely many points satisfying 284 | the unary filter and we need to restrict our search over a grid of points [center of the voxels]) 285 | :param dtype: torch type of the default voxel grid (can be safely omitted if voxels is supplied) 286 | :param device: torch device of the default voxel grid (can be safely omitted if voxels is supplied) 287 | :return: 288 | """ 289 | model_voxels = self.get_voxel_view(voxels, dtype=dtype, device=device) 290 | interior = unary_filter(model_voxels.raw_data) 291 | indices = interior.nonzero() 292 | # these points are in object frame 293 | return model_voxels.ensure_value_key(indices) 294 | 295 | 296 | class SphereSDF(ObjectFrameSDF): 297 | """SDF for a geometric primitive, the sphere centered at the origin""" 298 | 299 | def __init__(self, radius): 300 | self.radius = radius 301 | 302 | def __call__(self, points_in_object_frame): 303 | return self.apply(points_in_object_frame, self.radius) 304 | 305 | @staticmethod 306 | def forward(ctx, points_in_object_frame, radius): 307 | dist_to_origin = torch.linalg.norm(points_in_object_frame, dim=-1) 308 | dist = dist_to_origin - radius 309 | grad = points_in_object_frame / (dist_to_origin.unsqueeze(-1) + 1e-12) 310 | 311 | ctx.save_for_backward(grad) 312 | ctx.num_inputs = 2 313 | return dist, grad 314 | 315 | def surface_bounding_box(self, padding=0., padding_ratio=0.): 316 | length = self.radius + padding + padding_ratio * self.radius 317 | return torch.tensor([[-length, length], [-length, length], [-length, length]]) 318 | 319 | 320 | class MeshSDF(ObjectFrameSDF): 321 | """SDF generated from direct ray-tracing calls to the mesh. This is relatively expensive.""" 322 | 323 | def __init__(self, obj_factory: ObjectFactory, vis=None): 324 | self.obj_factory = obj_factory 325 | self.vis = vis 326 | 327 | def surface_bounding_box(self, **kwargs): 328 | return torch.tensor(self.obj_factory.bounding_box(**kwargs)) 329 | 330 | def __call__(self, points_in_object_frame): 331 | return self.apply(points_in_object_frame, self.obj_factory, self.vis) 332 | 333 | @staticmethod 334 | def forward(ctx, points_in_object_frame, obj_factory, vis=None): 335 | N, d = points_in_object_frame.shape[-2:] 336 | 337 | # compute SDF value for new sampled points 338 | res = obj_factory.object_frame_closest_point(points_in_object_frame) 339 | ctx.save_for_backward(res.gradient) 340 | ctx.num_inputs = 3 341 | 342 | # points are transformed to link frame, thus it needs to compare against the object in link frame 343 | # objId is not in link frame and shouldn't be moved 344 | if vis is not None: 345 | for i in range(N): 346 | vis.draw_point("test_point", points_in_object_frame[..., i, :], color=(1, 0, 0), length=0.005) 347 | vis.draw_2d_line(f"test_grad", points_in_object_frame[..., i, :], 348 | res.gradient[..., i, :].detach().cpu(), color=(0, 0, 0), 349 | size=2., scale=0.03) 350 | vis.draw_point("test_point_surf", res.closest[..., i, :].detach().cpu(), color=(0, 1, 0), 351 | length=0.005, 352 | label=f'{res.distance[..., i].item():.5f}') 353 | return res.distance, res.gradient 354 | 355 | 356 | class ComposedSDF(ObjectFrameSDF): 357 | def __init__(self, sdfs: typing.Sequence[ObjectFrameSDF], obj_frame_to_each_frame: pk.Transform3d = None): 358 | """ 359 | 360 | :param sdfs: S Object frame SDFs 361 | :param obj_frame_to_each_frame: [B*]S x 4 x 4 transforms from the shared object frame to the frame of each SDF 362 | These transforms are potentially arbitrarily batched B. Since Transform3D can only have one batch dimension, 363 | they are flattened 364 | """ 365 | self.sdfs = sdfs 366 | self.obj_frame_to_link_frame: typing.Optional[pk.Transform3d] = None 367 | self.link_frame_to_obj_frame: typing.Optional[typing.Sequence[pk.Transform3d]] = None 368 | self.tsf_batch = None 369 | self.set_transforms(obj_frame_to_each_frame) 370 | 371 | def surface_bounding_box(self, **kwargs): 372 | bounds = [] 373 | tsf = self.obj_frame_to_link_frame.inverse() 374 | for i, sdf in enumerate(self.sdfs): 375 | pts = sdf.surface_bounding_box(**kwargs) 376 | pts = tsf[self.ith_transform_slice(i)].transform_points( 377 | pts.to(dtype=tsf.dtype, device=tsf.device).transpose(0, 1)) 378 | # edge case where the batch is a single element 379 | if self.tsf_batch is not None and len(pts.shape) == 2: 380 | pts = pts.unsqueeze(0) 381 | bounds.append(pts) 382 | bounds = torch.stack(bounds) 383 | 384 | # min over everything except the batch dimensions and the last dimension 385 | if self.tsf_batch is not None: 386 | # ignore the batch dimension 387 | dims = (0,) + tuple(range(2, len(bounds.shape) - 1)) 388 | else: 389 | dims = tuple(range(len(bounds.shape) - 1)) 390 | mins = bounds.amin(dim=dims) 391 | maxs = bounds.amax(dim=dims) 392 | return torch.stack((mins, maxs), dim=-1) 393 | 394 | def set_transforms(self, tsf: pk.Transform3d, batch_dim=None): 395 | self.obj_frame_to_link_frame = tsf 396 | self.link_frame_to_obj_frame = [] 397 | self.tsf_batch = batch_dim 398 | # assume a single batch dimension when not given B x N x 4 x 4 399 | if tsf is not None: 400 | S = len(self.sdfs) 401 | S_tsf = len(self.obj_frame_to_link_frame) 402 | if self.tsf_batch is None and (S_tsf != S): 403 | self.tsf_batch = (S_tsf / S,) 404 | m = tsf.get_matrix().inverse() 405 | for i in range(S): 406 | self.link_frame_to_obj_frame.append( 407 | pk.Transform3d(matrix=m[self.ith_transform_slice(i)])) 408 | 409 | def ith_transform_slice(self, i): 410 | if self.tsf_batch is None: 411 | return slice(i, i + 1) 412 | else: 413 | total_to_slice = math.prod(list(self.tsf_batch)) 414 | return slice(i * total_to_slice, (i + 1) * total_to_slice) 415 | 416 | def __call__(self, points_in_object_frame): 417 | return self.apply(points_in_object_frame, self.sdfs, self.obj_frame_to_link_frame, self.tsf_batch, 418 | self.link_frame_to_obj_frame) 419 | 420 | @staticmethod 421 | def forward(ctx, points_in_object_frame, sdfs, obj_frame_to_link_frame, tsf_batch, link_frame_to_obj_frame): 422 | pts_shape = points_in_object_frame.shape 423 | # flatten it for the transform 424 | points_in_object_frame = points_in_object_frame.view(-1, 3) 425 | flat_shape = points_in_object_frame.shape 426 | S = len(sdfs) 427 | # pts[i] are now points in the ith SDF's frame 428 | pts = obj_frame_to_link_frame.transform_points(points_in_object_frame) 429 | # S x B x N x 3 430 | if tsf_batch is not None: 431 | pts = pts.reshape(S, *tsf_batch, *flat_shape) 432 | sdfv = [] 433 | sdfg = [] 434 | for i, sdf in enumerate(sdfs): 435 | # B x N for v and B x N x 3 for g 436 | v, g = sdf(pts[i]) 437 | # need to transform the gradient back to the object frame 438 | g = link_frame_to_obj_frame[i].transform_normals(g) 439 | sdfv.append(v) 440 | sdfg.append(g) 441 | 442 | # attempt at doing things in higher dimensions 443 | sdfv = torch.cat(sdfv) 444 | sdfg = torch.cat(sdfg) 445 | 446 | # easier solution for flattening 447 | v = sdfv.reshape(S, -1) 448 | g = sdfg.reshape(S, -1, 3) 449 | # ensure S is the first dimension and take min across S (the different links) 450 | closest = torch.argmin(v, 0) 451 | 452 | all = torch.arange(0, v.shape[1]) 453 | # B*N for vv and B*N x 3 for gg 454 | vv = v[closest, all] 455 | gg = g[closest, all] 456 | 457 | if tsf_batch is not None: 458 | # retrieve the original query points batch dimensions - note that they are after configuration batch 459 | vv = vv.reshape(*tsf_batch, *pts_shape[:-1]) 460 | gg = gg.reshape(*tsf_batch, *pts_shape[:-1], 3) 461 | 462 | ctx.save_for_backward(gg) 463 | ctx.num_inputs = 5 464 | return vv, gg 465 | 466 | 467 | class OutOfBoundsStrategy(enum.Enum): 468 | LOOKUP_GT_SDF = 0 469 | BOUNDING_BOX = 1 # will also always under-approximate the SDF value, but more accurate than sphere approximation 470 | 471 | 472 | class CachedSDF(ObjectFrameSDF): 473 | """SDF via looking up precomputed voxel grids requiring a ground truth SDF to default to on uncached queries.""" 474 | 475 | def __init__(self, object_name, resolution, range_per_dim, gt_sdf: ObjectFrameSDF, 476 | out_of_bounds_strategy=OutOfBoundsStrategy.BOUNDING_BOX, 477 | device="cpu", clean_cache=False, 478 | debug_check_sdf=False, cache_path="sdf_cache.pkl"): 479 | """ 480 | 481 | :param object_name: str readable name of the object; combined with the resolution and range for cache 482 | :param resolution: side length of each voxel cell 483 | :param range_per_dim: (min, max) sequence for each dimension (e.g. 3 for 3D) 484 | :param gt_sdf: ground truth SDF used to generate the cache and default to on queries outside of the cache 485 | :param out_of_bounds_strategy: what to do when a query is outside the cached range. 486 | LOOKUP_GT_SDF: use the ground truth SDF for the value and gradient (relatively expensive) 487 | BOUNDING_BOX: use the distance to the bounding box (under-approximates the SDF value) 488 | :param device: pytorch compatible device 489 | :param clean_cache: whether to ignore the existing cache and force recomputation 490 | :param debug_check_sdf: check that the generated SDF matches the ground truth SDF 491 | :param cache_path: path where to store the SDF cache for efficient loading 492 | """ 493 | self.device = device 494 | # cache for signed distance field to object 495 | self.voxels = None 496 | # voxel grid can't handle vector values yet 497 | self.voxels_grad = None 498 | self.out_of_bounds_strategy = out_of_bounds_strategy 499 | 500 | cached_underlying_sdf = None 501 | cached_underlying_sdf_grad = None 502 | 503 | self.gt_sdf = gt_sdf 504 | self.resolution = resolution 505 | 506 | bb = np.array(range_per_dim) 507 | r = bb[:, 1] - bb[:, 0] 508 | num_voxel = r // resolution 509 | if min(num_voxel) < 10: 510 | logger.warning(f"Resolution {resolution} is too high for {object_name}, only getting {num_voxel} voxels.") 511 | 512 | range_per_dim = get_divisible_range_by_resolution(resolution, range_per_dim) 513 | self.ranges = range_per_dim 514 | 515 | self.name = f"{object_name} {resolution} {tuple(range_per_dim)}" 516 | self.debug_check_sdf = debug_check_sdf 517 | 518 | if os.path.exists(cache_path): 519 | data = torch.load(cache_path) or {} 520 | try: 521 | cached_underlying_sdf, cached_underlying_sdf_grad = data[self.name] 522 | logger.info("cached sdf for %s loaded from %s", self.name, cache_path) 523 | except (ValueError, KeyError): 524 | logger.info("cached sdf invalid %s from %s, recreating", self.name, cache_path) 525 | else: 526 | data = {} 527 | 528 | # if we didn't load anything, then we need to create the cache and save to it 529 | if cached_underlying_sdf is None or clean_cache: 530 | if gt_sdf is None: 531 | raise RuntimeError("Cached SDF did not find the cache and requires an initialize queryable SDF") 532 | 533 | coords, pts = get_coordinates_and_points_in_grid(self.resolution, self.ranges) 534 | sdf_val, sdf_grad = gt_sdf(pts) 535 | cached_underlying_sdf = sdf_val.reshape([len(coord) for coord in coords]) 536 | cached_underlying_sdf_grad = sdf_grad.squeeze(0) 537 | # cached_underlying_sdf_grad = sdf_grad.reshape(cached_underlying_sdf.shape + (3,)) 538 | # confirm the values work 539 | if self.debug_check_sdf: 540 | debug_view = torch_view.TorchMultidimView(cached_underlying_sdf, self.ranges, 541 | invalid_value=self._fallback_sdf_value_func) 542 | query = debug_view[pts] 543 | assert torch.allclose(sdf_val, query) 544 | 545 | data[self.name] = cached_underlying_sdf, cached_underlying_sdf_grad 546 | 547 | torch.save(data, cache_path) 548 | logger.info("caching sdf for %s to %s", self.name, cache_path) 549 | 550 | cached_underlying_sdf = cached_underlying_sdf.to(device=device) 551 | cached_underlying_sdf_grad = cached_underlying_sdf_grad.to(device=device) 552 | self.voxels = torch_view.TorchMultidimView(cached_underlying_sdf, range_per_dim, 553 | invalid_value=self._fallback_sdf_value_func) 554 | self.voxels_grad = cached_underlying_sdf_grad.squeeze() 555 | 556 | self.bb = self.surface_bounding_box().to(device=device) 557 | 558 | def surface_bounding_box(self, **kwargs): 559 | return self.gt_sdf.surface_bounding_box(**kwargs) 560 | 561 | def _fallback_sdf_value_func(self, *args, **kwargs): 562 | sdf_val, _ = self.gt_sdf(*args, **kwargs) 563 | sdf_val = sdf_val.to(device=self.device) 564 | return sdf_val 565 | 566 | def __call__(self, points_in_object_frame): 567 | return self.apply(points_in_object_frame, self.voxels, self.voxels_grad, self.bb, self.out_of_bounds_strategy, 568 | self.device, self.gt_sdf) 569 | 570 | @staticmethod 571 | def forward(ctx, points_in_object_frame, voxels, voxels_grad, bb, out_of_bounds_strategy, device, gt_sdf): 572 | # check when points are out of cached range and use ground truth sdf for both value and grad 573 | keys = voxels.ensure_index_key(points_in_object_frame) 574 | keys_ravelled = voxels.ravel_multi_index(keys, voxels.shape) 575 | 576 | inbound_keys = voxels.get_valid_values(points_in_object_frame) 577 | out_of_bound_keys = ~inbound_keys 578 | 579 | # logger.info(f"out of bound keys: {out_of_bound_keys.sum()}/{out_of_bound_keys.numel()}") 580 | 581 | dtype = points_in_object_frame.dtype 582 | val = torch.zeros(keys_ravelled.shape, device=device, dtype=dtype) 583 | grad = torch.zeros(keys.shape, device=device, dtype=dtype) 584 | 585 | val[inbound_keys] = voxels.raw_data[keys_ravelled[inbound_keys]] 586 | grad[inbound_keys] = voxels_grad[keys_ravelled[inbound_keys]] 587 | 588 | points_oob = points_in_object_frame[out_of_bound_keys] 589 | if out_of_bounds_strategy == OutOfBoundsStrategy.LOOKUP_GT_SDF: 590 | val[out_of_bound_keys], grad[out_of_bound_keys] = gt_sdf(points_oob) 591 | elif out_of_bounds_strategy == OutOfBoundsStrategy.BOUNDING_BOX: 592 | if bb.dtype != dtype: 593 | bb = bb.to(dtype=dtype) 594 | # distance to bounding box 595 | dmin = bb[:, 0] - points_oob 596 | dmin_active = dmin > 0 597 | dmin[~dmin_active] = 0 598 | dmax = points_oob - bb[:, 1] 599 | dmax_active = dmax > 0 600 | dmax[~dmax_active] = 0 601 | dtotal = dmin + dmax 602 | # convert to gradient; for dmin, the dtotal component should be negative; for dmax, positive 603 | dtotal[dmin_active] = -dtotal[dmin_active] 604 | dist = dtotal.norm(dim=-1) 605 | # normalize gradient 606 | grad[out_of_bound_keys] = dtotal / dist.unsqueeze(-1) 607 | val[out_of_bound_keys] = dist 608 | 609 | # # comparison with ground truth 610 | # if self.debug_check_sdf: 611 | # val_gt, grad_gt = self.gt_sdf(points_oob) 612 | # diff = val_gt - val[out_of_bound_keys] 613 | # # always under-approximate the SDF value 614 | # assert torch.all(diff > 0) 615 | # # cosine similarity to compare the gradient vectors 616 | # diff_grad = torch.cosine_similarity(grad_gt, grad[out_of_bound_keys], dim=-1) 617 | # assert torch.all(diff_grad > 0.7) 618 | # assert diff_grad.mean() > 0.95 619 | 620 | # if self.debug_check_sdf: 621 | # val_gt = self._fallback_sdf_value_func(points_in_object_frame) 622 | # # the ones that are valid should be close enough to the ground truth 623 | # diff = torch.abs(val - val_gt) 624 | # close_enough = diff < self.resolution 625 | # within_bounds = self.voxels.get_valid_values(points_in_object_frame) 626 | # assert torch.all(close_enough[within_bounds]) 627 | ctx.save_for_backward(grad) 628 | ctx.num_inputs = 7 629 | return val, grad 630 | 631 | def outside_surface(self, points_in_object_frame, surface_level=0): 632 | keys = self.voxels.ensure_index_key(points_in_object_frame) 633 | keys_ravelled = self.voxels.ravel_multi_index(keys, self.voxels.shape) 634 | 635 | inbound_keys = self.voxels.get_valid_values(points_in_object_frame) 636 | 637 | # assume out of bound keys are outside 638 | outside = torch.ones(keys_ravelled.shape, device=self.device, dtype=torch.bool) 639 | outside[inbound_keys] = self.voxels.raw_data[keys_ravelled[inbound_keys]] > surface_level 640 | return outside 641 | 642 | def get_voxel_view(self, voxels: VoxelGrid = None, dtype=torch.float, device='cpu') -> torch_view.TorchMultidimView: 643 | if voxels is None: 644 | return self.voxels 645 | 646 | pts = voxels.get_voxel_center_points() 647 | sdf_val, sdf_grad = self.gt_sdf(pts.unsqueeze(0)) 648 | sdf_val = sdf_val.to(device=self.device) 649 | cached_underlying_sdf = sdf_val.reshape([len(coord) for coord in voxels.coords]) 650 | 651 | return torch_view.TorchMultidimView(cached_underlying_sdf, voxels.range_per_dim, 652 | invalid_value=self._fallback_sdf_value_func) 653 | 654 | 655 | def sample_mesh_points(obj_factory: ObjectFactory = None, num_points=100, seed=0, name="", 656 | clean_cache=False, dtype=torch.float, min_init_sample_points=200, 657 | dbpath='model_points_cache.pkl', device="cpu", cache=None): 658 | given_cache = cache is not None 659 | if cache is not None or os.path.exists(dbpath): 660 | if cache is None: 661 | cache = torch.load(dbpath) 662 | 663 | if name not in cache: 664 | cache[name] = {} 665 | if seed not in cache[name]: 666 | cache[name][seed] = {} 667 | if not clean_cache and num_points in cache[name][seed]: 668 | res = cache[name][seed][num_points] 669 | res = list(v.to(device=device, dtype=dtype) if v is not None else None for v in res) 670 | return *res[:-1], cache 671 | else: 672 | cache = {name: {seed: {}}} 673 | 674 | if obj_factory is None: 675 | raise RuntimeError(f"Expect model points to be cached for {name} {seed} {num_points} in {dbpath}") 676 | 677 | if obj_factory._mesh is None: 678 | obj_factory.precompute_sdf() 679 | 680 | mesh = obj_factory._mesh 681 | 682 | with rand.SavedRNG(): 683 | rand.seed(seed) 684 | o3d.utility.random.seed(seed) 685 | 686 | # because the point sampling is not dispersed, we do the dispersion ourselves 687 | # we accomplish this by sampling more points than we need then randomly selecting a subset 688 | sample_num_points = max(min_init_sample_points, 2 * num_points) 689 | 690 | # assume mesh is in object frame 691 | # pcd = mesh.sample_points_poisson_disk(number_of_points=num_points, init_factor=init_factor, seed=seed) 692 | pcd = mesh.sample_points_uniformly(number_of_points=sample_num_points) 693 | points = np.asarray(pcd.points) 694 | 695 | # subsample 696 | points = np.random.permutation(points)[:num_points] 697 | 698 | res = obj_factory.object_frame_closest_point(points, compute_normal=True) 699 | 700 | points = torch.tensor(points) 701 | normals = res.normal 702 | 703 | cache[name][seed][num_points] = points, normals.cpu(), None 704 | # otherwise assume will be saved by the user 705 | if not given_cache: 706 | torch.save(cache, dbpath) 707 | 708 | return points.to(device=device, dtype=dtype), normals.to(device=device, dtype=dtype), cache 709 | -------------------------------------------------------------------------------- /src/pytorch_volumetric/visualization.py: -------------------------------------------------------------------------------- 1 | import copy 2 | 3 | import torch 4 | 5 | from pytorch_volumetric import voxel 6 | from pytorch_volumetric import sdf 7 | from pytorch_volumetric import model_to_sdf 8 | from matplotlib import pyplot as plt 9 | import matplotlib.colors 10 | 11 | 12 | def fmt(x): 13 | s = f"{x:.1f}" 14 | if s.endswith("0"): 15 | s = f"{x:.0f}" 16 | if x == 0: 17 | return "surface" 18 | return rf"{s}" if plt.rcParams["text.usetex"] else f"{s}" 19 | 20 | 21 | def draw_sdf_slice(s: sdf.ObjectFrameSDF, query_range, resolution=0.01, interior_padding=0.2, 22 | cmap="Greys_r", device="cpu", plot_grad=False, do_plot=True): 23 | """ 24 | 25 | :param s: SDF to query on 26 | :param query_range: (min, max) for each dimension x,y,z. One dimension must have min=max to be sliced along, with 27 | the other dimensions shown. Note that this should be given in the SDF's frame. 28 | :param resolution: 29 | :param interior_padding: 30 | :param cmap: matplotlib compatible colormap 31 | :param device: pytorch device 32 | :param plot_grad: whether to plot the gradient field 33 | :return: 34 | """ 35 | coords, pts = voxel.get_coordinates_and_points_in_grid(resolution, query_range, device=device) 36 | # add a small amount of noise to avoid querying regular grid 37 | pts += torch.randn_like(pts) * 1e-6 38 | dim_labels = ['x', 'y', 'z'] 39 | slice_dim = None 40 | for i in range(len(dim_labels)): 41 | if len(coords[i]) == 1: 42 | slice_dim = i 43 | break 44 | 45 | # to properly draw a slice, the coords for that dimension must have only 1 element 46 | if slice_dim is None: 47 | raise RuntimeError(f"Sliced SDF requires a single query value for the sliced, but all query dimensions > 1") 48 | 49 | shown_dims = [i for i in range(3) if i != slice_dim] 50 | 51 | sdf_val, sdf_grad = s(pts) 52 | norm = matplotlib.colors.Normalize(vmin=sdf_val.min().cpu() - interior_padding, vmax=sdf_val.max().cpu()) 53 | 54 | x = coords[shown_dims[0]].cpu() 55 | z = coords[shown_dims[1]].cpu() 56 | v = sdf_val.reshape(len(x), len(z)).transpose(0, 1).cpu() 57 | ax = None 58 | cset1 = None 59 | cset2 = None 60 | if do_plot: 61 | ax = plt.gca() 62 | ax.set_xlabel(dim_labels[shown_dims[0]]) 63 | ax.set_ylabel(dim_labels[shown_dims[1]]) 64 | cset1 = ax.contourf(x, z, v, norm=norm, cmap=cmap) 65 | cset2 = ax.contour(x, z, v, colors='k', levels=[0], linestyles='dashed') 66 | if plot_grad: 67 | sdf_grad_uv = sdf_grad.reshape(len(x), len(z), 3).permute(1, 0, 2).cpu() 68 | # subsample arrows 69 | subsample_n = 5 70 | ax.quiver(x[::subsample_n], 71 | z[::subsample_n], 72 | sdf_grad_uv[::subsample_n, ::subsample_n, shown_dims[0]], 73 | sdf_grad_uv[::subsample_n, ::subsample_n, shown_dims[1]], color='g') 74 | ax.clabel(cset2, cset2.levels, inline=True, fontsize=13, fmt=fmt) 75 | plt.colorbar(cset1) 76 | # fig = plt.gcf() 77 | # fig.canvas.draw() 78 | plt.draw() 79 | plt.pause(0.005) 80 | return sdf_val, sdf_grad, pts, ax, cset1, cset2, v 81 | 82 | 83 | def get_transformed_meshes(robot_sdf: model_to_sdf.RobotSDF, obj_to_world_tsf=None): 84 | """Get the meshes of each link of the robot, transformed to the world frame. 85 | Each link is assumed to be a MeshSDF. 86 | You can use this like: 87 | 88 | import open3d as o3d 89 | meshes = get_transformed_meshes(robot_sdf) 90 | o3d.visualization.draw_geometries(meshes) 91 | """ 92 | 93 | meshes = [] 94 | # link to obj in the form of (object) H (link) 95 | tsfs = robot_sdf.sdf.obj_frame_to_link_frame.inverse() 96 | # given a transform in the form of (world) H (object) 97 | if obj_to_world_tsf is not None: 98 | # compose the transform to get (world) H (link) 99 | tsfs = obj_to_world_tsf.compose(tsfs) 100 | tsfs = tsfs.get_matrix() 101 | for i in range(len(robot_sdf.sdf_to_link_name)): 102 | # assuming they are individually MeshSDFs 103 | mesh = copy.deepcopy(robot_sdf.sdf.sdfs[i].obj_factory._mesh) 104 | mesh = mesh.transform(tsfs[i].cpu().numpy()) 105 | meshes.append(mesh) 106 | return meshes 107 | -------------------------------------------------------------------------------- /src/pytorch_volumetric/volume.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | 4 | def is_inside(points: torch.tensor, range_per_dim: torch.tensor): 5 | """Return whether the points are inside the range 6 | :param points: N x d geometric points; d is the number of dimensions 7 | :param range_per_dim: d x 2 range of values per dimension, each row is (min, max) 8 | :return: N boolean tensor 9 | """ 10 | return torch.all((range_per_dim[:, 0] <= points) & (points <= range_per_dim[:, 1]), dim=-1) 11 | -------------------------------------------------------------------------------- /src/pytorch_volumetric/voxel.py: -------------------------------------------------------------------------------- 1 | import abc 2 | import copy 3 | import math 4 | 5 | import numpy as np 6 | import torch 7 | from multidim_indexing import torch_view 8 | 9 | 10 | def get_divisible_range_by_resolution(resolution, range_per_dim): 11 | # ensure value range divides resolution evenly 12 | temp_range = [] 13 | for low, high in range_per_dim: 14 | span = high - low 15 | span = round(span / resolution) 16 | temp_range.append((low, low + span * resolution)) 17 | return temp_range 18 | 19 | 20 | def get_coordinates_and_points_in_grid(resolution, range_per_dim, dtype=torch.float, device='cpu', get_points=True): 21 | # create points along the value ranges 22 | coords = [torch.arange(low, high + 0.9 * resolution, resolution, dtype=dtype, device=device) for low, high in 23 | range_per_dim] 24 | pts = torch.cartesian_prod(*coords) if get_points else None 25 | return coords, pts 26 | 27 | 28 | class Voxels(abc.ABC): 29 | @abc.abstractmethod 30 | def get_known_pos_and_values(self): 31 | """Return the position (N x 3) and values (N) of known voxels""" 32 | 33 | @abc.abstractmethod 34 | def __getitem__(self, pts): 35 | """Return the values (N) at the positions (N x 3)""" 36 | 37 | @abc.abstractmethod 38 | def __setitem__(self, pts, value): 39 | """Set the values (N) at the positions (N x 3)""" 40 | 41 | 42 | class VoxelGrid(Voxels): 43 | def __init__(self, resolution, range_per_dim, dtype=torch.float, device='cpu'): 44 | self.resolution = resolution 45 | self.invalid_val = 0 46 | self.dtype = dtype 47 | self.device = device 48 | self._create_voxels(resolution, range_per_dim) 49 | 50 | def _create_voxels(self, resolution, range_per_dim): 51 | self.range_per_dim = get_divisible_range_by_resolution(resolution, range_per_dim) 52 | self.coords, self.pts = get_coordinates_and_points_in_grid(resolution, self.range_per_dim, device=self.device) 53 | # underlying data 54 | self._data = torch.zeros([len(coord) for coord in self.coords], dtype=self.dtype, device=self.device) 55 | self.voxels = torch_view.TorchMultidimView(self._data, self.range_per_dim, invalid_value=self.invalid_val) 56 | self.range_per_dim = np.array(self.range_per_dim) 57 | 58 | def get_known_pos_and_values(self): 59 | known = self.voxels.raw_data != self.invalid_val 60 | indices = known.nonzero() 61 | # these points are in object frame 62 | pos = self.voxels.ensure_value_key(indices) 63 | val = self.voxels.raw_data[indices] 64 | return pos, val 65 | 66 | def resize_to_fit(self): 67 | """Resize voxel grid to fit to the known points""" 68 | known_pos, known_val = self.get_known_pos_and_values() 69 | if known_pos.numel() == 0: 70 | return 71 | min = known_pos.min(dim=0).values 72 | max = known_pos.max(dim=0).values 73 | # fit to the min and max of data 74 | range_per_dim = copy.deepcopy(self.range_per_dim) 75 | for dim in range(len(min)): 76 | range_per_dim[dim] = (min[dim].item() - self.resolution, max[dim].item() + self.resolution) 77 | self._create_voxels(self.resolution, range_per_dim) 78 | self.__setitem__(known_pos, known_val) 79 | 80 | def get_voxel_values(self): 81 | """Get the raw value of the voxels without any coordinate information""" 82 | return self._data 83 | 84 | def get_voxel_center_points(self): 85 | return self.pts 86 | 87 | def __getitem__(self, pts): 88 | return self.voxels[pts] 89 | 90 | def __setitem__(self, pts, value): 91 | self.voxels[pts] = value 92 | 93 | 94 | class ExpandingVoxelGrid(VoxelGrid): 95 | def __setitem__(self, pts, value): 96 | if pts.numel() > 0: 97 | # if this query goes outside the range, expand the range in increments of the resolution 98 | min = pts.min(dim=0).values 99 | max = pts.max(dim=0).values 100 | range_per_dim = copy.deepcopy(self.range_per_dim) 101 | for dim in range(len(min)): 102 | over = (max[dim] - self.range_per_dim[dim][1]).item() 103 | under = (self.range_per_dim[dim][0] - min[dim]).item() 104 | # adjust in increments of resolution 105 | if over > 0: 106 | range_per_dim[dim][1] += math.ceil(over / self.resolution) * self.resolution 107 | if under > 0: 108 | range_per_dim[dim][0] -= math.ceil(under / self.resolution) * self.resolution 109 | if not np.allclose(range_per_dim, self.range_per_dim): 110 | # transfer over values 111 | known_pos, known_values = self.get_known_pos_and_values() 112 | self._create_voxels(self.resolution, range_per_dim) 113 | super().__setitem__(known_pos, known_values) 114 | 115 | return super().__setitem__(pts, value) 116 | 117 | 118 | class VoxelSet(Voxels): 119 | def __init__(self, positions, values): 120 | self.positions = positions 121 | self.values = values 122 | 123 | def __getitem__(self, pts): 124 | raise RuntimeError("Cannot get arbitrary points on a voxel set") 125 | 126 | def __setitem__(self, pts, value): 127 | self.positions = torch.cat((self.positions, pts.view(-1, self.positions.shape[-1])), dim=0) 128 | self.values = torch.cat((self.values, value)) 129 | 130 | def get_known_pos_and_values(self): 131 | return self.positions, self.values 132 | 133 | 134 | def bounds_contain_another_bounds(outer_bounds: np.array, inner_bounds: np.array): 135 | """Return whether outer_bounds contains inner_bounds""" 136 | return np.all(outer_bounds[:, 0] <= inner_bounds[:, 0]) and np.all(outer_bounds[:, 1] >= inner_bounds[:, 1]) 137 | 138 | 139 | def voxel_down_sample(points, resolution, range_per_dim=None, ignore_flat_dim=False): 140 | """ 141 | Down sample point clouds to the center of a voxel grid with a given resolution. 142 | Much faster than open3d's voxel_down_sample but at the cost of more memory usage since they 143 | loop over points while we process all points in parallel. 144 | :param points: N x D point cloud 145 | :param resolution: Voxel size 146 | :param range_per_dim: Range of the voxel grid, if None, will be determined by the points (you may want to specify 147 | smaller range than the range the points to ignore outliers) 148 | :return: 149 | """ 150 | if points.shape[0] == 0: 151 | return points 152 | data_bounds = np.stack((points.min(dim=0)[0].cpu().numpy() - resolution * 2, 153 | points.max(dim=0)[0].cpu().numpy() + resolution * 2)).T 154 | if range_per_dim is None or bounds_contain_another_bounds(range_per_dim, data_bounds): 155 | range_per_dim = data_bounds 156 | 157 | # special case for flat dimensions; assumes only last dimension can be flat 158 | flat_z = ignore_flat_dim and range_per_dim[-1][0] == range_per_dim[-1][1] 159 | flat_z_val = range_per_dim[-1][0] 160 | if flat_z: 161 | range_per_dim = range_per_dim[:-1] 162 | points = points[..., :-1] 163 | 164 | device = points.device 165 | voxel = VoxelGrid(resolution, range_per_dim, device=device, dtype=torch.bool) 166 | voxel[points] = 1 167 | pts, _ = voxel.get_known_pos_and_values() 168 | 169 | if flat_z: 170 | pts = torch.cat((pts, torch.ones((pts.shape[0], 1), device=device) * flat_z_val), dim=-1) 171 | return pts 172 | -------------------------------------------------------------------------------- /tests/YcbPowerDrill/collision_vhacd.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None.004 5 | Ns 500.000001 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.800000 0.800000 0.800000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.450000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /tests/YcbPowerDrill/collision_vhacd.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.82 (sub 6) OBJ File: '' 2 | # www.blender.org 3 | mtllib collision_vhacd.mtl 4 | o textured_simple_reoriented.004_hull_1_textured_simple_reoriente 5 | v 0.050582 0.020140 -0.001364 6 | v 0.049423 0.021676 0.012930 7 | v -0.031361 -0.015462 0.036928 8 | v -0.016585 -0.022182 0.023486 9 | v -0.005836 -0.004709 0.036928 10 | v -0.033374 0.012747 0.036928 11 | v 0.024386 0.008724 0.027518 12 | v -0.017927 -0.024189 -0.002030 13 | v 0.004231 -0.014790 0.005355 14 | v 0.026443 0.064086 0.002540 15 | v 0.043871 0.042972 -0.003378 16 | v 0.025057 0.057748 -0.003378 17 | v -0.045445 0.018736 -0.000611 18 | v -0.043452 -0.004709 0.000653 19 | v -0.031684 -0.030454 0.013015 20 | v -0.000730 0.051332 0.004613 21 | v -0.057940 0.011598 0.012465 22 | v -0.001809 0.040955 0.028192 23 | v -0.040096 -0.006725 0.036928 24 | v 0.037820 0.036923 0.028865 25 | v 0.008268 0.028867 0.034241 26 | v 0.043200 0.047676 0.008042 27 | v 0.038491 0.017451 -0.004048 28 | v 0.022373 0.055741 0.022816 29 | v 0.029766 0.052371 0.026174 30 | v 0.043200 0.026851 0.025504 31 | vt 0.237584 0.157199 32 | vt 0.375063 0.085750 33 | vt 0.475074 0.271528 34 | vt 0.218854 0.457125 35 | vt 0.756266 0.414351 36 | vt 0.362577 0.064410 37 | vt 1.000000 0.535730 38 | vt 0.568740 0.164344 39 | vt 0.775005 1.000000 40 | vt 0.937559 0.778487 41 | vt 0.762510 0.935589 42 | vt 0.107444 0.520134 43 | vt 0.125086 0.271528 44 | vt 0.238348 0.006259 45 | vt 0.509264 0.843350 46 | vt 0.000990 0.445991 47 | vt 0.512542 0.757041 48 | vt 0.156311 0.250093 49 | vt 0.881259 0.714172 50 | vt 0.606301 0.628518 51 | vt 0.931316 0.828501 52 | vt 0.983274 0.560069 53 | vt 0.887503 0.507140 54 | vt 0.737537 0.914250 55 | vt 0.806323 0.878420 56 | vt 0.931316 0.607083 57 | vn 0.2904 -0.6893 0.6638 58 | vn 0.0000 -0.0000 1.0000 59 | vn 0.4767 -0.7012 0.5302 60 | vn 0.4741 -0.7251 -0.4995 61 | vn 0.4396 0.5597 -0.7024 62 | vn -0.1542 -0.0662 -0.9858 63 | vn -0.4902 -0.5491 -0.6769 64 | vn -0.3663 0.6766 -0.6388 65 | vn -0.5086 0.7168 0.4770 66 | vn -0.6908 0.2385 0.6825 67 | vn 0.1779 0.0040 0.9840 68 | vn 0.0399 0.0630 0.9972 69 | vn -0.0830 0.3903 0.9170 70 | vn -0.0897 0.3849 0.9186 71 | vn -0.0903 0.3860 0.9181 72 | vn 0.7184 0.6570 -0.2284 73 | vn 0.9592 0.2768 -0.0577 74 | vn 0.9689 0.2418 0.0526 75 | vn 0.0963 -0.8439 0.5278 76 | vn 0.4171 -0.9020 0.1111 77 | vn 0.3961 -0.9180 -0.0201 78 | vn -0.6442 -0.6441 0.4125 79 | vn 0.2856 -0.4285 -0.8572 80 | vn -0.0408 0.0069 -0.9991 81 | vn -0.0408 0.0098 -0.9991 82 | vn -0.0409 -0.0000 -0.9992 83 | vn 0.2211 -0.0210 -0.9750 84 | vn -0.0409 0.0030 -0.9992 85 | vn 0.0177 0.0225 -0.9996 86 | vn 0.2184 0.8862 0.4086 87 | vn -0.1382 0.5302 0.8365 88 | vn 0.1138 0.2278 0.9670 89 | vn -0.0825 0.3902 0.9170 90 | vn 0.6254 0.7305 0.2742 91 | vn 0.7617 0.4741 0.4416 92 | vn -0.8100 -0.5096 -0.2901 93 | vn -0.8289 -0.5147 0.2191 94 | vn -0.3898 0.8765 0.2825 95 | vn -0.5083 0.7966 0.3273 96 | vn -0.4320 0.8326 0.3466 97 | vn 0.3127 -0.1461 0.9385 98 | vn 0.4123 -0.3337 0.8477 99 | vn 0.6130 -0.5762 0.5405 100 | vn 0.8595 0.3284 0.3917 101 | vn 0.9015 0.2781 0.3317 102 | vn -0.5771 0.8093 -0.1097 103 | vn -0.3689 0.6174 -0.6948 104 | vn -0.6924 -0.0974 -0.7149 105 | vn 0.5624 -0.7810 0.2718 106 | vn 0.5584 -0.7607 0.3309 107 | vn 0.6087 -0.7821 0.1334 108 | usemtl None.004 109 | s 1 110 | f 3/1/1 4/2/1 5/3/1 111 | f 6/4/2 3/1/2 5/3/2 112 | f 4/2/3 7/5/3 5/3/3 113 | f 8/6/4 1/7/4 9/8/4 114 | f 10/9/5 11/10/5 12/11/5 115 | f 13/12/6 8/6/6 14/13/6 116 | f 8/6/7 15/14/7 14/13/7 117 | f 10/9/8 12/11/8 16/15/8 118 | f 17/16/9 6/4/9 18/17/9 119 | f 6/4/10 17/16/10 19/18/10 120 | f 3/1/2 6/4/2 19/18/2 121 | f 5/3/11 20/19/11 21/20/11 122 | f 6/4/12 5/3/12 21/20/12 123 | f 18/17/13 6/4/14 21/20/15 124 | f 11/10/16 10/9/16 22/21/16 125 | f 1/7/17 11/10/17 22/21/17 126 | f 2/22/18 1/7/18 22/21/18 127 | f 4/2/19 3/1/19 15/14/19 128 | f 9/8/20 4/2/20 15/14/20 129 | f 8/6/21 9/8/21 15/14/21 130 | f 3/1/22 19/18/22 15/14/22 131 | f 1/7/23 8/6/23 23/23/23 132 | f 8/6/24 13/12/25 23/23/26 133 | f 11/10/27 1/7/27 23/23/27 134 | f 13/12/25 12/11/28 23/23/26 135 | f 12/11/29 11/10/29 23/23/29 136 | f 10/9/30 24/24/30 25/25/30 137 | f 24/24/31 18/17/31 25/25/31 138 | f 21/20/32 20/19/32 25/25/32 139 | f 18/17/13 21/20/15 25/25/33 140 | f 22/21/34 10/9/34 25/25/34 141 | f 20/19/35 22/21/35 25/25/35 142 | f 14/13/36 15/14/36 17/16/36 143 | f 15/14/37 19/18/37 17/16/37 144 | f 24/24/38 10/9/38 16/15/38 145 | f 17/16/39 18/17/39 16/15/39 146 | f 18/17/40 24/24/40 16/15/40 147 | f 20/19/41 5/3/41 26/26/41 148 | f 5/3/42 7/5/42 26/26/42 149 | f 7/5/43 2/22/43 26/26/43 150 | f 22/21/44 20/19/44 26/26/44 151 | f 2/22/45 22/21/45 26/26/45 152 | f 17/16/46 16/15/46 13/12/46 153 | f 16/15/47 12/11/47 13/12/47 154 | f 14/13/48 17/16/48 13/12/48 155 | f 7/5/49 4/2/49 2/22/49 156 | f 4/2/50 9/8/50 2/22/50 157 | f 9/8/51 1/7/51 2/22/51 158 | o textured_simple_reoriented.004_hull_2_textured_simple_reoriente 159 | v -0.025946 0.020080 0.088949 160 | v -0.038250 0.006831 0.089812 161 | v 0.006062 0.006674 0.090118 162 | v -0.026664 -0.015465 0.090001 163 | v -0.003274 0.010779 0.036927 164 | v -0.017687 -0.014461 0.036985 165 | v -0.022631 0.018792 0.036928 166 | v -0.038084 0.008041 0.036928 167 | v -0.003660 -0.002798 0.037345 168 | v -0.031368 -0.015465 0.036928 169 | v -0.007183 0.023497 0.090001 170 | v -0.027008 -0.018324 0.067343 171 | v -0.041445 -0.002027 0.043656 172 | v -0.038084 -0.012104 0.065813 173 | v -0.040101 -0.004041 0.079242 174 | v -0.012559 -0.014121 0.090001 175 | v -0.003155 -0.008746 0.084624 176 | v -0.006927 0.022603 0.066506 177 | v 0.002221 0.011399 0.063137 178 | v -0.038084 -0.010760 0.036928 179 | v -0.034052 -0.010090 0.090001 180 | v 0.002221 0.018122 0.089326 181 | v -0.011215 0.018118 0.036928 182 | vt 0.000000 0.790158 183 | vt 0.015801 0.464246 184 | vt 0.000000 0.400068 185 | vt 1.000000 0.314309 186 | vt 1.000000 1.000000 187 | vt 0.994292 0.084838 188 | vt 0.000000 0.071470 189 | vt 0.024544 0.765372 190 | vt 0.000000 0.214281 191 | vt 1.000000 0.728560 192 | vt 0.574176 0.306603 193 | vt 0.979585 0.327744 194 | vt 0.126769 0.000000 195 | vt 0.544250 0.071470 196 | vt 0.797279 0.028579 197 | vt 1.000000 0.614243 198 | vt 0.898687 0.814213 199 | vt 0.560537 0.726410 200 | vt 0.493829 0.928530 201 | vt 0.000000 0.071470 202 | vt 1.000000 0.157208 203 | vt 0.987282 0.928530 204 | vt 0.000000 0.642822 205 | vn 0.0107 -0.0080 -0.9999 206 | vn 0.0090 -0.0050 -0.9999 207 | vn 0.0001 -0.0014 -1.0000 208 | vn -0.0021 0.0043 1.0000 209 | vn -0.0058 0.0016 1.0000 210 | vn -0.0165 0.0260 0.9995 211 | vn 0.0016 -0.0008 -1.0000 212 | vn 0.0523 -0.0322 -0.9981 213 | vn 0.0030 0.0008 -1.0000 214 | vn -0.0182 0.0227 0.9996 215 | vn 0.0732 -0.9919 -0.1037 216 | vn -0.5702 0.8196 -0.0566 217 | vn -0.9612 -0.2750 0.0207 218 | vn 0.0006 -0.0062 1.0000 219 | vn 0.6285 -0.5658 0.5336 220 | vn 0.0941 -0.9879 0.1232 221 | vn -0.9477 0.3193 0.0043 222 | vn -0.7322 0.6809 0.0133 223 | vn -0.9911 0.1254 0.0445 224 | vn -0.1723 0.9844 -0.0353 225 | vn -0.1769 0.9834 -0.0394 226 | vn 0.8671 -0.4929 -0.0713 227 | vn 0.7376 0.6534 -0.1701 228 | vn 0.9803 -0.1157 -0.1598 229 | vn 0.9783 -0.0341 -0.2043 230 | vn -0.8946 0.0000 -0.4469 231 | vn -0.9373 -0.3482 -0.0162 232 | vn -0.0000 0.0000 -1.0000 233 | vn -0.5734 -0.8184 -0.0381 234 | vn -0.4985 -0.8591 0.1160 235 | vn -0.4883 -0.8726 -0.0120 236 | vn 0.6375 -0.7636 -0.1029 237 | vn 0.4782 -0.8773 -0.0406 238 | vn 0.3141 -0.9491 -0.0243 239 | vn 0.0099 0.0137 0.9999 240 | vn -0.5804 -0.7978 0.1632 241 | vn -0.8980 -0.2185 0.3820 242 | vn -0.8383 -0.5137 0.1825 243 | vn 0.1356 0.1136 0.9842 244 | vn 0.4945 0.8687 -0.0277 245 | vn 0.9467 0.3121 -0.0801 246 | vn 0.7383 0.6533 -0.1677 247 | vn 0.0000 0.0001 -1.0000 248 | vn 0.6643 0.7188 -0.2053 249 | vn 0.0582 0.9857 -0.1579 250 | vn -0.0807 0.1392 0.9870 251 | usemtl None.004 252 | s 1 253 | f 31/27/52 32/28/53 33/29/54 254 | f 30/30/55 29/31/56 28/32/57 255 | f 33/29/54 32/28/53 34/33/58 256 | f 32/28/53 31/27/52 35/34/59 257 | f 34/33/58 32/28/53 36/35/60 258 | f 28/32/57 29/31/56 37/36/61 259 | f 36/35/62 32/28/62 38/37/62 260 | f 33/29/63 34/33/63 27/38/63 261 | f 39/39/64 40/40/64 41/41/64 262 | f 29/31/56 30/30/55 42/42/65 263 | f 43/43/66 29/31/66 42/42/66 264 | f 30/30/67 38/37/67 42/42/67 265 | f 34/33/68 39/39/68 28/32/68 266 | f 27/38/69 34/33/69 28/32/69 267 | f 39/39/70 41/41/70 28/32/70 268 | f 33/29/71 27/38/71 44/44/71 269 | f 27/38/72 37/36/72 44/44/72 270 | f 29/31/73 43/43/73 35/34/73 271 | f 31/27/74 44/44/74 45/45/74 272 | f 29/31/75 35/34/75 45/45/75 273 | f 35/34/76 31/27/76 45/45/76 274 | f 39/39/77 34/33/77 46/46/77 275 | f 40/40/78 39/39/78 46/46/78 276 | f 34/33/58 36/35/60 46/46/79 277 | f 36/35/80 40/40/80 46/46/80 278 | f 30/30/81 40/40/81 38/37/81 279 | f 40/40/82 36/35/82 38/37/82 280 | f 35/34/83 43/43/83 32/28/83 281 | f 43/43/84 42/42/84 32/28/84 282 | f 42/42/85 38/37/85 32/28/85 283 | f 30/30/55 28/32/57 47/47/86 284 | f 40/40/87 30/30/87 47/47/87 285 | f 28/32/88 41/41/88 47/47/88 286 | f 41/41/89 40/40/89 47/47/89 287 | f 37/36/90 29/31/90 48/48/90 288 | f 44/44/91 37/36/91 48/48/91 289 | f 29/31/92 45/45/92 48/48/92 290 | f 45/45/93 44/44/93 48/48/93 291 | f 31/27/52 33/29/54 49/49/94 292 | f 44/44/95 31/27/95 49/49/95 293 | f 33/29/96 44/44/96 49/49/96 294 | f 28/32/57 37/36/61 27/38/97 295 | o textured_simple_reoriented.004_hull_3_textured_simple_reoriente 296 | v -0.032031 -0.004712 0.113514 297 | v -0.030694 -0.012774 0.091345 298 | v -0.023816 -0.012537 0.113573 299 | v -0.024829 0.017916 0.113405 300 | v -0.006335 -0.010310 0.113310 301 | v -0.025988 -0.015464 0.090001 302 | v -0.036736 -0.004712 0.091345 303 | v -0.036827 0.008491 0.090078 304 | v -0.021289 0.021479 0.090001 305 | v 0.010512 0.032268 0.113594 306 | v 0.001550 0.019462 0.090001 307 | v 0.002313 -0.002955 0.089850 308 | v -0.005842 -0.010762 0.090675 309 | v -0.013228 -0.014119 0.099408 310 | v -0.032706 0.006703 0.113514 311 | v 0.009611 0.004019 0.113514 312 | v 0.021551 0.018457 0.098864 313 | v 0.022377 0.019462 0.113512 314 | v 0.002894 0.030209 0.098738 315 | v 0.015618 0.032571 0.098291 316 | v 0.018347 0.030882 0.113512 317 | v 0.021942 0.025442 0.096005 318 | v -0.008523 0.024163 0.090001 319 | vt 0.089985 0.225432 320 | vt 0.112347 0.056400 321 | vt 0.231296 0.074422 322 | vt 0.215272 0.690359 323 | vt 0.515653 0.117374 324 | vt 0.191058 0.000000 325 | vt 0.011290 0.225432 326 | vt 0.019669 0.494979 327 | vt 0.269653 0.774568 328 | vt 0.803842 0.990417 329 | vt 0.651653 0.732278 330 | vt 0.658066 0.266548 331 | vt 0.528016 0.098585 332 | vt 0.404479 0.028200 333 | vt 0.078695 0.464766 334 | vt 0.786479 0.408491 335 | vt 0.978640 0.714610 336 | vt 1.000000 0.732278 337 | vt 0.674132 0.957606 338 | vt 0.881387 1.000000 339 | vt 0.932595 0.971716 340 | vt 0.982773 0.854676 341 | vt 0.483174 0.830842 342 | vn -0.2958 -0.3062 0.9048 343 | vn -0.6742 -0.7062 0.2162 344 | vn -0.3131 -0.3305 0.8904 345 | vn 0.0026 -0.0008 1.0000 346 | vn 0.0032 0.0001 1.0000 347 | vn -0.2163 -0.0947 -0.9717 348 | vn -0.0016 0.0002 -1.0000 349 | vn -0.0045 0.0022 -1.0000 350 | vn -0.0056 0.0007 -1.0000 351 | vn 0.0015 -0.0019 1.0000 352 | vn 0.1307 0.0088 -0.9914 353 | vn 0.0644 0.0089 -0.9979 354 | vn 0.6928 -0.7206 0.0295 355 | vn 0.0754 -0.1823 -0.9803 356 | vn 0.0148 -0.9924 0.1219 357 | vn 0.2293 -0.9576 -0.1742 358 | vn 0.0127 0.0008 0.9999 359 | vn -0.9766 -0.0577 0.2073 360 | vn -0.9847 0.0099 0.1739 361 | vn -0.6411 0.7672 0.0198 362 | vn -0.8034 0.5661 0.1845 363 | vn -0.3753 0.9231 0.0837 364 | vn -0.0074 -0.0026 1.0000 365 | vn 0.6682 -0.7438 0.0131 366 | vn 0.7707 -0.6371 0.0002 367 | vn 0.7511 -0.6591 -0.0374 368 | vn -0.4238 -0.3176 -0.8482 369 | vn -0.7889 -0.5912 0.1674 370 | vn -0.3560 0.9330 0.0533 371 | vn -0.1810 0.8994 -0.3978 372 | vn -0.1838 0.9821 -0.0419 373 | vn 0.1745 0.9816 0.0777 374 | vn 0.0051 -0.0016 1.0000 375 | vn 0.0111 0.0039 0.9999 376 | vn 0.7376 0.6727 -0.0576 377 | vn 0.9392 0.3314 0.0899 378 | vn 0.1577 0.4256 -0.8911 379 | vn 0.0000 0.0000 -1.0000 380 | vn -0.1915 0.9107 -0.3661 381 | vn 0.1662 0.3562 -0.9195 382 | vn 0.2781 0.0159 -0.9604 383 | vn 0.9958 -0.0766 -0.0509 384 | vn 0.6664 -0.3141 -0.6762 385 | vn 0.4404 -0.8974 0.0275 386 | vn 0.1268 -0.9709 0.2032 387 | vn -0.4577 -0.8762 0.1510 388 | usemtl None.004 389 | s 1 390 | f 50/50/98 51/51/99 52/52/100 391 | f 53/53/101 52/52/100 54/54/102 392 | f 52/52/100 53/53/101 50/50/98 393 | f 55/55/103 56/56/103 57/57/103 394 | f 58/58/104 55/55/105 57/57/106 395 | f 53/53/101 54/54/102 59/59/107 396 | f 55/55/105 58/58/104 60/60/108 397 | f 55/55/105 60/60/108 61/61/109 398 | f 54/54/110 62/62/110 61/61/110 399 | f 62/62/111 55/55/111 61/61/111 400 | f 52/52/112 55/55/112 63/63/112 401 | f 55/55/113 62/62/113 63/63/113 402 | f 50/50/98 53/53/101 64/64/114 403 | f 56/56/115 50/50/115 64/64/115 404 | f 57/57/116 56/56/116 64/64/116 405 | f 53/53/117 58/58/117 57/57/117 406 | f 64/64/118 53/53/118 57/57/118 407 | f 59/59/119 58/58/119 53/53/119 408 | f 59/59/107 54/54/102 65/65/120 409 | f 54/54/121 61/61/121 65/65/121 410 | f 66/66/122 67/67/122 65/65/122 411 | f 61/61/123 66/66/123 65/65/123 412 | f 56/56/124 55/55/124 51/51/124 413 | f 50/50/125 56/56/125 51/51/125 414 | f 58/58/126 59/59/126 68/68/126 415 | f 69/69/127 58/58/127 68/68/127 416 | f 69/69/128 68/68/128 59/59/128 417 | f 70/70/129 69/69/129 59/59/129 418 | f 67/67/130 70/70/131 59/59/107 419 | f 65/65/120 67/67/130 59/59/107 420 | f 69/69/132 70/70/132 71/71/132 421 | f 70/70/133 67/67/133 71/71/133 422 | f 69/69/134 71/71/134 72/72/134 423 | f 60/60/108 58/58/104 72/72/135 424 | f 58/58/136 69/69/136 72/72/136 425 | f 71/71/137 60/60/137 72/72/137 426 | f 60/60/108 71/71/138 61/61/109 427 | f 71/71/139 67/67/139 66/66/139 428 | f 61/61/140 71/71/140 66/66/140 429 | f 63/63/141 62/62/141 54/54/141 430 | f 52/52/142 63/63/142 54/54/142 431 | f 51/51/143 55/55/143 52/52/143 432 | o textured_simple_reoriented.004_hull_4_textured_simple_reoriente 433 | v -0.021842 -0.015411 0.120667 434 | v -0.009867 -0.011432 0.113514 435 | v -0.003596 -0.007681 0.120795 436 | v 0.026022 0.035983 0.120970 437 | v -0.037510 -0.012974 0.120566 438 | v 0.014887 0.033027 0.113585 439 | v 0.000872 -0.004718 0.113514 440 | v -0.030687 0.011400 0.113514 441 | v -0.020278 0.020078 0.120892 442 | v 0.023073 0.019472 0.113558 443 | v -0.024696 -0.012323 0.113504 444 | v 0.029964 0.024418 0.120730 445 | v -0.036476 0.003055 0.120976 446 | v -0.033843 -0.000126 0.113515 447 | v 0.021789 0.036815 0.120461 448 | v -0.020196 0.020724 0.113503 449 | v 0.027749 0.033568 0.118217 450 | vt 0.239162 0.005627 451 | vt 0.415921 0.078003 452 | vt 0.506495 0.154839 453 | vt 0.784240 0.914703 454 | vt 0.574204 0.207794 455 | vt 0.109054 0.519380 456 | vt 0.020318 0.064166 457 | vt 0.943700 0.989947 458 | vt 0.266496 0.677760 459 | vt 0.891817 0.684669 460 | vt 0.202764 0.074993 461 | vt 0.996969 0.774889 462 | vt 0.032136 0.352032 463 | vt 0.074406 0.298117 464 | vt 0.870504 0.999795 465 | vt 0.265922 0.693756 466 | vt 0.970345 0.947921 467 | vn 0.3855 -0.9123 0.1379 468 | vn 0.1269 0.0780 -0.9888 469 | vn 0.0440 -0.0388 -0.9983 470 | vn -0.3138 0.1793 -0.9324 471 | vn 0.0059 -0.0098 0.9999 472 | vn -0.0127 0.0465 0.9988 473 | vn -0.2587 0.4911 0.8318 474 | vn 0.3736 -0.5099 0.7749 475 | vn 0.4954 -0.3442 -0.7976 476 | vn -0.1515 -0.1132 -0.9820 477 | vn 0.0754 -0.0769 0.9942 478 | vn -0.1421 0.1401 0.9799 479 | vn -0.4018 -0.0058 -0.9157 480 | vn -0.0066 -0.0010 1.0000 481 | vn -0.0966 0.2674 0.9587 482 | vn -0.3230 0.3624 -0.8742 483 | vn -0.3248 0.9275 -0.1849 484 | vn 0.0008 -0.0012 -1.0000 485 | vn 0.9041 0.3019 0.3024 486 | vn 0.3247 0.1979 -0.9249 487 | vn -0.3679 0.9267 0.0770 488 | vn 0.6419 -0.7627 0.0796 489 | vn 0.7163 -0.6570 -0.2351 490 | vn -0.7085 0.6752 0.2054 491 | vn -0.6632 0.7462 0.0579 492 | vn -0.8786 0.2406 -0.4125 493 | vn 0.2243 0.7981 -0.5593 494 | vn 0.1881 0.7681 -0.6120 495 | vn -0.4425 -0.3311 -0.8334 496 | vn 0.7256 -0.0133 -0.6880 497 | vn -0.9311 0.0693 -0.3580 498 | vn -0.1422 -0.9283 -0.3435 499 | vn 0.0549 -0.9088 -0.4136 500 | vn 0.5300 -0.8478 -0.0198 501 | usemtl None.004 502 | s 1 503 | f 73/73/144 74/74/144 75/75/144 504 | f 78/76/145 79/77/146 80/78/147 505 | f 77/79/148 76/80/149 81/81/150 506 | f 76/80/149 77/79/148 75/75/151 507 | f 79/77/146 78/76/145 82/82/152 508 | f 80/78/147 79/77/146 83/83/153 509 | f 76/80/149 75/75/151 84/84/154 510 | f 77/79/148 81/81/150 85/85/155 511 | f 80/78/147 83/83/153 86/86/156 512 | f 75/75/151 77/79/148 73/73/157 513 | f 81/81/150 76/80/149 87/87/158 514 | f 78/76/145 80/78/147 88/88/159 515 | f 87/87/160 78/76/160 88/88/160 516 | f 83/83/153 79/77/146 74/74/161 517 | f 76/80/162 84/84/162 89/89/162 518 | f 82/82/152 78/76/145 89/89/163 519 | f 87/87/158 88/88/164 81/81/150 520 | f 75/75/151 79/77/165 84/84/154 521 | f 84/84/166 79/77/146 82/82/152 522 | f 81/81/150 80/78/167 85/85/155 523 | f 80/78/147 81/81/168 88/88/159 524 | f 85/85/169 80/78/147 86/86/156 525 | f 76/80/170 89/89/170 87/87/170 526 | f 89/89/171 78/76/171 87/87/171 527 | f 77/79/172 86/86/156 83/83/153 528 | f 82/82/173 89/89/173 84/84/173 529 | f 85/85/174 86/86/174 77/79/174 530 | f 77/79/175 83/83/175 73/73/175 531 | f 83/83/176 74/74/176 73/73/176 532 | f 74/74/177 79/77/165 75/75/151 533 | o textured_simple_reoriented.004_hull_5_textured_simple_reoriente 534 | v -0.005844 -0.008743 0.120905 535 | v 0.017451 0.000750 0.129425 536 | v -0.038223 -0.027687 0.125333 537 | v 0.026040 0.040579 0.122932 538 | v 0.037391 0.029902 0.132527 539 | v -0.052863 -0.004712 0.132324 540 | v -0.042490 -0.028950 0.132619 541 | v -0.024966 0.016438 0.120601 542 | v 0.031596 0.022842 0.121299 543 | v 0.023048 0.040292 0.132324 544 | v -0.004928 0.034340 0.129936 545 | v -0.041945 -0.016902 0.120887 546 | v 0.031103 0.032901 0.120905 547 | v -0.053117 -0.007082 0.125622 548 | v -0.051520 -0.021506 0.129636 549 | v -0.048827 -0.021506 0.124264 550 | v -0.054881 -0.013445 0.132324 551 | vt 0.532791 0.284359 552 | vt 0.778014 0.424318 553 | vt 0.178181 0.021324 554 | vt 0.021926 0.343189 555 | vt 0.137325 0.009288 556 | vt 0.999195 0.851131 557 | vt 0.333620 0.638416 558 | vt 0.939906 0.747235 559 | vt 0.550240 0.908969 560 | vt 0.846705 1.000000 561 | vt 0.875265 0.998669 562 | vt 0.141349 0.166163 563 | vt 0.934223 0.892132 564 | vt 0.036518 0.098090 565 | vt 0.030415 0.302985 566 | vt 0.065777 0.098090 567 | vt 0.000000 0.215736 568 | vn 0.4515 -0.8413 -0.2973 569 | vn -0.0094 0.0083 0.9999 570 | vn -0.0858 -0.0845 0.9927 571 | vn 0.0287 -0.0370 0.9989 572 | vn -0.0381 0.0250 -0.9990 573 | vn 0.0156 -0.0288 -0.9995 574 | vn 0.0084 -0.0057 -0.9999 575 | vn -0.1288 0.2173 0.9676 576 | vn 0.5721 0.7938 0.2065 577 | vn -0.0064 0.0107 0.9999 578 | vn -0.2051 0.9781 -0.0354 579 | vn -0.1718 0.0791 -0.9819 580 | vn -0.0205 0.0882 -0.9959 581 | vn 0.7705 0.5786 -0.2676 582 | vn 0.4758 -0.5555 -0.6819 583 | vn 0.8819 0.0248 -0.4708 584 | vn -0.8724 -0.2183 -0.4373 585 | vn -0.4190 -0.0392 -0.9072 586 | vn -0.6202 0.7466 -0.2406 587 | vn -0.6468 0.7596 -0.0682 588 | vn -0.1150 -0.0730 0.9907 589 | vn -0.9734 0.2249 -0.0427 590 | vn -0.8727 -0.2182 -0.4369 591 | vn -0.2829 0.1360 -0.9495 592 | vn -0.2895 0.6769 -0.6768 593 | vn -0.0588 0.2183 -0.9741 594 | vn -0.5547 -0.7842 -0.2781 595 | vn -0.5995 -0.4668 0.6501 596 | vn 0.0809 -0.3559 -0.9310 597 | vn -0.1559 -0.4220 -0.8931 598 | vn -0.4329 -0.8108 -0.3941 599 | vn 0.1548 -0.2086 0.9657 600 | vn 0.8267 -0.5574 -0.0762 601 | vn 0.4460 -0.8886 0.1072 602 | usemtl None.004 603 | s 1 604 | f 90/90/178 91/91/178 92/92/178 605 | f 95/93/179 96/94/180 94/95/181 606 | f 97/96/182 98/97/183 90/90/184 607 | f 100/98/185 95/93/185 99/99/185 608 | f 94/95/186 93/100/186 99/99/186 609 | f 95/93/179 94/95/181 99/99/187 610 | f 99/99/188 93/100/188 100/98/188 611 | f 97/96/182 90/90/184 101/101/189 612 | f 98/97/183 97/96/182 102/102/190 613 | f 93/100/191 94/95/191 102/102/191 614 | f 91/91/192 90/90/192 98/97/192 615 | f 102/102/193 94/95/193 98/97/193 616 | f 104/103/194 103/104/194 105/105/194 617 | f 103/104/195 101/101/195 105/105/195 618 | f 103/104/196 95/93/196 100/98/196 619 | f 97/96/197 103/104/197 100/98/197 620 | f 96/94/180 95/93/179 106/106/198 621 | f 95/93/199 103/104/199 106/106/199 622 | f 103/104/200 104/103/200 106/106/200 623 | f 101/101/189 103/104/201 97/96/182 624 | f 100/98/202 93/100/202 97/96/202 625 | f 93/100/203 102/102/190 97/96/182 626 | f 104/103/204 105/105/204 96/94/204 627 | f 106/106/198 104/103/205 96/94/180 628 | f 101/101/206 90/90/206 92/92/206 629 | f 105/105/207 101/101/207 92/92/207 630 | f 96/94/208 105/105/208 92/92/208 631 | f 96/94/180 91/91/209 94/95/181 632 | f 91/91/210 98/97/210 94/95/210 633 | f 91/91/211 96/94/211 92/92/211 634 | o textured_simple_reoriented.004_hull_6_textured_simple_reoriente 635 | v -0.068317 -0.014796 0.152487 636 | v -0.065538 -0.013496 0.141982 637 | v 0.004363 -0.013075 0.147921 638 | v -0.054896 -0.012798 0.132290 639 | v -0.064953 -0.012780 0.170610 640 | v -0.038080 -0.028228 0.132325 641 | v -0.014571 -0.012780 0.132329 642 | v -0.023481 -0.013152 0.178421 643 | v 0.003017 -0.014001 0.166154 644 | v -0.054474 -0.030854 0.137233 645 | v -0.058233 -0.026893 0.175319 646 | v -0.054204 -0.012780 0.178003 647 | v -0.039307 -0.037091 0.170415 648 | v -0.045473 -0.027555 0.178003 649 | v -0.054018 -0.037875 0.166151 650 | v -0.041580 -0.041862 0.152928 651 | v -0.066299 -0.020172 0.164570 652 | v -0.060251 -0.030917 0.146434 653 | v -0.051520 -0.038979 0.147778 654 | v -0.040244 -0.039044 0.145238 655 | vt 0.996916 0.356414 656 | vt 0.185262 0.000000 657 | vt 0.046371 0.814124 658 | vt 0.740854 0.000085 659 | vt 0.416798 0.000000 660 | vt 0.619665 0.963367 661 | vt 0.969927 0.740489 662 | vt 0.193517 0.115983 663 | vt 0.139001 0.914260 664 | vt 0.194539 0.971335 665 | vt 0.400334 0.802018 666 | vt 0.314890 0.971335 667 | vt 0.204525 0.706903 668 | vt 0.366913 0.444733 669 | vt 0.048290 0.209942 670 | vt 0.000000 0.428742 671 | vt 0.027817 0.685685 672 | vt 0.111185 0.300026 673 | vt 0.231536 0.328605 674 | vt 0.386748 0.276641 675 | vn 0.0040 1.0000 0.0089 676 | vn -0.0179 0.9998 -0.0030 677 | vn -0.0049 1.0000 -0.0039 678 | vn -0.0005 0.9998 0.0195 679 | vn -0.0814 -0.0909 -0.9925 680 | vn -0.1278 -0.1416 -0.9816 681 | vn 0.0010 -0.0012 -1.0000 682 | vn 0.0076 0.9999 0.0071 683 | vn 0.0573 0.9968 0.0549 684 | vn -0.2381 -0.2617 -0.9353 685 | vn 0.4536 -0.6901 -0.5639 686 | vn -0.5667 0.0051 0.8239 687 | vn 0.0124 0.9998 -0.0180 688 | vn -0.1842 -0.6822 0.7076 689 | vn -0.0137 -0.0081 0.9999 690 | vn -0.2104 -0.1243 0.9697 691 | vn -0.1736 -0.6696 0.7222 692 | vn -0.0258 -0.9635 0.2663 693 | vn 0.4851 -0.8576 0.1710 694 | vn -0.0863 0.9960 -0.0231 695 | vn -0.4628 0.8864 -0.0127 696 | vn -0.8426 -0.2404 0.4820 697 | vn -0.9833 0.0329 0.1789 698 | vn -0.8998 -0.4341 -0.0429 699 | vn -0.8227 -0.5653 0.0606 700 | vn -0.8024 -0.5326 0.2691 701 | vn -0.4851 -0.8743 -0.0138 702 | vn -0.6737 -0.7375 -0.0473 703 | vn -0.5126 -0.8584 -0.0182 704 | vn -0.2873 -0.9576 0.0185 705 | vn -0.0856 -0.9306 -0.3558 706 | vn 0.3513 -0.4975 0.7932 707 | vn 0.2933 -0.4720 0.8314 708 | vn 0.5307 -0.8476 -0.0039 709 | vn 0.4509 -0.7200 -0.5275 710 | vn 0.5007 -0.8373 -0.2198 711 | vn -0.0724 -0.7586 -0.6475 712 | vn -0.5957 -0.7083 -0.3788 713 | vn -0.1355 -0.8028 -0.5806 714 | vn -0.8965 -0.3436 -0.2797 715 | vn -0.6499 -0.2148 -0.7290 716 | vn -0.7871 -0.3658 -0.4967 717 | usemtl None.004 718 | s 1 719 | f 109/107/212 110/108/213 111/109/214 720 | f 110/108/213 109/107/212 113/110/215 721 | f 112/111/216 110/108/217 113/110/218 722 | f 109/107/212 111/109/214 114/112/219 723 | f 115/113/220 109/107/220 114/112/220 724 | f 110/108/217 112/111/216 116/114/221 725 | f 112/111/222 113/110/222 109/107/222 726 | f 111/109/223 117/115/223 118/116/223 727 | f 114/112/219 111/109/214 118/116/224 728 | f 117/115/225 119/117/225 120/118/225 729 | f 114/112/226 118/116/226 120/118/226 730 | f 118/116/227 117/115/227 120/118/227 731 | f 119/117/228 117/115/228 121/119/228 732 | f 122/120/229 119/117/229 121/119/229 733 | f 122/120/230 115/113/230 119/117/230 734 | f 111/109/214 110/108/213 108/121/231 735 | f 107/122/232 111/109/232 108/121/232 736 | f 117/115/233 111/109/233 123/123/233 737 | f 111/109/234 107/122/234 123/123/234 738 | f 107/122/235 124/124/235 123/123/235 739 | f 124/124/236 121/119/236 123/123/236 740 | f 121/119/237 117/115/237 123/123/237 741 | f 121/119/238 124/124/239 125/125/240 742 | f 122/120/241 121/119/238 125/125/240 743 | f 126/126/242 122/120/242 125/125/242 744 | f 115/113/243 114/112/243 119/117/243 745 | f 114/112/244 120/118/244 119/117/244 746 | f 115/113/245 122/120/245 109/107/245 747 | f 112/111/246 109/107/246 126/126/246 748 | f 109/107/247 122/120/247 126/126/247 749 | f 112/111/248 126/126/248 116/114/248 750 | f 125/125/249 124/124/249 116/114/249 751 | f 126/126/250 125/125/250 116/114/250 752 | f 124/124/251 107/122/251 108/121/251 753 | f 110/108/252 116/114/252 108/121/252 754 | f 116/114/253 124/124/253 108/121/253 755 | o textured_simple_reoriented.004_hull_7_textured_simple_reoriente 756 | v 0.025734 0.001328 0.176662 757 | v 0.006123 -0.012541 0.175594 758 | v 0.047207 0.012862 0.160867 759 | v -0.032709 0.013420 0.171957 760 | v 0.019687 0.013420 0.132330 761 | v -0.056102 -0.010907 0.132374 762 | v -0.025991 0.013420 0.132330 763 | v -0.003596 0.013536 0.182738 764 | v 0.030430 0.012746 0.178000 765 | v -0.063608 -0.012778 0.141064 766 | v -0.013224 -0.012780 0.132330 767 | v -0.044103 0.012701 0.157541 768 | v -0.042593 -0.012824 0.179841 769 | v 0.025734 0.002000 0.136359 770 | v 0.006476 -0.012364 0.138783 771 | v 0.044147 0.013018 0.145025 772 | v -0.063808 -0.001644 0.150724 773 | v -0.068303 -0.012778 0.161208 774 | v -0.063608 -0.003373 0.165242 775 | v -0.058144 -0.012159 0.176653 776 | vt 0.828440 0.891920 777 | vt 0.657632 0.859078 778 | vt 0.990617 0.625171 779 | vt 0.313573 0.797260 780 | vt 0.775167 0.000000 781 | vt 0.121518 0.000000 782 | vt 0.372757 0.000000 783 | vt 0.562009 0.990708 784 | vt 0.869810 0.918840 785 | vt 0.041362 0.175720 786 | vt 0.485231 0.000000 787 | vt 0.218549 0.508621 788 | vt 0.226497 0.945960 789 | vt 0.828440 0.081060 790 | vt 0.656931 0.130198 791 | vt 0.978169 0.279744 792 | vt 0.041881 0.372372 793 | vt 0.000000 0.580999 794 | vt 0.041362 0.662160 795 | vt 0.095650 0.889614 796 | vn 0.5622 -0.8085 0.1740 797 | vn -0.1764 0.8772 0.4465 798 | vn 0.0092 0.9999 0.0051 799 | vn 0.0070 0.9999 0.0081 800 | vn -0.0005 -0.0002 -1.0000 801 | vn -0.4020 0.4965 -0.7693 802 | vn -0.0973 0.1186 -0.9882 803 | vn -0.0108 0.9999 -0.0018 804 | vn -0.0544 0.9892 0.1364 805 | vn 0.0279 0.9990 0.0341 806 | vn -0.0118 -0.9978 -0.0654 807 | vn -0.0426 -0.9685 -0.2453 808 | vn 0.0061 -0.9999 -0.0160 809 | vn -0.0010 0.0012 -1.0000 810 | vn -0.0519 0.9986 -0.0088 811 | vn -0.1428 -0.6954 0.7043 812 | vn 0.0103 -0.9999 0.0009 813 | vn 0.1883 -0.2366 -0.9532 814 | vn 0.2776 -0.5096 -0.8144 815 | vn 0.5787 -0.8017 -0.1495 816 | vn 0.5491 -0.8279 -0.1142 817 | vn 0.5567 -0.8215 -0.1238 818 | vn 0.5786 -0.8014 -0.1518 819 | vn 0.6699 -0.3521 0.6536 820 | vn -0.9586 0.1766 -0.2234 821 | vn -0.9035 0.4240 0.0629 822 | vn -0.8248 0.1822 0.5352 823 | vn -0.0105 -0.9999 0.0119 824 | vn -0.0980 -0.9556 0.2780 825 | vn -0.6083 0.7871 0.1021 826 | vn -0.5047 0.7848 0.3597 827 | vn -0.4618 0.5821 0.6693 828 | vn -0.3098 0.4608 0.8317 829 | vn -0.4816 0.8148 -0.3227 830 | vn -0.7227 0.4455 -0.5284 831 | vn -0.5258 0.6498 -0.5489 832 | vn 0.0126 0.9999 0.0074 833 | vn 0.4586 -0.0697 -0.8859 834 | vn -0.1905 0.1757 0.9658 835 | vn 0.1321 -0.1688 0.9768 836 | vn 0.0858 -0.2334 0.9686 837 | vn 0.1067 -0.2255 0.9684 838 | vn 0.0226 -0.9997 -0.0046 839 | vn 0.5261 -0.8504 0.0010 840 | usemtl None.004 841 | s 1 842 | f 127/127/254 128/128/254 129/129/254 843 | f 130/130/255 129/129/256 131/131/257 844 | f 131/131/258 132/132/259 133/133/260 845 | f 130/130/255 131/131/257 133/133/261 846 | f 129/129/256 130/130/255 134/134/262 847 | f 129/129/263 134/134/263 135/135/263 848 | f 136/136/264 132/132/265 137/137/266 849 | f 132/132/259 131/131/258 137/137/267 850 | f 130/130/255 133/133/261 138/138/268 851 | f 136/136/264 137/137/266 139/139/269 852 | f 137/137/266 128/128/270 139/139/269 853 | f 137/137/271 131/131/271 140/140/271 854 | f 141/141/272 137/137/272 140/140/272 855 | f 140/140/273 142/142/274 129/129/275 856 | f 141/141/276 140/140/273 129/129/275 857 | f 129/129/277 135/135/277 127/127/277 858 | f 143/143/278 136/136/278 144/144/278 859 | f 145/145/279 143/143/279 144/144/279 860 | f 146/146/280 145/145/280 144/144/280 861 | f 136/136/264 139/139/269 144/144/281 862 | f 139/139/269 146/146/282 144/144/281 863 | f 143/143/283 145/145/283 138/138/283 864 | f 145/145/284 130/130/284 138/138/284 865 | f 130/130/285 145/145/285 146/146/285 866 | f 134/134/262 130/130/255 146/146/286 867 | f 138/138/287 133/133/287 143/143/287 868 | f 132/132/288 136/136/288 143/143/288 869 | f 133/133/260 132/132/259 143/143/289 870 | f 131/131/257 129/129/256 142/142/290 871 | f 140/140/291 131/131/291 142/142/291 872 | f 134/134/292 146/146/282 139/139/269 873 | f 127/127/293 135/135/293 134/134/293 874 | f 139/139/294 128/128/294 134/134/294 875 | f 128/128/295 127/127/295 134/134/295 876 | f 128/128/270 137/137/266 141/141/296 877 | f 129/129/297 128/128/297 141/141/297 878 | o textured_simple_reoriented.004_hull_8_textured_simple_reoriente 879 | v 0.030825 0.042770 0.132100 880 | v -0.000094 0.042833 0.138273 881 | v 0.071162 0.042918 0.173862 882 | v 0.068050 0.042980 0.139049 883 | v 0.030427 0.042980 0.182034 884 | v 0.080809 0.042306 0.152481 885 | v 0.046554 0.013420 0.150466 886 | v 0.056636 0.021487 0.165235 887 | v -0.025312 0.013423 0.132325 888 | v 0.022410 0.013514 0.132311 889 | v 0.019602 0.027338 0.184448 890 | v 0.033601 0.013124 0.177626 891 | v 0.048566 0.036932 0.181360 892 | v -0.037404 0.013423 0.146435 893 | v -0.019277 0.029543 0.137707 894 | v -0.037404 0.018125 0.159194 895 | v -0.014055 0.013397 0.180816 896 | v -0.019277 0.027529 0.178004 897 | v -0.001961 0.041110 0.176642 898 | v -0.037404 0.013423 0.167255 899 | v 0.040306 0.014656 0.139341 900 | v 0.047899 0.035587 0.133003 901 | v 0.080809 0.042306 0.161883 902 | vt 0.316719 0.117332 903 | vt 0.915468 0.794203 904 | vt 0.892068 0.129998 905 | vt 0.573803 0.961043 906 | vt 0.577926 0.000097 907 | vt 1.000000 0.389684 908 | vt 0.710226 0.350727 909 | vt 0.795513 0.636262 910 | vt 0.102290 0.000000 911 | vt 0.501463 0.000097 912 | vt 0.490969 0.994791 913 | vt 0.596989 0.860094 914 | vt 0.727247 0.948013 915 | vt 0.000000 0.272794 916 | vt 0.153342 0.104052 917 | vt 0.000000 0.519469 918 | vt 0.200472 0.919741 919 | vt 0.296632 0.846589 920 | vt 0.153342 0.883130 921 | vt 0.000000 0.675315 922 | vt 0.653585 0.123861 923 | vt 0.721604 0.013108 924 | vt 1.000000 0.571456 925 | vn -0.0009 1.0000 -0.0045 926 | vn -0.0010 1.0000 -0.0003 927 | vn -0.0022 1.0000 -0.0001 928 | vn 0.0007 1.0000 -0.0039 929 | vn -0.0019 0.9998 -0.0198 930 | vn 0.0561 0.9984 -0.0032 931 | vn 0.6453 -0.7636 -0.0234 932 | vn -0.0307 -0.9286 -0.3699 933 | vn -0.0731 0.1323 -0.9885 934 | vn -0.0004 -0.8036 -0.5952 935 | vn 0.1892 -0.2671 0.9449 936 | vn 0.0714 0.1037 0.9920 937 | vn -0.6787 0.4483 -0.5817 938 | vn -0.7074 0.6632 -0.2444 939 | vn -0.5681 0.8218 -0.0426 940 | vn 0.1654 -0.9836 0.0714 941 | vn 0.1031 -0.9939 0.0383 942 | vn -0.2100 -0.9102 0.3570 943 | vn 0.0595 -0.3837 0.9215 944 | vn -0.0645 0.9970 0.0416 945 | vn -0.5478 0.8365 0.0109 946 | vn -0.5973 0.7802 0.1856 947 | vn -1.0000 0.0000 0.0000 948 | vn -0.6966 0.6197 0.3615 949 | vn -0.5022 -0.0135 0.8646 950 | vn -0.1371 -0.9626 0.2336 951 | vn 0.0008 -1.0000 -0.0003 952 | vn 0.4249 -0.8844 0.1930 953 | vn 0.3796 -0.3715 0.8473 954 | vn 0.1778 0.4348 0.8828 955 | vn 0.5367 -0.6018 0.5914 956 | vn 0.5794 -0.5735 -0.5791 957 | vn 0.6010 -0.6839 -0.4135 958 | vn 0.1342 -0.9738 -0.1835 959 | vn -0.1732 0.2662 0.9482 960 | vn -0.1550 0.2923 0.9437 961 | vn -0.1614 0.1349 0.9776 962 | vn 0.3938 -0.3943 -0.8303 963 | vn 0.3674 -0.3889 -0.8448 964 | vn 0.3849 -0.3922 -0.8355 965 | vn 0.1738 0.2948 -0.9396 966 | vn 0.3584 -0.3872 -0.8495 967 | vn 0.0444 -0.0200 -0.9988 968 | vn 0.0633 0.9980 0.0000 969 | vn 0.6526 -0.7577 0.0000 970 | vn 0.5996 -0.6134 0.5141 971 | vn -0.2424 0.3877 -0.8894 972 | vn -0.1832 0.3434 -0.9212 973 | usemtl None.004 974 | s 1 975 | f 148/147/298 149/148/299 150/149/300 976 | f 149/148/299 148/147/298 151/150/301 977 | f 148/147/298 150/149/300 147/151/302 978 | f 150/149/303 149/148/303 152/152/303 979 | f 153/153/304 152/152/304 154/154/304 980 | f 155/155/305 147/151/306 156/156/307 981 | f 157/157/308 158/158/308 159/159/308 982 | f 151/150/309 157/157/309 159/159/309 983 | f 155/155/310 160/160/310 161/161/310 984 | f 160/160/311 162/162/311 161/161/311 985 | f 162/162/312 148/147/312 161/161/312 986 | f 153/153/313 158/158/314 163/163/315 987 | f 158/158/316 157/157/316 163/163/316 988 | f 151/150/317 148/147/317 165/164/317 989 | f 148/147/318 162/162/318 165/164/318 990 | f 162/162/319 164/165/319 165/164/319 991 | f 162/162/320 160/160/320 166/166/320 992 | f 164/165/321 162/162/321 166/166/321 993 | f 163/163/315 164/165/322 166/166/323 994 | f 160/160/324 163/163/315 166/166/323 995 | f 153/153/313 154/154/325 158/158/314 996 | f 159/159/326 158/158/326 149/148/326 997 | f 151/150/327 159/159/327 149/148/327 998 | f 158/158/328 154/154/328 149/148/328 999 | f 150/149/329 152/152/329 167/167/329 1000 | f 152/152/330 153/153/330 167/167/330 1001 | f 160/160/324 155/155/305 156/156/307 1002 | f 167/167/331 153/153/331 156/156/331 1003 | f 163/163/315 160/160/324 156/156/307 1004 | f 153/153/313 163/163/315 156/156/307 1005 | f 151/150/332 165/164/332 157/157/332 1006 | f 165/164/333 164/165/333 157/157/333 1007 | f 164/165/334 163/163/334 157/157/334 1008 | f 150/149/335 167/167/336 168/168/337 1009 | f 147/151/338 150/149/338 168/168/338 1010 | f 167/167/336 156/156/339 168/168/337 1011 | f 156/156/340 147/151/340 168/168/340 1012 | f 152/152/341 149/148/341 169/169/341 1013 | f 154/154/342 152/152/342 169/169/342 1014 | f 149/148/343 154/154/343 169/169/343 1015 | f 155/155/344 161/161/344 148/147/344 1016 | f 147/151/306 155/155/305 148/147/345 1017 | o textured_simple_reoriented.004_hull_9_textured_simple_reoriente 1018 | v 0.077979 0.066228 0.137201 1019 | v 0.070978 0.076742 0.141843 1020 | v 0.090900 0.075895 0.164566 1021 | v 0.068729 0.078584 0.169942 1022 | v 0.080195 0.063130 0.178169 1023 | v 0.077160 0.042827 0.171471 1024 | v 0.030380 0.043443 0.132251 1025 | v 0.068729 0.042980 0.139044 1026 | v 0.029111 0.043464 0.182228 1027 | v 0.008272 0.043655 0.139721 1028 | v 0.010293 0.042984 0.175986 1029 | v 0.084854 0.057092 0.140392 1030 | v 0.088208 0.077242 0.148450 1031 | v 0.096624 0.064204 0.158119 1032 | v 0.088878 0.048358 0.151812 1033 | v 0.088208 0.051714 0.169271 1034 | v 0.082162 0.081944 0.159865 1035 | v 0.067138 0.082270 0.158781 1036 | v 0.005403 0.047473 0.167861 1037 | v 0.005308 0.047969 0.147596 1038 | vt 0.947742 0.648595 1039 | vt 0.701439 0.756744 1040 | vt 0.827469 0.895173 1041 | vt 0.772252 0.743699 1042 | vt 0.312919 -0.001638 1043 | vt 0.701439 0.135167 1044 | vt 0.052258 0.878332 1045 | vt 0.029806 0.148786 1046 | vt 0.880575 0.162284 1047 | vt 0.803779 0.104276 1048 | vt 0.917836 0.324388 1049 | vt 0.994811 0.520361 1050 | vt 0.925279 0.392022 1051 | vt 0.917836 0.743246 1052 | vt 0.284743 1.003790 1053 | vt 0.850669 0.554024 1054 | vt 0.726529 0.205704 1055 | vt 0.684038 0.533178 1056 | vt 0.012482 0.753222 1057 | vt 0.002949 0.297982 1058 | vn 0.2568 0.5957 0.7611 1059 | vn -0.0061 -1.0000 0.0003 1060 | vn -0.0056 -0.9999 -0.0090 1061 | vn -0.0118 -0.9999 -0.0016 1062 | vn -0.0155 -0.9997 -0.0176 1063 | vn 0.6190 0.2006 -0.7593 1064 | vn 0.8111 0.0957 -0.5770 1065 | vn 0.3064 -0.9482 -0.0841 1066 | vn 0.5444 -0.5626 -0.6221 1067 | vn 0.6380 -0.7513 0.1689 1068 | vn 0.8508 -0.2274 -0.4737 1069 | vn 0.8709 -0.4754 0.1248 1070 | vn -0.0017 -1.0000 0.0097 1071 | vn 0.0024 -0.9976 0.0696 1072 | vn 0.2017 -0.3339 0.9208 1073 | vn 0.2343 0.7846 0.5740 1074 | vn 0.0424 0.9526 -0.3013 1075 | vn 0.1005 0.9376 -0.3330 1076 | vn 0.5793 0.8146 -0.0287 1077 | vn -0.2707 0.5753 0.7718 1078 | vn -0.0883 0.4162 0.9050 1079 | vn -0.3658 0.7038 0.6090 1080 | vn -0.6989 -0.7147 0.0257 1081 | vn -0.8322 -0.5544 -0.0096 1082 | vn -0.2628 0.5490 -0.7934 1083 | vn -0.4003 0.8405 -0.3651 1084 | vn -0.3751 0.7462 -0.5500 1085 | vn 0.8691 0.4833 -0.1048 1086 | vn 0.4391 -0.3400 0.8316 1087 | vn 0.7724 0.0283 0.6345 1088 | vn 0.7714 0.0468 0.6346 1089 | vn -0.4239 0.8403 0.3379 1090 | vn -0.0020 0.9495 0.3139 1091 | vn -0.4882 0.8724 0.0236 1092 | vn 0.1709 -0.1452 -0.9745 1093 | vn 0.2253 -0.1657 -0.9601 1094 | vn 0.2875 0.5405 -0.7907 1095 | vn -0.0757 0.3601 -0.9298 1096 | usemtl None.004 1097 | s 1 1098 | f 172/170/346 173/171/346 174/172/346 1099 | f 175/173/347 176/174/348 177/175/349 1100 | f 176/174/350 180/176/350 179/177/350 1101 | f 181/178/351 170/179/351 182/180/351 1102 | f 183/181/352 181/178/352 182/180/352 1103 | f 175/173/353 177/175/353 184/182/353 1104 | f 177/175/354 181/178/354 184/182/354 1105 | f 185/183/355 175/173/355 184/182/355 1106 | f 181/178/356 183/181/356 184/182/356 1107 | f 183/181/357 185/183/357 184/182/357 1108 | f 180/176/358 176/174/348 175/173/347 1109 | f 178/184/359 180/176/358 175/173/347 1110 | f 174/172/360 178/184/360 175/173/360 1111 | f 173/171/361 172/170/361 186/185/361 1112 | f 171/186/362 187/187/362 186/185/362 1113 | f 182/180/363 171/186/363 186/185/363 1114 | f 172/170/364 182/180/364 186/185/364 1115 | f 173/171/365 180/176/365 178/184/365 1116 | f 174/172/366 173/171/366 178/184/366 1117 | f 180/176/367 173/171/367 188/188/367 1118 | f 179/177/368 180/176/368 188/188/368 1119 | f 189/189/369 179/177/369 188/188/369 1120 | f 179/177/370 171/186/370 176/174/370 1121 | f 187/187/371 171/186/371 189/189/371 1122 | f 171/186/372 179/177/372 189/189/372 1123 | f 182/180/373 172/170/373 183/181/373 1124 | f 175/173/374 185/183/374 174/172/374 1125 | f 183/181/375 172/170/375 174/172/375 1126 | f 185/183/376 183/181/376 174/172/376 1127 | f 188/188/377 173/171/377 187/187/377 1128 | f 173/171/378 186/185/378 187/187/378 1129 | f 189/189/379 188/188/379 187/187/379 1130 | f 177/175/380 176/174/380 170/179/380 1131 | f 181/178/381 177/175/381 170/179/381 1132 | f 182/180/382 170/179/382 171/186/382 1133 | f 170/179/383 176/174/383 171/186/383 1134 | -------------------------------------------------------------------------------- /tests/YcbPowerDrill/model.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /tests/YcbPowerDrill/texture_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UM-ARM-Lab/pytorch_volumetric/6e90a1ca7c7e25e49c82b6c8520f50f8d8f925a1/tests/YcbPowerDrill/texture_map.png -------------------------------------------------------------------------------- /tests/YcbPowerDrill/textured_simple_reoriented.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl material_0.008 5 | Ns 0.000000 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.639216 0.639216 0.639216 8 | Ks 1.000000 1.000000 1.000000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.450000 11 | d 1.000000 12 | illum 2 13 | map_Kd texture_map.png 14 | -------------------------------------------------------------------------------- /tests/offset_wrench.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /tests/offset_wrench_nogrip.obj: -------------------------------------------------------------------------------- 1 | # Exported by SolidWorks 2 | g 3 | v 0.09479585 -0.01211035 0.00830313 4 | v 0.09479585 -0.01211035 -0.00100313 5 | v 0.09623943 -0.01155567 0.00830313 6 | v 0.09623943 -0.01155567 -0.00100313 7 | v 0.09760490 -0.01082965 0.00830313 8 | v 0.09760490 -0.01082965 -0.00100313 9 | v 0.09887200 -0.00994304 0.00830313 10 | v 0.09887200 -0.00994304 -0.00100313 11 | v 0.10002194 -0.00890900 0.00830313 12 | v 0.10002194 -0.00890900 -0.00100313 13 | v 0.10103768 -0.00774286 0.00830313 14 | v 0.10103768 -0.00774286 -0.00100313 15 | v 0.10190414 -0.00646190 0.00830313 16 | v 0.10190414 -0.00646190 -0.00100313 17 | v 0.09720842 0.01106275 0.00830313 18 | v 0.09720842 0.01106275 -0.00100313 19 | v 0.09581757 0.01173885 0.00830313 20 | v 0.09509386 0.01201216 -0.00100313 21 | v 0.09435484 0.01224090 0.00830313 22 | v 0.09435484 0.01224090 -0.00100313 23 | v 0.09284195 0.01256143 0.00830313 24 | v 0.09284195 0.01256143 -0.00100313 25 | v 0.09130130 0.01269570 0.00830313 26 | v 0.09130130 0.01269570 -0.00100313 27 | v 0.08975577 0.01264173 0.00830313 28 | v 0.09052771 0.01269226 -0.00100313 29 | v 0.08822824 0.01240029 0.00830313 30 | v 0.08822824 0.01240029 -0.00100313 31 | v 0.09082653 -0.00686503 0.00870000 32 | v 0.09008930 -0.00661135 0.00870000 33 | v 0.08828635 -0.01149933 0.00870000 34 | v 0.09340975 -0.01203844 0.00870000 35 | v 0.09467632 -0.01173190 0.00870000 36 | v 0.09160458 -0.00681511 0.00870000 37 | v 0.10129056 -0.00473194 0.00870000 38 | v 0.10078742 -0.00449007 0.00870000 39 | v 0.10022934 -0.00450411 0.00870000 40 | v 0.07986800 -0.00634932 0.00870000 41 | v 0.08130010 -0.00683814 0.00870000 42 | v 0.08665625 0.00454476 0.00870000 43 | v 0.08811213 0.00621885 0.00870000 44 | v 0.09424910 0.01185837 0.00870000 45 | v 0.09278348 0.01216889 0.00870000 46 | v 0.09129098 0.01229896 0.00870000 47 | v 0.08979374 0.01224667 0.00870000 48 | v 0.08831395 0.01201279 0.00870000 49 | v 0.08952730 -0.01189714 0.00870000 50 | v 0.09081091 -0.01212184 0.00870000 51 | v 0.09211319 -0.01216924 0.00870000 52 | v 0.09607480 -0.01119456 0.00870000 53 | v 0.09739759 -0.01049122 0.00870000 54 | v 0.09862510 -0.00963232 0.00870000 55 | v 0.08654626 0.00266773 0.00870000 56 | v 0.08690175 0.00528475 0.00870000 57 | v 0.08182842 0.00793268 0.00870000 58 | v 0.07982329 0.00684754 0.00870000 59 | v 0.07764360 0.00617894 0.00870000 60 | v 0.09566611 0.01137202 0.00870000 61 | v 0.09673690 0.00852985 0.00870000 62 | v 0.09701351 0.01071704 0.00870000 63 | v 0.08700065 0.01162134 0.00870000 64 | v 0.08575711 0.01104548 0.00870000 65 | v 0.08741336 0.00587305 0.00870000 66 | v 0.08460907 0.01029713 0.00870000 67 | v 0.08358031 0.00939178 0.00870000 68 | v 0.10156249 -0.00625997 0.00870000 69 | v 0.10172594 -0.00572618 0.00870000 70 | v 0.10162813 -0.00517657 0.00870000 71 | v 0.10072310 -0.00750089 0.00870000 72 | v 0.09973910 -0.00863060 0.00870000 73 | v 0.08391133 -0.00835513 0.00870000 74 | v 0.08504534 -0.00935708 0.00870000 75 | v 0.08950669 -0.00609325 0.00870000 76 | v 0.08602421 -0.01021730 0.00870000 77 | v 0.08711133 -0.01093587 0.00870000 78 | v 0.07838414 -0.00605260 0.00870000 79 | v 0.07687419 -0.00595313 0.00870000 80 | v 0.07537487 0.00595313 0.00870000 81 | v -0.00900000 -0.00595313 0.00870000 82 | v -0.00900000 0.00595313 0.00870000 83 | v -0.01016140 -0.00583874 0.00870000 84 | v 0.08847292 -0.00452269 0.00870000 85 | v 0.08764831 -0.00283290 0.00870000 86 | v 0.08265569 -0.00751062 0.00870000 87 | v 0.08704639 -0.00105159 0.00870000 88 | v 0.08667701 0.00079202 0.00870000 89 | v 0.09761206 0.00981161 0.00870000 90 | v 0.09742196 0.01033650 0.00870000 91 | v 0.09754203 0.00925776 0.00870000 92 | v 0.09722724 0.00879673 0.00870000 93 | v -0.01320949 -0.00420950 0.00870000 94 | v -0.01394984 -0.00330738 0.00870000 95 | v -0.01449997 -0.00227816 0.00870000 96 | v -0.01230738 0.00494984 0.00870000 97 | v -0.01320949 0.00420950 0.00870000 98 | v -0.01394984 0.00330738 0.00870000 99 | v -0.01127816 0.00549997 0.00870000 100 | v -0.01483874 0.00116140 0.00870000 101 | v -0.01495313 0.00000000 0.00870000 102 | v -0.01230738 -0.00494984 0.00870000 103 | v -0.01127816 -0.00549997 0.00870000 104 | v -0.01449997 0.00227816 0.00870000 105 | v -0.01016140 0.00583874 0.00870000 106 | v -0.01483874 -0.00116140 0.00870000 107 | v 0.08828635 -0.01149933 -0.00140000 108 | v 0.09008930 -0.00661135 -0.00140000 109 | v 0.09082653 -0.00686503 -0.00140000 110 | v 0.09160458 -0.00681511 -0.00140000 111 | v 0.09081091 -0.01212184 -0.00140000 112 | v 0.08952730 -0.01189714 -0.00140000 113 | v 0.09973910 -0.00863060 -0.00140000 114 | v 0.09862510 -0.00963232 -0.00140000 115 | v 0.08831395 0.01201279 -0.00140000 116 | v 0.09054157 0.01229563 -0.00140000 117 | v 0.08811213 0.00621885 -0.00140000 118 | v 0.09754203 0.00925776 -0.00140000 119 | v 0.09722724 0.00879673 -0.00140000 120 | v 0.09673690 0.00852985 -0.00140000 121 | v 0.07838438 0.00635356 -0.00140000 122 | v 0.07982329 0.00684754 -0.00140000 123 | v 0.08665625 0.00454476 -0.00140000 124 | v 0.08118447 0.00752699 -0.00140000 125 | v 0.08690175 0.00528475 -0.00140000 126 | v 0.08244414 0.00838004 -0.00140000 127 | v 0.08741336 0.00587305 -0.00140000 128 | v 0.10072310 -0.00750089 -0.00140000 129 | v 0.10022934 -0.00450411 -0.00140000 130 | v 0.10156249 -0.00625997 -0.00140000 131 | v 0.08358031 0.00939178 -0.00140000 132 | v 0.08497963 0.01056486 -0.00140000 133 | v 0.08657707 0.01144936 -0.00140000 134 | v 0.09739759 -0.01049122 -0.00140000 135 | v 0.09607480 -0.01119456 -0.00140000 136 | v 0.09467632 -0.01173190 -0.00140000 137 | v 0.09701351 0.01071704 -0.00140000 138 | v 0.09742196 0.01033650 -0.00140000 139 | v 0.09761206 0.00981161 -0.00140000 140 | v 0.08711133 -0.01093587 -0.00140000 141 | v 0.08602421 -0.01021730 -0.00140000 142 | v 0.08950669 -0.00609325 -0.00140000 143 | v 0.08504534 -0.00935708 -0.00140000 144 | v 0.08847292 -0.00452269 -0.00140000 145 | v 0.08329741 -0.00791222 -0.00140000 146 | v 0.08764831 -0.00283290 -0.00140000 147 | v 0.08704639 -0.00105159 -0.00140000 148 | v 0.09340975 -0.01203844 -0.00140000 149 | v 0.09211319 -0.01216924 -0.00140000 150 | v 0.09496502 0.01163678 -0.00140000 151 | v 0.09278348 0.01216889 -0.00140000 152 | v 0.08667701 0.00079202 -0.00140000 153 | v 0.08130010 -0.00683814 -0.00140000 154 | v 0.08654626 0.00266773 -0.00140000 155 | v -0.00900000 0.00595313 -0.00140000 156 | v 0.07537487 0.00595313 -0.00140000 157 | v 0.07687419 -0.00595313 -0.00140000 158 | v 0.07689289 0.00605367 -0.00140000 159 | v 0.07913095 -0.00617655 -0.00140000 160 | v 0.10162813 -0.00517657 -0.00140000 161 | v 0.10172594 -0.00572618 -0.00140000 162 | v 0.10129056 -0.00473194 -0.00140000 163 | v 0.10078742 -0.00449007 -0.00140000 164 | v -0.01016140 0.00583874 -0.00140000 165 | v -0.01449997 -0.00227816 -0.00140000 166 | v -0.01394984 -0.00330738 -0.00140000 167 | v -0.01495313 0.00000000 -0.00140000 168 | v -0.01483874 0.00116140 -0.00140000 169 | v -0.01127816 0.00549997 -0.00140000 170 | v -0.00900000 -0.00595313 -0.00140000 171 | v -0.01016140 -0.00583874 -0.00140000 172 | v -0.01127816 -0.00549997 -0.00140000 173 | v -0.01230738 -0.00494984 -0.00140000 174 | v -0.01320949 -0.00420950 -0.00140000 175 | v -0.01483874 -0.00116140 -0.00140000 176 | v -0.01449997 0.00227816 -0.00140000 177 | v -0.01394984 0.00330738 -0.00140000 178 | v -0.01230738 0.00494984 -0.00140000 179 | v -0.01320949 0.00420950 -0.00140000 180 | v 0.08982355 -0.00585427 0.00830313 181 | v 0.08982355 -0.00585427 -0.00100313 182 | v 0.08881772 -0.00432615 0.00830313 183 | v 0.08881772 -0.00432615 -0.00100313 184 | v 0.08801540 -0.00268204 0.00830313 185 | v 0.08801540 -0.00268204 -0.00100313 186 | v 0.08742974 -0.00094888 0.00830313 187 | v 0.08742974 -0.00094888 -0.00100313 188 | v 0.08707035 0.00084491 0.00830313 189 | v 0.08707035 0.00084491 -0.00100313 190 | v 0.08694312 0.00266992 0.00830313 191 | v 0.08694312 0.00266992 -0.00100313 192 | v 0.08705014 0.00449623 0.00830313 193 | v 0.08705014 0.00449623 -0.00100313 194 | v 0.08821485 0.00583550 0.00830313 195 | v 0.08821485 0.00583550 -0.00100313 196 | v 0.09683962 0.00814650 0.00830313 197 | v 0.09683962 0.00814650 -0.00100313 198 | v 0.10012662 -0.00412076 0.00830313 199 | v 0.10012662 -0.00412076 -0.00100313 200 | v 0.09150185 -0.00643176 0.00830313 201 | v 0.09150185 -0.00643176 -0.00100313 202 | v 0.09347650 -0.01242966 -0.00100313 203 | v 0.09347650 -0.01242966 0.00830313 204 | v 0.09212592 -0.01256591 -0.00100313 205 | v 0.09212592 -0.01256591 0.00830313 206 | v 0.09076938 -0.01251654 -0.00100313 207 | v 0.09076938 -0.01251654 0.00830313 208 | v 0.08943228 -0.01228247 -0.00100313 209 | v 0.08943228 -0.01228247 0.00830313 210 | v 0.08813964 -0.01186809 -0.00100313 211 | v 0.08813964 -0.01186809 0.00830313 212 | v 0.08691565 -0.01128115 -0.00100313 213 | v 0.08691565 -0.01128115 0.00830313 214 | v 0.08578323 -0.01053264 -0.00100313 215 | v 0.08578323 -0.01053264 0.00830313 216 | v 0.08476358 -0.00963657 -0.00100313 217 | v 0.08476358 -0.00963657 0.00830313 218 | v 0.08686021 0.01199254 0.00830313 219 | v 0.08641899 0.01181340 -0.00100313 220 | v 0.08556486 0.01139268 0.00830313 221 | v 0.08556486 0.01139268 -0.00100313 222 | v 0.08436899 0.01061315 0.00830313 223 | v 0.08475498 0.01089204 -0.00100313 224 | v 0.08329737 0.00967008 0.00830313 225 | v 0.08329737 0.00967008 -0.00100313 226 | v 0.07537487 0.00635000 0.00830313 227 | v 0.07537487 0.00635000 -0.00100313 228 | v -0.00900000 0.00635000 0.00830313 229 | v -0.00900000 0.00635000 -0.00100313 230 | v -0.01023882 0.00622799 0.00830313 231 | v -0.01023882 0.00622799 -0.00100313 232 | v -0.01143004 0.00586664 0.00830313 233 | v -0.01143004 0.00586664 -0.00100313 234 | v -0.01252787 0.00527983 0.00830313 235 | v -0.01252787 0.00527983 -0.00100313 236 | v -0.01349013 0.00449013 0.00830313 237 | v -0.01349013 0.00449013 -0.00100313 238 | v -0.01427983 0.00352787 0.00830313 239 | v -0.01427983 0.00352787 -0.00100313 240 | v -0.01486663 0.00243004 0.00830313 241 | v -0.01486663 0.00243004 -0.00100313 242 | v -0.01522799 0.00123882 0.00830313 243 | v -0.01522799 0.00123882 -0.00100313 244 | v -0.01535000 0.00000000 0.00830313 245 | v -0.01535000 0.00000000 -0.00100313 246 | v -0.01522799 -0.00123882 0.00830313 247 | v -0.01522799 -0.00123882 -0.00100313 248 | v -0.01486663 -0.00243004 0.00830313 249 | v -0.01486663 -0.00243004 -0.00100313 250 | v -0.01427983 -0.00352787 0.00830313 251 | v -0.01427983 -0.00352787 -0.00100313 252 | v -0.01349013 -0.00449013 0.00830313 253 | v -0.01349013 -0.00449013 -0.00100313 254 | v -0.01252787 -0.00527983 0.00830313 255 | v -0.01252787 -0.00527983 -0.00100313 256 | v -0.01143004 -0.00586664 0.00830313 257 | v -0.01143004 -0.00586664 -0.00100313 258 | v -0.01023882 -0.00622799 0.00830313 259 | v -0.01023882 -0.00622799 -0.00100313 260 | v -0.00900000 -0.00635000 0.00830313 261 | v -0.00900000 -0.00635000 -0.00100313 262 | v 0.07687419 -0.00635000 0.00830313 263 | v 0.07687419 -0.00635000 -0.00100313 264 | v 0.08098414 0.00786960 -0.00100313 265 | v 0.08160588 0.00826130 0.00830313 266 | v 0.08220038 0.00869323 -0.00100313 267 | v 0.07684054 0.00644708 -0.00100313 268 | v 0.07756537 0.00656803 0.00830313 269 | v 0.07828060 0.00673662 -0.00100313 270 | v 0.07828060 0.00673662 0.00830313 271 | v 0.07966990 0.00721358 -0.00100313 272 | v 0.07966990 0.00721358 0.00830313 273 | v 0.08098414 0.00786960 0.00830313 274 | v 0.07833207 -0.00644605 0.00830313 275 | v 0.07905313 -0.00656572 -0.00100313 276 | v 0.07976476 -0.00673253 0.00830313 277 | v 0.07976476 -0.00673253 -0.00100313 278 | v 0.08114747 -0.00720450 0.00830313 279 | v 0.08114747 -0.00720450 -0.00100313 280 | v 0.08245632 -0.00785379 0.00830313 281 | v 0.08245632 -0.00785379 -0.00100313 282 | v 0.08307592 -0.00824154 -0.00100313 283 | v 0.08366868 -0.00866918 0.00830313 284 | v 0.10212209 -0.00575019 -0.00100313 285 | v 0.10212209 -0.00575019 0.00830313 286 | v 0.10199168 -0.00501736 -0.00100313 287 | v 0.10199168 -0.00501736 0.00830313 288 | v 0.10154157 -0.00442454 -0.00100313 289 | v 0.10154157 -0.00442454 0.00830313 290 | v 0.10087073 -0.00410203 -0.00100313 291 | v 0.10087073 -0.00410203 0.00830313 292 | v 0.09087941 -0.00647169 0.00830313 293 | v 0.09087941 -0.00647169 -0.00100313 294 | v 0.09028963 -0.00626875 0.00830313 295 | v 0.09028963 -0.00626875 -0.00100313 296 | v 0.08724654 0.00508822 0.00830313 297 | v 0.08724654 0.00508822 -0.00100313 298 | v 0.08765583 0.00555886 0.00830313 299 | v 0.08765583 0.00555886 -0.00100313 300 | v 0.09749339 0.00850233 -0.00100313 301 | v 0.09749339 0.00850233 0.00830313 302 | v 0.09791311 0.00911705 -0.00100313 303 | v 0.09791311 0.00911705 0.00830313 304 | v 0.09800651 0.00985551 -0.00100313 305 | v 0.09800651 0.00985551 0.00830313 306 | v 0.09775304 0.01055536 -0.00100313 307 | v 0.09775304 0.01055536 0.00830313 308 | v 0.08693478 0.00451044 0.00858376 309 | v 0.08674469 0.00266882 0.00864683 310 | v 0.08702016 0.00449992 0.00845500 311 | v 0.08688995 0.00266963 0.00850156 312 | v 0.08966513 -0.00597376 0.00864683 313 | v 0.08864532 -0.00442442 0.00864683 314 | v 0.08978110 -0.00588629 0.00850156 315 | v 0.08877152 -0.00435248 0.00850156 316 | v 0.08796622 -0.00270225 0.00850156 317 | v 0.08737838 -0.00096264 0.00850156 318 | v 0.08701765 0.00083783 0.00850156 319 | v 0.08783185 -0.00275747 0.00864683 320 | v 0.08723806 -0.00100024 0.00864683 321 | v 0.08687369 0.00081847 0.00864683 322 | v 0.09151561 -0.00648312 0.00850156 323 | v 0.09087232 -0.00652439 0.00850156 324 | v 0.09155321 -0.00662344 0.00864683 325 | v 0.09085297 -0.00666836 0.00864683 326 | v 0.09026279 -0.00631465 0.00850156 327 | v 0.09018947 -0.00644005 0.00864683 328 | v 0.08816349 0.00602717 0.00864683 329 | v 0.08750615 0.00575282 0.00866979 330 | v 0.08820109 0.00588686 0.00850156 331 | v 0.08758481 0.00565088 0.00858376 332 | v 0.08722029 0.00510318 0.00845500 333 | v 0.08763738 0.00558278 0.00845500 334 | v 0.08714555 0.00514578 0.00858376 335 | v 0.08703369 0.00520954 0.00866979 336 | v 0.10019004 -0.00435741 0.00866979 337 | v 0.10015671 -0.00423304 0.00858376 338 | v 0.10013445 -0.00414994 0.00845500 339 | v 0.09678827 0.00833817 0.00864683 340 | v 0.09682586 0.00819786 0.00850156 341 | v 0.10081931 -0.00434157 0.00866979 342 | v 0.10187755 -0.00573537 0.00866979 343 | v 0.10176725 -0.00511564 0.00866979 344 | v 0.10138661 -0.00461430 0.00866979 345 | v 0.10084633 -0.00421569 0.00858376 346 | v 0.10146805 -0.00451457 0.00858376 347 | v 0.10188520 -0.00506399 0.00858376 348 | v 0.10200607 -0.00574316 0.00858376 349 | v 0.10086439 -0.00413157 0.00845500 350 | v 0.10169323 -0.00633725 0.00866979 351 | v 0.10173331 -0.00636094 0.00864683 352 | v 0.10185837 -0.00643485 0.00850156 353 | v 0.10152247 -0.00444794 0.00845500 354 | v 0.10196400 -0.00502948 0.00845500 355 | v 0.10209194 -0.00574836 0.00845500 356 | v 0.09718231 0.01101643 0.00850156 357 | v 0.09770868 0.01052604 0.00850156 358 | v 0.09711096 0.01088989 0.00864683 359 | v 0.09758749 0.01044593 0.00864683 360 | v 0.09780929 0.00983356 0.00864683 361 | v 0.09772757 0.00918741 0.00864683 362 | v 0.09736032 0.00864953 0.00864683 363 | v 0.09770868 0.01052604 0.00850156 364 | v 0.09795366 0.00984963 0.00850156 365 | v 0.09786340 0.00913590 0.00850156 366 | v 0.09745773 0.00854178 0.00850156 367 | v 0.10088038 -0.00762188 0.00864683 368 | v 0.10099553 -0.00771044 0.00850156 369 | v 0.09988053 -0.00876980 0.00864683 370 | v 0.09874855 -0.00978768 0.00864683 371 | v 0.09750125 -0.01066043 0.00864683 372 | v 0.09615712 -0.01137511 0.00864683 373 | v 0.09473608 -0.01192112 0.00864683 374 | v 0.09998405 -0.00887170 0.00850156 375 | v 0.09883891 -0.00990141 0.00850156 376 | v 0.09757713 -0.01078431 0.00850156 377 | v 0.09621738 -0.01150729 0.00850156 378 | v 0.09477983 -0.01205964 0.00850156 379 | v 0.08823972 0.01234838 0.00850156 380 | v 0.08976085 0.01258880 0.00850156 381 | v 0.08827110 0.01220654 0.00864683 382 | v 0.08977475 0.01244420 0.00864683 383 | v 0.09129614 0.01249733 0.00864683 384 | v 0.09281271 0.01236516 0.00864683 385 | v 0.09430197 0.01204963 0.00864683 386 | v 0.09574184 0.01155543 0.00864683 387 | v 0.09129992 0.01264255 0.00850156 388 | v 0.09283412 0.01250884 0.00850156 389 | v 0.09434068 0.01218965 0.00850156 390 | v 0.09579728 0.01168971 0.00850156 391 | v 0.09346756 -0.01237725 0.00850156 392 | v 0.09344312 -0.01223405 0.00864683 393 | v 0.09211956 -0.01236757 0.00864683 394 | v 0.09079015 -0.01231919 0.00864683 395 | v 0.08947979 -0.01208980 0.00864683 396 | v 0.08821300 -0.01168371 0.00864683 397 | v 0.08701349 -0.01110851 0.00864683 398 | v 0.08590372 -0.01037497 0.00864683 399 | v 0.08490446 -0.00949682 0.00864683 400 | v 0.09212422 -0.01251276 0.00850156 401 | v 0.09077495 -0.01246366 0.00850156 402 | v 0.08944501 -0.01223084 0.00850156 403 | v 0.08815929 -0.01181868 0.00850156 404 | v 0.08694187 -0.01123489 0.00850156 405 | v 0.08581552 -0.01049039 0.00850156 406 | v 0.08480133 -0.00959913 0.00850156 407 | v 0.08338024 0.00958857 0.00858376 408 | v 0.08448903 0.01045514 0.00864683 409 | v 0.08331890 0.00964890 0.00845500 410 | v 0.08440115 0.01057081 0.00850156 411 | v 0.08566098 0.01121908 0.00864683 412 | v 0.08693043 0.01180694 0.00864683 413 | v 0.08559062 0.01134617 0.00850156 414 | v 0.08687903 0.01194281 0.00850156 415 | v 0.07687419 -0.00615156 0.00864683 416 | v 0.07835811 -0.00624933 0.00864683 417 | v 0.07687419 -0.00629683 0.00850156 418 | v 0.07833905 -0.00639334 0.00850156 419 | v 0.07977859 -0.00668119 0.00850156 420 | v 0.08116792 -0.00715541 0.00850156 421 | v 0.08248303 -0.00780781 0.00850156 422 | v 0.08370118 -0.00862710 0.00850156 423 | v 0.07981638 -0.00654092 0.00864683 424 | v 0.08122378 -0.00702132 0.00864683 425 | v 0.08255601 -0.00768220 0.00864683 426 | v 0.08379002 -0.00851215 0.00864683 427 | v 0.07537487 0.00615156 0.00864683 428 | v 0.07761365 0.00632784 0.00866979 429 | v 0.07537487 0.00629683 0.00850156 430 | v 0.07758827 0.00645407 0.00858376 431 | v 0.08162283 0.00823628 0.00845500 432 | v 0.07757132 0.00653842 0.00845500 433 | v 0.07968157 0.00718571 0.00845500 434 | v 0.08167107 0.00816505 0.00858376 435 | v 0.07971482 0.00710637 0.00858376 436 | v 0.08174326 0.00805844 0.00866979 437 | v 0.07976458 0.00698762 0.00866979 438 | v -0.00900000 -0.00615156 0.00864683 439 | v -0.00900000 -0.00629683 0.00850156 440 | v -0.00900000 0.00615156 0.00864683 441 | v -0.00900000 0.00629683 0.00850156 442 | v -0.01022845 -0.00617584 0.00850156 443 | v -0.01020011 -0.00603336 0.00864683 444 | v -0.01135410 -0.00568330 0.00864683 445 | v -0.01241762 -0.00511484 0.00864683 446 | v -0.01334981 -0.00434981 0.00864683 447 | v -0.01411484 -0.00341763 0.00864683 448 | v -0.01468330 -0.00235410 0.00864683 449 | v -0.01503336 -0.00120011 0.00864683 450 | v -0.01515156 0.00000000 0.00864683 451 | v -0.01503336 0.00120011 0.00864683 452 | v -0.01468330 0.00235410 0.00864683 453 | v -0.01411484 0.00341763 0.00864683 454 | v -0.01334981 0.00434981 0.00864683 455 | v -0.01241762 0.00511484 0.00864683 456 | v -0.01135410 0.00568330 0.00864683 457 | v -0.01020011 0.00603336 0.00864683 458 | v -0.01140969 -0.00581751 0.00850156 459 | v -0.01249833 -0.00523562 0.00850156 460 | v -0.01345253 -0.00445253 0.00850156 461 | v -0.01423562 -0.00349833 0.00850156 462 | v -0.01481751 -0.00240969 0.00850156 463 | v -0.01517584 -0.00122845 0.00850156 464 | v -0.01529683 0.00000000 0.00850156 465 | v -0.01517584 0.00122845 0.00850156 466 | v -0.01481751 0.00240969 0.00850156 467 | v -0.01423562 0.00349833 0.00850156 468 | v -0.01345253 0.00445253 0.00850156 469 | v -0.01249833 0.00523562 0.00850156 470 | v -0.01140969 0.00581751 0.00850156 471 | v -0.01022845 0.00617584 0.00850156 472 | v 0.09678827 0.00833817 -0.00134683 473 | v 0.08816349 0.00602717 -0.00134683 474 | v 0.09682586 0.00819786 -0.00120156 475 | v 0.08820109 0.00588686 -0.00120156 476 | v 0.09683962 0.00814650 -0.00100313 477 | v 0.08685319 0.00452049 -0.00134683 478 | v 0.08707414 0.00518649 -0.00134683 479 | v 0.08699737 0.00450273 -0.00120156 480 | v 0.08720034 0.00511455 -0.00120156 481 | v 0.08753460 0.00571596 -0.00134683 482 | v 0.08762335 0.00560095 -0.00120156 483 | v 0.09719359 0.01103643 -0.00115500 484 | v 0.09715134 0.01096149 -0.00128376 485 | v 0.09770868 0.01052604 -0.00120156 486 | v 0.09795366 0.00984963 -0.00120156 487 | v 0.09786340 0.00913590 -0.00120156 488 | v 0.09745773 0.00854178 -0.00120156 489 | v 0.09711096 0.01088989 -0.00134683 490 | v 0.09758749 0.01044593 -0.00134683 491 | v 0.09780929 0.00983356 -0.00134683 492 | v 0.09772757 0.00918741 -0.00134683 493 | v 0.09736032 0.00864953 -0.00134683 494 | v 0.08978110 -0.00588629 -0.00120156 495 | v 0.08877152 -0.00435248 -0.00120156 496 | v 0.08966513 -0.00597376 -0.00134683 497 | v 0.08864532 -0.00442442 -0.00134683 498 | v 0.08783185 -0.00275747 -0.00134683 499 | v 0.08723806 -0.00100024 -0.00134683 500 | v 0.08687369 0.00081847 -0.00134683 501 | v 0.08674469 0.00266882 -0.00134683 502 | v 0.08796622 -0.00270225 -0.00120156 503 | v 0.08737838 -0.00096264 -0.00120156 504 | v 0.08701765 0.00083783 -0.00120156 505 | v 0.08688995 0.00266963 -0.00120156 506 | v 0.09505612 0.01190222 -0.00128376 507 | v 0.08828116 0.01216108 -0.00136979 508 | v 0.08825335 0.01228680 -0.00128376 509 | v 0.09053176 0.01257609 -0.00128376 510 | v 0.08823477 0.01237080 -0.00115500 511 | v 0.09052877 0.01266207 -0.00115500 512 | v 0.09508405 0.01198359 -0.00115500 513 | v 0.09283750 0.01253155 -0.00115500 514 | v 0.09282482 0.01244646 -0.00128376 515 | v 0.09053626 0.01244741 -0.00136979 516 | v 0.09280586 0.01231911 -0.00136979 517 | v 0.09501432 0.01178043 -0.00136979 518 | v 0.09155321 -0.00662344 -0.00134683 519 | v 0.09085297 -0.00666836 -0.00134683 520 | v 0.09087232 -0.00652439 -0.00120156 521 | v 0.09151561 -0.00648312 -0.00120156 522 | v 0.09018947 -0.00644005 -0.00134683 523 | v 0.09026279 -0.00631465 -0.00120156 524 | v 0.08643103 0.01178569 -0.00115500 525 | v 0.08331890 0.00964890 -0.00115500 526 | v 0.08477208 0.01086713 -0.00115500 527 | v 0.08482078 0.01079621 -0.00128376 528 | v 0.08489366 0.01069006 -0.00136979 529 | v 0.08343883 0.00953093 -0.00134683 530 | v 0.08338024 0.00958857 -0.00128376 531 | v 0.08646529 0.01170677 -0.00128376 532 | v 0.08651658 0.01158867 -0.00136979 533 | v 0.10019004 -0.00435741 -0.00136979 534 | v 0.10015671 -0.00423304 -0.00128376 535 | v 0.10013445 -0.00414994 -0.00115500 536 | v 0.08223304 0.00865127 -0.00120156 537 | v 0.08232227 0.00853664 -0.00134683 538 | v 0.08108430 0.00769830 -0.00134683 539 | v 0.07974659 0.00703056 -0.00134683 540 | v 0.07833249 0.00654509 -0.00134683 541 | v 0.07686672 0.00625038 -0.00134683 542 | v 0.07537487 0.00615156 -0.00134683 543 | v 0.08101099 0.00782370 -0.00120156 544 | v 0.07969044 0.00716454 -0.00120156 545 | v 0.07829451 0.00668530 -0.00120156 546 | v 0.07684755 0.00639437 -0.00120156 547 | v 0.07537487 0.00629683 -0.00120156 548 | v 0.10081931 -0.00434157 -0.00136979 549 | v 0.10084633 -0.00421569 -0.00128376 550 | v 0.10086439 -0.00413157 -0.00115500 551 | v 0.10209194 -0.00574836 -0.00115500 552 | v 0.10196400 -0.00502948 -0.00115500 553 | v 0.10152247 -0.00444794 -0.00115500 554 | v 0.10146805 -0.00451457 -0.00128376 555 | v 0.10188520 -0.00506399 -0.00128376 556 | v 0.10200607 -0.00574316 -0.00128376 557 | v 0.10187813 -0.00644653 -0.00115500 558 | v 0.10185837 -0.00643485 -0.00120156 559 | v 0.10180407 -0.00640276 -0.00128376 560 | v 0.10138661 -0.00461430 -0.00136979 561 | v 0.10176725 -0.00511564 -0.00136979 562 | v 0.10187755 -0.00573537 -0.00136979 563 | v 0.10169323 -0.00633725 -0.00136979 564 | v -0.00900000 0.00615156 -0.00134683 565 | v -0.00900000 0.00629683 -0.00120156 566 | v 0.10099553 -0.00771044 -0.00120156 567 | v 0.10088038 -0.00762188 -0.00134683 568 | v 0.09998405 -0.00887170 -0.00120156 569 | v 0.09883891 -0.00990141 -0.00120156 570 | v 0.09757713 -0.01078431 -0.00120156 571 | v 0.09621738 -0.01150729 -0.00120156 572 | v 0.09477983 -0.01205964 -0.00120156 573 | v 0.09988053 -0.00876980 -0.00134683 574 | v 0.09874855 -0.00978768 -0.00134683 575 | v 0.09750125 -0.01066043 -0.00134683 576 | v 0.09615712 -0.01137511 -0.00134683 577 | v 0.09473608 -0.01192112 -0.00134683 578 | v -0.00900000 -0.00615156 -0.00134683 579 | v -0.01020011 -0.00603336 -0.00134683 580 | v -0.00900000 -0.00629683 -0.00120156 581 | v -0.01022845 -0.00617584 -0.00120156 582 | v -0.01140969 -0.00581751 -0.00120156 583 | v -0.01249833 -0.00523562 -0.00120156 584 | v -0.01345253 -0.00445253 -0.00120156 585 | v -0.01423562 -0.00349833 -0.00120156 586 | v -0.01481751 -0.00240969 -0.00120156 587 | v -0.01517584 -0.00122845 -0.00120156 588 | v -0.01529683 0.00000000 -0.00120156 589 | v -0.01517584 0.00122845 -0.00120156 590 | v -0.01481751 0.00240969 -0.00120156 591 | v -0.01423562 0.00349833 -0.00120156 592 | v -0.01345253 0.00445253 -0.00120156 593 | v -0.01249833 0.00523562 -0.00120156 594 | v -0.01140969 0.00581751 -0.00120156 595 | v -0.01022845 0.00617584 -0.00120156 596 | v -0.01135410 -0.00568330 -0.00134683 597 | v -0.01241762 -0.00511484 -0.00134683 598 | v -0.01334981 -0.00434981 -0.00134683 599 | v -0.01411484 -0.00341763 -0.00134683 600 | v -0.01468330 -0.00235410 -0.00134683 601 | v -0.01503336 -0.00120011 -0.00134683 602 | v -0.01515156 0.00000000 -0.00134683 603 | v -0.01503336 0.00120011 -0.00134683 604 | v -0.01468330 0.00235410 -0.00134683 605 | v -0.01411484 0.00341763 -0.00134683 606 | v -0.01334981 0.00434981 -0.00134683 607 | v -0.01241762 0.00511484 -0.00134683 608 | v -0.01135410 0.00568330 -0.00134683 609 | v -0.01020011 0.00603336 -0.00134683 610 | v 0.09344312 -0.01223405 -0.00134683 611 | v 0.09346756 -0.01237725 -0.00120156 612 | v 0.09212422 -0.01251276 -0.00120156 613 | v 0.09077495 -0.01246366 -0.00120156 614 | v 0.08944501 -0.01223084 -0.00120156 615 | v 0.08815929 -0.01181868 -0.00120156 616 | v 0.08694187 -0.01123489 -0.00120156 617 | v 0.08581552 -0.01049039 -0.00120156 618 | v 0.08478503 -0.00961530 -0.00115500 619 | v 0.08484611 -0.00955471 -0.00128376 620 | v 0.09211956 -0.01236757 -0.00134683 621 | v 0.09079015 -0.01231919 -0.00134683 622 | v 0.08947979 -0.01208980 -0.00134683 623 | v 0.08821300 -0.01168371 -0.00134683 624 | v 0.08701349 -0.01110851 -0.00134683 625 | v 0.08590372 -0.01037497 -0.00134683 626 | v 0.08490446 -0.00949682 -0.00134683 627 | v 0.07687419 -0.00610500 -0.00136979 628 | v 0.07687419 -0.00623376 -0.00128376 629 | v 0.07687419 -0.00629683 -0.00120156 630 | v 0.08115909 -0.00717661 -0.00115500 631 | v 0.07905906 -0.00653609 -0.00115500 632 | v 0.07910117 -0.00632548 -0.00136979 633 | v 0.07907592 -0.00645173 -0.00128376 634 | v 0.08124169 -0.00697834 -0.00136979 635 | v 0.08119218 -0.00709719 -0.00128376 636 | v 0.08309278 -0.00821647 -0.00115500 637 | v 0.08314079 -0.00814508 -0.00128376 638 | v 0.08321265 -0.00803824 -0.00136979 639 | f 1 2 3 640 | f 3 2 4 641 | f 3 4 5 642 | f 5 4 6 643 | f 5 6 7 644 | f 7 6 8 645 | f 7 8 9 646 | f 9 8 10 647 | f 9 10 11 648 | f 11 10 12 649 | f 11 12 13 650 | f 13 12 14 651 | f 15 16 17 652 | f 17 16 18 653 | f 17 18 19 654 | f 19 18 20 655 | f 19 20 21 656 | f 21 20 22 657 | f 21 22 23 658 | f 23 22 24 659 | f 23 24 25 660 | f 25 24 26 661 | f 25 26 27 662 | f 27 26 28 663 | f 29 30 31 664 | f 32 33 34 665 | f 35 36 37 666 | f 38 39 40 667 | f 41 42 43 668 | f 43 44 41 669 | f 41 44 45 670 | f 41 45 46 671 | f 31 47 29 672 | f 29 47 48 673 | f 29 48 34 674 | f 34 48 49 675 | f 34 49 32 676 | f 33 50 34 677 | f 34 50 51 678 | f 34 51 52 679 | f 40 39 53 680 | f 54 55 40 681 | f 40 55 56 682 | f 40 56 57 683 | f 42 41 58 684 | f 58 41 59 685 | f 58 59 60 686 | f 46 61 41 687 | f 41 61 62 688 | f 41 62 63 689 | f 63 62 64 690 | f 63 64 54 691 | f 54 64 65 692 | f 54 65 55 693 | f 66 67 68 694 | f 68 35 66 695 | f 66 35 37 696 | f 66 37 69 697 | f 69 37 34 698 | f 69 34 70 699 | f 70 34 52 700 | f 71 72 73 701 | f 73 72 74 702 | f 73 74 30 703 | f 30 74 75 704 | f 30 75 31 705 | f 38 40 76 706 | f 76 40 57 707 | f 76 57 77 708 | f 77 57 78 709 | f 77 78 79 710 | f 79 78 80 711 | f 79 80 81 712 | f 73 82 71 713 | f 71 82 83 714 | f 71 83 84 715 | f 84 83 85 716 | f 84 85 39 717 | f 39 85 86 718 | f 39 86 53 719 | f 87 88 89 720 | f 89 88 60 721 | f 89 60 90 722 | f 90 60 59 723 | f 91 80 92 724 | f 92 80 93 725 | f 94 95 96 726 | f 97 98 99 727 | f 91 100 80 728 | f 80 100 101 729 | f 80 101 81 730 | f 94 96 97 731 | f 97 96 102 732 | f 97 102 98 733 | f 80 103 93 734 | f 93 103 97 735 | f 93 97 104 736 | f 104 97 99 737 | f 105 106 107 738 | f 108 109 107 739 | f 107 109 110 740 | f 107 110 105 741 | f 108 111 112 742 | f 113 114 115 743 | f 116 117 118 744 | f 119 120 121 745 | f 121 120 122 746 | f 121 122 123 747 | f 123 122 124 748 | f 123 124 125 749 | f 111 108 126 750 | f 126 108 127 751 | f 126 127 128 752 | f 124 129 125 753 | f 125 129 130 754 | f 125 130 115 755 | f 115 130 131 756 | f 115 131 113 757 | f 112 132 108 758 | f 108 132 133 759 | f 108 133 134 760 | f 135 136 137 761 | f 105 138 106 762 | f 106 138 139 763 | f 106 139 140 764 | f 140 139 141 765 | f 140 141 142 766 | f 142 141 143 767 | f 142 143 144 768 | f 144 143 145 769 | f 134 146 108 770 | f 108 146 147 771 | f 108 147 109 772 | f 137 116 135 773 | f 135 116 118 774 | f 135 118 148 775 | f 148 118 115 776 | f 148 115 149 777 | f 149 115 114 778 | f 145 143 150 779 | f 150 143 151 780 | f 150 151 152 781 | f 153 154 155 782 | f 155 154 156 783 | f 155 156 157 784 | f 157 156 119 785 | f 157 119 151 786 | f 151 119 121 787 | f 151 121 152 788 | f 158 159 160 789 | f 160 159 128 790 | f 160 128 161 791 | f 161 128 127 792 | f 162 153 163 793 | f 163 153 164 794 | f 165 166 167 795 | f 155 168 153 796 | f 153 168 169 797 | f 153 169 170 798 | f 170 171 153 799 | f 153 171 172 800 | f 153 172 164 801 | f 162 163 167 802 | f 167 163 173 803 | f 167 173 165 804 | f 166 174 167 805 | f 167 174 175 806 | f 167 175 176 807 | f 176 175 177 808 | f 178 179 180 809 | f 180 179 181 810 | f 180 181 182 811 | f 182 181 183 812 | f 182 183 184 813 | f 184 183 185 814 | f 184 185 186 815 | f 186 185 187 816 | f 186 187 188 817 | f 188 187 189 818 | f 188 189 190 819 | f 190 189 191 820 | f 192 193 194 821 | f 194 193 195 822 | f 196 197 198 823 | f 198 197 199 824 | f 2 1 200 825 | f 200 1 201 826 | f 200 201 202 827 | f 202 201 203 828 | f 202 203 204 829 | f 204 203 205 830 | f 204 205 206 831 | f 206 205 207 832 | f 206 207 208 833 | f 208 207 209 834 | f 208 209 210 835 | f 210 209 211 836 | f 210 211 212 837 | f 212 211 213 838 | f 212 213 214 839 | f 214 213 215 840 | f 27 28 216 841 | f 216 28 217 842 | f 216 217 218 843 | f 218 217 219 844 | f 218 219 220 845 | f 220 219 221 846 | f 220 221 222 847 | f 222 221 223 848 | f 224 225 226 849 | f 226 225 227 850 | f 226 227 228 851 | f 228 227 229 852 | f 228 229 230 853 | f 230 229 231 854 | f 230 231 232 855 | f 232 231 233 856 | f 232 233 234 857 | f 234 233 235 858 | f 234 235 236 859 | f 236 235 237 860 | f 236 237 238 861 | f 238 237 239 862 | f 238 239 240 863 | f 240 239 241 864 | f 240 241 242 865 | f 242 241 243 866 | f 242 243 244 867 | f 244 243 245 868 | f 244 245 246 869 | f 246 245 247 870 | f 246 247 248 871 | f 248 247 249 872 | f 248 249 250 873 | f 250 249 251 874 | f 250 251 252 875 | f 252 251 253 876 | f 252 253 254 877 | f 254 253 255 878 | f 254 255 256 879 | f 256 255 257 880 | f 256 257 258 881 | f 258 257 259 882 | f 258 259 260 883 | f 260 259 261 884 | f 262 263 264 885 | f 264 263 222 886 | f 264 222 223 887 | f 225 224 265 888 | f 265 224 266 889 | f 265 266 267 890 | f 267 266 268 891 | f 267 268 269 892 | f 269 268 270 893 | f 269 270 262 894 | f 262 270 271 895 | f 262 271 263 896 | f 260 261 272 897 | f 272 261 273 898 | f 272 273 274 899 | f 274 273 275 900 | f 274 275 276 901 | f 276 275 277 902 | f 276 277 278 903 | f 277 279 278 904 | f 278 279 280 905 | f 278 280 281 906 | f 281 280 214 907 | f 281 214 215 908 | f 13 14 282 909 | f 13 282 283 910 | f 283 282 284 911 | f 283 284 285 912 | f 285 284 286 913 | f 285 286 287 914 | f 287 286 288 915 | f 287 288 289 916 | f 289 288 197 917 | f 289 197 196 918 | f 198 199 290 919 | f 290 199 291 920 | f 290 291 292 921 | f 292 291 293 922 | f 292 293 178 923 | f 178 293 179 924 | f 190 191 294 925 | f 294 191 295 926 | f 294 295 296 927 | f 296 295 297 928 | f 296 297 192 929 | f 192 297 193 930 | f 194 195 298 931 | f 194 298 299 932 | f 299 298 300 933 | f 299 300 301 934 | f 301 300 302 935 | f 301 302 303 936 | f 303 302 304 937 | f 303 304 305 938 | f 305 304 16 939 | f 305 16 15 940 | f 308 306 309 941 | f 73 310 311 942 | f 310 312 313 943 | f 312 178 180 944 | f 312 180 313 945 | f 313 180 182 946 | f 313 182 314 947 | f 314 182 184 948 | f 314 184 315 949 | f 315 184 186 950 | f 315 186 316 951 | f 316 186 188 952 | f 316 188 309 953 | f 309 188 190 954 | f 309 190 308 955 | f 310 313 311 956 | f 311 313 314 957 | f 311 314 317 958 | f 317 314 315 959 | f 317 315 318 960 | f 318 315 316 961 | f 318 316 319 962 | f 319 316 309 963 | f 319 309 307 964 | f 307 309 306 965 | f 73 311 82 966 | f 82 311 317 967 | f 82 317 83 968 | f 83 317 318 969 | f 83 318 85 970 | f 85 318 319 971 | f 85 319 86 972 | f 86 319 307 973 | f 86 307 53 974 | f 53 307 306 975 | f 53 306 40 976 | f 198 290 320 977 | f 320 290 321 978 | f 320 321 322 979 | f 322 321 323 980 | f 322 323 34 981 | f 34 323 29 982 | f 290 292 321 983 | f 321 292 324 984 | f 321 324 323 985 | f 323 324 325 986 | f 323 325 29 987 | f 29 325 30 988 | f 292 178 324 989 | f 324 178 312 990 | f 324 312 325 991 | f 325 312 310 992 | f 325 310 30 993 | f 30 310 73 994 | f 306 308 330 995 | f 308 190 294 996 | f 308 294 330 997 | f 330 294 296 998 | f 330 296 331 999 | f 331 296 192 1000 | f 331 192 328 1001 | f 306 330 332 1002 | f 332 330 331 1003 | f 332 331 329 1004 | f 329 331 328 1005 | f 306 332 333 1006 | f 333 332 329 1007 | f 333 329 327 1008 | f 327 329 328 1009 | f 327 328 326 1010 | f 54 40 306 1011 | f 306 333 54 1012 | f 54 333 327 1013 | f 54 327 63 1014 | f 63 327 326 1015 | f 63 326 41 1016 | f 37 334 34 1017 | f 34 334 322 1018 | f 322 334 335 1019 | f 322 335 320 1020 | f 196 198 336 1021 | f 336 198 320 1022 | f 336 320 335 1023 | f 41 326 59 1024 | f 59 326 337 1025 | f 337 326 328 1026 | f 337 328 338 1027 | f 192 194 328 1028 | f 328 194 338 1029 | f 66 340 67 1030 | f 67 340 341 1031 | f 67 341 68 1032 | f 68 341 342 1033 | f 68 342 35 1034 | f 35 342 339 1035 | f 35 339 36 1036 | f 36 339 334 1037 | f 36 334 37 1038 | f 336 335 343 1039 | f 343 335 334 1040 | f 334 339 343 1041 | f 343 339 342 1042 | f 343 342 344 1043 | f 344 342 341 1044 | f 344 341 345 1045 | f 345 341 340 1046 | f 345 340 346 1047 | f 196 336 347 1048 | f 66 348 340 1049 | f 340 348 349 1050 | f 340 349 346 1051 | f 346 349 350 1052 | f 336 343 347 1053 | f 347 343 344 1054 | f 347 344 351 1055 | f 351 344 345 1056 | f 351 345 352 1057 | f 352 345 346 1058 | f 352 346 353 1059 | f 353 346 350 1060 | f 196 347 289 1061 | f 289 347 351 1062 | f 289 351 287 1063 | f 287 351 352 1064 | f 287 352 285 1065 | f 285 352 353 1066 | f 285 353 283 1067 | f 283 353 350 1068 | f 283 350 13 1069 | f 15 354 355 1070 | f 354 356 357 1071 | f 356 60 88 1072 | f 356 88 357 1073 | f 357 88 87 1074 | f 357 87 358 1075 | f 358 87 89 1076 | f 358 89 359 1077 | f 359 89 90 1078 | f 359 90 360 1079 | f 360 90 59 1080 | f 360 59 337 1081 | f 354 357 361 1082 | f 361 357 358 1083 | f 361 358 362 1084 | f 362 358 359 1085 | f 362 359 363 1086 | f 363 359 360 1087 | f 363 360 364 1088 | f 364 360 337 1089 | f 364 337 338 1090 | f 15 361 305 1091 | f 305 361 362 1092 | f 305 362 303 1093 | f 303 362 363 1094 | f 303 363 301 1095 | f 301 363 364 1096 | f 301 364 299 1097 | f 299 364 338 1098 | f 299 338 194 1099 | f 348 66 69 1100 | f 365 349 348 1101 | f 366 350 349 1102 | f 11 13 350 1103 | f 348 69 365 1104 | f 365 69 70 1105 | f 365 70 367 1106 | f 367 70 52 1107 | f 367 52 368 1108 | f 368 52 51 1109 | f 368 51 369 1110 | f 369 51 50 1111 | f 369 50 370 1112 | f 370 50 33 1113 | f 370 33 371 1114 | f 349 365 366 1115 | f 366 365 367 1116 | f 366 367 372 1117 | f 372 367 368 1118 | f 372 368 373 1119 | f 373 368 369 1120 | f 373 369 374 1121 | f 374 369 370 1122 | f 374 370 375 1123 | f 375 370 371 1124 | f 375 371 376 1125 | f 350 366 11 1126 | f 11 366 372 1127 | f 11 372 9 1128 | f 9 372 373 1129 | f 9 373 7 1130 | f 7 373 374 1131 | f 7 374 5 1132 | f 5 374 375 1133 | f 5 375 3 1134 | f 3 375 376 1135 | f 3 376 1 1136 | f 27 377 378 1137 | f 377 379 380 1138 | f 379 46 45 1139 | f 379 45 380 1140 | f 380 45 44 1141 | f 380 44 381 1142 | f 381 44 43 1143 | f 381 43 382 1144 | f 382 43 42 1145 | f 382 42 383 1146 | f 383 42 58 1147 | f 383 58 384 1148 | f 384 58 60 1149 | f 384 60 356 1150 | f 377 380 378 1151 | f 378 380 381 1152 | f 378 381 385 1153 | f 385 381 382 1154 | f 385 382 386 1155 | f 386 382 383 1156 | f 386 383 387 1157 | f 387 383 384 1158 | f 387 384 388 1159 | f 388 384 356 1160 | f 388 356 354 1161 | f 27 378 25 1162 | f 25 378 385 1163 | f 25 385 23 1164 | f 23 385 386 1165 | f 23 386 21 1166 | f 21 386 387 1167 | f 21 387 19 1168 | f 19 387 388 1169 | f 19 388 17 1170 | f 17 388 354 1171 | f 17 354 15 1172 | f 1 376 389 1173 | f 376 371 390 1174 | f 371 33 32 1175 | f 371 32 390 1176 | f 390 32 49 1177 | f 390 49 391 1178 | f 391 49 48 1179 | f 391 48 392 1180 | f 392 48 47 1181 | f 392 47 393 1182 | f 393 47 31 1183 | f 393 31 394 1184 | f 394 31 75 1185 | f 394 75 395 1186 | f 395 75 74 1187 | f 395 74 396 1188 | f 396 74 72 1189 | f 396 72 397 1190 | f 376 390 389 1191 | f 389 390 391 1192 | f 389 391 398 1193 | f 398 391 392 1194 | f 398 392 399 1195 | f 399 392 393 1196 | f 399 393 400 1197 | f 400 393 394 1198 | f 400 394 401 1199 | f 401 394 395 1200 | f 401 395 402 1201 | f 402 395 396 1202 | f 402 396 403 1203 | f 403 396 397 1204 | f 403 397 404 1205 | f 1 389 201 1206 | f 201 389 398 1207 | f 201 398 203 1208 | f 203 398 399 1209 | f 203 399 205 1210 | f 205 399 400 1211 | f 205 400 207 1212 | f 207 400 401 1213 | f 207 401 209 1214 | f 209 401 402 1215 | f 209 402 211 1216 | f 211 402 403 1217 | f 211 403 213 1218 | f 213 403 404 1219 | f 213 404 215 1220 | f 405 65 64 1221 | f 408 407 405 1222 | f 220 222 407 1223 | f 405 64 406 1224 | f 406 64 62 1225 | f 406 62 409 1226 | f 409 62 61 1227 | f 409 61 410 1228 | f 410 61 46 1229 | f 410 46 379 1230 | f 405 406 408 1231 | f 408 406 409 1232 | f 408 409 411 1233 | f 411 409 410 1234 | f 411 410 412 1235 | f 412 410 379 1236 | f 412 379 377 1237 | f 407 408 220 1238 | f 220 408 411 1239 | f 220 411 218 1240 | f 218 411 412 1241 | f 218 412 216 1242 | f 216 412 377 1243 | f 216 377 27 1244 | f 77 413 414 1245 | f 413 415 416 1246 | f 415 260 272 1247 | f 415 272 416 1248 | f 416 272 274 1249 | f 416 274 417 1250 | f 417 274 276 1251 | f 417 276 418 1252 | f 418 276 278 1253 | f 418 278 419 1254 | f 419 278 281 1255 | f 419 281 420 1256 | f 420 281 215 1257 | f 420 215 404 1258 | f 413 416 414 1259 | f 414 416 417 1260 | f 414 417 421 1261 | f 421 417 418 1262 | f 421 418 422 1263 | f 422 418 419 1264 | f 422 419 423 1265 | f 423 419 420 1266 | f 423 420 424 1267 | f 424 420 404 1268 | f 424 404 397 1269 | f 77 414 76 1270 | f 76 414 421 1271 | f 76 421 38 1272 | f 38 421 422 1273 | f 38 422 39 1274 | f 39 422 423 1275 | f 39 423 84 1276 | f 84 423 424 1277 | f 84 424 71 1278 | f 71 424 397 1279 | f 71 397 72 1280 | f 427 425 428 1281 | f 407 222 263 1282 | f 268 430 431 1283 | f 407 263 429 1284 | f 429 263 271 1285 | f 429 271 431 1286 | f 431 271 270 1287 | f 431 270 268 1288 | f 268 266 430 1289 | f 430 266 224 1290 | f 430 224 427 1291 | f 432 405 407 1292 | f 407 429 432 1293 | f 432 429 431 1294 | f 432 431 433 1295 | f 433 431 430 1296 | f 433 430 428 1297 | f 428 430 427 1298 | f 405 432 434 1299 | f 434 432 433 1300 | f 434 433 435 1301 | f 435 433 428 1302 | f 435 428 426 1303 | f 426 428 425 1304 | f 55 65 405 1305 | f 405 434 55 1306 | f 55 434 435 1307 | f 55 435 56 1308 | f 56 435 426 1309 | f 56 426 57 1310 | f 57 426 425 1311 | f 57 425 78 1312 | f 77 79 436 1313 | f 77 436 413 1314 | f 413 436 437 1315 | f 413 437 415 1316 | f 415 437 258 1317 | f 415 258 260 1318 | f 80 78 425 1319 | f 80 425 438 1320 | f 438 425 439 1321 | f 439 425 427 1322 | f 439 427 226 1323 | f 226 427 224 1324 | f 258 437 440 1325 | f 437 436 441 1326 | f 436 79 81 1327 | f 436 81 441 1328 | f 441 81 101 1329 | f 441 101 442 1330 | f 442 101 100 1331 | f 442 100 443 1332 | f 443 100 91 1333 | f 443 91 444 1334 | f 444 91 92 1335 | f 444 92 445 1336 | f 445 92 93 1337 | f 445 93 446 1338 | f 446 93 104 1339 | f 446 104 447 1340 | f 447 104 99 1341 | f 447 99 448 1342 | f 448 99 98 1343 | f 448 98 449 1344 | f 449 98 102 1345 | f 449 102 450 1346 | f 450 102 96 1347 | f 450 96 451 1348 | f 451 96 95 1349 | f 451 95 452 1350 | f 452 95 94 1351 | f 452 94 453 1352 | f 453 94 97 1353 | f 453 97 454 1354 | f 454 97 103 1355 | f 454 103 455 1356 | f 455 103 80 1357 | f 455 80 438 1358 | f 437 441 440 1359 | f 440 441 442 1360 | f 440 442 456 1361 | f 456 442 443 1362 | f 456 443 457 1363 | f 457 443 444 1364 | f 457 444 458 1365 | f 458 444 445 1366 | f 458 445 459 1367 | f 459 445 446 1368 | f 459 446 460 1369 | f 460 446 447 1370 | f 460 447 461 1371 | f 461 447 448 1372 | f 461 448 462 1373 | f 462 448 449 1374 | f 462 449 463 1375 | f 463 449 450 1376 | f 463 450 464 1377 | f 464 450 451 1378 | f 464 451 465 1379 | f 465 451 452 1380 | f 465 452 466 1381 | f 466 452 453 1382 | f 466 453 467 1383 | f 467 453 454 1384 | f 467 454 468 1385 | f 468 454 455 1386 | f 468 455 469 1387 | f 469 455 438 1388 | f 469 438 439 1389 | f 258 440 256 1390 | f 256 440 456 1391 | f 256 456 254 1392 | f 254 456 457 1393 | f 254 457 252 1394 | f 252 457 458 1395 | f 252 458 250 1396 | f 250 458 459 1397 | f 250 459 248 1398 | f 248 459 460 1399 | f 248 460 246 1400 | f 246 460 461 1401 | f 246 461 244 1402 | f 244 461 462 1403 | f 244 462 242 1404 | f 242 462 463 1405 | f 242 463 240 1406 | f 240 463 464 1407 | f 240 464 238 1408 | f 238 464 465 1409 | f 238 465 236 1410 | f 236 465 466 1411 | f 236 466 234 1412 | f 234 466 467 1413 | f 234 467 232 1414 | f 232 467 468 1415 | f 232 468 230 1416 | f 230 468 469 1417 | f 230 469 228 1418 | f 228 469 439 1419 | f 228 439 226 1420 | f 115 118 470 1421 | f 115 470 471 1422 | f 471 470 472 1423 | f 471 472 473 1424 | f 473 472 474 1425 | f 473 474 193 1426 | f 121 123 475 1427 | f 475 123 476 1428 | f 475 476 477 1429 | f 477 476 478 1430 | f 477 478 191 1431 | f 191 478 295 1432 | f 123 125 476 1433 | f 476 125 479 1434 | f 476 479 478 1435 | f 478 479 480 1436 | f 478 480 295 1437 | f 295 480 297 1438 | f 125 115 479 1439 | f 479 115 471 1440 | f 479 471 480 1441 | f 480 471 473 1442 | f 480 473 297 1443 | f 297 473 193 1444 | f 481 16 304 1445 | f 483 482 481 1446 | f 481 304 483 1447 | f 483 304 302 1448 | f 483 302 484 1449 | f 484 302 300 1450 | f 484 300 485 1451 | f 485 300 298 1452 | f 485 298 486 1453 | f 486 298 474 1454 | f 486 474 472 1455 | f 135 487 488 1456 | f 488 487 482 1457 | f 482 483 488 1458 | f 488 483 484 1459 | f 488 484 489 1460 | f 489 484 485 1461 | f 489 485 490 1462 | f 490 485 486 1463 | f 490 486 491 1464 | f 491 486 472 1465 | f 491 472 470 1466 | f 135 488 136 1467 | f 136 488 489 1468 | f 136 489 137 1469 | f 137 489 490 1470 | f 137 490 116 1471 | f 116 490 491 1472 | f 116 491 117 1473 | f 117 491 470 1474 | f 117 470 118 1475 | f 179 492 493 1476 | f 492 494 495 1477 | f 494 140 142 1478 | f 494 142 495 1479 | f 495 142 144 1480 | f 495 144 496 1481 | f 496 144 145 1482 | f 496 145 497 1483 | f 497 145 150 1484 | f 497 150 498 1485 | f 498 150 152 1486 | f 498 152 499 1487 | f 499 152 121 1488 | f 499 121 475 1489 | f 492 495 493 1490 | f 493 495 496 1491 | f 493 496 500 1492 | f 500 496 497 1493 | f 500 497 501 1494 | f 501 497 498 1495 | f 501 498 502 1496 | f 502 498 499 1497 | f 502 499 503 1498 | f 503 499 475 1499 | f 503 475 477 1500 | f 179 493 181 1501 | f 181 493 500 1502 | f 181 500 183 1503 | f 183 500 501 1504 | f 183 501 185 1505 | f 185 501 502 1506 | f 185 502 187 1507 | f 187 502 503 1508 | f 187 503 189 1509 | f 189 503 477 1510 | f 189 477 191 1511 | f 504 482 487 1512 | f 505 506 507 1513 | f 506 508 509 1514 | f 508 28 26 1515 | f 18 510 20 1516 | f 20 510 511 1517 | f 508 26 509 1518 | f 509 26 24 1519 | f 509 24 511 1520 | f 511 24 22 1521 | f 511 22 20 1522 | f 18 16 510 1523 | f 510 16 481 1524 | f 510 481 482 1525 | f 506 509 507 1526 | f 507 509 511 1527 | f 507 511 512 1528 | f 512 511 510 1529 | f 512 510 504 1530 | f 504 510 482 1531 | f 505 507 513 1532 | f 513 507 512 1533 | f 513 512 514 1534 | f 514 512 504 1535 | f 514 504 515 1536 | f 515 504 487 1537 | f 515 487 135 1538 | f 135 148 515 1539 | f 515 148 149 1540 | f 515 149 514 1541 | f 514 149 114 1542 | f 514 114 513 1543 | f 513 114 113 1544 | f 513 113 505 1545 | f 108 107 516 1546 | f 516 107 517 1547 | f 291 199 518 1548 | f 518 199 519 1549 | f 518 519 517 1550 | f 517 519 516 1551 | f 107 106 517 1552 | f 517 106 520 1553 | f 517 520 518 1554 | f 518 520 521 1555 | f 518 521 291 1556 | f 291 521 293 1557 | f 106 140 520 1558 | f 520 140 494 1559 | f 520 494 521 1560 | f 521 494 492 1561 | f 521 492 293 1562 | f 293 492 179 1563 | f 219 217 522 1564 | f 223 221 523 1565 | f 523 221 524 1566 | f 523 524 525 1567 | f 526 527 525 1568 | f 525 527 528 1569 | f 130 129 526 1570 | f 526 129 527 1571 | f 221 219 524 1572 | f 524 219 522 1573 | f 524 522 525 1574 | f 525 522 529 1575 | f 525 529 526 1576 | f 526 529 530 1577 | f 526 530 130 1578 | f 130 530 131 1579 | f 217 28 522 1580 | f 522 28 508 1581 | f 522 508 529 1582 | f 529 508 506 1583 | f 529 506 530 1584 | f 530 506 505 1585 | f 530 505 131 1586 | f 131 505 113 1587 | f 127 108 516 1588 | f 127 516 531 1589 | f 531 516 532 1590 | f 532 516 519 1591 | f 532 519 533 1592 | f 533 519 199 1593 | f 533 199 197 1594 | f 523 528 534 1595 | f 528 527 535 1596 | f 527 129 124 1597 | f 523 534 223 1598 | f 527 124 535 1599 | f 535 124 122 1600 | f 535 122 536 1601 | f 536 122 120 1602 | f 536 120 537 1603 | f 537 120 119 1604 | f 537 119 538 1605 | f 538 119 156 1606 | f 538 156 539 1607 | f 539 156 154 1608 | f 539 154 540 1609 | f 528 535 534 1610 | f 534 535 536 1611 | f 534 536 541 1612 | f 541 536 537 1613 | f 541 537 542 1614 | f 542 537 538 1615 | f 542 538 543 1616 | f 543 538 539 1617 | f 543 539 544 1618 | f 544 539 540 1619 | f 544 540 545 1620 | f 223 534 264 1621 | f 264 534 541 1622 | f 264 541 262 1623 | f 262 541 542 1624 | f 262 542 269 1625 | f 269 542 543 1626 | f 269 543 267 1627 | f 267 543 544 1628 | f 267 544 265 1629 | f 265 544 545 1630 | f 265 545 225 1631 | f 127 531 546 1632 | f 531 532 547 1633 | f 532 533 548 1634 | f 14 549 282 1635 | f 282 549 550 1636 | f 282 550 284 1637 | f 284 550 551 1638 | f 284 551 286 1639 | f 286 551 548 1640 | f 286 548 288 1641 | f 288 548 533 1642 | f 288 533 197 1643 | f 532 548 547 1644 | f 547 548 551 1645 | f 547 551 552 1646 | f 552 551 550 1647 | f 552 550 553 1648 | f 553 550 549 1649 | f 553 549 554 1650 | f 14 555 549 1651 | f 549 555 556 1652 | f 549 556 554 1653 | f 554 556 557 1654 | f 531 547 546 1655 | f 546 547 552 1656 | f 546 552 558 1657 | f 558 552 553 1658 | f 558 553 559 1659 | f 559 553 554 1660 | f 559 554 560 1661 | f 560 554 557 1662 | f 560 557 561 1663 | f 127 546 161 1664 | f 161 546 558 1665 | f 161 558 160 1666 | f 160 558 559 1667 | f 160 559 158 1668 | f 158 559 560 1669 | f 158 560 159 1670 | f 159 560 561 1671 | f 159 561 128 1672 | f 154 153 562 1673 | f 154 562 540 1674 | f 540 562 563 1675 | f 540 563 545 1676 | f 545 563 227 1677 | f 545 227 225 1678 | f 555 14 12 1679 | f 557 556 564 1680 | f 564 556 555 1681 | f 561 557 565 1682 | f 126 128 561 1683 | f 555 12 564 1684 | f 564 12 10 1685 | f 564 10 566 1686 | f 566 10 8 1687 | f 566 8 567 1688 | f 567 8 6 1689 | f 567 6 568 1690 | f 568 6 4 1691 | f 568 4 569 1692 | f 569 4 2 1693 | f 569 2 570 1694 | f 557 564 565 1695 | f 565 564 566 1696 | f 565 566 571 1697 | f 571 566 567 1698 | f 571 567 572 1699 | f 572 567 568 1700 | f 572 568 573 1701 | f 573 568 569 1702 | f 573 569 574 1703 | f 574 569 570 1704 | f 574 570 575 1705 | f 561 565 126 1706 | f 126 565 571 1707 | f 126 571 111 1708 | f 111 571 572 1709 | f 111 572 112 1710 | f 112 572 573 1711 | f 112 573 132 1712 | f 132 573 574 1713 | f 132 574 133 1714 | f 133 574 575 1715 | f 133 575 134 1716 | f 168 576 577 1717 | f 576 578 579 1718 | f 578 259 257 1719 | f 578 257 579 1720 | f 579 257 255 1721 | f 579 255 580 1722 | f 580 255 253 1723 | f 580 253 581 1724 | f 581 253 251 1725 | f 581 251 582 1726 | f 582 251 249 1727 | f 582 249 583 1728 | f 583 249 247 1729 | f 583 247 584 1730 | f 584 247 245 1731 | f 584 245 585 1732 | f 585 245 243 1733 | f 585 243 586 1734 | f 586 243 241 1735 | f 586 241 587 1736 | f 587 241 239 1737 | f 587 239 588 1738 | f 588 239 237 1739 | f 588 237 589 1740 | f 589 237 235 1741 | f 589 235 590 1742 | f 590 235 233 1743 | f 590 233 591 1744 | f 591 233 231 1745 | f 591 231 592 1746 | f 592 231 229 1747 | f 592 229 593 1748 | f 593 229 227 1749 | f 593 227 563 1750 | f 576 579 577 1751 | f 577 579 580 1752 | f 577 580 594 1753 | f 594 580 581 1754 | f 594 581 595 1755 | f 595 581 582 1756 | f 595 582 596 1757 | f 596 582 583 1758 | f 596 583 597 1759 | f 597 583 584 1760 | f 597 584 598 1761 | f 598 584 585 1762 | f 598 585 599 1763 | f 599 585 586 1764 | f 599 586 600 1765 | f 600 586 587 1766 | f 600 587 601 1767 | f 601 587 588 1768 | f 601 588 602 1769 | f 602 588 589 1770 | f 602 589 603 1771 | f 603 589 590 1772 | f 603 590 604 1773 | f 604 590 591 1774 | f 604 591 605 1775 | f 605 591 592 1776 | f 605 592 606 1777 | f 606 592 593 1778 | f 606 593 607 1779 | f 607 593 563 1780 | f 607 563 562 1781 | f 168 577 169 1782 | f 169 577 594 1783 | f 169 594 170 1784 | f 170 594 595 1785 | f 170 595 171 1786 | f 171 595 596 1787 | f 171 596 172 1788 | f 172 596 597 1789 | f 172 597 164 1790 | f 164 597 598 1791 | f 164 598 163 1792 | f 163 598 599 1793 | f 163 599 173 1794 | f 173 599 600 1795 | f 173 600 165 1796 | f 165 600 601 1797 | f 165 601 166 1798 | f 166 601 602 1799 | f 166 602 174 1800 | f 174 602 603 1801 | f 174 603 175 1802 | f 175 603 604 1803 | f 175 604 177 1804 | f 177 604 605 1805 | f 177 605 176 1806 | f 176 605 606 1807 | f 176 606 167 1808 | f 167 606 607 1809 | f 167 607 162 1810 | f 162 607 562 1811 | f 162 562 153 1812 | f 134 575 608 1813 | f 575 570 609 1814 | f 570 2 200 1815 | f 570 200 609 1816 | f 609 200 202 1817 | f 609 202 610 1818 | f 610 202 204 1819 | f 610 204 611 1820 | f 611 204 206 1821 | f 611 206 612 1822 | f 612 206 208 1823 | f 612 208 613 1824 | f 613 208 210 1825 | f 613 210 614 1826 | f 614 210 212 1827 | f 614 212 615 1828 | f 212 214 616 1829 | f 212 616 615 1830 | f 615 616 617 1831 | f 575 609 608 1832 | f 608 609 610 1833 | f 608 610 618 1834 | f 618 610 611 1835 | f 618 611 619 1836 | f 619 611 612 1837 | f 619 612 620 1838 | f 620 612 613 1839 | f 620 613 621 1840 | f 621 613 614 1841 | f 621 614 622 1842 | f 622 614 615 1843 | f 622 615 623 1844 | f 623 615 617 1845 | f 623 617 624 1846 | f 134 608 146 1847 | f 146 608 618 1848 | f 146 618 147 1849 | f 147 618 619 1850 | f 147 619 109 1851 | f 109 619 620 1852 | f 109 620 110 1853 | f 110 620 621 1854 | f 110 621 105 1855 | f 105 621 622 1856 | f 105 622 138 1857 | f 138 622 623 1858 | f 138 623 139 1859 | f 139 623 624 1860 | f 139 624 141 1861 | f 155 625 168 1862 | f 168 625 576 1863 | f 576 625 626 1864 | f 576 626 578 1865 | f 578 626 627 1866 | f 578 627 259 1867 | f 259 627 261 1868 | f 279 277 628 1869 | f 275 273 629 1870 | f 630 631 626 1871 | f 155 157 625 1872 | f 625 157 630 1873 | f 625 630 626 1874 | f 273 261 629 1875 | f 629 261 627 1876 | f 629 627 631 1877 | f 631 627 626 1878 | f 157 151 630 1879 | f 630 151 632 1880 | f 630 632 631 1881 | f 631 632 633 1882 | f 631 633 629 1883 | f 629 633 628 1884 | f 629 628 275 1885 | f 275 628 277 1886 | f 280 279 634 1887 | f 634 279 628 1888 | f 634 628 635 1889 | f 635 628 633 1890 | f 635 633 636 1891 | f 636 633 632 1892 | f 636 632 143 1893 | f 143 632 151 1894 | f 214 280 616 1895 | f 616 280 634 1896 | f 616 634 617 1897 | f 617 634 635 1898 | f 617 635 636 1899 | f 617 636 624 1900 | f 624 636 143 1901 | f 624 143 141 1902 | 1903 | -------------------------------------------------------------------------------- /tests/probe.obj: -------------------------------------------------------------------------------- 1 | # Exported by SolidWorks 2 | g 3 | v 0.00241494 0.00064659 0.02488309 4 | v 0.00250000 -0.00000000 0.02487469 5 | v 0.00335583 0.00500000 0.02477374 6 | v -0.00057560 0.00243283 0.02499337 7 | v 0.00000000 0.00250000 0.02500000 8 | v -0.00112162 0.00500000 0.02497483 9 | v 0.00125000 0.00216506 0.02496873 10 | v 0.00176747 0.00176806 0.02493744 11 | v 0.00216506 0.00125000 0.02490607 12 | v 0.00064961 0.00241413 0.02499156 13 | v 0.00241495 -0.00064654 0.02488309 14 | v 0.00216506 -0.00125000 0.02490607 15 | v 0.00335583 -0.00500000 0.02477374 16 | v 0.00176750 -0.00176803 0.02493744 17 | v 0.00125000 -0.00216506 0.02496873 18 | v 0.00064777 -0.00241462 0.02499161 19 | v -0.00112162 -0.00500000 0.02497483 20 | v 0.00000000 -0.00250000 0.02500000 21 | v -0.00216506 -0.00125000 0.02490607 22 | v -0.00241474 -0.00064734 0.02488311 23 | v -0.00556302 -0.00500000 0.02437320 24 | v -0.00176774 -0.00176779 0.02493742 25 | v -0.00057709 -0.00243248 0.02499334 26 | v -0.00112162 -0.00223427 0.02497483 27 | v -0.00125003 -0.00216505 0.02496873 28 | v -0.00250000 -0.00000000 0.02487469 29 | v -0.00556302 0.00500000 0.02437320 30 | v -0.00241460 0.00064785 0.02488312 31 | v -0.00216506 0.00125000 0.02490607 32 | v -0.00176805 0.00176749 0.02493740 33 | v -0.00125000 0.00216506 0.02496873 34 | v -0.00112764 0.00223124 0.02497456 35 | v -0.00982563 0.00500000 0.02298820 36 | v -0.00982563 -0.00500000 0.02298820 37 | v -0.01377242 0.00500000 0.02086433 38 | v -0.01377242 -0.00500000 0.02086433 39 | v -0.01727657 0.00500000 0.01806987 40 | v -0.01727657 -0.00500000 0.01806987 41 | v -0.02022543 0.00500000 0.01469463 42 | v -0.02022543 -0.00500000 0.01469463 43 | v -0.02252422 0.00500000 0.01084709 44 | v -0.02252422 -0.00500000 0.01084709 45 | v -0.02409907 0.00500000 0.00665092 46 | v -0.02409907 -0.00500000 0.00665092 47 | v -0.02489936 0.00500000 0.00224098 48 | v -0.02489936 -0.00500000 0.00224098 49 | v -0.02489936 0.00500000 -0.00224098 50 | v -0.02489936 -0.00500000 -0.00224098 51 | v -0.02409907 0.00500000 -0.00665092 52 | v -0.02409907 -0.00500000 -0.00665092 53 | v -0.02252422 0.00500000 -0.01084709 54 | v -0.02252422 -0.00500000 -0.01084709 55 | v -0.02022543 0.00500000 -0.01469463 56 | v -0.02022543 -0.00500000 -0.01469463 57 | v -0.01727657 0.00500000 -0.01806987 58 | v -0.01727657 -0.00500000 -0.01806987 59 | v -0.01377242 0.00500000 -0.02086433 60 | v -0.01377242 -0.00500000 -0.02086433 61 | v -0.00982563 0.00500000 -0.02298820 62 | v -0.00982563 -0.00500000 -0.02298820 63 | v -0.00556302 0.00500000 -0.02437320 64 | v -0.00556302 -0.00500000 -0.02437320 65 | v -0.00112162 0.00500000 -0.02497483 66 | v -0.00112162 -0.00500000 -0.02497483 67 | v 0.00335583 0.00500000 -0.02477374 68 | v 0.00335583 -0.00500000 -0.02477374 69 | v 0.00772543 0.00500000 -0.02377641 70 | v 0.00772543 -0.00500000 -0.02377641 71 | v 0.01184672 0.00500000 -0.02201489 72 | v 0.01184672 -0.00500000 -0.02201489 73 | v 0.01558725 0.00500000 -0.01954579 74 | v 0.01558725 -0.00500000 -0.01954579 75 | v 0.01882679 0.00500000 -0.01644847 76 | v 0.01882679 -0.00500000 -0.01644847 77 | v 0.02146122 0.00500000 -0.01282248 78 | v 0.02146122 -0.00500000 -0.01282248 79 | v 0.02340587 0.00500000 -0.00878437 80 | v 0.02340587 -0.00500000 -0.00878437 81 | v 0.02459824 0.00500000 -0.00446392 82 | v 0.02459824 -0.00500000 -0.00446392 83 | v 0.02500000 0.00500000 0.00000000 84 | v 0.02500000 -0.00500000 -0.00000000 85 | v 0.02459824 0.00500000 0.00446392 86 | v 0.02459824 -0.00500000 0.00446392 87 | v 0.02340587 0.00500000 0.00878437 88 | v 0.02340587 -0.00500000 0.00878437 89 | v 0.02146122 0.00500000 0.01282248 90 | v 0.02146122 -0.00500000 0.01282248 91 | v 0.01882679 0.00500000 0.01644847 92 | v 0.01882679 -0.00500000 0.01644847 93 | v 0.01558725 0.00500000 0.01954579 94 | v 0.01558725 -0.00500000 0.01954579 95 | v 0.01184672 0.00500000 0.02201489 96 | v 0.01184672 -0.00500000 0.02201489 97 | v 0.00772543 0.00500000 0.02377641 98 | v 0.00772543 -0.00500000 0.02377641 99 | v 0.00000000 0.00500000 0.00000000 100 | v 0.00000000 -0.00500000 -0.00000000 101 | v 0.00250000 -0.00000000 0.04250000 102 | v 0.00230970 0.00095671 0.04250000 103 | v 0.00216506 0.00125000 0.04250000 104 | v 0.00176777 0.00176777 0.04250000 105 | v 0.00125000 0.00216506 0.04250000 106 | v -0.00250000 -0.00000000 0.04250000 107 | v -0.00230970 -0.00095671 0.04250000 108 | v -0.00216506 -0.00125000 0.04250000 109 | v -0.00176777 -0.00176777 0.04250000 110 | v -0.00125000 -0.00216506 0.04250000 111 | v -0.00095671 -0.00230970 0.04250000 112 | v 0.00000000 -0.00250000 0.04250000 113 | v 0.00095671 -0.00230970 0.04250000 114 | v 0.00125000 -0.00216506 0.04250000 115 | v 0.00176777 -0.00176777 0.04250000 116 | v 0.00216506 -0.00125000 0.04250000 117 | v 0.00230970 -0.00095671 0.04250000 118 | v 0.00095671 0.00230970 0.04250000 119 | v 0.00000000 0.00250000 0.04250000 120 | v -0.00095671 0.00230970 0.04250000 121 | v -0.00125000 0.00216506 0.04250000 122 | v -0.00176777 0.00176777 0.04250000 123 | v -0.00216506 0.00125000 0.04250000 124 | v -0.00230970 0.00095671 0.04250000 125 | v -0.00067650 0.00067650 0.04480970 126 | v -0.00088388 0.00036612 0.04480970 127 | v -0.00000000 -0.00000000 0.04500000 128 | v -0.00036612 0.00088388 0.04480970 129 | v 0.00000000 0.00095671 0.04480970 130 | v 0.00036612 0.00088388 0.04480970 131 | v 0.00067650 0.00067650 0.04480970 132 | v 0.00088388 0.00036612 0.04480970 133 | v 0.00095671 -0.00000000 0.04480970 134 | v 0.00088388 -0.00036612 0.04480970 135 | v 0.00067650 -0.00067650 0.04480970 136 | v 0.00036612 -0.00088388 0.04480970 137 | v -0.00000000 -0.00095671 0.04480970 138 | v -0.00036612 -0.00088388 0.04480970 139 | v -0.00067650 -0.00067650 0.04480970 140 | v -0.00088388 -0.00036612 0.04480970 141 | v -0.00095671 -0.00000000 0.04480970 142 | v -0.00088388 -0.00213388 0.04345671 143 | v -0.00163320 -0.00163320 0.04345671 144 | v -0.00000000 -0.00230970 0.04345671 145 | v 0.00088388 -0.00213388 0.04345671 146 | v 0.00163320 -0.00163320 0.04345671 147 | v 0.00213388 -0.00088388 0.04345671 148 | v 0.00230970 -0.00000000 0.04345671 149 | v 0.00213388 0.00088388 0.04345671 150 | v 0.00163320 0.00163320 0.04345671 151 | v 0.00088388 0.00213388 0.04345671 152 | v 0.00000000 0.00230970 0.04345671 153 | v -0.00088388 0.00213388 0.04345671 154 | v -0.00163320 0.00163320 0.04345671 155 | v -0.00213388 0.00088388 0.04345671 156 | v -0.00230970 -0.00000000 0.04345671 157 | v -0.00213388 -0.00088388 0.04345671 158 | v -0.00163320 0.00067650 0.04426777 159 | v -0.00125000 0.00125000 0.04426777 160 | v -0.00067650 0.00163320 0.04426777 161 | v 0.00000000 0.00176777 0.04426777 162 | v 0.00067650 0.00163320 0.04426777 163 | v 0.00125000 0.00125000 0.04426777 164 | v 0.00163320 0.00067650 0.04426777 165 | v 0.00176777 -0.00000000 0.04426777 166 | v 0.00163320 -0.00067650 0.04426777 167 | v 0.00125000 -0.00125000 0.04426777 168 | v 0.00067650 -0.00163320 0.04426777 169 | v -0.00000000 -0.00176777 0.04426777 170 | v -0.00067650 -0.00163320 0.04426777 171 | v -0.00125000 -0.00125000 0.04426777 172 | v -0.00163320 -0.00067650 0.04426777 173 | v -0.00176777 -0.00000000 0.04426777 174 | f 1 2 3 175 | f 4 5 6 176 | f 7 8 3 177 | f 3 8 9 178 | f 3 9 1 179 | f 6 5 3 180 | f 3 5 10 181 | f 3 10 7 182 | f 11 12 13 183 | f 13 12 14 184 | f 14 15 13 185 | f 13 15 16 186 | f 13 16 17 187 | f 17 16 18 188 | f 19 20 21 189 | f 19 21 22 190 | f 18 23 17 191 | f 17 23 24 192 | f 17 24 21 193 | f 21 24 25 194 | f 21 25 22 195 | f 20 26 27 196 | f 27 26 28 197 | f 27 28 29 198 | f 29 30 27 199 | f 27 30 31 200 | f 27 31 6 201 | f 6 31 32 202 | f 6 32 4 203 | f 20 27 21 204 | f 21 27 33 205 | f 21 33 34 206 | f 34 33 35 207 | f 34 35 36 208 | f 36 35 37 209 | f 36 37 38 210 | f 38 37 39 211 | f 38 39 40 212 | f 40 39 41 213 | f 40 41 42 214 | f 42 41 43 215 | f 42 43 44 216 | f 44 43 45 217 | f 44 45 46 218 | f 46 45 47 219 | f 46 47 48 220 | f 48 47 49 221 | f 48 49 50 222 | f 50 49 51 223 | f 50 51 52 224 | f 52 51 53 225 | f 52 53 54 226 | f 54 53 55 227 | f 54 55 56 228 | f 56 55 57 229 | f 56 57 58 230 | f 58 57 59 231 | f 58 59 60 232 | f 60 59 61 233 | f 60 61 62 234 | f 62 61 63 235 | f 62 63 64 236 | f 64 63 65 237 | f 64 65 66 238 | f 66 65 67 239 | f 66 67 68 240 | f 68 67 69 241 | f 68 69 70 242 | f 70 69 71 243 | f 70 71 72 244 | f 72 71 73 245 | f 72 73 74 246 | f 74 73 75 247 | f 74 75 76 248 | f 76 75 77 249 | f 76 77 78 250 | f 78 77 79 251 | f 78 79 80 252 | f 80 79 81 253 | f 80 81 82 254 | f 82 81 83 255 | f 82 83 84 256 | f 84 83 85 257 | f 84 85 86 258 | f 86 85 87 259 | f 86 87 88 260 | f 88 87 89 261 | f 88 89 90 262 | f 90 89 91 263 | f 90 91 92 264 | f 92 91 93 265 | f 92 93 94 266 | f 94 93 95 267 | f 94 95 96 268 | f 96 95 3 269 | f 96 3 13 270 | f 13 3 2 271 | f 13 2 11 272 | f 81 79 97 273 | f 81 97 83 274 | f 89 87 97 275 | f 97 87 85 276 | f 97 85 83 277 | f 95 93 97 278 | f 97 93 91 279 | f 97 91 89 280 | f 27 6 97 281 | f 97 6 3 282 | f 97 3 95 283 | f 37 35 97 284 | f 97 35 33 285 | f 97 33 27 286 | f 43 41 97 287 | f 97 41 39 288 | f 97 39 37 289 | f 49 47 97 290 | f 97 47 45 291 | f 97 45 43 292 | f 55 53 97 293 | f 97 53 51 294 | f 97 51 49 295 | f 61 59 97 296 | f 97 59 57 297 | f 97 57 55 298 | f 67 65 97 299 | f 97 65 63 300 | f 97 63 61 301 | f 73 71 97 302 | f 97 71 69 303 | f 97 69 67 304 | f 79 77 97 305 | f 97 77 75 306 | f 97 75 73 307 | f 82 84 98 308 | f 82 98 80 309 | f 74 76 98 310 | f 98 76 78 311 | f 98 78 80 312 | f 68 70 98 313 | f 98 70 72 314 | f 98 72 74 315 | f 62 64 98 316 | f 98 64 66 317 | f 98 66 68 318 | f 56 58 98 319 | f 98 58 60 320 | f 98 60 62 321 | f 50 52 98 322 | f 98 52 54 323 | f 98 54 56 324 | f 44 46 98 325 | f 98 46 48 326 | f 98 48 50 327 | f 38 40 98 328 | f 98 40 42 329 | f 98 42 44 330 | f 21 34 98 331 | f 98 34 36 332 | f 98 36 38 333 | f 96 13 98 334 | f 98 13 17 335 | f 98 17 21 336 | f 90 92 98 337 | f 98 92 94 338 | f 98 94 96 339 | f 84 86 98 340 | f 98 86 88 341 | f 98 88 90 342 | f 99 1 100 343 | f 100 1 9 344 | f 100 9 101 345 | f 101 9 8 346 | f 101 8 102 347 | f 102 8 7 348 | f 102 7 103 349 | f 104 20 105 350 | f 105 20 19 351 | f 105 19 106 352 | f 106 19 22 353 | f 106 22 107 354 | f 107 22 25 355 | f 107 25 108 356 | f 108 25 24 357 | f 108 24 109 358 | f 109 24 23 359 | f 109 23 110 360 | f 110 23 18 361 | f 110 18 111 362 | f 111 18 16 363 | f 111 16 112 364 | f 112 16 15 365 | f 112 15 113 366 | f 113 15 14 367 | f 113 14 114 368 | f 114 14 12 369 | f 114 12 115 370 | f 115 12 11 371 | f 115 11 99 372 | f 99 11 2 373 | f 99 2 1 374 | f 103 7 116 375 | f 116 7 10 376 | f 116 10 117 377 | f 117 10 5 378 | f 117 5 118 379 | f 118 5 4 380 | f 118 4 119 381 | f 4 32 119 382 | f 119 32 31 383 | f 119 31 120 384 | f 120 31 30 385 | f 120 30 121 386 | f 121 30 29 387 | f 121 29 122 388 | f 122 29 28 389 | f 122 28 104 390 | f 104 28 26 391 | f 104 26 20 392 | f 123 124 125 393 | f 123 125 126 394 | f 127 126 125 395 | f 128 127 125 396 | f 129 128 125 397 | f 130 129 125 398 | f 131 130 125 399 | f 132 131 125 400 | f 133 132 125 401 | f 134 133 125 402 | f 135 134 125 403 | f 136 135 125 404 | f 137 136 125 405 | f 138 137 125 406 | f 139 138 125 407 | f 124 139 125 408 | f 108 140 141 409 | f 108 109 140 410 | f 140 109 110 411 | f 140 110 142 412 | f 142 110 111 413 | f 142 111 143 414 | f 143 111 112 415 | f 143 112 144 416 | f 112 113 144 417 | f 144 113 114 418 | f 144 114 145 419 | f 114 115 145 420 | f 145 115 99 421 | f 145 99 146 422 | f 146 99 100 423 | f 146 100 147 424 | f 147 100 101 425 | f 147 101 148 426 | f 101 102 148 427 | f 148 102 103 428 | f 148 103 149 429 | f 103 116 149 430 | f 149 116 117 431 | f 149 117 150 432 | f 150 117 118 433 | f 150 118 151 434 | f 151 118 119 435 | f 151 119 152 436 | f 119 120 152 437 | f 152 120 121 438 | f 152 121 153 439 | f 121 122 153 440 | f 153 122 104 441 | f 153 104 154 442 | f 154 104 105 443 | f 154 105 155 444 | f 155 105 106 445 | f 155 106 141 446 | f 141 106 107 447 | f 141 107 108 448 | f 154 156 153 449 | f 153 156 157 450 | f 153 157 152 451 | f 152 157 158 452 | f 152 158 151 453 | f 151 158 159 454 | f 151 159 150 455 | f 150 159 160 456 | f 150 160 149 457 | f 149 160 161 458 | f 149 161 148 459 | f 148 161 162 460 | f 148 162 147 461 | f 147 162 163 462 | f 147 163 146 463 | f 146 163 164 464 | f 146 164 145 465 | f 145 164 165 466 | f 145 165 144 467 | f 144 165 166 468 | f 144 166 143 469 | f 143 166 167 470 | f 143 167 142 471 | f 142 167 168 472 | f 142 168 140 473 | f 140 168 169 474 | f 140 169 141 475 | f 141 169 170 476 | f 141 170 155 477 | f 155 170 171 478 | f 155 171 154 479 | f 154 171 156 480 | f 171 124 156 481 | f 156 124 123 482 | f 156 123 157 483 | f 157 123 126 484 | f 157 126 158 485 | f 158 126 127 486 | f 158 127 159 487 | f 159 127 128 488 | f 159 128 160 489 | f 160 128 129 490 | f 160 129 161 491 | f 161 129 130 492 | f 161 130 162 493 | f 162 130 131 494 | f 162 131 163 495 | f 163 131 132 496 | f 163 132 164 497 | f 164 132 133 498 | f 164 133 165 499 | f 165 133 134 500 | f 165 134 166 501 | f 166 134 135 502 | f 166 135 167 503 | f 167 135 136 504 | f 167 136 168 505 | f 168 136 137 506 | f 168 137 169 507 | f 169 137 138 508 | f 169 138 170 509 | f 170 138 139 510 | f 170 139 171 511 | f 171 139 124 512 | 513 | -------------------------------------------------------------------------------- /tests/pv_sdf_debug/box_template.obj: -------------------------------------------------------------------------------- 1 | # Created by Open3D 2 | # object name: box_template 3 | # number of vertices: 8 4 | # number of triangles: 12 5 | v 1 1 1 6 | vn 0.333333 0.333333 0.333333 7 | v -1 1 1 8 | vn -0.333333 0.333333 0.333333 9 | v 1 -1 1 10 | vn 0.333333 -0.333333 0.333333 11 | v -1 -1 1 12 | vn -0.333333 -0.333333 0.333333 13 | v 1 1 -1 14 | vn 0.333333 0.333333 -0.333333 15 | v 1 -1 -1 16 | vn 0.333333 -0.333333 -0.333333 17 | v -1 1 -1 18 | vn -0.333333 0.333333 -0.333333 19 | v -1 -1 -1 20 | vn -0.333333 -0.333333 -0.333333 21 | f 2//2 7//7 4//4 22 | f 3//3 4//4 6//6 23 | f 4//4 8//8 6//6 24 | f 5//5 6//6 7//7 25 | f 2//2 5//5 7//7 26 | f 1//1 5//5 2//2 27 | f 6//6 8//8 7//7 28 | f 1//1 3//3 5//5 29 | f 2//2 4//4 3//3 30 | f 4//4 7//7 8//8 31 | f 3//3 6//6 5//5 32 | f 1//1 2//2 3//3 33 | -------------------------------------------------------------------------------- /tests/pv_sdf_debug/scene_mesh_gt.obj: -------------------------------------------------------------------------------- 1 | # Created by Open3D 2 | # object name: scene_mesh_gt 3 | # number of vertices: 14 4 | # number of triangles: 22 5 | v 0.4 0.4 0.2 6 | v 0.4 0.4 0 7 | v 0.4 -0.4 0.2 8 | v 1.2 -0.4 0.2 9 | v 1.2 -0.4 0 10 | v 0.4 -0.4 0 11 | v 1.2 0.4 0 12 | v 1.2 0.4 0.2 13 | v -0.3 0.4 0 14 | v -0.3 0.4 -0.2 15 | v -0.3 -0.4 0 16 | v 1.2 -0.4 -0.2 17 | v -0.3 -0.4 -0.2 18 | v 1.2 0.4 -0.2 19 | f 6 2 9 20 | f 5 12 14 21 | f 10 12 13 22 | f 7 14 9 23 | f 10 13 11 24 | f 11 13 12 25 | f 9 14 10 26 | f 5 11 12 27 | f 6 9 11 28 | f 1 2 3 29 | f 10 14 12 30 | f 1 7 2 31 | f 4 7 8 32 | f 3 5 4 33 | f 9 10 11 34 | f 1 8 7 35 | f 5 14 7 36 | f 2 6 3 37 | f 3 6 5 38 | f 1 4 8 39 | f 1 3 4 40 | f 4 5 7 41 | -------------------------------------------------------------------------------- /tests/pv_sdf_debug/scene_mesh_overlap.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Default_OBJ.001 5 | Ns 250.000000 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.450000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /tests/pv_sdf_debug/scene_mesh_overlap.obj: -------------------------------------------------------------------------------- 1 | # Blender v3.2.2 OBJ File: '' 2 | # www.blender.org 3 | mtllib scene_mesh_overlap.mtl 4 | o scene_mesh_wrong 5 | v 0.400000 0.400000 0.153853 6 | v 0.400000 0.400000 -0.046147 7 | v 0.400000 -0.400000 0.153853 8 | v 1.200000 -0.400000 0.153853 9 | v 1.200000 -0.400000 -0.046147 10 | v 0.400000 -0.400000 -0.046147 11 | v 1.200000 0.400000 -0.046147 12 | v 1.200000 0.400000 0.153853 13 | v -0.300000 0.400000 0.000000 14 | v -0.300000 0.400000 -0.200000 15 | v -0.300000 -0.400000 -0.000000 16 | v 1.200000 -0.400000 -0.000000 17 | v 1.200000 -0.400000 -0.200000 18 | v -0.300000 -0.400000 -0.200000 19 | v 1.200000 0.400000 -0.200000 20 | v 1.200000 0.400000 0.000000 21 | vn -0.5774 0.5774 0.5774 22 | vn -0.5774 0.5774 -0.5774 23 | vn -0.5774 -0.5774 0.5774 24 | vn 0.5774 -0.5774 0.5774 25 | vn 0.5774 -0.5774 -0.5774 26 | vn -0.5774 -0.5774 -0.5774 27 | vn 0.5774 0.5774 -0.5774 28 | vn 0.5774 0.5774 0.5774 29 | usemtl Default_OBJ.001 30 | s 1 31 | f 1//1 2//2 3//3 32 | f 4//4 3//3 5//5 33 | f 3//3 6//6 5//5 34 | f 7//7 5//5 2//2 35 | f 1//1 7//7 2//2 36 | f 8//8 7//7 1//1 37 | f 5//5 6//6 2//2 38 | f 8//8 4//4 7//7 39 | f 1//1 3//3 4//4 40 | f 3//3 2//2 6//6 41 | f 4//4 5//5 7//7 42 | f 8//8 1//1 4//4 43 | f 9//1 10//2 11//3 44 | f 12//4 11//3 13//5 45 | f 11//3 14//6 13//5 46 | f 15//7 13//5 10//2 47 | f 9//1 15//7 10//2 48 | f 16//8 15//7 9//1 49 | f 13//5 14//6 10//2 50 | f 16//8 12//4 15//7 51 | f 9//1 11//3 12//4 52 | f 11//3 10//2 14//6 53 | f 12//4 13//5 15//7 54 | f 16//8 9//1 12//4 55 | -------------------------------------------------------------------------------- /tests/pv_sdf_debug/scene_mesh_separated.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Default_OBJ.001 5 | Ns 250.000000 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.800000 0.800000 0.800000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.450000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /tests/pv_sdf_debug/scene_mesh_separated.obj: -------------------------------------------------------------------------------- 1 | # Blender v3.2.2 OBJ File: '' 2 | # www.blender.org 3 | mtllib scene_mesh_separated.mtl 4 | o scene_mesh_wrong 5 | v 0.400000 0.400000 0.253449 6 | v 0.400000 0.400000 0.053449 7 | v 0.400000 -0.400000 0.253449 8 | v 1.200000 -0.400000 0.253449 9 | v 1.200000 -0.400000 0.053449 10 | v 0.400000 -0.400000 0.053449 11 | v 1.200000 0.400000 0.053449 12 | v 1.200000 0.400000 0.253449 13 | v -0.300000 0.400000 0.000000 14 | v -0.300000 0.400000 -0.200000 15 | v -0.300000 -0.400000 -0.000000 16 | v 1.200000 -0.400000 -0.000000 17 | v 1.200000 -0.400000 -0.200000 18 | v -0.300000 -0.400000 -0.200000 19 | v 1.200000 0.400000 -0.200000 20 | v 1.200000 0.400000 0.000000 21 | vn -0.5774 0.5774 0.5774 22 | vn -0.5774 0.5774 -0.5774 23 | vn -0.5774 -0.5774 0.5774 24 | vn 0.5774 -0.5774 0.5774 25 | vn 0.5774 -0.5774 -0.5774 26 | vn -0.5774 -0.5774 -0.5774 27 | vn 0.5774 0.5774 -0.5774 28 | vn 0.5774 0.5774 0.5774 29 | usemtl Default_OBJ.001 30 | s 1 31 | f 1//1 2//2 3//3 32 | f 4//4 3//3 5//5 33 | f 3//3 6//6 5//5 34 | f 7//7 5//5 2//2 35 | f 1//1 7//7 2//2 36 | f 8//8 7//7 1//1 37 | f 5//5 6//6 2//2 38 | f 8//8 4//4 7//7 39 | f 1//1 3//3 4//4 40 | f 3//3 2//2 6//6 41 | f 4//4 5//5 7//7 42 | f 8//8 1//1 4//4 43 | f 9//1 10//2 11//3 44 | f 12//4 11//3 13//5 45 | f 11//3 14//6 13//5 46 | f 15//7 13//5 10//2 47 | f 9//1 15//7 10//2 48 | f 16//8 15//7 9//1 49 | f 13//5 14//6 10//2 50 | f 16//8 12//4 15//7 51 | f 9//1 11//3 12//4 52 | f 11//3 10//2 14//6 53 | f 12//4 13//5 15//7 54 | f 16//8 9//1 12//4 55 | -------------------------------------------------------------------------------- /tests/pv_sdf_debug/scene_mesh_wrong.obj: -------------------------------------------------------------------------------- 1 | # Created by Open3D 2 | # object name: scene_mesh_wrong 3 | # number of vertices: 16 4 | # number of triangles: 24 5 | v 0.4 0.4 0.2 6 | vn -0.333333 0.333333 0.333333 7 | v 0.4 0.4 0 8 | vn -0.333333 0.333333 -0.333333 9 | v 0.4 -0.4 0.2 10 | vn -0.333333 -0.333333 0.333333 11 | v 1.2 -0.4 0.2 12 | vn 0.333333 -0.333333 0.333333 13 | v 1.2 -0.4 0 14 | vn 0.333333 -0.333333 -0.333333 15 | v 0.4 -0.4 0 16 | vn -0.333333 -0.333333 -0.333333 17 | v 1.2 0.4 0 18 | vn 0.333333 0.333333 -0.333333 19 | v 1.2 0.4 0.2 20 | vn 0.333333 0.333333 0.333333 21 | v -0.3 0.4 0 22 | vn -0.333333 0.333333 0.333333 23 | v -0.3 0.4 -0.2 24 | vn -0.333333 0.333333 -0.333333 25 | v -0.3 -0.4 0 26 | vn -0.333333 -0.333333 0.333333 27 | v 1.2 -0.4 0 28 | vn 0.333333 -0.333333 0.333333 29 | v 1.2 -0.4 -0.2 30 | vn 0.333333 -0.333333 -0.333333 31 | v -0.3 -0.4 -0.2 32 | vn -0.333333 -0.333333 -0.333333 33 | v 1.2 0.4 -0.2 34 | vn 0.333333 0.333333 -0.333333 35 | v 1.2 0.4 0 36 | vn 0.333333 0.333333 0.333333 37 | f 1//1 2//2 3//3 38 | f 4//4 3//3 5//5 39 | f 3//3 6//6 5//5 40 | f 7//7 5//5 2//2 41 | f 1//1 7//7 2//2 42 | f 8//8 7//7 1//1 43 | f 5//5 6//6 2//2 44 | f 8//8 4//4 7//7 45 | f 1//1 3//3 4//4 46 | f 3//3 2//2 6//6 47 | f 4//4 5//5 7//7 48 | f 8//8 1//1 4//4 49 | f 9//9 10//10 11//11 50 | f 12//12 11//11 13//13 51 | f 11//11 14//14 13//13 52 | f 15//15 13//13 10//10 53 | f 9//9 15//15 10//10 54 | f 16//16 15//15 9//9 55 | f 13//13 14//14 10//10 56 | f 16//16 12//12 15//15 57 | f 9//9 11//11 12//12 58 | f 11//11 10//10 14//14 59 | f 12//12 13//13 15//15 60 | f 16//16 9//9 12//12 61 | -------------------------------------------------------------------------------- /tests/pv_sdf_debug/test_export_composed_sdf.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | 3 | import pytorch_volumetric as pv 4 | import numpy as np 5 | import open3d as o3d 6 | import trimesh 7 | import matplotlib.pyplot as plt 8 | import plotly.express as px 9 | def create_box_mesh(scale, trans): 10 | FILE_DIR = pathlib.Path(__file__).parent.joinpath('box_template.obj') 11 | mesh = o3d.io.read_triangle_mesh(str(FILE_DIR)) 12 | v = np.array(mesh.vertices) 13 | v = v * np.array(scale).reshape(1, 3) + np.array(trans).reshape(1, 3) 14 | mesh.vertices = o3d.utility.Vector3dVector(v) 15 | return mesh 16 | 17 | 18 | def test_sdf(): 19 | t_mesh = create_box_mesh((0.4, 0.4, 0.1), (0.8, 0, 0.1)) 20 | t_mesh = trimesh.Trimesh(vertices=t_mesh.vertices, faces=t_mesh.triangles) 21 | f_mesh = create_box_mesh((0.75, 0.4, 0.1), (0.45, 0, -0.15)) 22 | f_mesh = trimesh.Trimesh(vertices=f_mesh.vertices, faces=f_mesh.triangles) 23 | scene_mesh = trimesh.boolean.union([t_mesh, f_mesh], engine='blender') 24 | scene_path = pathlib.Path(__file__).parent.joinpath('scene_mesh_wrong.obj') 25 | trimesh.exchange.obj.export_obj(scene_mesh, scene_path) 26 | 27 | scene_path = pathlib.Path(__file__).parent.joinpath('scene_mesh_wrong.obj') 28 | # scene_path = pathlib.Path(__file__).parent.joinpath('scene_mesh_gt.obj') 29 | # scene_path = pathlib.Path(__file__).parent.joinpath('scene_mesh_separated.obj') 30 | # scene_path = pathlib.Path(__file__).parent.joinpath('scene_mesh_overlap.obj') 31 | scene_sdf = pv.MeshSDF(pv.MeshObjectFactory(scene_path)) 32 | query_range = np.array([ 33 | [-0.5, 2], 34 | [0, 0], 35 | [-0.2, 0.3], 36 | ]) 37 | ret = pv.draw_sdf_slice(scene_sdf, query_range) 38 | v = ret[-1] 39 | px.imshow(v).show() 40 | plt.show() 41 | 42 | if __name__ == "__main__": 43 | test_sdf() -------------------------------------------------------------------------------- /tests/test_chamfer.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import os 3 | import pytorch_volumetric as pv 4 | import pytorch_kinematics as pk 5 | from pytorch_seed import seed 6 | from pytorch_volumetric import sample_mesh_points 7 | import matplotlib.pyplot as plt 8 | import seaborn as sns 9 | 10 | plt.switch_backend('Qt5Agg') 11 | TEST_DIR = os.path.dirname(__file__) 12 | 13 | visualize = True 14 | 15 | 16 | def do_test_chamfer_distance(mesh): 17 | d = "cuda" if torch.cuda.is_available() else "cpu" 18 | dtype = torch.float 19 | B = 300 20 | N = 1000 21 | seed(3) 22 | 23 | # press n to visualize the normals / gradients 24 | obj = pv.MeshObjectFactory(os.path.join(TEST_DIR, mesh)) 25 | 26 | # sample points on the obj mesh surface uniformly 27 | pts, normals, _ = sample_mesh_points(obj, name=mesh, num_points=N, device=d, dtype=dtype) 28 | 29 | # sample a random transform 30 | gt_tf = pk.Transform3d(pos=torch.randn(3, device=d), rot=pk.random_rotation(device=d), device=d) 31 | pts_world = gt_tf.transform_points(pts) 32 | 33 | # giving it the transform should have 0 distance 34 | world_to_object = gt_tf.inverse().get_matrix().repeat(B, 1, 1) 35 | 36 | err = pv.batch_chamfer_dist(world_to_object, pts_world, obj) 37 | assert err.shape == (B,) 38 | assert torch.allclose(err, torch.zeros_like(err), atol=1e-4) 39 | 40 | # randomly pertrub the transform 41 | perturbed_tf = gt_tf.sample_perturbations(B, radian_sigma=0.1, translation_sigma=0.1) 42 | world_to_object_perturbed = perturbed_tf.inverse().get_matrix() 43 | 44 | # ChamferDistance gives the sum of all the distances while we take their mean, so we need to multiply by N 45 | err = pv.batch_chamfer_dist(world_to_object_perturbed, pts_world, obj, scale=1) * N 46 | # compare the error to the chamfer distance between the transformed points and the original points 47 | perturbed_pts = perturbed_tf.transform_points(pts) 48 | 49 | try: 50 | from chamferdist import ChamferDistance 51 | d = ChamferDistance() 52 | gt_dist = d(pts_world.repeat(B, 1, 1), perturbed_pts, reduction=None) 53 | # gt_dist_r = d(pts_world.repeat(B, 1, 1), perturbed_tf.transform_points(pts), reduction=None, reverse=True) 54 | # gt_dist_b = 0.5 * d(pts_world.repeat(B, 1, 1), perturbed_tf.transform_points(pts), reduction=None, bidirectional=True) 55 | # there are some differences because ours compares the unidirectional chamfer distance to a mesh vs point cloud 56 | # they are always overestimating the distance due to not finding the actual closest point 57 | assert torch.all(err < gt_dist) 58 | assert torch.all(gt_dist - err < 0.05 * gt_dist) 59 | except ImportError: 60 | print("pip install chamferdist to test against an accelerated implementation of chamfer distance") 61 | 62 | # compare against a manual chamfer distance calculation 63 | all_dists = torch.cdist(pts_world, perturbed_pts) 64 | gt_dist_manual = torch.min(all_dists, dim=2).values.square().sum(dim=1) 65 | assert torch.all(err < gt_dist_manual) 66 | assert torch.all(gt_dist_manual - err < 0.05 * gt_dist_manual) 67 | 68 | # compare the chamfer distance to the matrix distance of the transform (norm of their difference) 69 | # use the p=2 induced vector norm (spectral norm) 70 | mat_diff = world_to_object_perturbed - world_to_object 71 | # get the max singular value of the matrix 72 | _, singular_values, _ = torch.svd(mat_diff, compute_uv=False) 73 | mat_norm = singular_values[:, 0] 74 | 75 | if visualize: 76 | sns.displot(mat_norm.cpu().numpy(), kind="kde", fill=True) 77 | # plot the mat_norm with respect to the chamfer distance 78 | plt.figure() 79 | plt.scatter(mat_norm.cpu().numpy(), err.cpu().numpy()) 80 | plt.xlabel("Matrix Norm") 81 | plt.ylabel("Chamfer Distance (mm^2)") 82 | # set x and y min limit to 0 83 | plt.xlim(0) 84 | plt.ylim(0) 85 | plt.show() 86 | 87 | 88 | def do_test_plausible_diversity(mesh): 89 | d = "cuda" if torch.cuda.is_available() else "cpu" 90 | dtype = torch.float 91 | B = 10 92 | tol = 1e-4 93 | seed(3) 94 | 95 | # press n to visualize the normals / gradients 96 | obj = pv.MeshObjectFactory(os.path.join(TEST_DIR, mesh)) 97 | 98 | # sample a random transform 99 | gt_tf = pk.Transform3d(pos=torch.randn(3, device=d), rot=pk.random_rotation(device=d), dtype=dtype, device=d) 100 | # sample perturbations around it to get a set of plausible gt transforms 101 | gt_tf = gt_tf.sample_perturbations(B, radian_sigma=0.05, translation_sigma=0.01) 102 | 103 | # plausible diversity of itself should be 0 104 | pd = pv.PlausibleDiversity(obj) 105 | pd_ret = pd(gt_tf.inverse().get_matrix(), gt_tf.get_matrix()) 106 | assert pd_ret.plausibility < tol 107 | assert pd_ret.coverage < tol 108 | 109 | # removing some transforms should keep plausibility at 0, but increase coverage error 110 | partial_tf = pk.Transform3d(matrix=gt_tf.get_matrix()[:B // 2]) 111 | pd_ret = pd(partial_tf.inverse().get_matrix(), gt_tf.get_matrix(), bidirectional=True) 112 | assert pd_ret.plausibility < tol 113 | assert pd_ret.coverage > tol 114 | 115 | # # compare the computed distances against the ground truth 116 | # pts = pd.model_points_eval 117 | # # transform the points using both sets of transforms 118 | # pts_gt = gt_tf.transform_points(pts) 119 | # pts_partial = partial_tf.transform_points(pts) 120 | # # compute the chamfer distance between the transformed points 121 | 122 | # going the other way should have the opposite effect 123 | pd_ret_other = pd(gt_tf.inverse().get_matrix(), partial_tf.get_matrix(), bidirectional=True) 124 | assert pd_ret_other.plausibility > tol 125 | assert pd_ret_other.coverage < tol 126 | 127 | # should also be symmetric when created as bidirectional 128 | # could still have some numerical error due to inverting the matrix 129 | assert torch.allclose(pd_ret.plausibility, pd_ret_other.coverage) 130 | assert torch.allclose(pd_ret.coverage, pd_ret_other.plausibility, rtol=0.06) 131 | 132 | 133 | def test_chamfer_distance(): 134 | do_test_chamfer_distance("probe.obj") 135 | do_test_chamfer_distance("offset_wrench_nogrip.obj") 136 | 137 | 138 | def test_plausible_diversity(): 139 | do_test_plausible_diversity("probe.obj") 140 | do_test_plausible_diversity("offset_wrench_nogrip.obj") 141 | 142 | 143 | if __name__ == "__main__": 144 | test_chamfer_distance() 145 | test_plausible_diversity() 146 | -------------------------------------------------------------------------------- /tests/test_model_to_sdf.py: -------------------------------------------------------------------------------- 1 | import pymeshlab 2 | import os 3 | import math 4 | import torch 5 | import time 6 | from matplotlib import pyplot as plt 7 | import numpy as np 8 | from timeit import default_timer as timer 9 | import open3d as o3d 10 | import pytorch_kinematics as pk 11 | import pytorch_volumetric as pv 12 | 13 | import matplotlib 14 | from matplotlib import cm 15 | 16 | import pybullet as p 17 | import pybullet_data 18 | 19 | import logging 20 | 21 | plt.switch_backend('Qt5Agg') 22 | 23 | logger = logging.getLogger(__file__) 24 | logging.basicConfig(level=logging.INFO, force=True, 25 | format='[%(levelname)s %(asctime)s %(pathname)s:%(lineno)d] %(message)s', 26 | datefmt='%m-%d %H:%M:%S') 27 | 28 | TEST_DIR = os.path.dirname(__file__) 29 | 30 | visualize = True 31 | 32 | 33 | def _make_robot_translucent(robot_id, alpha=0.4): 34 | def make_transparent(link): 35 | link_id = link[1] 36 | rgba = list(link[7]) 37 | rgba[3] = alpha 38 | p.changeVisualShape(robot_id, link_id, rgbaColor=rgba) 39 | 40 | visual_data = p.getVisualShapeData(robot_id) 41 | for link in visual_data: 42 | make_transparent(link) 43 | 44 | 45 | def test_urdf_to_sdf(): 46 | urdf = "kuka_iiwa/model.urdf" 47 | search_path = pybullet_data.getDataPath() 48 | full_urdf = os.path.join(search_path, urdf) 49 | chain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), "lbr_iiwa_link_7") 50 | d = "cuda" if torch.cuda.is_available() else "cpu" 51 | 52 | chain = chain.to(device=d) 53 | # use MeshSDF or CachedSDF for much faster lookup 54 | s = pv.RobotSDF(chain, path_prefix=os.path.join(search_path, "kuka_iiwa"), ) 55 | # link_sdf_cls=pv.cache_link_sdf_factory(resolution=0.02, padding=0.1, device=d)) 56 | th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0], device=d) 57 | 58 | s.set_joint_configuration(th) 59 | 60 | # plt.ion() 61 | # plt.show() 62 | 63 | if visualize: 64 | save_dir = "intermediate_meshes" 65 | os.makedirs(save_dir, exist_ok=True) 66 | # toggles - g:GUI w:wireframe j:joint axis a:AABB i:interrupt 67 | p.connect(p.GUI) 68 | p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 69 | p.setAdditionalSearchPath(search_path) 70 | armId = p.loadURDF(urdf, [0, 0, 0], useFixedBase=True) 71 | for i, q in enumerate(th): 72 | p.resetJointState(armId, i, q.item()) 73 | 74 | _make_robot_translucent(armId, alpha=0.5) 75 | 76 | from base_experiments.env.pybullet_env import DebugDrawer 77 | vis = DebugDrawer(0.3, 0.5) 78 | vis.toggle_3d(True) 79 | vis.set_camera_position([-0.35, 0, 0.35], yaw=-70, pitch=-15) 80 | 81 | # record video 82 | p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "video.mp4") 83 | 84 | # expected SDF value range 85 | norm = matplotlib.colors.Normalize(vmin=-0.15, vmax=0.35) 86 | m = cm.ScalarMappable(norm=norm, cmap=cm.jet) 87 | sdf_id = None 88 | 89 | # sweep across to animate it 90 | for x in np.linspace(-0.8, 0.5, 100): 91 | query_range = np.array([ 92 | [x, x], 93 | [-0.3, 0.3], 94 | [-0.2, 0.8], 95 | ]) 96 | 97 | resolution = 0.01 98 | # ret = pv.draw_sdf_slice(s, query_range, resolution=resolution, device=s.device, do_plot=False) 99 | # sdf_val = ret[0] 100 | # pts = ret[2] 101 | coords, pts = pv.get_coordinates_and_points_in_grid(resolution, query_range, device=s.device) 102 | 103 | # query multiple times and select the median to remove artifacts 104 | extra_queries = 10 105 | sdf_val, sdf_grad = s( 106 | pts + torch.randn((extra_queries, pts.shape[0], pts.shape[1]), device=s.device) * 1e-4) 107 | sdf_val = sdf_val.reshape(extra_queries, -1) 108 | sdf_val = sdf_val.median(dim=0).values 109 | 110 | sdf_color = m.to_rgba(sdf_val.cpu()) 111 | 112 | # Assume grid dimensions (rows and cols). You need to know or compute these. 113 | rows = len(coords[2]) 114 | cols = len(coords[1]) 115 | 116 | # Generating the faces (triangulating the grid cells) 117 | faces = [] 118 | for c in range(cols - 1): 119 | for r in range(rows - 1): 120 | # Compute indices of the vertices of the quadrilateral cell in column-major order 121 | idx0 = c * rows + r 122 | idx1 = c * rows + (r + 1) 123 | idx2 = (c + 1) * rows + (r + 1) 124 | idx3 = (c + 1) * rows + r 125 | 126 | # Two triangles per grid cell 127 | faces.append([idx0, idx1, idx2]) 128 | faces.append([idx0, idx2, idx3]) 129 | 130 | faces = np.array(faces) 131 | 132 | # surface = sdf_val.abs() < 0.005 133 | # set alpha 134 | 135 | # draw mesh of level set 136 | # Create the mesh 137 | this_mesh = pymeshlab.Mesh(vertex_matrix=pts.cpu().numpy(), face_matrix=faces, 138 | v_color_matrix=sdf_color, 139 | ) 140 | # create and save mesh 141 | ms = pymeshlab.MeshSet() 142 | ms.add_mesh(this_mesh, "sdf") 143 | 144 | # UV map and turn vertex coloring into a texture 145 | base_name = f"sdf_{x}" 146 | ms.compute_texcoord_parametrization_triangle_trivial_per_wedge() 147 | ms.compute_texmap_from_color(textname=f"tex_{base_name}") 148 | 149 | # print(f"has vertex colors: {this_mesh.has_vertex_color()}") 150 | # print(f"has face colors: {this_mesh.has_face_color()}") 151 | # print(f"has vertex tex coords: {this_mesh.has_vertex_tex_coord()}") 152 | 153 | # Check vertex colors are set correctly 154 | # assert ms.current_mesh().vertex_number() == len(sdf_color), "Mismatch in vertex counts" 155 | # # check vertex colors 156 | # mvc = ms.current_mesh().vertex_color_matrix() 157 | # print(mvc.shape) 158 | # print(mvc) 159 | # print(sdf_color.shape) 160 | # print(sdf_color) 161 | # assert np.allclose(mvc, sdf_color), "Mismatch in vertex colors" 162 | 163 | fn = os.path.join(save_dir, f"mesh_{base_name}.obj") 164 | ms.save_current_mesh(fn, save_vertex_color=True) 165 | 166 | prev_sdf_id = sdf_id 167 | visId = p.createVisualShape(p.GEOM_MESH, fileName=fn) 168 | sdf_id = p.createMultiBody(0, baseVisualShapeIndex=visId, basePosition=[0, 0, 0]) 169 | if prev_sdf_id is not None: 170 | p.removeBody(prev_sdf_id) 171 | 172 | 173 | def test_batch_over_configurations(): 174 | urdf = "kuka_iiwa/model.urdf" 175 | search_path = pybullet_data.getDataPath() 176 | full_urdf = os.path.join(search_path, urdf) 177 | chain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), "lbr_iiwa_link_7") 178 | d = "cuda" if torch.cuda.is_available() else "cpu" 179 | 180 | chain = chain.to(device=d) 181 | s = pv.RobotSDF(chain, path_prefix=os.path.join(search_path, "kuka_iiwa"), 182 | link_sdf_cls=pv.cache_link_sdf_factory(resolution=0.02, padding=1.0, device=d)) 183 | 184 | th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0], device=d) 185 | N = 20 186 | th_perturbation = torch.randn(N - 1, 7, device=d) * 0.1 187 | th = torch.cat((th.view(1, -1), th_perturbation + th)) 188 | 189 | s.set_joint_configuration(th) 190 | 191 | y = 0.02 192 | query_range = np.array([ 193 | [-1, 0.5], 194 | [y, y], 195 | [-0.2, 0.8], 196 | ]) 197 | 198 | coords, pts = pv.get_coordinates_and_points_in_grid(0.01, query_range, device=s.device) 199 | 200 | start = timer() 201 | all_sdf_val, all_sdf_grad = s(pts) 202 | elapsed = timer() - start 203 | logger.info("configurations: %d points: %d elapsed: %fms time per config and point: %fms", N, len(pts), 204 | elapsed * 1000, elapsed * 1000 / N / len(pts)) 205 | 206 | for i in range(N): 207 | th_i = th[i] 208 | s.set_joint_configuration(th_i) 209 | sdf_val, sdf_grad = s(pts) 210 | 211 | assert torch.allclose(sdf_val, all_sdf_val[i]) 212 | assert torch.allclose(sdf_grad, all_sdf_grad[i], atol=1e-6) 213 | 214 | 215 | def test_bounding_box(): 216 | if visualize: 217 | urdf = "kuka_iiwa/model.urdf" 218 | search_path = pybullet_data.getDataPath() 219 | full_urdf = os.path.join(search_path, urdf) 220 | chain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), "lbr_iiwa_link_7") 221 | d = "cuda" if torch.cuda.is_available() else "cpu" 222 | 223 | chain = chain.to(device=d) 224 | # use MeshSDF or CachedSDF for much faster lookup 225 | s = pv.RobotSDF(chain, path_prefix=os.path.join(search_path, "kuka_iiwa"), ) 226 | th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0], device=d) 227 | 228 | s.set_joint_configuration(th) 229 | 230 | # toggles - g:GUI w:wireframe j:joint axis a:AABB i:interrupt 231 | p.connect(p.GUI if visualize else p.DIRECT) 232 | p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 233 | p.setAdditionalSearchPath(search_path) 234 | armId = p.loadURDF(urdf, [0, 0, 0], useFixedBase=True) 235 | # p.resetBasePositionAndOrientation(armId, [0, 0, 0], [0, 0, 0, 1]) 236 | for i, q in enumerate(th): 237 | p.resetJointState(armId, i, q.item()) 238 | 239 | try: 240 | from base_experiments.env.env import draw_ordered_end_points, draw_AABB 241 | from base_experiments.env.pybullet_env import DebugDrawer 242 | delay = 0.2 243 | vis = DebugDrawer(1., 1.5) 244 | vis.toggle_3d(True) 245 | vis.set_camera_position([-0.1, 0, 0], yaw=-30, pitch=-20) 246 | # draw bounding box for each link (set breakpoints here to better see the link frame bounding box) 247 | bbs = s.link_bounding_boxes() 248 | for i in range(len(s.sdf.sdfs)): 249 | bb = bbs[i] 250 | draw_ordered_end_points(vis, bb) 251 | time.sleep(delay) 252 | # total aabb 253 | aabb = s.surface_bounding_box(padding=0) 254 | draw_AABB(vis, aabb.cpu().numpy()) 255 | time.sleep(delay) 256 | except ImportError as e: 257 | print(e) 258 | 259 | time.sleep(1) 260 | p.disconnect() 261 | 262 | 263 | def test_single_link_robot(): 264 | full_urdf = os.path.join(TEST_DIR, 'offset_wrench.urdf') 265 | chain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), "offset_wrench") 266 | d = "cuda" if torch.cuda.is_available() else "cpu" 267 | 268 | chain = chain.to(device=d) 269 | # paths to the link meshes are specified with their relative path inside the URDF 270 | # we need to give them the path prefix as we need their absolute path to load 271 | sdf = pv.RobotSDF(chain, path_prefix=TEST_DIR, 272 | link_sdf_cls=pv.cache_link_sdf_factory(resolution=0.001, padding=0.05, device=d)) 273 | trans_x, trans_y, trans_z = 0.0, 0.0, 0.0 274 | rot_x, rot_y, rot_z = 0.0, 0.0, 0.0 275 | trans = torch.tensor([trans_x, trans_y, trans_z], device=d) 276 | rot = torch.tensor([rot_x, rot_y, rot_z], device=d) 277 | H = torch.eye(4, device=d) 278 | H[:-1, -1] = trans 279 | H[:-1, :-1] = pk.euler_angles_to_matrix(rot, 'XYZ') 280 | 281 | th = torch.cat((trans, rot), dim=0) 282 | sdf.set_joint_configuration(th.view(1, -1)) 283 | query_range = sdf.surface_bounding_box(padding=0.05)[0] 284 | # M x 3 points 285 | coords, pts = pv.get_coordinates_and_points_in_grid(0.001, query_range, device=sdf.device) 286 | 287 | sdf_val, sdf_grad = sdf(pts) 288 | # because we passed in th with size (1, 6), the output is also (1, M) and (1, M, 3) meaning we have a batch of 1 289 | sdf_val = sdf_val[0] 290 | sdf_grad = sdf_grad[0] 291 | near_surface = sdf_val.abs() < 0.001 292 | surf_pts = pts[near_surface] 293 | surf_norms = sdf_grad[near_surface] 294 | pc = o3d.geometry.PointCloud() 295 | pc.points = o3d.utility.Vector3dVector(surf_pts.cpu()) 296 | pc.normals = o3d.utility.Vector3dVector(surf_norms.cpu()) 297 | if visualize: 298 | o3d.visualization.draw_geometries([pc]) 299 | 300 | # test multiple joint configurations 301 | B = 5 302 | th = th.view(1, -1).repeat(B, 1) 303 | sdf.set_joint_configuration(th) 304 | query_range = sdf.surface_bounding_box(padding=0.05) 305 | assert query_range.shape == (B, 3, 2) 306 | for i in range(1, B): 307 | assert torch.allclose(query_range[0], query_range[i]) 308 | 309 | # test non-batch query when we have a batch of configurations 310 | BB = 10 311 | N = 100 312 | assert surf_pts.shape[0] > BB * N 313 | test_pts = surf_pts[:BB * N] 314 | sdf_vals, sdf_grads = sdf(test_pts) 315 | 316 | assert sdf_vals.shape == (B, BB * N) 317 | assert sdf_grads.shape == (B, BB * N, 3) 318 | assert torch.allclose(sdf_vals.abs(), torch.zeros_like(sdf_vals), atol=1e-3) 319 | 320 | # test batch query when we have a batch of configurations 321 | batch_pts = test_pts.view(BB, N, 3) 322 | # will return with batch order Configuration x Point Query Batch x Num data point 323 | batch_sdf_vals, batch_sdf_grads = sdf(batch_pts) 324 | assert batch_sdf_vals.shape == (B, BB, N) 325 | assert batch_sdf_grads.shape == (B, BB, N, 3) 326 | assert torch.allclose(batch_sdf_vals, sdf_vals.view(B, BB, N)) 327 | 328 | 329 | if __name__ == "__main__": 330 | test_urdf_to_sdf() 331 | # test_batch_over_configurations() 332 | # test_bounding_box() 333 | # test_single_link_robot() 334 | -------------------------------------------------------------------------------- /tests/test_sdf.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | 4 | import open3d as o3d 5 | import pytorch_kinematics as pk 6 | import torch 7 | 8 | import pytorch_volumetric as pv 9 | from pytorch_volumetric import sample_mesh_points 10 | 11 | TEST_DIR = os.path.dirname(__file__) 12 | 13 | 14 | def do_test_gradients_at_surface_pts(mesh): 15 | d = "cuda" if torch.cuda.is_available() else "cpu" 16 | 17 | # press n to visualize the normals / gradients 18 | obj = pv.MeshObjectFactory(os.path.join(TEST_DIR, mesh)) 19 | sdf = pv.MeshSDF(obj) 20 | 21 | # sample points on the obj mesh surface uniformly 22 | pts, normals, _ = sample_mesh_points(obj, name=mesh, num_points=1000) 23 | 24 | # query the sdf value and gradient at the sampled points 25 | sdf_vals, sdf_grads = sdf(pts) 26 | 27 | assert torch.allclose(sdf_vals.abs(), torch.zeros_like(sdf_vals), atol=1e-4) 28 | 29 | # test batch query 30 | batch_pts = pts.view(10, 100, -1) 31 | batch_sdf_vals, batch_sdf_grads = sdf(batch_pts) 32 | assert batch_sdf_vals.shape == (10, 100) 33 | assert torch.allclose(batch_sdf_vals.abs(), torch.zeros_like(batch_sdf_vals), atol=1e-4) 34 | 35 | print("Press L to turn off the light to better compare colors") 36 | print("Press N to visualize the normals / gradients") 37 | print("Press Q to move onto the next visualization") 38 | 39 | pcd = o3d.geometry.PointCloud() 40 | pcd.points = o3d.utility.Vector3dVector(pts.cpu()) 41 | pcd.normals = o3d.utility.Vector3dVector(normals.cpu()) 42 | pcd.paint_uniform_color([0, 1, 0]) 43 | o3d.visualization.draw_geometries([sdf.obj_factory._mesh, pcd]) 44 | 45 | # compare the gradient against the surface normals plotted before - should be the same 46 | pcd.normals = o3d.utility.Vector3dVector(sdf_grads.cpu()) 47 | # color based on sdf value 48 | colors = torch.zeros_like(sdf_vals).view(-1, 1).repeat(1, 3) 49 | colors[:, 0] = (sdf_vals - sdf_vals.min()) / (sdf_vals.max() - sdf_vals.min()) 50 | pcd.colors = o3d.utility.Vector3dVector(colors.cpu()) 51 | o3d.visualization.draw_geometries([sdf.obj_factory._mesh, pcd]) 52 | 53 | # sample points in the bounding box 54 | coords, pts = pv.get_coordinates_and_points_in_grid(0.002, obj.bounding_box(0.01), device=d) 55 | # randomly downsample to some number of points 56 | pts = pts[torch.randperm(len(pts))[:1000]] 57 | # query the sdf value and gradient at the sampled points 58 | sdf_vals, sdf_grads = sdf(pts) 59 | # visualize the sdf value and gradient at the sampled points 60 | pcd.points = o3d.utility.Vector3dVector(pts.cpu()) 61 | pcd.normals = o3d.utility.Vector3dVector(sdf_grads.cpu()) 62 | colors = torch.zeros_like(sdf_vals).view(-1, 1).repeat(1, 3) 63 | colors[:, 0] = (sdf_vals - sdf_vals.min()) / (sdf_vals.max() - sdf_vals.min()) 64 | colors[:, 1] = 1 65 | pcd.colors = o3d.utility.Vector3dVector(colors.cpu()) 66 | o3d.visualization.draw_geometries([sdf.obj_factory._mesh, pcd]) 67 | 68 | 69 | def test_compose_sdf(): 70 | d = "cuda" if torch.cuda.is_available() else "cpu" 71 | 72 | obj = pv.MeshObjectFactory("YcbPowerDrill/textured_simple_reoriented.obj") 73 | 74 | # 2 drills in the world 75 | sdf1 = pv.MeshSDF(obj) 76 | sdf2 = pv.MeshSDF(obj) 77 | # need to specify the transform of each SDF frame 78 | tsf1 = pk.Translate(0.1, 0, 0, device=d) 79 | tsf2 = pk.Translate(-0.2, 0, 0.2, device=d) 80 | sdf = pv.ComposedSDF([sdf1, sdf2], tsf1.stack(tsf2)) 81 | # sample points in the bounding box 82 | 83 | coords, pts = pv.get_coordinates_and_points_in_grid(0.002, obj.bounding_box(0.01), device=d) 84 | # randomly downsample to some number of points 85 | pts = pts[torch.randperm(len(pts))[:1000]] 86 | # query the sdf value and gradient at the sampled points 87 | sdf_vals, sdf_grads = sdf(pts) 88 | # visualize the sdf value and gradient at the sampled points 89 | pcd = o3d.geometry.PointCloud() 90 | pcd.points = o3d.utility.Vector3dVector(pts.cpu()) 91 | pcd.normals = o3d.utility.Vector3dVector(sdf_grads.cpu()) 92 | colors = torch.zeros_like(sdf_vals).view(-1, 1).repeat(1, 3) 93 | colors[:, 0] = (sdf_vals - sdf_vals.min()) / (sdf_vals.max() - sdf_vals.min()) 94 | colors[:, 1] = 1 95 | pcd.colors = o3d.utility.Vector3dVector(colors.cpu()) 96 | o3d.visualization.draw_geometries([sdf1.obj_factory._mesh, pcd]) 97 | 98 | 99 | def test_differentiability(): 100 | # for a sdf_vals, sdf_grads = sdf(pts) call, d(sdf_vals)/d(pts) should be sdf_grads 101 | d = "cuda" if torch.cuda.is_available() else "cpu" 102 | 103 | obj = pv.MeshObjectFactory("YcbPowerDrill/textured_simple_reoriented.obj") 104 | 105 | # sample points in the bounding box 106 | coords, pts = pv.get_coordinates_and_points_in_grid(0.002, obj.bounding_box(0.01), device=d) 107 | # randomly downsample to some number of points 108 | pts = pts[torch.randperm(len(pts))[:1000]] 109 | pts.requires_grad = True 110 | 111 | # test with various classes of SDF 112 | 113 | sdf = pv.MeshSDF(obj) 114 | # query the sdf value and gradient at the sampled points 115 | sdf_vals, sdf_grads = sdf(pts) 116 | sdf_vals.sum().backward() 117 | assert torch.allclose(pts.grad, sdf_grads, atol=1e-4) 118 | # clear the gradients 119 | pts.grad.zero_() 120 | 121 | sdf = pv.ComposedSDF([pv.MeshSDF(obj)], None) 122 | sdf.set_transforms(pk.Translate(0.1, 0, 0, device=d), batch_dim=(1,)) 123 | sdf_vals, sdf_grads = sdf(pts) 124 | sdf_vals.sum().backward() 125 | assert torch.allclose(pts.grad, sdf_grads, atol=1e-4) 126 | pts.grad.zero_() 127 | 128 | sdf = pv.CachedSDF('drill', resolution=0.01, range_per_dim=obj.bounding_box(padding=0.1), gt_sdf=pv.MeshSDF(obj)) 129 | sdf_vals, sdf_grads = sdf(pts) 130 | sdf_vals.sum().backward() 131 | assert torch.allclose(pts.grad, sdf_grads, atol=1e-4) 132 | pts.grad.zero_() 133 | 134 | 135 | def test_gradients_at_surface_pts(): 136 | do_test_gradients_at_surface_pts("probe.obj") 137 | do_test_gradients_at_surface_pts("offset_wrench_nogrip.obj") 138 | 139 | 140 | def test_lookup_performance(): 141 | N = 20000 142 | d = "cuda" if torch.cuda.is_available() else "cpu" 143 | obj = pv.MeshObjectFactory("YcbPowerDrill/textured_simple_reoriented.obj") 144 | 145 | # sample points in the bounding box 146 | coords, orig_pts = pv.get_coordinates_and_points_in_grid(0.002, obj.bounding_box(0.01), device=d) 147 | 148 | for _ in range(10): 149 | # randomly downsample to some number of points 150 | pts = orig_pts[torch.randperm(len(orig_pts))[:N]] 151 | N = len(pts) 152 | 153 | # test with various classes of SDF 154 | sdf = pv.MeshSDF(obj) 155 | # profile 156 | start = time.time() 157 | sdf_vals, sdf_grads = sdf(pts) 158 | num_hash = sdf_vals.sum() 159 | print(f"Time taken for {N} points: {time.time() - start:.4f} s for {d}; hash: {num_hash.item()}") 160 | 161 | cache_sdf = pv.CachedSDF('drill', resolution=0.01, range_per_dim=obj.bounding_box(padding=0.1), gt_sdf=sdf) 162 | start = time.time() 163 | sdf_vals, sdf_grads = cache_sdf(pts) 164 | num_hash = sdf_vals.sum() 165 | print(f"Time taken for {N} points: {time.time() - start:.4f} s for {d}; hash: {num_hash.item()} cached") 166 | 167 | pts.requires_grad = True 168 | start = time.time() 169 | sdf_vals, sdf_grads = sdf(pts) 170 | num_hash = sdf_vals.sum() 171 | print( 172 | f"Time taken for {N} points: {time.time() - start:.4f} s for {d}; hash: {num_hash.detach().item()} with grad") 173 | pts.requires_grad = False 174 | 175 | 176 | if __name__ == "__main__": 177 | test_gradients_at_surface_pts() 178 | test_compose_sdf() 179 | test_differentiability() 180 | test_lookup_performance() 181 | -------------------------------------------------------------------------------- /tests/test_voxel_sdf.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import torch 3 | import pytorch_volumetric as pv 4 | 5 | to_plot = True 6 | 7 | 8 | def test_voxel_down_sample(): 9 | N = 100 10 | 11 | def f(x, y): 12 | return torch.sin(x) + 2 * torch.cos(y) 13 | 14 | x = torch.linspace(-2, 2, N) 15 | y = torch.linspace(-2, 2, N) 16 | xx, yy = torch.meshgrid(x, y) 17 | values = f(xx, yy).flatten() 18 | pts = torch.stack((xx.flatten(), yy.flatten(), values), dim=-1) 19 | 20 | bounds = 4 21 | prev_resolution = bounds / N 22 | new_resolution = 0.2 23 | reduce_factor = prev_resolution / new_resolution 24 | pts_reduced = pv.voxel_down_sample(pts, new_resolution) 25 | 26 | values_reduced = f(pts_reduced[:, 0], pts_reduced[:, 1]) 27 | assert pts_reduced.shape[0] < pts.shape[0] * reduce_factor 28 | # expect an error of around the new resolution 29 | assert torch.allclose(values_reduced, pts_reduced[:, 2], atol=new_resolution * 2) 30 | # plt.scatter(pts[:, 0], pts[:, 1], c=values) 31 | # plot 3d 32 | if to_plot: 33 | fig = plt.figure() 34 | ax = fig.add_subplot(111, projection='3d') 35 | # plot 3d surface 36 | ax.plot_surface(xx, yy, values.reshape(xx.shape)) 37 | ax.scatter(pts[:, 0], pts[:, 1], pts[:, 2]) 38 | ax.scatter(pts_reduced[:, 0], pts_reduced[:, 1], pts_reduced[:, 2]) 39 | plt.show() 40 | 41 | 42 | if __name__ == "__main__": 43 | test_voxel_down_sample() 44 | --------------------------------------------------------------------------------