├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── data ├── Squirrel.mtl ├── Squirrel_texture.png ├── Squirrel_visual.obj ├── pointcloud_color.pcd └── rgbd │ ├── r_0.png │ ├── r_0_depth_0019.png │ ├── r_1.png │ ├── r_1_depth_0019.png │ ├── r_2.png │ ├── r_2_depth_0019.png │ ├── r_3.png │ ├── r_3_depth_0019.png │ ├── r_4.png │ ├── r_4_depth_0019.png │ ├── r_5.png │ ├── r_5_depth_0019.png │ ├── r_6.png │ ├── r_6_depth_0019.png │ ├── r_7.png │ ├── r_7_depth_0019.png │ ├── r_8.png │ ├── r_8_depth_0019.png │ ├── r_9.png │ ├── r_9_depth_0019.png │ └── transforms.json ├── examples ├── multiview_rgbd_fusion.py ├── pointcloud_io.py ├── render_mesh_nvisii.py ├── render_mesh_pyrender.py └── visualize_3d_pointcloud.py ├── requirements-extra.txt ├── requirements.txt ├── setup.cfg ├── setup.py ├── tests └── unit │ └── test_io.py └── utils3d ├── __init__.py ├── mesh ├── __init__.py ├── io.py └── utils.py ├── pointcloud ├── __init__.py ├── io.py ├── utils.py └── visualization.py ├── render ├── __init__.py ├── nvisii.py └── pyrender.py ├── rgbd ├── __init__.py ├── fusion.py ├── io.py └── utils.py └── utils ├── __init__.py ├── transform.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v3.4.0 4 | hooks: 5 | # list of supported hooks: https://pre-commit.com/hooks.html 6 | - id: trailing-whitespace 7 | - id: end-of-file-fixer 8 | - id: check-yaml 9 | # - id: check-added-large-files 10 | - id: debug-statements 11 | - id: detect-private-key 12 | 13 | # python code formatting 14 | - repo: https://github.com/psf/black 15 | rev: 20.8b1 16 | hooks: 17 | - id: black 18 | args: [--line-length, "99"] 19 | 20 | # python import sorting 21 | - repo: https://github.com/PyCQA/isort 22 | rev: 5.8.0 23 | hooks: 24 | - id: isort 25 | args: ["--profile", "black", "--filter-files"] 26 | 27 | # python code analysis 28 | - repo: https://github.com/PyCQA/flake8 29 | rev: 3.9.2 30 | hooks: 31 | - id: flake8 32 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Zhenyu Jiang 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # utils3D 2 | A small library of 3D related utilities used in my research. 3 | 4 | ## Installation 5 | 6 | ### Install via GitHub 7 | 8 | ```bash 9 | pip install git+https://github.com/Steve-Tod/utils3d --upgrade 10 | ``` 11 | 12 | ### Install locally 13 | 14 | ```bash 15 | git clone git@github.com:Steve-Tod/utils3d.git 16 | cd utils3d 17 | pip install -e . 18 | ``` 19 | 20 | ### Optional 21 | 22 | Install pre-commit to automatically format the code when commit. 23 | 24 | ``` 25 | pip install pre-commit 26 | pre-commit install 27 | ``` 28 | 29 | Install extra package for: 30 | 31 | - Photo realistic rendering (`utils3d.render.nvisii`) 32 | 33 | ``` 34 | pip install -r requirements-extra.txt 35 | 36 | ``` 37 | -------------------------------------------------------------------------------- /data/Squirrel.mtl: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Google LLC. 2 | # 3 | # This work is licensed under the Creative Commons Attribution 4.0 4 | # International License. To view a copy of this license, visit 5 | # http://creativecommons.org/licenses/by/4.0/ or send a letter 6 | # to Creative Commons, PO Box 1866, Mountain View, CA 94042, USA. 7 | newmtl material_0 8 | # shader_type beckmann 9 | map_Kd Squirrel_texture.png 10 | 11 | # Kd: Diffuse reflectivity. 12 | Kd 1.000000 1.000000 1.000000 13 | -------------------------------------------------------------------------------- /data/Squirrel_texture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/Squirrel_texture.png -------------------------------------------------------------------------------- /data/pointcloud_color.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/pointcloud_color.pcd -------------------------------------------------------------------------------- /data/rgbd/r_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_0.png -------------------------------------------------------------------------------- /data/rgbd/r_0_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_0_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_1.png -------------------------------------------------------------------------------- /data/rgbd/r_1_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_1_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_2.png -------------------------------------------------------------------------------- /data/rgbd/r_2_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_2_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_3.png -------------------------------------------------------------------------------- /data/rgbd/r_3_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_3_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_4.png -------------------------------------------------------------------------------- /data/rgbd/r_4_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_4_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_5.png -------------------------------------------------------------------------------- /data/rgbd/r_5_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_5_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_6.png -------------------------------------------------------------------------------- /data/rgbd/r_6_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_6_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_7.png -------------------------------------------------------------------------------- /data/rgbd/r_7_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_7_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_8.png -------------------------------------------------------------------------------- /data/rgbd/r_8_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_8_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/r_9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_9.png -------------------------------------------------------------------------------- /data/rgbd/r_9_depth_0019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/data/rgbd/r_9_depth_0019.png -------------------------------------------------------------------------------- /data/rgbd/transforms.json: -------------------------------------------------------------------------------- 1 | { 2 | "camera_angle_x": 0.691111147403717, 3 | "frames": [ 4 | { 5 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_0", 6 | "rotation": 0.6283185307179586, 7 | "transform_matrix": [ 8 | [ 9 | 0.8592766523361206, 10 | -0.4537835419178009, 11 | 0.2360597848892212, 12 | 0.951587438583374 13 | ], 14 | [ 15 | 0.5115112066268921, 16 | 0.7623012065887451, 17 | -0.3965517282485962, 18 | -1.5985511541366577 19 | ], 20 | [ 21 | -1.4901161193847656e-08, 22 | 0.4614948034286499, 23 | 0.8871428966522217, 24 | 3.5761873722076416 25 | ], 26 | [ 27 | 0.0, 28 | 0.0, 29 | 0.0, 30 | 1.0 31 | ] 32 | ] 33 | }, 34 | { 35 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_1", 36 | "rotation": 0.6283185307179586, 37 | "transform_matrix": [ 38 | [ 39 | 0.7008286118507385, 40 | -0.10256122797727585, 41 | 0.7059181928634644, 42 | 2.8456473350524902 43 | ], 44 | [ 45 | 0.7133297324180603, 46 | 0.10076384246349335, 47 | -0.6935469508171082, 48 | -2.7957770824432373 49 | ], 50 | [ 51 | 0.0, 52 | 0.989609956741333, 53 | 0.14377816021442413, 54 | 0.5795882940292358 55 | ], 56 | [ 57 | 0.0, 58 | 0.0, 59 | 0.0, 60 | 1.0 61 | ] 62 | ] 63 | }, 64 | { 65 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_2", 66 | "rotation": 0.6283185307179586, 67 | "transform_matrix": [ 68 | [ 69 | -0.5528996586799622, 70 | 0.19426241517066956, 71 | -0.8102863430976868, 72 | -3.266368865966797 73 | ], 74 | [ 75 | -0.8332477807998657, 76 | -0.1289023756980896, 77 | 0.5376636385917664, 78 | 2.167391777038574 79 | ], 80 | [ 81 | 0.0, 82 | 0.9724434614181519, 83 | 0.2331387996673584, 84 | 0.9398126006126404 85 | ], 86 | [ 87 | 0.0, 88 | 0.0, 89 | 0.0, 90 | 1.0 91 | ] 92 | ] 93 | }, 94 | { 95 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_3", 96 | "rotation": 0.6283185307179586, 97 | "transform_matrix": [ 98 | [ 99 | -0.8820494413375854, 100 | 0.4372628927230835, 101 | -0.17547060549259186, 102 | -0.707344651222229 103 | ], 104 | [ 105 | -0.4711568355560303, 106 | -0.8185968399047852, 107 | 0.3284973204135895, 108 | 1.324215054512024 109 | ], 110 | [ 111 | 0.0, 112 | 0.3724250793457031, 113 | 0.9280622601509094, 114 | 3.7411386966705322 115 | ], 116 | [ 117 | 0.0, 118 | 0.0, 119 | 0.0, 120 | 1.0 121 | ] 122 | ] 123 | }, 124 | { 125 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_4", 126 | "rotation": 0.6283185307179586, 127 | "transform_matrix": [ 128 | [ 129 | -0.9990124702453613, 130 | -0.03346855938434601, 131 | 0.029221588745713234, 132 | 0.11779599636793137 133 | ], 134 | [ 135 | 0.04443023353815079, 136 | -0.7525395154953003, 137 | 0.6570464968681335, 138 | 2.648639440536499 139 | ], 140 | [ 141 | 0.0, 142 | 0.657696008682251, 143 | 0.7532833218574524, 144 | 3.0365824699401855 145 | ], 146 | [ 147 | 0.0, 148 | 0.0, 149 | 0.0, 150 | 1.0 151 | ] 152 | ] 153 | }, 154 | { 155 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_5", 156 | "rotation": 0.6283185307179586, 157 | "transform_matrix": [ 158 | [ 159 | -0.1428895890712738, 160 | 0.9301019906997681, 161 | -0.33836793899536133, 162 | -1.3640047311782837 163 | ], 164 | [ 165 | -0.9897387027740479, 166 | -0.1342797875404358, 167 | 0.04885053262114525, 168 | 0.19692279398441315 169 | ], 170 | [ 171 | 0.0, 172 | 0.3418760299682617, 173 | 0.9397450685501099, 174 | 3.788233518600464 175 | ], 176 | [ 177 | 0.0, 178 | 0.0, 179 | 0.0, 180 | 1.0 181 | ] 182 | ] 183 | }, 184 | { 185 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_6", 186 | "rotation": 0.6283185307179586, 187 | "transform_matrix": [ 188 | [ 189 | -0.34585410356521606, 190 | 0.14360204339027405, 191 | -0.9272342324256897, 192 | -3.7378008365631104 193 | ], 194 | [ 195 | -0.9382883906364441, 196 | -0.05293187499046326, 197 | 0.34177955985069275, 198 | 1.3777574300765991 199 | ], 200 | [ 201 | -3.725290742551124e-09, 202 | 0.9882189631462097, 203 | 0.15304681658744812, 204 | 0.6169514656066895 205 | ], 206 | [ 207 | 0.0, 208 | 0.0, 209 | 0.0, 210 | 1.0 211 | ] 212 | ] 213 | }, 214 | { 215 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_7", 216 | "rotation": 0.6283185307179586, 217 | "transform_matrix": [ 218 | [ 219 | -0.16158853471279144, 220 | -0.2685190439224243, 221 | 0.9496244788169861, 222 | 3.8280587196350098 223 | ], 224 | [ 225 | 0.9868583083152771, 226 | -0.043967410922050476, 227 | 0.15549185872077942, 228 | 0.6268077492713928 229 | ], 230 | [ 231 | 3.725290742551124e-09, 232 | 0.9622703790664673, 233 | 0.27209484577178955, 234 | 1.0968494415283203 235 | ], 236 | [ 237 | 0.0, 238 | 0.0, 239 | 0.0, 240 | 1.0 241 | ] 242 | ] 243 | }, 244 | { 245 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_8", 246 | "rotation": 0.6283185307179586, 247 | "transform_matrix": [ 248 | [ 249 | -0.9744495153427124, 250 | -0.22459661960601807, 251 | 0.0021242694929242134, 252 | 0.008563205599784851 253 | ], 254 | [ 255 | 0.22460666298866272, 256 | -0.9744059443473816, 257 | 0.009216081351041794, 258 | 0.037151217460632324 259 | ], 260 | [ 261 | 0.0, 262 | 0.009453319944441319, 263 | 0.9999552369117737, 264 | 4.030948638916016 265 | ], 266 | [ 267 | 0.0, 268 | 0.0, 269 | 0.0, 270 | 1.0 271 | ] 272 | ] 273 | }, 274 | { 275 | "file_path": "/Users/zhenyu/Downloads/blender/train_start/r_9", 276 | "rotation": 0.6283185307179586, 277 | "transform_matrix": [ 278 | [ 279 | 0.6303862929344177, 280 | -0.750603199005127, 281 | 0.19801023602485657, 282 | 0.798204779624939 283 | ], 284 | [ 285 | 0.7762816548347473, 286 | 0.6095339059829712, 287 | -0.16079594194889069, 288 | -0.6481891870498657 289 | ], 290 | [ 291 | 0.0, 292 | 0.2550751566886902, 293 | 0.9669212102890015, 294 | 3.8977839946746826 295 | ], 296 | [ 297 | 0.0, 298 | 0.0, 299 | 0.0, 300 | 1.0 301 | ] 302 | ] 303 | } 304 | ] 305 | } 306 | -------------------------------------------------------------------------------- /examples/multiview_rgbd_fusion.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import glob 3 | import json 4 | import os 5 | 6 | import numpy as np 7 | from PIL import Image 8 | 9 | from utils3d.pointcloud.visualization import visualize_3d_point_cloud_o3d 10 | from utils3d.rgbd.io import read_rgbd 11 | from utils3d.rgbd.utils import tsdf_fusion 12 | 13 | 14 | def main(): 15 | parser = argparse.ArgumentParser() 16 | parser.add_argument( 17 | "-i", 18 | "--input", 19 | type=str, 20 | default="../data/rgbd", 21 | help="Input rgbd path.", 22 | ) 23 | 24 | parser.add_argument( 25 | "-t", 26 | "--transform", 27 | type=str, 28 | default="../data/rgbd/transforms.json", 29 | help="Input transformation matrix.", 30 | ) 31 | 32 | args = parser.parse_args() 33 | 34 | rgb_path_dict = {} 35 | depth_path_dict = {} 36 | 37 | with open(args.transform) as f: 38 | transform_dict = json.load(f) 39 | 40 | rgbd_list = [] 41 | transform_list = [] 42 | 43 | for idx in range(10): 44 | # rgb_p = f'../data/rgbd/r_{idx}.png' 45 | # depth_p = f'../data/rgbd/r_{idx}_depth_0019.png' 46 | rgb_p = f"/home/zhenyu/Downloads/real_music_box/imgs_1/{idx}_color.png" 47 | depth_p = f"/home/zhenyu/Downloads/real_music_box/imgs_1/{idx}_depth.png" 48 | rgbd_list.append(read_rgbd(rgb_p, depth_p, depth_scale=1000, depth_trunc=1)) 49 | transform_list.append(np.array(transform_dict["frames"][idx]["transform_matrix"])) 50 | # print(np.unique(np.asanyarray(rgbd_list[0].depth))) 51 | # compute intrinsics 52 | camera_angle_x = transform_dict["camera_angle_x"] 53 | img = Image.open(rgb_p) 54 | w, h = img.size 55 | fx = w / 2.0 / np.tan(camera_angle_x / 2) 56 | fy = h / 2.0 / np.tan(camera_angle_x / 2) * (w / h) 57 | intrinsics = {"height": h, "width": w, "fx": fx, "fy": fy, "cx": w / 2, "cy": h / 2} 58 | 59 | # fusion 60 | tsdf_vol = tsdf_fusion(rgbd_list, transform_list, intrinsics) 61 | # verts, faces, norms, colors = tsdf_vol.get_mesh() 62 | point_cloud = tsdf_vol.get_point_cloud() 63 | 64 | visualize_3d_point_cloud_o3d(point_cloud[:, :3], point_cloud[:, 3:6]) 65 | 66 | 67 | if __name__ == "__main__": 68 | main() 69 | -------------------------------------------------------------------------------- /examples/pointcloud_io.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | import numpy as np 4 | 5 | from utils3d.pointcloud.io import read_pointcloud, write_pointcloud 6 | from utils3d.pointcloud.utils import sample_pointcloud 7 | 8 | 9 | def main(): 10 | parser = argparse.ArgumentParser() 11 | parser.add_argument( 12 | "-i", 13 | "--input", 14 | type=str, 15 | default="../data/pointcloud_color.pcd", 16 | help="Input point cloud path.", 17 | ) 18 | parser.add_argument("-o", "--output", type=str, help="Output point cloud path.") 19 | args = parser.parse_args() 20 | 21 | xyz, color = read_pointcloud(args.input) 22 | xyz, color = sample_pointcloud(xyz, color, 2048) 23 | 24 | print(f"Saving point cloud of shape {xyz.shape} to {args.output}") 25 | write_pointcloud(xyz, args.output, color=color) 26 | 27 | 28 | if __name__ == "__main__": 29 | main() 30 | -------------------------------------------------------------------------------- /examples/render_mesh_nvisii.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | 6 | from utils3d.mesh.io import read_mesh 7 | from utils3d.render.nvisii import NViSIIRenderer 8 | from utils3d.utils.utils import get_pose 9 | 10 | 11 | def main(): 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument( 14 | "-i", 15 | "--input", 16 | type=str, 17 | default="../data/Squirrel_visual.obj", 18 | help="Input mesh path.", 19 | ) 20 | args = parser.parse_args() 21 | 22 | NViSIIRenderer.init() 23 | renderer = NViSIIRenderer() 24 | 25 | mesh_pose_dict = {"mesh": (args.input, [1.0] * 3, np.eye(4))} 26 | 27 | mesh = read_mesh(args.input) 28 | center = mesh.bounds.mean(0) 29 | scale = np.sqrt(((mesh.bounds[1] - mesh.bounds[0]) ** 2).sum()) 30 | camera_pose = get_pose(scale * 1, center=center, ax=np.pi / 3, az=np.pi / 3) 31 | # camera_pose = get_pose(scale * 1, center=center, ax=0, az=np.pi) 32 | light_pose = get_pose(scale * 2, center=center, ax=np.pi / 3, az=np.pi / 3) 33 | 34 | renderer.reset(camera_pose, light_pose) 35 | renderer.update_objects(mesh_pose_dict) 36 | img = renderer.render() 37 | 38 | fig, ax = plt.subplots(1, 1, dpi=150) 39 | ax.imshow(img) 40 | plt.show() 41 | 42 | NViSIIRenderer.deinit() 43 | 44 | 45 | if __name__ == "__main__": 46 | main() 47 | -------------------------------------------------------------------------------- /examples/render_mesh_pyrender.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | 6 | from utils3d.mesh.io import read_mesh 7 | from utils3d.render.pyrender import PyRenderer 8 | from utils3d.utils.utils import get_pose 9 | 10 | 11 | def main(): 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument( 14 | "-i", 15 | "--input", 16 | type=str, 17 | default="../data/Squirrel_visual.obj", 18 | help="Input mesh path.", 19 | ) 20 | args = parser.parse_args() 21 | 22 | renderer = PyRenderer() 23 | 24 | mesh = read_mesh(args.input) 25 | center = mesh.bounds.mean(0) 26 | scale = np.sqrt(((mesh.bounds[1] - mesh.bounds[0]) ** 2).sum()) 27 | 28 | surface_point_cloud = mesh.sample(2048) 29 | 30 | camera_pose = get_pose(scale * 1, center=center, ax=np.pi / 3, az=np.pi / 3) 31 | light_pose = get_pose(scale * 2, center=center, ax=np.pi / 3, az=np.pi / 3) 32 | img, depth = renderer.render_mesh(mesh, camera_pose=camera_pose, light_pose=light_pose) 33 | 34 | pc_light_pose = get_pose(scale * 4, center=center, ax=np.pi / 3, az=np.pi / 3) 35 | pc_img, _ = renderer.render_pointcloud( 36 | surface_point_cloud, 37 | camera_pose=camera_pose, 38 | light_pose=pc_light_pose, 39 | radius=scale * 0.01, 40 | colors=[102, 204, 102, 102], 41 | ) 42 | 43 | fig, axs = plt.subplots(1, 3, dpi=150) 44 | axs[0].imshow(img) 45 | axs[1].imshow(pc_img) 46 | axs[2].imshow(depth) 47 | plt.show() 48 | 49 | 50 | if __name__ == "__main__": 51 | main() 52 | -------------------------------------------------------------------------------- /examples/visualize_3d_pointcloud.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | from utils3d.pointcloud.io import read_pointcloud 4 | from utils3d.pointcloud.utils import normalize_pointcloud 5 | from utils3d.pointcloud.visualization import ( 6 | visualize_3d_point_cloud_mpl, 7 | visualize_3d_point_cloud_o3d, 8 | ) 9 | 10 | 11 | def main(): 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument( 14 | "-t", "--tool", type=str, choices=["o3d", "mpl"], help="Which tool for visualization." 15 | ) 16 | parser.add_argument( 17 | "-i", 18 | "--input", 19 | type=str, 20 | default="../data/pointcloud_color.pcd", 21 | help="Input point cloud path.", 22 | ) 23 | parser.add_argument("--norm", action="store_true", help="Normalize data.") 24 | args = parser.parse_args() 25 | 26 | xyz, color = read_pointcloud(args.input) 27 | 28 | # normalize point cloud 29 | if args.norm: 30 | xyz, _, _ = normalize_pointcloud(xyz) 31 | 32 | if args.tool == "o3d": 33 | visualize_3d_point_cloud_o3d(xyz, color=color) 34 | else: 35 | visualize_3d_point_cloud_mpl(xyz, color=color) 36 | 37 | 38 | if __name__ == "__main__": 39 | main() 40 | -------------------------------------------------------------------------------- /requirements-extra.txt: -------------------------------------------------------------------------------- 1 | nvisii 2 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | . 2 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [isort] 2 | line_length = 99 3 | profile = black 4 | filter_files = True 5 | 6 | 7 | [flake8] 8 | max_line_length = 99 9 | show_source = True 10 | format = pylint 11 | ignore = 12 | F401 # Module imported but unused 13 | W503 # line break before binary operator 14 | W504 # Line break occurred after a binary operator 15 | F841 # Local variable name is assigned to but never used 16 | F403 # from module import * 17 | E501 # Line too long 18 | E402 # module level import not at top of file 19 | exclude = 20 | .git 21 | __pycache__ 22 | data/* 23 | tests/* 24 | notebooks/* 25 | logs/* 26 | utils3d/utils/pyrender.py # have to change environ first 27 | 28 | 29 | [tool:pytest] 30 | python_files = tests/* 31 | log_cli = True 32 | markers = 33 | slow 34 | addopts = 35 | --durations=0 36 | --strict-markers 37 | --doctest-modules 38 | filterwarnings = 39 | ignore::DeprecationWarning 40 | ignore::UserWarning 41 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | setup( 4 | name="utils3d", # change "src" folder name to your project name 5 | version="0.0.0", 6 | description="some 3D related utilities", 7 | author="Zhenyu Jiang", 8 | author_email="stevetod98@gmail.com", 9 | url="https://github.com/Steve-Tod/utils3d", # replace with your own github project link 10 | install_requires=[ 11 | "numpy", 12 | "matplotlib", 13 | "pillow", 14 | "trimesh", 15 | "open3d", 16 | "scipy", 17 | "pyrender", 18 | "pytest", 19 | "numba", 20 | "scikit-image", 21 | ], 22 | packages=find_packages(), 23 | ) 24 | -------------------------------------------------------------------------------- /tests/unit/test_io.py: -------------------------------------------------------------------------------- 1 | import os 2 | import tempfile 3 | 4 | import pytest 5 | import trimesh 6 | 7 | from utils3d.mesh.io import read_mesh, write_mesh 8 | from utils3d.pointcloud.io import read_pointcloud, write_pointcloud 9 | from utils3d.pointcloud.utils import sample_pointcloud 10 | 11 | 12 | @pytest.mark.parametrize("num_point", [1, 2048]) 13 | def test_pointcloud_io(num_point): 14 | xyz, color = read_pointcloud("data/pointcloud_color.pcd") 15 | xyz, color = sample_pointcloud(xyz, color, 2048) 16 | with tempfile.TemporaryDirectory() as tmpdirname: 17 | output = os.path.join(tmpdirname, "tmp.npy") 18 | write_pointcloud(xyz, output, color=color) 19 | 20 | output = os.path.join(tmpdirname, "tmp.pcd") 21 | write_pointcloud(xyz, output, color=color) 22 | 23 | output = os.path.join(tmpdirname, "tmp.txt") 24 | write_pointcloud(xyz, output, color=color) 25 | 26 | output = os.path.join(tmpdirname, "tmp.pts") 27 | write_pointcloud(xyz, output, color=color) 28 | 29 | 30 | def test_mesh_io(): 31 | mesh = read_mesh("data/Squirrel_visual.obj") 32 | with tempfile.TemporaryDirectory() as tmpdirname: 33 | output = os.path.join(tmpdirname, "tmp.stl") 34 | write_mesh(mesh, output) 35 | -------------------------------------------------------------------------------- /utils3d/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/utils3d/__init__.py -------------------------------------------------------------------------------- /utils3d/mesh/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/utils3d/mesh/__init__.py -------------------------------------------------------------------------------- /utils3d/mesh/io.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-01-12 5 | @desc 6 | """ 7 | 8 | import trimesh 9 | 10 | 11 | def read_mesh(path): 12 | return trimesh.load(path) 13 | 14 | 15 | def write_mesh(mesh, path): 16 | mesh.export(path) 17 | -------------------------------------------------------------------------------- /utils3d/mesh/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-01-12 5 | @desc Mesh utils 6 | """ 7 | import trimesh 8 | 9 | 10 | def as_mesh(scene_or_mesh): 11 | """Convert a possible scene to mesh 12 | 13 | Args: 14 | scene_or_mesh (trimesh.Scene or trimesh.Trimesh): input scene or mesh. 15 | 16 | Returns: 17 | trimesh.Trimesh: Converted mesh with only vertex and face data. 18 | """ 19 | if isinstance(scene_or_mesh, trimesh.Scene): 20 | if len(scene_or_mesh.geometry) == 0: 21 | mesh = None # empty scene 22 | else: 23 | # we lose texture information here 24 | mesh = trimesh.util.concatenate( 25 | tuple( 26 | trimesh.Trimesh(vertices=g.vertices, faces=g.faces, visual=g.visual) 27 | for g in scene_or_mesh.geometry.values() 28 | ) 29 | ) 30 | else: 31 | assert isinstance(scene_or_mesh, trimesh.Trimesh) 32 | mesh = scene_or_mesh 33 | return mesh 34 | -------------------------------------------------------------------------------- /utils3d/pointcloud/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/utils3d/pointcloud/__init__.py -------------------------------------------------------------------------------- /utils3d/pointcloud/io.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-01-11 5 | @desc Point cloud IO 6 | """ 7 | 8 | import os 9 | 10 | import numpy as np 11 | import open3d as o3d 12 | from numpy.lib import shape_base 13 | 14 | from utils3d.pointcloud.utils import assert_array_shape, np_to_o3d_pointcloud 15 | 16 | 17 | def read_pointcloud(path): 18 | """read point cloud 19 | 20 | Args: 21 | path (str): path to point cloud file 22 | 23 | Raises: 24 | NotImplementedError: if the loader for certain type of file is not implemented 25 | 26 | Returns: 27 | np.ndarry: loaded point cloud. 28 | np.ndarry or None: loaded color. 29 | """ 30 | _, ext = os.path.splitext(path) 31 | ext = ext[1:] # remove . 32 | if ext in ["txt", "pts"]: 33 | xyz = np.loadtxt(path) 34 | elif ext in ["npy"]: 35 | xyz = np.load(path) 36 | elif ext in ["ply", "pcd"]: 37 | pcd = o3d.io.read_point_cloud(path) 38 | xyz = np.asarray(pcd.points) 39 | if pcd.colors is not None: 40 | color = np.asarray(pcd.colors) 41 | xyz = np.concatenate((xyz, color), axis=1) 42 | else: 43 | raise NotImplementedError(f"No loader for {ext} files. Can't load {path}") 44 | assert_array_shape(xyz, shapes=((-1, 3), (-1, 6))) 45 | 46 | if xyz.shape[1] > 3: # with color 47 | color = xyz[:, 3:] 48 | xyz = xyz[:, :3] 49 | else: 50 | color = None 51 | 52 | return xyz, color 53 | 54 | 55 | def write_pointcloud(xyz, path, color=None): 56 | """save point cloud 57 | 58 | Args: 59 | xyz (np.ndarray): point cloud to save, N*3 or N*6 60 | path (str): path to point cloud file 61 | 62 | Raises: 63 | NotImplementedError: if the saver for certain type of file is not implemented 64 | 65 | """ 66 | if color is not None: 67 | assert_array_shape(color, shapes=((xyz.shape,))) 68 | xyz = np.concatenate((xyz, color), axis=1) 69 | 70 | _, ext = os.path.splitext(path) 71 | ext = ext[1:] # remove . 72 | if ext in ["txt", "pts"]: 73 | np.savetxt(path, xyz) 74 | elif ext in ["npy"]: 75 | np.save(path, xyz) 76 | elif ext in ["ply", "pcd"]: 77 | if xyz.shape[1] == 6: 78 | color = xyz[:, 3:] 79 | xyz = xyz[:, :3] 80 | else: 81 | color = None 82 | pcd = np_to_o3d_pointcloud(xyz, color=color) 83 | o3d.io.write_point_cloud(path, pcd) 84 | else: 85 | raise NotImplementedError(f"No saver for {ext} files. Can't save {path}") 86 | -------------------------------------------------------------------------------- /utils3d/pointcloud/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-01-12 5 | @desc Point cloud utilities 6 | """ 7 | 8 | import numpy as np 9 | import open3d as o3d 10 | 11 | 12 | def assert_array_shape(xyz, shapes=((-1, 3),)): 13 | """check array shape 14 | 15 | Args: 16 | xyz (np.ndarray): array 17 | shape (tuple of tuple of ints, optional): possible target shapes, -1 means arbitrary. Defaults to ((-1, 3)). 18 | 19 | Raises: 20 | ValueError. 21 | """ 22 | flags = {x: True for x in range(len(shapes))} 23 | for idx, shape in enumerate(shapes): 24 | if len(xyz.shape) != len(shape): 25 | flags[idx] = False 26 | 27 | for dim, num in enumerate(shape): 28 | if num == -1: 29 | continue 30 | elif xyz.shape[dim] != num: 31 | flags[idx] = False 32 | if sum(flags.values()) == 0: # None of the possible shape works 33 | raise ValueError(f"Input array {xyz.shape} is not in target shapes {shapes}!") 34 | 35 | 36 | def np_to_o3d_pointcloud(xyz, color=None): 37 | """convert numpy array to open3d point cloud 38 | 39 | Args: 40 | xyz (np.ndarray): point cloud 41 | color (np.ndarray, optional): colors of input point cloud. Can be N*3 or 3. Defaults to None. 42 | 43 | Returns: 44 | o3d.geometry.PointCloud: open3d point cloud 45 | """ 46 | assert_array_shape(xyz) 47 | pcd = o3d.geometry.PointCloud() 48 | pcd.points = o3d.utility.Vector3dVector(xyz) 49 | 50 | # add color 51 | if color is not None: 52 | if len(color.shape) == 2: 53 | # same number of colors as the points 54 | assert_array_shape(color, shapes=(xyz.shape,)) 55 | pcd.colors = o3d.utility.Vector3dVector(color) 56 | elif len(color.shape) == 1: 57 | # N*3 58 | color = np.tile(color, (xyz.shape[0], 1)) 59 | assert_array_shape(color, shapes=(xyz.shape,)) 60 | pcd.colors = o3d.utility.Vector3dVector(color) 61 | else: 62 | raise ValueError(f"Bad color with shape {color.shape}") 63 | 64 | return pcd 65 | 66 | 67 | def normalize_pointcloud(xyz, padding=0.0): 68 | """normalize point cloud to [-0.5, 0.5] 69 | 70 | Args: 71 | xyz (np.ndarray): input point cloud, N*3 72 | padding (float, optional): padding. Defaults to 0.0. 73 | 74 | Returns: 75 | np.ndarray: normalized point cloud 76 | np.ndarray: original center 77 | float: original scale 78 | """ 79 | assert_array_shape(xyz) 80 | bound_max = xyz.max(0) 81 | bound_min = xyz.min(0) 82 | center = (bound_max + bound_min) / 2 83 | scale = (bound_max - bound_min).max() 84 | scale = scale * (1 + padding) 85 | normalized_xyz = (xyz - center) / scale 86 | return normalized_xyz, center, scale 87 | 88 | 89 | def sample_pointcloud(xyz, color=None, num_points=2048): 90 | """random subsample point cloud 91 | 92 | Args: 93 | xyz (np.ndarray): input point cloud of N*3. 94 | color (np.ndarray, optional): color of the points, N*3 or None. Defaults to None. 95 | num_points (int, optional): number of subsampled point cloud. Defaults to 2048. 96 | 97 | Returns: 98 | np.ndarray: subsampled point cloud 99 | """ 100 | assert_array_shape(xyz) 101 | replace = num_points > xyz.shape[0] 102 | sample_idx = np.random.choice(np.arange(xyz.shape[0]), size=(num_points,), replace=replace) 103 | 104 | if color is not None: 105 | assert_array_shape(color, shapes=(xyz.shape,)) 106 | color = color[sample_idx] 107 | xyz = xyz[sample_idx] 108 | return xyz, color 109 | -------------------------------------------------------------------------------- /utils3d/pointcloud/visualization.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-01-11 5 | @desc Point cloud interactive visualization utils 6 | """ 7 | 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | import open3d as o3d 11 | from mpl_toolkits.mplot3d import Axes3D 12 | 13 | from utils3d.pointcloud.utils import assert_array_shape, np_to_o3d_pointcloud 14 | 15 | 16 | def visualize_3d_point_cloud_mpl( 17 | xyz, 18 | show=True, 19 | show_axis=True, 20 | in_u_sphere=False, 21 | marker=".", 22 | s=8, 23 | alpha=0.8, 24 | figsize=(5, 5), 25 | elev=10, 26 | azim=240, 27 | axis=None, 28 | title=None, 29 | lim=None, 30 | *args, 31 | **kwargs, 32 | ): 33 | """Visualize 3d point cloud with matplotlib. 34 | 35 | Args: 36 | xyz (np.ndarray): input point cloud as N*3 numpy array. 37 | show (bool, optional): show current plot. Defaults to True. 38 | show_axis (bool, optional): show axis grid. Defaults to True. 39 | in_u_sphere (bool, optional): plot in unit sphere. Defaults to False. 40 | marker (str, optional): marker shape. Defaults to '.'. 41 | s (int, optional): point size. Defaults to 8. 42 | alpha (float, optional): point transparency. Defaults to .8. 43 | figsize (tuple, optional): figure size. Defaults to (5, 5). 44 | elev (int, optional): elevation of view angle. Defaults to 10. 45 | azim (int, optional): azimuth of view angle. Defaults to 240. 46 | axis (mpl_toolkits.mplot3d.axes3d.Axes3D, optional): plot on given axis instead of creating a new. Defaults to None. 47 | title (str of None, optional): plot title. Defaults to None. 48 | lim (list of tuples of floats, optional): limits in three dimension. Defaults to None. 49 | 50 | Returns: 51 | matplotlib.figure.Figure: axis or figure where the point cloud is plotted on. 52 | """ 53 | 54 | assert_array_shape(xyz) 55 | # extract x y z from input points 56 | x = xyz.T[0] 57 | y = xyz.T[1] 58 | z = xyz.T[2] 59 | 60 | if axis is None: 61 | fig = plt.figure(figsize=figsize) 62 | ax = fig.add_subplot(111, projection="3d") 63 | else: 64 | ax = axis 65 | fig = axis 66 | 67 | if title is not None: 68 | plt.title(title) 69 | 70 | sc = ax.scatter(x, y, z, marker=marker, s=s, alpha=alpha, *args, **kwargs) 71 | ax.view_init(elev=elev, azim=azim) 72 | 73 | if lim: 74 | ax.set_xlim3d(*lim[0]) 75 | ax.set_ylim3d(*lim[1]) 76 | ax.set_zlim3d(*lim[2]) 77 | elif in_u_sphere: 78 | ax.set_xlim3d(-0.5, 0.5) 79 | ax.set_ylim3d(-0.5, 0.5) 80 | ax.set_zlim3d(-0.5, 0.5) 81 | else: 82 | lim = (min(np.min(x), np.min(y), np.min(z)), max(np.max(x), np.max(y), np.max(z))) 83 | ax.set_xlim(1.3 * lim[0], 1.3 * lim[1]) 84 | ax.set_ylim(1.3 * lim[0], 1.3 * lim[1]) 85 | ax.set_zlim(1.3 * lim[0], 1.3 * lim[1]) 86 | plt.tight_layout() 87 | 88 | if not show_axis: 89 | plt.axis("off") 90 | 91 | if show: 92 | plt.show() 93 | 94 | return fig 95 | 96 | 97 | def visualize_3d_point_cloud_o3d(xyz, color=None, **kwargs): 98 | """Visualize 3d point cloud with matplotlib. 99 | 100 | Args: 101 | xyz (np.ndarray): input point cloud as N*3 numpy array. 102 | color (np.ndarray, optional): colors of input point cloud. Can be N*3 or 3. Defaults to None. 103 | """ 104 | pcd = np_to_o3d_pointcloud(xyz, color) 105 | o3d.visualization.draw_geometries([pcd], **kwargs) 106 | -------------------------------------------------------------------------------- /utils3d/render/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/utils3d/render/__init__.py -------------------------------------------------------------------------------- /utils3d/render/nvisii.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-01-14 5 | @desc NViSII renderer 6 | """ 7 | 8 | from statistics import mode 9 | 10 | import numpy as np 11 | import nvisii 12 | 13 | from utils3d.utils.transform import Transform 14 | 15 | 16 | class NViSIIRenderer: 17 | def __init__( 18 | self, 19 | resolution=(640, 480), 20 | spp=256, 21 | camera_kwargs={"field_of_view": np.pi / 3.0}, 22 | light_kwargs={"intensity": 3}, 23 | ): 24 | """init function 25 | 26 | Args: 27 | resolution (tuple, optional): render resolution. Defaults to (640, 480). 28 | spp (int, optional): sample point per pixel. Defaults to 256. 29 | camera_kwargs (dict, optional): camera configs. Defaults to {'field_of_view': np.pi / 3.0}. 30 | light_kwargs (dict, optional): light configs. Defaults to {'intensity': 3}. 31 | """ 32 | self.spp = spp 33 | self.width, self.height = resolution 34 | self.camera_kwargs = camera_kwargs 35 | self.light_kwargs = light_kwargs 36 | 37 | def reset(self, camera_pose, light_pose, dome_light_color=(1.0, 1.0, 1.0)): 38 | """clear scenen, reset camera and light 39 | 40 | Args: 41 | camera_pose (np.ndarray): camera pose, 4*4 42 | light_pose (np.ndarray): light pose 4*4 43 | dome_light_color (tuple, optional): dome light color. Defaults to (1.0, 1.0, 1.0) 44 | """ 45 | nvisii.clear_all() 46 | # Camera 47 | self.camera = nvisii.entity.create(name="camera") 48 | self.camera.set_transform(nvisii.transform.create(name="camera_transform")) 49 | self.camera.set_camera( 50 | nvisii.camera.create_from_fov( 51 | name="camera_camera", aspect=self.width / float(self.height), **self.camera_kwargs 52 | ) 53 | ) 54 | nvisii.set_camera_entity(self.camera) 55 | self.set_camera(camera_pose) 56 | 57 | # Light 58 | self.light = nvisii.entity.create( 59 | name="light_0", 60 | mesh=nvisii.mesh.create_plane("light_0", flip_z=True), 61 | transform=nvisii.transform.create("light_1"), 62 | light=nvisii.light.create("light_1"), 63 | ) 64 | self.set_light(light_pose, **self.light_kwargs) 65 | 66 | # Dome color 67 | nvisii.set_dome_light_color(dome_light_color) 68 | 69 | # Floor 70 | # self.floor = nvisii.entity.create( 71 | # name='floor', 72 | # mesh=nvisii.mesh.create_plane('mesh_floor'), 73 | # transform=nvisii.transform.create('transform_floor'), 74 | # material=nvisii.material.create('material_floor')) 75 | # self.set_floor(self.opt['floor']['texture'], 76 | # self.opt['floor']['scale'], 77 | # self.opt['floor']['position']) 78 | 79 | self.objects = {} 80 | 81 | def update_objects(self, mesh_pose_dict): 82 | """update objects in the scene 83 | 84 | Args: 85 | mesh_pose_dict (dict): dict of {'name': (mesh_path: str, scale: tuple of ints, transform: np.ndarray)} 86 | 87 | Returns: 88 | list: list of new object names 89 | list: list of removed object names 90 | """ 91 | new_objects = [] 92 | removed_objects = [] 93 | for k, (path, scale, transform) in mesh_pose_dict.items(): 94 | transform = Transform.from_matrix(transform) 95 | if k not in self.objects.keys(): 96 | obj = nvisii.import_scene(path) 97 | obj.transforms[0].set_position(transform.translation) 98 | obj.transforms[0].set_rotation(transform.rotation.as_quat()) 99 | obj.transforms[0].set_scale(scale) 100 | self.objects[k] = obj 101 | new_objects.append(k) 102 | else: 103 | obj = self.objects[k] 104 | obj.transforms[0].set_position(transform.translation) 105 | obj.transforms[0].set_rotation(transform.rotation.as_quat()) 106 | obj.transforms[0].set_scale(scale) 107 | for k in self.objects.keys(): 108 | if k not in mesh_pose_dict.keys(): 109 | for obj in self.objects[k].entities: 110 | obj.remove(obj.get_name()) 111 | removed_objects.append(k) 112 | for k in removed_objects: 113 | self.objects.pop(k) 114 | 115 | return new_objects, removed_objects 116 | 117 | def render(self): 118 | """render current scene 119 | 120 | Returns: 121 | np.ndarray: rgb image of H*W*4 122 | """ 123 | rgb = nvisii.render(width=self.width, height=self.height, samples_per_pixel=self.spp) 124 | rgb = np.asarray(rgb).reshape(self.height, self.width, 4) 125 | # flip over x axis 126 | rgb = np.flip(rgb, axis=0) 127 | return rgb 128 | 129 | def set_camera(self, pose): 130 | """set camera pose 131 | 132 | Args: 133 | pose (np.ndarray): camera pose 134 | """ 135 | transform = Transform.from_matrix(pose) 136 | self.camera.get_transform().set_position(transform.translation) 137 | self.camera.get_transform().set_rotation(transform.rotation.as_quat()) 138 | 139 | def set_light(self, pose, intensity, scale=(1.0, 1.0, 1.0)): 140 | """set light pose 141 | 142 | Args: 143 | pose (np.ndarray): light pose 144 | intensity (float): light intensity 145 | scale (tuple, optional): light scale. Defaults to (1.0, 1.0, 1.0). 146 | """ 147 | self.light.get_light().set_intensity(intensity) 148 | self.light.get_transform().set_scale(scale) 149 | 150 | transform = Transform.from_matrix(pose) 151 | self.light.get_transform().set_position(transform.translation) 152 | self.light.get_transform().set_rotation(transform.rotation.as_quat()) 153 | 154 | # def set_floor(self, texture_path, scale, position): 155 | # if hasattr(self, 'floor_texture'): 156 | # self.floor_texture.remove(self.floor_texture.get_name()) 157 | # self.floor_texture = nvisii.texture.create_from_file( 158 | # name='floor_texture', path=texture_path) 159 | # self.floor.get_material().set_base_color_texture(self.floor_texture) 160 | # self.floor.get_material().set_roughness(0.4) 161 | # self.floor.get_material().set_specular(0) 162 | 163 | # self.floor.get_transform().set_scale(scale) 164 | # self.floor.get_transform().set_position(position) 165 | 166 | @staticmethod 167 | def init(): 168 | """init NViSII backend""" 169 | nvisii.initialize(headless=True, verbose=False) 170 | nvisii.enable_denoiser() 171 | 172 | @staticmethod 173 | def deinit(): 174 | """shut down NViSII backend""" 175 | nvisii.deinitialize() 176 | -------------------------------------------------------------------------------- /utils3d/render/pyrender.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-01-12 5 | @desc Pyrender utils 6 | """ 7 | import os 8 | from turtle import color 9 | 10 | os.environ["PYOPENGL_PLATFORM"] = "egl" 11 | import numpy as np 12 | import pyrender 13 | import trimesh 14 | 15 | from utils3d.utils.transform import Rotation, Transform 16 | from utils3d.utils.utils import get_pose 17 | 18 | 19 | class PyRenderer: 20 | def __init__( 21 | self, 22 | resolution=(640, 480), 23 | camera_kwargs={"yfov": np.pi / 3.0}, 24 | light_kwargs={"color": np.ones(3), "intensity": 3}, 25 | ): 26 | """Renderer init function 27 | 28 | Args: 29 | resolution (tuple, optional): render resolution, . Defaults to (640, 480). 30 | camera_kwargs (dict, optional): camera properties. Defaults to {'yfov': np.pi/ 3.0}. 31 | light_kwargs (dict, optional): light properties. Defaults to {'color': np.ones(3), 'intensity': 3}. 32 | """ 33 | self.renderer = pyrender.OffscreenRenderer(*resolution) 34 | self.camera = pyrender.PerspectiveCamera(**camera_kwargs) 35 | self.light = pyrender.SpotLight(**light_kwargs) 36 | 37 | def render_mesh(self, scene_or_mesh, camera_pose, light_pose): 38 | """render a mesh or a scene 39 | 40 | Args: 41 | scene_or_mesh (trimesh.Trimesh or trimesh.Scene): the scene or mesh to render 42 | camera_pose (np.ndarray): camera transformation 43 | light_pose (np.ndarray): light transformation 44 | 45 | Returns: 46 | np.ndarray: rendered rgb image 47 | np.ndarray: rendered depth image 48 | """ 49 | if isinstance(scene_or_mesh, trimesh.Trimesh): 50 | scene = trimesh.Scene(scene_or_mesh) 51 | else: 52 | scene = scene_or_mesh 53 | 54 | r_scene = pyrender.Scene() 55 | for mesh in scene.geometry.values(): 56 | o_mesh = pyrender.Mesh.from_trimesh(mesh, smooth=False) 57 | r_scene.add(o_mesh) 58 | r_scene.add(self.camera, name="camera", pose=camera_pose) 59 | r_scene.add(self.light, name="light", pose=light_pose) 60 | rgb, depth = self.renderer.render(r_scene) 61 | return rgb, depth 62 | 63 | def render_pointcloud( 64 | self, xyz, camera_pose, light_pose, radius=0.005, colors=[102, 102, 102, 102] 65 | ): 66 | """render a point cloud 67 | 68 | Args: 69 | xyz (np.ndarray): point cloud to render. 70 | camera_pose (np.ndarray): camera 71 | light_pose ([type]): [description] 72 | radius (float, optional): radius of each ball representing the point. Defaults to 0.005. 73 | colors (list or np.ndarray, optional): colors of the points. Defaults to [102, 102, 102, 102]. 74 | 75 | Returns: 76 | np.ndarray: rendered rgb image 77 | np.ndarray: rendered depth image 78 | """ 79 | r_scene = pyrender.Scene() 80 | tfs = np.tile(np.eye(4), (len(xyz), 1, 1)) 81 | tfs[:, :3, 3] = xyz 82 | if colors is None or isinstance(colors, list) or len(colors.shape) == 1: 83 | # one color for all points 84 | sm = trimesh.creation.uv_sphere(radius=radius) 85 | if colors is None: 86 | sm.visual.vertex_colors = [0.4, 0.4, 0.4, 0.8] 87 | else: 88 | sm.visual.vertex_colors = colors 89 | o_mesh = pyrender.Mesh.from_trimesh(sm, poses=tfs) 90 | 91 | r_scene.add(o_mesh) 92 | else: 93 | # different color for different pints 94 | for idx, tf in enumerate(tfs): 95 | sm = trimesh.creation.uv_sphere(radius=radius) 96 | sm.visual.vertex_colors = colors[idx] 97 | o_mesh = pyrender.Mesh.from_trimesh(sm, poses=tf) 98 | r_scene.add(o_mesh) 99 | 100 | r_scene.add(self.camera, name="camera", pose=camera_pose) 101 | r_scene.add(self.light, name="light", pose=light_pose) 102 | rgb, depth = self.renderer.render(r_scene) 103 | return rgb, depth 104 | -------------------------------------------------------------------------------- /utils3d/rgbd/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/utils3d/rgbd/__init__.py -------------------------------------------------------------------------------- /utils3d/rgbd/fusion.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Andy Zeng 2 | 3 | import numpy as np 4 | from numba import njit, prange 5 | from skimage import measure 6 | 7 | try: 8 | import pycuda.autoinit 9 | import pycuda.driver as cuda 10 | from pycuda.compiler import SourceModule 11 | 12 | FUSION_GPU_MODE = 1 13 | except Exception as err: 14 | print("Warning: {}".format(err)) 15 | print("Failed to import PyCUDA. Running fusion in CPU mode.") 16 | FUSION_GPU_MODE = 0 17 | 18 | 19 | class TSDFVolume: 20 | """Volumetric TSDF Fusion of RGB-D Images.""" 21 | 22 | def __init__(self, vol_bnds, voxel_size, use_gpu=True): 23 | """Constructor. 24 | 25 | Args: 26 | vol_bnds (ndarray): An ndarray of shape (3, 2). Specifies the 27 | xyz bounds (min/max) in meters. 28 | voxel_size (float): The volume discretization in meters. 29 | """ 30 | vol_bnds = np.asarray(vol_bnds) 31 | assert vol_bnds.shape == (3, 2), "[!] `vol_bnds` should be of shape (3, 2)." 32 | 33 | # Define voxel volume parameters 34 | self._vol_bnds = vol_bnds 35 | self._voxel_size = float(voxel_size) 36 | self._trunc_margin = 5 * self._voxel_size # truncation on SDF 37 | self._color_const = 256 * 256 38 | 39 | # Adjust volume bounds and ensure C-order contiguous 40 | self._vol_dim = ( 41 | np.ceil((self._vol_bnds[:, 1] - self._vol_bnds[:, 0]) / self._voxel_size) 42 | .copy(order="C") 43 | .astype(int) 44 | ) 45 | self._vol_bnds[:, 1] = self._vol_bnds[:, 0] + self._vol_dim * self._voxel_size 46 | self._vol_origin = self._vol_bnds[:, 0].copy(order="C").astype(np.float32) 47 | 48 | print( 49 | "Voxel volume size: {} x {} x {} - # points: {:,}".format( 50 | self._vol_dim[0], 51 | self._vol_dim[1], 52 | self._vol_dim[2], 53 | self._vol_dim[0] * self._vol_dim[1] * self._vol_dim[2], 54 | ) 55 | ) 56 | 57 | # Initialize pointers to voxel volume in CPU memory 58 | self._tsdf_vol_cpu = np.ones(self._vol_dim).astype(np.float32) 59 | # for computing the cumulative moving average of observations per voxel 60 | self._weight_vol_cpu = np.zeros(self._vol_dim).astype(np.float32) 61 | self._color_vol_cpu = np.zeros(self._vol_dim).astype(np.float32) 62 | 63 | self.gpu_mode = use_gpu and FUSION_GPU_MODE 64 | 65 | # Copy voxel volumes to GPU 66 | if self.gpu_mode: 67 | self._tsdf_vol_gpu = cuda.mem_alloc(self._tsdf_vol_cpu.nbytes) 68 | cuda.memcpy_htod(self._tsdf_vol_gpu, self._tsdf_vol_cpu) 69 | self._weight_vol_gpu = cuda.mem_alloc(self._weight_vol_cpu.nbytes) 70 | cuda.memcpy_htod(self._weight_vol_gpu, self._weight_vol_cpu) 71 | self._color_vol_gpu = cuda.mem_alloc(self._color_vol_cpu.nbytes) 72 | cuda.memcpy_htod(self._color_vol_gpu, self._color_vol_cpu) 73 | 74 | # Cuda kernel function (C++) 75 | self._cuda_src_mod = SourceModule( 76 | """ 77 | __global__ void integrate(float * tsdf_vol, 78 | float * weight_vol, 79 | float * color_vol, 80 | float * vol_dim, 81 | float * vol_origin, 82 | float * cam_intr, 83 | float * cam_pose, 84 | float * other_params, 85 | float * color_im, 86 | float * depth_im) { 87 | // Get voxel index 88 | int gpu_loop_idx = (int) other_params[0]; 89 | int max_threads_per_block = blockDim.x; 90 | int block_idx = blockIdx.z*gridDim.y*gridDim.x+blockIdx.y*gridDim.x+blockIdx.x; 91 | int voxel_idx = gpu_loop_idx*gridDim.x*gridDim.y*gridDim.z*max_threads_per_block+block_idx*max_threads_per_block+threadIdx.x; 92 | int vol_dim_x = (int) vol_dim[0]; 93 | int vol_dim_y = (int) vol_dim[1]; 94 | int vol_dim_z = (int) vol_dim[2]; 95 | if (voxel_idx > vol_dim_x*vol_dim_y*vol_dim_z) 96 | return; 97 | // Get voxel grid coordinates (note: be careful when casting) 98 | float voxel_x = floorf(((float)voxel_idx)/((float)(vol_dim_y*vol_dim_z))); 99 | float voxel_y = floorf(((float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z))/((float)vol_dim_z)); 100 | float voxel_z = (float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z-((int)voxel_y)*vol_dim_z); 101 | // Voxel grid coordinates to world coordinates 102 | float voxel_size = other_params[1]; 103 | float pt_x = vol_origin[0]+voxel_x*voxel_size; 104 | float pt_y = vol_origin[1]+voxel_y*voxel_size; 105 | float pt_z = vol_origin[2]+voxel_z*voxel_size; 106 | // World coordinates to camera coordinates 107 | float tmp_pt_x = pt_x-cam_pose[0*4+3]; 108 | float tmp_pt_y = pt_y-cam_pose[1*4+3]; 109 | float tmp_pt_z = pt_z-cam_pose[2*4+3]; 110 | float cam_pt_x = cam_pose[0*4+0]*tmp_pt_x+cam_pose[1*4+0]*tmp_pt_y+cam_pose[2*4+0]*tmp_pt_z; 111 | float cam_pt_y = cam_pose[0*4+1]*tmp_pt_x+cam_pose[1*4+1]*tmp_pt_y+cam_pose[2*4+1]*tmp_pt_z; 112 | float cam_pt_z = cam_pose[0*4+2]*tmp_pt_x+cam_pose[1*4+2]*tmp_pt_y+cam_pose[2*4+2]*tmp_pt_z; 113 | // Camera coordinates to image pixels 114 | int pixel_x = (int) roundf(cam_intr[0*3+0]*(cam_pt_x/cam_pt_z)+cam_intr[0*3+2]); 115 | int pixel_y = (int) roundf(cam_intr[1*3+1]*(cam_pt_y/cam_pt_z)+cam_intr[1*3+2]); 116 | // Skip if outside view frustum 117 | int im_h = (int) other_params[2]; 118 | int im_w = (int) other_params[3]; 119 | if (pixel_x < 0 || pixel_x >= im_w || pixel_y < 0 || pixel_y >= im_h || cam_pt_z<0) 120 | return; 121 | // Skip invalid depth 122 | float depth_value = depth_im[pixel_y*im_w+pixel_x]; 123 | if (depth_value == 0) 124 | return; 125 | // Integrate TSDF 126 | float trunc_margin = other_params[4]; 127 | float depth_diff = depth_value-cam_pt_z; 128 | if (depth_diff < -trunc_margin) 129 | return; 130 | float dist = fmin(1.0f,depth_diff/trunc_margin); 131 | float w_old = weight_vol[voxel_idx]; 132 | float obs_weight = other_params[5]; 133 | float w_new = w_old + obs_weight; 134 | weight_vol[voxel_idx] = w_new; 135 | tsdf_vol[voxel_idx] = (tsdf_vol[voxel_idx]*w_old+obs_weight*dist)/w_new; 136 | // Integrate color 137 | float old_color = color_vol[voxel_idx]; 138 | float old_b = floorf(old_color/(256*256)); 139 | float old_g = floorf((old_color-old_b*256*256)/256); 140 | float old_r = old_color-old_b*256*256-old_g*256; 141 | float new_color = color_im[pixel_y*im_w+pixel_x]; 142 | float new_b = floorf(new_color/(256*256)); 143 | float new_g = floorf((new_color-new_b*256*256)/256); 144 | float new_r = new_color-new_b*256*256-new_g*256; 145 | new_b = fmin(roundf((old_b*w_old+obs_weight*new_b)/w_new),255.0f); 146 | new_g = fmin(roundf((old_g*w_old+obs_weight*new_g)/w_new),255.0f); 147 | new_r = fmin(roundf((old_r*w_old+obs_weight*new_r)/w_new),255.0f); 148 | color_vol[voxel_idx] = new_b*256*256+new_g*256+new_r; 149 | }""" 150 | ) 151 | 152 | self._cuda_integrate = self._cuda_src_mod.get_function("integrate") 153 | 154 | # Determine block/grid size on GPU 155 | gpu_dev = cuda.Device(0) 156 | self._max_gpu_threads_per_block = gpu_dev.MAX_THREADS_PER_BLOCK 157 | n_blocks = int( 158 | np.ceil(float(np.prod(self._vol_dim)) / float(self._max_gpu_threads_per_block)) 159 | ) 160 | grid_dim_x = min(gpu_dev.MAX_GRID_DIM_X, int(np.floor(np.cbrt(n_blocks)))) 161 | grid_dim_y = min(gpu_dev.MAX_GRID_DIM_Y, int(np.floor(np.sqrt(n_blocks / grid_dim_x)))) 162 | grid_dim_z = min( 163 | gpu_dev.MAX_GRID_DIM_Z, 164 | int(np.ceil(float(n_blocks) / float(grid_dim_x * grid_dim_y))), 165 | ) 166 | self._max_gpu_grid_dim = np.array([grid_dim_x, grid_dim_y, grid_dim_z]).astype(int) 167 | self._n_gpu_loops = int( 168 | np.ceil( 169 | float(np.prod(self._vol_dim)) 170 | / float(np.prod(self._max_gpu_grid_dim) * self._max_gpu_threads_per_block) 171 | ) 172 | ) 173 | 174 | else: 175 | # Get voxel grid coordinates 176 | xv, yv, zv = np.meshgrid( 177 | range(self._vol_dim[0]), 178 | range(self._vol_dim[1]), 179 | range(self._vol_dim[2]), 180 | indexing="ij", 181 | ) 182 | self.vox_coords = ( 183 | np.concatenate([xv.reshape(1, -1), yv.reshape(1, -1), zv.reshape(1, -1)], axis=0) 184 | .astype(int) 185 | .T 186 | ) 187 | 188 | @staticmethod 189 | @njit(parallel=True) 190 | def vox2world(vol_origin, vox_coords, vox_size): 191 | """Convert voxel grid coordinates to world coordinates.""" 192 | vol_origin = vol_origin.astype(np.float32) 193 | vox_coords = vox_coords.astype(np.float32) 194 | cam_pts = np.empty_like(vox_coords, dtype=np.float32) 195 | for i in prange(vox_coords.shape[0]): 196 | for j in range(3): 197 | cam_pts[i, j] = vol_origin[j] + (vox_size * vox_coords[i, j]) 198 | return cam_pts 199 | 200 | @staticmethod 201 | @njit(parallel=True) 202 | def cam2pix(cam_pts, intr): 203 | """Convert camera coordinates to pixel coordinates.""" 204 | intr = intr.astype(np.float32) 205 | fx, fy = intr[0, 0], intr[1, 1] 206 | cx, cy = intr[0, 2], intr[1, 2] 207 | pix = np.empty((cam_pts.shape[0], 2), dtype=np.int64) 208 | for i in prange(cam_pts.shape[0]): 209 | pix[i, 0] = int(np.round((cam_pts[i, 0] * fx / cam_pts[i, 2]) + cx)) 210 | pix[i, 1] = int(np.round((cam_pts[i, 1] * fy / cam_pts[i, 2]) + cy)) 211 | return pix 212 | 213 | @staticmethod 214 | @njit(parallel=True) 215 | def integrate_tsdf(tsdf_vol, dist, w_old, obs_weight): 216 | """Integrate the TSDF volume.""" 217 | tsdf_vol_int = np.empty_like(tsdf_vol, dtype=np.float32) 218 | w_new = np.empty_like(w_old, dtype=np.float32) 219 | for i in prange(len(tsdf_vol)): 220 | w_new[i] = w_old[i] + obs_weight 221 | tsdf_vol_int[i] = (w_old[i] * tsdf_vol[i] + obs_weight * dist[i]) / w_new[i] 222 | return tsdf_vol_int, w_new 223 | 224 | def integrate(self, color_im, depth_im, cam_intr, cam_pose, obs_weight=1.0): 225 | """Integrate an RGB-D frame into the TSDF volume. 226 | 227 | Args: 228 | color_im (ndarray): An RGB image of shape (H, W, 3). 229 | depth_im (ndarray): A depth image of shape (H, W). 230 | cam_intr (ndarray): The camera intrinsics matrix of shape (3, 3). 231 | cam_pose (ndarray): The camera pose (i.e. extrinsics) of shape (4, 4). 232 | obs_weight (float): The weight to assign for the current observation. A higher 233 | value 234 | """ 235 | im_h, im_w = depth_im.shape 236 | 237 | # Fold RGB color image into a single channel image 238 | color_im = color_im.astype(np.float32) 239 | color_im = np.floor( 240 | color_im[..., 2] * self._color_const + color_im[..., 1] * 256 + color_im[..., 0] 241 | ) 242 | 243 | if self.gpu_mode: # GPU mode: integrate voxel volume (calls CUDA kernel) 244 | for gpu_loop_idx in range(self._n_gpu_loops): 245 | self._cuda_integrate( 246 | self._tsdf_vol_gpu, 247 | self._weight_vol_gpu, 248 | self._color_vol_gpu, 249 | cuda.InOut(self._vol_dim.astype(np.float32)), 250 | cuda.InOut(self._vol_origin.astype(np.float32)), 251 | cuda.InOut(cam_intr.reshape(-1).astype(np.float32)), 252 | cuda.InOut(cam_pose.reshape(-1).astype(np.float32)), 253 | cuda.InOut( 254 | np.asarray( 255 | [ 256 | gpu_loop_idx, 257 | self._voxel_size, 258 | im_h, 259 | im_w, 260 | self._trunc_margin, 261 | obs_weight, 262 | ], 263 | np.float32, 264 | ) 265 | ), 266 | cuda.InOut(color_im.reshape(-1).astype(np.float32)), 267 | cuda.InOut(depth_im.reshape(-1).astype(np.float32)), 268 | block=(self._max_gpu_threads_per_block, 1, 1), 269 | grid=( 270 | int(self._max_gpu_grid_dim[0]), 271 | int(self._max_gpu_grid_dim[1]), 272 | int(self._max_gpu_grid_dim[2]), 273 | ), 274 | ) 275 | else: # CPU mode: integrate voxel volume (vectorized implementation) 276 | # Convert voxel grid coordinates to pixel coordinates 277 | cam_pts = self.vox2world(self._vol_origin, self.vox_coords, self._voxel_size) 278 | cam_pts = rigid_transform(cam_pts, np.linalg.inv(cam_pose)) 279 | pix_z = cam_pts[:, 2] 280 | pix = self.cam2pix(cam_pts, cam_intr) 281 | pix_x, pix_y = pix[:, 0], pix[:, 1] 282 | 283 | # Eliminate pixels outside view frustum 284 | valid_pix = np.logical_and( 285 | pix_x >= 0, 286 | np.logical_and( 287 | pix_x < im_w, 288 | np.logical_and(pix_y >= 0, np.logical_and(pix_y < im_h, pix_z > 0)), 289 | ), 290 | ) 291 | depth_val = np.zeros(pix_x.shape) 292 | depth_val[valid_pix] = depth_im[pix_y[valid_pix], pix_x[valid_pix]] 293 | 294 | # Integrate TSDF 295 | depth_diff = depth_val - pix_z 296 | valid_pts = np.logical_and(depth_val > 0, depth_diff >= -self._trunc_margin) 297 | dist = np.minimum(1, depth_diff / self._trunc_margin) 298 | valid_vox_x = self.vox_coords[valid_pts, 0] 299 | valid_vox_y = self.vox_coords[valid_pts, 1] 300 | valid_vox_z = self.vox_coords[valid_pts, 2] 301 | w_old = self._weight_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] 302 | tsdf_vals = self._tsdf_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] 303 | valid_dist = dist[valid_pts] 304 | tsdf_vol_new, w_new = self.integrate_tsdf(tsdf_vals, valid_dist, w_old, obs_weight) 305 | self._weight_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = w_new 306 | self._tsdf_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = tsdf_vol_new 307 | 308 | # Integrate color 309 | old_color = self._color_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] 310 | old_b = np.floor(old_color / self._color_const) 311 | old_g = np.floor((old_color - old_b * self._color_const) / 256) 312 | old_r = old_color - old_b * self._color_const - old_g * 256 313 | new_color = color_im[pix_y[valid_pts], pix_x[valid_pts]] 314 | new_b = np.floor(new_color / self._color_const) 315 | new_g = np.floor((new_color - new_b * self._color_const) / 256) 316 | new_r = new_color - new_b * self._color_const - new_g * 256 317 | new_b = np.minimum(255.0, np.round((w_old * old_b + obs_weight * new_b) / w_new)) 318 | new_g = np.minimum(255.0, np.round((w_old * old_g + obs_weight * new_g) / w_new)) 319 | new_r = np.minimum(255.0, np.round((w_old * old_r + obs_weight * new_r) / w_new)) 320 | self._color_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = ( 321 | new_b * self._color_const + new_g * 256 + new_r 322 | ) 323 | 324 | def get_volume(self): 325 | if self.gpu_mode: 326 | cuda.memcpy_dtoh(self._tsdf_vol_cpu, self._tsdf_vol_gpu) 327 | cuda.memcpy_dtoh(self._color_vol_cpu, self._color_vol_gpu) 328 | return self._tsdf_vol_cpu, self._color_vol_cpu 329 | 330 | def get_point_cloud(self): 331 | """Extract a point cloud from the voxel volume.""" 332 | tsdf_vol, color_vol = self.get_volume() 333 | 334 | # Marching cubes 335 | verts = measure.marching_cubes(tsdf_vol, level=0)[0] 336 | verts_ind = np.round(verts).astype(int) 337 | verts = verts * self._voxel_size + self._vol_origin 338 | 339 | # Get vertex colors 340 | rgb_vals = color_vol[verts_ind[:, 0], verts_ind[:, 1], verts_ind[:, 2]] 341 | colors_b = np.floor(rgb_vals / self._color_const) 342 | colors_g = np.floor((rgb_vals - colors_b * self._color_const) / 256) 343 | colors_r = rgb_vals - colors_b * self._color_const - colors_g * 256 344 | colors = np.floor(np.asarray([colors_r, colors_g, colors_b])).T 345 | colors = colors.astype(np.uint8) 346 | 347 | pc = np.hstack([verts, colors]) 348 | return pc 349 | 350 | def get_mesh(self): 351 | """Compute a mesh from the voxel volume using marching cubes.""" 352 | tsdf_vol, color_vol = self.get_volume() 353 | 354 | # Marching cubes 355 | verts, faces, norms, vals = measure.marching_cubes(tsdf_vol, level=0) 356 | verts_ind = np.round(verts).astype(int) 357 | verts = ( 358 | verts * self._voxel_size + self._vol_origin 359 | ) # voxel grid coordinates to world coordinates 360 | 361 | # Get vertex colors 362 | rgb_vals = color_vol[verts_ind[:, 0], verts_ind[:, 1], verts_ind[:, 2]] 363 | colors_b = np.floor(rgb_vals / self._color_const) 364 | colors_g = np.floor((rgb_vals - colors_b * self._color_const) / 256) 365 | colors_r = rgb_vals - colors_b * self._color_const - colors_g * 256 366 | colors = np.floor(np.asarray([colors_r, colors_g, colors_b])).T 367 | colors = colors.astype(np.uint8) 368 | return verts, faces, norms, colors 369 | 370 | 371 | def rigid_transform(xyz, transform): 372 | """Applies a rigid transform to an (N, 3) pointcloud.""" 373 | xyz_h = np.hstack([xyz, np.ones((len(xyz), 1), dtype=np.float32)]) 374 | xyz_t_h = np.dot(transform, xyz_h.T).T 375 | return xyz_t_h[:, :3] 376 | 377 | 378 | def get_view_frustum(depth_im, cam_intr, cam_pose): 379 | """Get corners of 3D camera view frustum of depth image""" 380 | im_h = depth_im.shape[0] 381 | im_w = depth_im.shape[1] 382 | max_depth = np.max(depth_im) 383 | view_frust_pts = np.array( 384 | [ 385 | (np.array([0, 0, 0, im_w, im_w]) - cam_intr[0, 2]) 386 | * np.array([0, max_depth, max_depth, max_depth, max_depth]) 387 | / cam_intr[0, 0], 388 | (np.array([0, 0, im_h, 0, im_h]) - cam_intr[1, 2]) 389 | * np.array([0, max_depth, max_depth, max_depth, max_depth]) 390 | / cam_intr[1, 1], 391 | np.array([0, max_depth, max_depth, max_depth, max_depth]), 392 | ] 393 | ) 394 | view_frust_pts = rigid_transform(view_frust_pts.T, cam_pose).T 395 | return view_frust_pts 396 | 397 | 398 | def meshwrite(filename, verts, faces, norms, colors): 399 | """Save a 3D mesh to a polygon .ply file.""" 400 | # Write header 401 | ply_file = open(filename, "w") 402 | ply_file.write("ply\n") 403 | ply_file.write("format ascii 1.0\n") 404 | ply_file.write("element vertex %d\n" % (verts.shape[0])) 405 | ply_file.write("property float x\n") 406 | ply_file.write("property float y\n") 407 | ply_file.write("property float z\n") 408 | ply_file.write("property float nx\n") 409 | ply_file.write("property float ny\n") 410 | ply_file.write("property float nz\n") 411 | ply_file.write("property uchar red\n") 412 | ply_file.write("property uchar green\n") 413 | ply_file.write("property uchar blue\n") 414 | ply_file.write("element face %d\n" % (faces.shape[0])) 415 | ply_file.write("property list uchar int vertex_index\n") 416 | ply_file.write("end_header\n") 417 | 418 | # Write vertex list 419 | for i in range(verts.shape[0]): 420 | ply_file.write( 421 | "%f %f %f %f %f %f %d %d %d\n" 422 | % ( 423 | verts[i, 0], 424 | verts[i, 1], 425 | verts[i, 2], 426 | norms[i, 0], 427 | norms[i, 1], 428 | norms[i, 2], 429 | colors[i, 0], 430 | colors[i, 1], 431 | colors[i, 2], 432 | ) 433 | ) 434 | 435 | # Write face list 436 | for i in range(faces.shape[0]): 437 | ply_file.write("3 %d %d %d\n" % (faces[i, 0], faces[i, 1], faces[i, 2])) 438 | 439 | ply_file.close() 440 | 441 | 442 | def pcwrite(filename, xyzrgb): 443 | """Save a point cloud to a polygon .ply file.""" 444 | xyz = xyzrgb[:, :3] 445 | rgb = xyzrgb[:, 3:].astype(np.uint8) 446 | 447 | # Write header 448 | ply_file = open(filename, "w") 449 | ply_file.write("ply\n") 450 | ply_file.write("format ascii 1.0\n") 451 | ply_file.write("element vertex %d\n" % (xyz.shape[0])) 452 | ply_file.write("property float x\n") 453 | ply_file.write("property float y\n") 454 | ply_file.write("property float z\n") 455 | ply_file.write("property uchar red\n") 456 | ply_file.write("property uchar green\n") 457 | ply_file.write("property uchar blue\n") 458 | ply_file.write("end_header\n") 459 | 460 | # Write vertex list 461 | for i in range(xyz.shape[0]): 462 | ply_file.write( 463 | "%f %f %f %d %d %d\n" 464 | % ( 465 | xyz[i, 0], 466 | xyz[i, 1], 467 | xyz[i, 2], 468 | rgb[i, 0], 469 | rgb[i, 1], 470 | rgb[i, 2], 471 | ) 472 | ) 473 | -------------------------------------------------------------------------------- /utils3d/rgbd/io.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-03-03 5 | @desc RGBD IO 6 | """ 7 | 8 | import os 9 | 10 | import numpy as np 11 | import open3d as o3d 12 | from PIL import Image 13 | 14 | 15 | def read_rgbd(rgb_path, depth_path, depth_scale=1000, depth_trunc=1): 16 | """read rgbd images 17 | 18 | Args: 19 | rgb_path (str): path to rgb image 20 | depth_path (str): path to depth image 21 | depth_scale (float, optional): The scale of depth, multiplicative. Defaults to 0.001. 22 | depth_trunc (float, optional): The truncate scale of depth. Defaults to 0.7. 23 | 24 | Returns: 25 | o3d.geometry.RGBDImage: Merged RGBD Image 26 | """ 27 | depth_img = np.array(Image.open(depth_path)) 28 | depth_img[depth_img == depth_img.max()] == 0 29 | depth_img = depth_img.astype(np.float32) 30 | if len(depth_img.shape) == 3: 31 | depth_img = np.ascontiguousarray(depth_img[:, :, 0]) 32 | 33 | rgb_img = np.array(Image.open(rgb_path)) 34 | 35 | rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( 36 | o3d.geometry.Image(rgb_img), 37 | o3d.geometry.Image(depth_img), 38 | depth_scale=depth_scale, 39 | depth_trunc=depth_trunc, 40 | convert_rgb_to_intensity=False, 41 | ) 42 | return rgbd 43 | -------------------------------------------------------------------------------- /utils3d/rgbd/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-03-03 5 | @desc RGBD utils 6 | """ 7 | from bdb import set_trace 8 | 9 | import numpy as np 10 | import open3d as o3d 11 | 12 | from utils3d.rgbd.fusion import TSDFVolume 13 | 14 | 15 | def backproject_rgbd(rgbd, intrinsics): 16 | """back project rgbd to point cloud 17 | 18 | Args: 19 | rgbd (o3d.geometry.RGBDImage): RGBD Image 20 | intrinsics (dict): intrinsics with keys: width, height, fx, fy, cx, cy 21 | 22 | Returns: 23 | o3d.geometry.PointCloud: back projected point cloud 24 | """ 25 | intrinsics_o3d = o3d.camera.PinholeCameraIntrinsic(**intrinsics) 26 | pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, intrinsics_o3d) 27 | return pcd 28 | 29 | 30 | def tsdf_fusion(rgbd_list, transform_list, intrinsics, bounds=None, voxel_size=0.0): 31 | """fuse multiview rgbd into tsdsf 32 | 33 | Args: 34 | rgbd_list (list[o3d.geometry.RGBDImage]): list of RGBD images 35 | transform_list (list[np.ndarray]): list of transformations (4X4) 36 | intrinsics (dict): _description_ 37 | bounds (np.ndarray, optional): scene boundary, 3X2. Defaults to None. 38 | voxel_size (float, optional): size of voxel. Defaults to 0. 39 | 40 | Returns: 41 | TSDFVolumn: tsdf volumn 42 | """ 43 | if bounds is None: 44 | # automatic determine bounds 45 | pc_list = [] 46 | for rgbd, transform in zip(rgbd_list, transform_list): 47 | pcd = backproject_rgbd(rgbd, intrinsics) 48 | pcd.transform(transform) 49 | pc_list.append(np.asarray(pcd.points)) 50 | pcd = o3d.geometry.PointCloud() 51 | pcd.points = o3d.utility.Vector3dVector(np.concatenate(pc_list, axis=0)) 52 | bounds = np.stack((pcd.get_min_bound(), pcd.get_max_bound()), axis=1) 53 | # automatic determine voxel size 54 | if voxel_size == 0: 55 | voxel_size = (bounds[:, 1] - bounds[:, 0]).max() / 100 56 | tsdf = TSDFVolume(bounds, voxel_size, use_gpu=True) 57 | 58 | intrinsics_mat = np.eye(3) 59 | intrinsics_mat[0, 0] = intrinsics["fx"] 60 | intrinsics_mat[1, 1] = intrinsics["fy"] 61 | intrinsics_mat[0, 2] = intrinsics["cx"] 62 | intrinsics_mat[1, 2] = intrinsics["cy"] 63 | for rgbd, transform in zip(rgbd_list, transform_list): 64 | rgb = rgbd.color 65 | depth = rgbd.depth 66 | tsdf.integrate(np.asarray(rgb), np.asarray(depth), intrinsics_mat, transform) 67 | return tsdf 68 | -------------------------------------------------------------------------------- /utils3d/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Steve-Tod/utils3d/513be2d59e410c3159a9db011802f875aacdc20a/utils3d/utils/__init__.py -------------------------------------------------------------------------------- /utils3d/utils/transform.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.spatial.transform 3 | 4 | 5 | class Rotation(scipy.spatial.transform.Rotation): 6 | @classmethod 7 | def identity(cls): 8 | return cls.from_quat([0.0, 0.0, 0.0, 1.0]) 9 | 10 | 11 | class Transform(object): 12 | """Rigid spatial transform between coordinate systems in 3D space. 13 | 14 | Attributes: 15 | rotation (scipy.spatial.transform.Rotation) 16 | translation (np.ndarray) 17 | """ 18 | 19 | def __init__(self, rotation, translation): 20 | assert isinstance(rotation, scipy.spatial.transform.Rotation) 21 | assert isinstance(translation, (np.ndarray, list)) 22 | 23 | self.rotation = rotation 24 | self.translation = np.asarray(translation, np.double) 25 | 26 | def as_matrix(self): 27 | """Represent as a 4x4 matrix.""" 28 | return np.vstack( 29 | (np.c_[self.rotation.as_matrix(), self.translation], [0.0, 0.0, 0.0, 1.0]) 30 | ) 31 | 32 | def to_dict(self): 33 | """Serialize Transform object into a dictionary.""" 34 | return { 35 | "rotation": self.rotation.as_quat().tolist(), 36 | "translation": self.translation.tolist(), 37 | } 38 | 39 | def to_list(self): 40 | return np.r_[self.rotation.as_quat(), self.translation] 41 | 42 | def __mul__(self, other): 43 | """Compose this transform with another.""" 44 | rotation = self.rotation * other.rotation 45 | translation = self.rotation.apply(other.translation) + self.translation 46 | return self.__class__(rotation, translation) 47 | 48 | def transform_point(self, point): 49 | return self.rotation.apply(point) + self.translation 50 | 51 | def transform_vector(self, vector): 52 | return self.rotation.apply(vector) 53 | 54 | def inverse(self): 55 | """Compute the inverse of this transform.""" 56 | rotation = self.rotation.inv() 57 | translation = -rotation.apply(self.translation) 58 | return self.__class__(rotation, translation) 59 | 60 | @classmethod 61 | def from_matrix(cls, m): 62 | """Initialize from a 4x4 matrix.""" 63 | rotation = Rotation.from_matrix(m[:3, :3]) 64 | translation = m[:3, 3] 65 | return cls(rotation, translation) 66 | 67 | @classmethod 68 | def from_dict(cls, dictionary): 69 | rotation = Rotation.from_quat(dictionary["rotation"]) 70 | translation = np.asarray(dictionary["translation"]) 71 | return cls(rotation, translation) 72 | 73 | @classmethod 74 | def from_list(cls, list): 75 | rotation = Rotation.from_quat(list[:4]) 76 | translation = list[4:] 77 | return cls(rotation, translation) 78 | 79 | @classmethod 80 | def identity(cls): 81 | """Initialize with the identity transformation.""" 82 | rotation = Rotation.from_quat([0.0, 0.0, 0.0, 1.0]) 83 | translation = np.array([0.0, 0.0, 0.0]) 84 | return cls(rotation, translation) 85 | 86 | @classmethod 87 | def look_at(cls, eye, center, up): 88 | """Initialize with a LookAt matrix. 89 | 90 | Returns: 91 | T_eye_ref, the transform from camera to the reference frame, w.r.t. 92 | which the input arguments were defined. 93 | """ 94 | eye = np.asarray(eye) 95 | center = np.asarray(center) 96 | 97 | forward = center - eye 98 | forward /= np.linalg.norm(forward) 99 | 100 | right = np.cross(forward, up) 101 | right /= np.linalg.norm(right) 102 | 103 | up = np.asarray(up) / np.linalg.norm(up) 104 | up = np.cross(right, forward) 105 | 106 | m = np.eye(4, 4) 107 | m[:3, 0] = right 108 | m[:3, 1] = -up 109 | m[:3, 2] = forward 110 | m[:3, 3] = eye 111 | 112 | return cls.from_matrix(m).inverse() 113 | -------------------------------------------------------------------------------- /utils3d/utils/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | @author Zhenyu Jiang 3 | @email stevetod98@gmail.com 4 | @date 2022-01-14 5 | @desc 6 | """ 7 | 8 | import numpy as np 9 | 10 | from utils3d.utils.transform import Rotation, Transform 11 | 12 | 13 | def get_pose(distance, center=np.zeros(3), ax=0, ay=0, az=0): 14 | """generate camera pose from distance, center and euler angles 15 | 16 | Args: 17 | distance (float): distance from camera to center 18 | center (np.ndarray, optional): the look at center. Defaults to np.zeros(3). 19 | ax (float, optional): euler angle x. Defaults to 0. 20 | ay (float, optional): euler angle around y axis. Defaults to 0. 21 | az (float, optional): euler angle around z axis. Defaults to 0. 22 | 23 | Returns: 24 | np.ndarray: camera pose of 4*4 numpy array 25 | """ 26 | rotation = Rotation.from_euler("xyz", (ax, ay, az)) 27 | vec = np.array([0, 0, distance]) 28 | translation = rotation.as_matrix().dot(vec) + center 29 | camera_pose = Transform(rotation, translation).as_matrix() 30 | return camera_pose 31 | --------------------------------------------------------------------------------