├── .gitignore
├── LICENSE
├── README.md
├── configs
├── Replica
│ ├── office0.yaml
│ ├── office1.yaml
│ ├── office2.yaml
│ ├── office3.yaml
│ ├── office4.yaml
│ ├── replica.yaml
│ ├── room0.yaml
│ ├── room1.yaml
│ └── room2.yaml
├── Synthetic
│ ├── breakfast_room.yaml
│ ├── complete_kitchen.yaml
│ ├── green_room.yaml
│ ├── grey_white_room.yaml
│ ├── morning_apartment.yaml
│ ├── synthetic.yaml
│ ├── thin_geometry.yaml
│ └── whiteroom.yaml
└── benchmark_standard.yaml
├── environment.yml
├── external
└── NumpyMarchingCubes
│ ├── marching_cubes
│ ├── __init__.py
│ └── src
│ │ ├── _mcubes.cpp
│ │ ├── _mcubes.pyx
│ │ ├── marching_cubes.cpp
│ │ ├── marching_cubes.h
│ │ ├── pyarray_symbol.h
│ │ ├── pyarraymodule.h
│ │ ├── pywrapper.cpp
│ │ ├── pywrapper.h
│ │ ├── sparsegrid3.h
│ │ └── tables.h
│ └── setup.py
├── requirements.txt
├── run_benchmark.py
└── src
├── NeRFs
├── decoder.py
├── decoder_hybrid.py
├── encoder.py
├── encoder_hybrid.py
├── keyframe_global.py
├── representaion.py
└── representaion_hybrid.py
├── config.py
├── datasets
├── dataset.py
└── dataset_utils.py
├── standard_SLAM.py
├── tools
└── pose_eval.py
└── utils
├── common_utils.py
├── mesh_utils_eslam.py
├── mesher_utils.py
├── pose_eval_utils.py
├── pose_utils.py
├── render_utils.py
└── vis_utils.py
/.gitignore:
--------------------------------------------------------------------------------
1 | .history
2 | .vscode
3 | __pycache__/
4 | output/
5 | Co-SLAM/
6 | *.pyc
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | Apache License
2 | Version 2.0, January 2004
3 | http://www.apache.org/licenses/
4 |
5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
6 |
7 | 1. Definitions.
8 |
9 | "License" shall mean the terms and conditions for use, reproduction,
10 | and distribution as defined by Sections 1 through 9 of this document.
11 |
12 | "Licensor" shall mean the copyright owner or entity authorized by
13 | the copyright owner that is granting the License.
14 |
15 | "Legal Entity" shall mean the union of the acting entity and all
16 | other entities that control, are controlled by, or are under common
17 | control with that entity. For the purposes of this definition,
18 | "control" means (i) the power, direct or indirect, to cause the
19 | direction or management of such entity, whether by contract or
20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the
21 | outstanding shares, or (iii) beneficial ownership of such entity.
22 |
23 | "You" (or "Your") shall mean an individual or Legal Entity
24 | exercising permissions granted by this License.
25 |
26 | "Source" form shall mean the preferred form for making modifications,
27 | including but not limited to software source code, documentation
28 | source, and configuration files.
29 |
30 | "Object" form shall mean any form resulting from mechanical
31 | transformation or translation of a Source form, including but
32 | not limited to compiled object code, generated documentation,
33 | and conversions to other media types.
34 |
35 | "Work" shall mean the work of authorship, whether in Source or
36 | Object form, made available under the License, as indicated by a
37 | copyright notice that is included in or attached to the work
38 | (an example is provided in the Appendix below).
39 |
40 | "Derivative Works" shall mean any work, whether in Source or Object
41 | form, that is based on (or derived from) the Work and for which the
42 | editorial revisions, annotations, elaborations, or other modifications
43 | represent, as a whole, an original work of authorship. For the purposes
44 | of this License, Derivative Works shall not include works that remain
45 | separable from, or merely link (or bind by name) to the interfaces of,
46 | the Work and Derivative Works thereof.
47 |
48 | "Contribution" shall mean any work of authorship, including
49 | the original version of the Work and any modifications or additions
50 | to that Work or Derivative Works thereof, that is intentionally
51 | submitted to Licensor for inclusion in the Work by the copyright owner
52 | or by an individual or Legal Entity authorized to submit on behalf of
53 | the copyright owner. For the purposes of this definition, "submitted"
54 | means any form of electronic, verbal, or written communication sent
55 | to the Licensor or its representatives, including but not limited to
56 | communication on electronic mailing lists, source code control systems,
57 | and issue tracking systems that are managed by, or on behalf of, the
58 | Licensor for the purpose of discussing and improving the Work, but
59 | excluding communication that is conspicuously marked or otherwise
60 | designated in writing by the copyright owner as "Not a Contribution."
61 |
62 | "Contributor" shall mean Licensor and any individual or Legal Entity
63 | on behalf of whom a Contribution has been received by Licensor and
64 | subsequently incorporated within the Work.
65 |
66 | 2. Grant of Copyright License. Subject to the terms and conditions of
67 | this License, each Contributor hereby grants to You a perpetual,
68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
69 | copyright license to reproduce, prepare Derivative Works of,
70 | publicly display, publicly perform, sublicense, and distribute the
71 | Work and such Derivative Works in Source or Object form.
72 |
73 | 3. Grant of Patent License. Subject to the terms and conditions of
74 | this License, each Contributor hereby grants to You a perpetual,
75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
76 | (except as stated in this section) patent license to make, have made,
77 | use, offer to sell, sell, import, and otherwise transfer the Work,
78 | where such license applies only to those patent claims licensable
79 | by such Contributor that are necessarily infringed by their
80 | Contribution(s) alone or by combination of their Contribution(s)
81 | with the Work to which such Contribution(s) was submitted. If You
82 | institute patent litigation against any entity (including a
83 | cross-claim or counterclaim in a lawsuit) alleging that the Work
84 | or a Contribution incorporated within the Work constitutes direct
85 | or contributory patent infringement, then any patent licenses
86 | granted to You under this License for that Work shall terminate
87 | as of the date such litigation is filed.
88 |
89 | 4. Redistribution. You may reproduce and distribute copies of the
90 | Work or Derivative Works thereof in any medium, with or without
91 | modifications, and in Source or Object form, provided that You
92 | meet the following conditions:
93 |
94 | (a) You must give any other recipients of the Work or
95 | Derivative Works a copy of this License; and
96 |
97 | (b) You must cause any modified files to carry prominent notices
98 | stating that You changed the files; and
99 |
100 | (c) You must retain, in the Source form of any Derivative Works
101 | that You distribute, all copyright, patent, trademark, and
102 | attribution notices from the Source form of the Work,
103 | excluding those notices that do not pertain to any part of
104 | the Derivative Works; and
105 |
106 | (d) If the Work includes a "NOTICE" text file as part of its
107 | distribution, then any Derivative Works that You distribute must
108 | include a readable copy of the attribution notices contained
109 | within such NOTICE file, excluding those notices that do not
110 | pertain to any part of the Derivative Works, in at least one
111 | of the following places: within a NOTICE text file distributed
112 | as part of the Derivative Works; within the Source form or
113 | documentation, if provided along with the Derivative Works; or,
114 | within a display generated by the Derivative Works, if and
115 | wherever such third-party notices normally appear. The contents
116 | of the NOTICE file are for informational purposes only and
117 | do not modify the License. You may add Your own attribution
118 | notices within Derivative Works that You distribute, alongside
119 | or as an addendum to the NOTICE text from the Work, provided
120 | that such additional attribution notices cannot be construed
121 | as modifying the License.
122 |
123 | You may add Your own copyright statement to Your modifications and
124 | may provide additional or different license terms and conditions
125 | for use, reproduction, or distribution of Your modifications, or
126 | for any such Derivative Works as a whole, provided Your use,
127 | reproduction, and distribution of the Work otherwise complies with
128 | the conditions stated in this License.
129 |
130 | 5. Submission of Contributions. Unless You explicitly state otherwise,
131 | any Contribution intentionally submitted for inclusion in the Work
132 | by You to the Licensor shall be under the terms and conditions of
133 | this License, without any additional terms or conditions.
134 | Notwithstanding the above, nothing herein shall supersede or modify
135 | the terms of any separate license agreement you may have executed
136 | with Licensor regarding such Contributions.
137 |
138 | 6. Trademarks. This License does not grant permission to use the trade
139 | names, trademarks, service marks, or product names of the Licensor,
140 | except as required for reasonable and customary use in describing the
141 | origin of the Work and reproducing the content of the NOTICE file.
142 |
143 | 7. Disclaimer of Warranty. Unless required by applicable law or
144 | agreed to in writing, Licensor provides the Work (and each
145 | Contributor provides its Contributions) on an "AS IS" BASIS,
146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 | implied, including, without limitation, any warranties or conditions
148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 | PARTICULAR PURPOSE. You are solely responsible for determining the
150 | appropriateness of using or redistributing the Work and assume any
151 | risks associated with Your exercise of permissions under this License.
152 |
153 | 8. Limitation of Liability. In no event and under no legal theory,
154 | whether in tort (including negligence), contract, or otherwise,
155 | unless required by applicable law (such as deliberate and grossly
156 | negligent acts) or agreed to in writing, shall any Contributor be
157 | liable to You for damages, including any direct, indirect, special,
158 | incidental, or consequential damages of any character arising as a
159 | result of this License or out of the use or inability to use the
160 | Work (including but not limited to damages for loss of goodwill,
161 | work stoppage, computer failure or malfunction, or any and all
162 | other commercial damages or losses), even if such Contributor
163 | has been advised of the possibility of such damages.
164 |
165 | 9. Accepting Warranty or Additional Liability. While redistributing
166 | the Work or Derivative Works thereof, You may choose to offer,
167 | and charge a fee for, acceptance of support, warranty, indemnity,
168 | or other liability obligations and/or rights consistent with this
169 | License. However, in accepting such obligations, You may act only
170 | on Your own behalf and on Your sole responsibility, not on behalf
171 | of any other Contributor, and only if You agree to indemnify,
172 | defend, and hold each Contributor harmless for any liability
173 | incurred by, or claims asserted against, such Contributor by reason
174 | of your accepting any such warranty or additional liability.
175 |
176 | END OF TERMS AND CONDITIONS
177 |
178 | APPENDIX: How to apply the Apache License to your work.
179 |
180 | To apply the Apache License to your work, attach the following
181 | boilerplate notice, with the fields enclosed by brackets "[]"
182 | replaced with your own identifying information. (Don't include
183 | the brackets!) The text should be enclosed in the appropriate
184 | comment syntax for the file format. We also recommend that a
185 | file or class name and description of purpose be included on the
186 | same "printed page" as the copyright notice for easier
187 | identification within third-party archives.
188 |
189 | Copyright [yyyy] [name of copyright owner]
190 |
191 | Licensed under the Apache License, Version 2.0 (the "License");
192 | you may not use this file except in compliance with the License.
193 | You may obtain a copy of the License at
194 |
195 | http://www.apache.org/licenses/LICENSE-2.0
196 |
197 | Unless required by applicable law or agreed to in writing, software
198 | distributed under the License is distributed on an "AS IS" BASIS,
199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 | See the License for the specific language governing permissions and
201 | limitations under the License.
202 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 |
2 |
3 | [CVPR 2024]
4 |
5 | Benchmarking Implicit Neural Representation and Geometric Rendering in Real-Time RGB-D SLAM
6 |
7 | [Project Page](https://vlis2022.github.io/nerf-slam-benchmark/) | [Paper](https://arxiv.org/abs/2403.19473)
8 |
9 |
10 |
11 |
12 | ## Installation
13 |
14 | First you have to make sure that you have all dependencies in place.
15 | The simplest way to do so, is to use [anaconda](https://www.anaconda.com/).
16 |
17 | For linux, you need to install **libopenexr-dev** before creating the environment:
18 | ```bash
19 | sudo apt-get install libopenexr-dev
20 | ```
21 | You can then create an anaconda environment called `benchmark-release`:
22 | ```bash
23 | git clone https://github.com/thua919/NeRF-SLAM-Benchmark-CVPR24.git
24 | cd NeRF-SLAM-Benchmark-CVPR24
25 | conda env create -f environment.yaml
26 | conda activate benchmark-release
27 | ```
28 |
29 | Alternatively, if you run into installation errors, we recommend the following commands:
30 |
31 | ```bash
32 | # Create conda environment
33 | conda create -n benchmark-release python=3.7
34 | conda activate benchmark-release
35 |
36 | # Install the pytorch first (Please check the cuda version)
37 | pip install torch==1.10.1+cu113 torchvision==0.11.2+cu113 torchaudio==0.10.1 -f https://download.pytorch.org/whl/cu113/torch_stable.html
38 |
39 | # Install all the dependencies via pip (Note here pytorch3d and tinycudann requires ~10min to build)
40 | pip install -r requirements.txt
41 |
42 | # For tinycudann, if you cannot access network when you use GPUs, you can also try build from source as below:
43 | git clone --recursive https://github.com/nvlabs/tiny-cuda-nn
44 | cd tiny-cuda-nn/bindings/torch
45 | python setup.py install
46 |
47 | # Build extension (marching cubes from neuralRGBD)
48 | cd external/NumpyMarchingCubes
49 | python setup.py install
50 | ```
51 |
52 |
53 | ## Quick Run
54 |
55 | Download the data as below and the data is saved into the `Datasets/` folder,
56 | ```bash
57 | mkdir -p Datasets
58 | cd Datasets
59 | # Replica Dataset for Lab Scenario
60 | wget https://cvg-data.inf.ethz.ch/nice-slam/data/Replica.zip
61 | unzip Replica.zip
62 | # NeuralRGBD Dataset for Practical Scenario
63 | wget http://kaldir.vc.in.tum.de/neural_rgbd/neural_rgbd_data.zip
64 | unzip neural_rgbd_data.zip
65 | ```
66 | and then you can run benchmark for, e.g. `room0` sequence of Replica Dataset by:
67 | ```bash
68 | python run_benchmark.py configs/Replica/room0.yaml
69 | ```
70 |
71 | ## Evaluation
72 |
73 | ### ATE/PSNR/DepthL1/Time
74 | To monitor the evaluation metrics throughout SLAM running, please use tensorboard:
75 | ```bash
76 | tensorboard --logdir output/Replica/room0/ --port=8008
77 | ```
78 |
79 | ### Acc./Comp.
80 | Upon finishing the SLAM process, you will see the final reconstruction in the folder with `.ply` ending. To measure the quality of the reconstruction, we recommend you to evaluate the meshes using the repository [here](https://github.com/JingwenWang95/neural_slam_eval). Note that our results are evaluated by its `Neural-RGBD/GO-Surf` mesh culling strategy:
81 |
82 | ```bash
83 | ## Replica ##
84 | # mesh culling
85 | INPUT_MESH=/home/ps/data/tongyanhua/NeRF-SLAM-Benchmark-CVPR24/Replica/room0/mesh_track1999.ply # path_to_your_mesh
86 | python cull_mesh.py --config configs/Replica/room0.yaml --input_mesh $INPUT_MESH --remove_occlusion --gt_pose
87 | # mesh evaluation
88 | REC_MESH=/home/ps/data/tongyanhua/NeRF-SLAM-Benchmark-CVPR24/Replica/room0/mesh_track1999_cull_occlusion.ply # path_to_your_culled_mesh
89 | GT_MESH=/home/ps/data/tongyanhua/Datasets/Replica/room0_mesh.ply # path_to_gt_mesh: we use gt mesh without any culling for replica evaluation
90 | python eval_recon.py --rec_mesh $REC_MESH --gt_mesh $GT_MESH --dataset_type Replica -3d
91 |
92 |
93 | ## Neural RGBD ##
94 | INPUT_MESH=/home/ps/data/tongyanhua/NeRF-SLAM-Benchmark-CVPR24/synthetic/gr_mesh_track1441.ply
95 | python cull_mesh.py --config configs/Synthetic/gr.yaml --input_mesh $INPUT_MESH --remove_occlusion --gt_pose
96 | REC_MESH=/home/ps/data/tongyanhua/NeRF-SLAM-Benchmark-CVPR24/synthetic/gr_mesh_track1441_cull_occlusion.ply
97 | GT_MESH=/home/ps/data/tongyanhua/Datasets/neural_rgbd_data/green_room/gt_mesh_cull_virt_cams.ply # path_to_gt_mesh: we use culled gt mesh for neuralrgbd evaluation
98 | python eval_recon.py --rec_mesh $REC_MESH --gt_mesh $GT_MESH --dataset_type RGBD -3d
99 |
100 | ```
101 | You may need to download ground truth meshes by youself, following the instruction and data directories of section `Run Evaluation` in the [repository](https://github.com/JingwenWang95/neural_slam_eval).
102 |
103 |
104 | ### ScanNet for Explicit Hybrid Encoding
105 |
106 | Please find our implementation [here](https://github.com/thua919/explicit_hybrid_encoding).
107 |
108 |
109 | ## Related Repositories
110 | We would like to extend our gratitude to the authors of
111 | [NICE-SLAM](https://github.com/cvg/nice-slam),
112 | [ESLAM](https://github.com/idiap/ESLAM),
113 | [COSLAM](https://github.com/HengyiWang/Co-SLAM),
114 | [TensoRF](https://github.com/apchenstu/TensoRF), and
115 | [NeuS](https://github.com/Totoro97/NeuS)
116 | for their exceptional work.
117 |
118 |
119 | ## Citing
120 | If you find our work useful, please consider citing:
121 | ```BibTeX
122 | @inproceedings{nerfslam24hua,
123 | author = {Tongyan Hua and Lin Wang},
124 | title = {Benchmarking Implicit Neural Representation and Geometric Rendering in Real-Time RGB-D SLAM},
125 | booktitle = {Proceedings of the IEEE international conference on Computer Vision and Pattern Recognition (CVPR)},
126 | month = {June},
127 | year = {2024},
128 | }
129 | ```
130 |
131 | ### Acknowledgement
132 | This paper is supported by the National Natural Science Foundation of China (NSF).
133 |
134 |
--------------------------------------------------------------------------------
/configs/Replica/office0.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Replica/replica.yaml
2 |
3 | mapping:
4 | bound: [[-4.5,4.9],[-5.7,4.4],[-3.7,4.3]]
5 | marching_cubes_bound: [[-4.5,4.9],[-5.7,4.4],[-3.7,4.3]]
6 | data:
7 | dataset: 'replica'
8 | datadir: Datasets/Replica/office0
9 | output: output/Replica/office0
10 | downsample: 1
11 |
12 |
13 |
--------------------------------------------------------------------------------
/configs/Replica/office1.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Replica/replica.yaml
2 |
3 | mapping:
4 | bound: [[-4.3,5.5],[-4.1,5.0],[-3.5,4.2]]
5 | marching_cubes_bound: [[-4.3,5.5],[-4.1,5.0],[-3.5,4.2]]
6 | data:
7 | dataset: 'replica'
8 | datadir: Datasets/Replica/office1
9 | output: output/Replica/office1
10 | downsample: 1
11 |
12 |
13 |
--------------------------------------------------------------------------------
/configs/Replica/office2.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Replica/replica.yaml
2 |
3 | mapping:
4 | bound: [[-4.0,3.6],[-3.4,5.9],[-1.8,2.1]]
5 | marching_cubes_bound: [[-4.0,3.6],[-3.4,5.9],[-1.8,2.1]]
6 | data:
7 | dataset: 'replica'
8 | datadir: Datasets/Replica/office2
9 | output: output/Replica/office2
10 | downsample: 1
11 |
12 |
13 |
--------------------------------------------------------------------------------
/configs/Replica/office3.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Replica/replica.yaml
2 |
3 | mapping:
4 | bound: [[-5.7,4.1],[-6.5,3.9],[-1.8,2.5]]
5 | marching_cubes_bound: [[-5.7,4.1],[-6.5,3.9],[-1.8,2.5]]
6 | data:
7 | dataset: 'replica'
8 | datadir: Datasets/Replica/office3
9 | output: output/Replica/office3
10 | downsample: 1
--------------------------------------------------------------------------------
/configs/Replica/office4.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Replica/replica.yaml
2 |
3 | mapping:
4 | bound: [[-2.7,6.8],[-3.8,5.7],[-2.7,3.1]]
5 | marching_cubes_bound: [[-2.7,6.8],[-3.8,5.7],[-2.7,3.1]]
6 | data:
7 | dataset: 'replica'
8 | datadir: Datasets/Replica/office4
9 | output: output/Replica/office4
10 | downsample: 1
11 |
12 |
13 |
--------------------------------------------------------------------------------
/configs/Replica/replica.yaml:
--------------------------------------------------------------------------------
1 |
2 | cam:
3 | H: 680
4 | W: 1200
5 | fx: 600.0
6 | fy: 600.0
7 | cx: 599.5
8 | cy: 339.5
9 | png_depth_scale: 6553.5 #for depth image in png format
10 | crop_edge: 0
11 | crop_size: # like this tum [384,512]
12 | istortion: # like this tum [0.2312, -0.7849, -0.0033, -0.0001, 0.9172]
13 | near: 0
14 | far: 5
15 | depth_trunc: 100.
--------------------------------------------------------------------------------
/configs/Replica/room0.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Replica/replica.yaml
2 |
3 | mapping:
4 | bound: [[-2.9,8.9],[-3.2,5.5],[-3.5,3.3]]
5 | marching_cubes_bound: [[-2.9,8.9],[-3.2,5.5],[-3.5,3.3]]
6 |
7 |
8 | data:
9 | dataset: 'replica'
10 | datadir: Datasets/Replica/room0
11 | output: output/Replica/room0
12 | downsample: 1
13 |
14 |
15 |
--------------------------------------------------------------------------------
/configs/Replica/room1.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Replica/replica.yaml
2 |
3 | mapping:
4 | bound: [[-6.0,1.8],[-3.6,3.3],[-2.0,1.9]]
5 | marching_cubes_bound: [[-6.0,1.8],[-3.6,3.3],[-2.0,1.9]]
6 | data:
7 | dataset: 'replica'
8 | datadir: Datasets/Replica/room1
9 | output: output/Replica/room1
10 | downsample: 1
11 |
12 |
13 |
--------------------------------------------------------------------------------
/configs/Replica/room2.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Replica/replica.yaml
2 |
3 | mapping:
4 | bound: [[-3.3,8.5],[-5.7,4.2],[-5.4,3.2]]
5 | marching_cubes_bound: [[-3.3,8.5],[-5.7,4.2],[-5.4,3.2]]
6 | data:
7 | dataset: 'replica'
8 | datadir: Datasets/Replica/room2
9 | output: output/Replica/room2
10 | downsample: 1
11 |
12 |
13 |
--------------------------------------------------------------------------------
/configs/Synthetic/breakfast_room.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Synthetic/synthetic.yaml
2 | mapping:
3 | bound: [[-2.4, 2.],[-0.6, 2.9],[-1.8, 3.1]]
4 | marching_cubes_bound: [[-2.4, 2.],[-0.6, 2.9],[-1.8, 3.1]]
5 |
6 | data:
7 | dataset: 'synthetic'
8 | datadir: Datasets/breakfast_room
9 | output: output/Synthetic/br
10 | downsample: 1
11 |
12 | cam:
13 | near: 0
14 | far: 5
--------------------------------------------------------------------------------
/configs/Synthetic/complete_kitchen.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Synthetic/synthetic.yaml
2 | mapping:
3 | bound: [[-5.7, 3.8],[-0.2, 3.3],[-6.6, 3.6]]
4 | marching_cubes_bound: [[-5.7, 3.8],[-0.2, 3.3],[-6.6, 3.6]]
5 |
6 | data:
7 | dataset: 'synthetic'
8 | datadir: Datasets/complete_kitchen
9 | output: output/Synthetic/ck
10 | downsample: 1
11 |
12 | cam:
13 | near: 0
14 | far: 8
--------------------------------------------------------------------------------
/configs/Synthetic/green_room.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Synthetic/synthetic.yaml
2 | mapping:
3 | bound: [[-2.6, 5.6],[-0.3, 3.0],[0.2, 5.1]]
4 | marching_cubes_bound: [[-2.6, 5.6],[-0.3, 3.0],[0.2, 5.1]]
5 |
6 | data:
7 | dataset: 'synthetic'
8 | datadir: Datasets/green_room
9 | output: output/Synthetic/gr
10 | downsample: 1
11 |
12 | cam:
13 | near: 0
14 | far: 8
--------------------------------------------------------------------------------
/configs/Synthetic/grey_white_room.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Synthetic/synthetic.yaml
2 | mapping:
3 | bound: [[-0.7, 5.4],[-0.2, 3.1],[-3.9, 0.8]]
4 | marching_cubes_bound: [[-0.7, 5.4],[-0.2, 3.1],[-3.9, 0.8]]
5 |
6 | data:
7 | dataset: 'synthetic'
8 | datadir: Datasets/grey_white_room
9 | output: output/Synthetic/gwr
10 | downsample: 1
11 |
12 | cam:
13 | near: 0
14 | far: 4
--------------------------------------------------------------------------------
/configs/Synthetic/morning_apartment.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Synthetic/synthetic.yaml
2 | mapping:
3 | bound: [[-1.5, 2.2],[-0.3, 2.2],[-2.3, 1.9]]
4 | marching_cubes_bound: [[-1.5, 2.2],[-0.3, 2.2],[-2.3, 1.9]]
5 |
6 | data:
7 | dataset: 'synthetic'
8 | datadir: Datasets/morning_apartment
9 | output: output/Synthetic/ma
10 | downsample: 1
11 |
--------------------------------------------------------------------------------
/configs/Synthetic/synthetic.yaml:
--------------------------------------------------------------------------------
1 | cam:
2 | H: 480
3 | W: 640
4 | fx: 554.2562584220408
5 | fy: 554.2562584220408
6 | cx: 320
7 | cy: 240
8 | png_depth_scale: 1000. #for depth image in png format
9 | crop_edge: 0
10 | near: 0
11 | far: 4
12 | depth_trunc: 100.
--------------------------------------------------------------------------------
/configs/Synthetic/thin_geometry.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Synthetic/synthetic.yaml
2 | mapping:
3 | bound: [[-2.5, 1.1],[-0.3, 1.1],[0.1, 3.9]]
4 | marching_cubes_bound: [[-2.5, 1.1],[-0.3, 1.1],[0.1, 3.9]]
5 |
6 | data:
7 | dataset: 'synthetic'
8 | datadir: Datasets/thin_geometry
9 | output: output/Synthetic/tg
10 | downsample: 1
11 |
12 | cam:
13 | near: 0
14 | far: 4
--------------------------------------------------------------------------------
/configs/Synthetic/whiteroom.yaml:
--------------------------------------------------------------------------------
1 | inherit_from: configs/Synthetic/synthetic.yaml
2 | mapping:
3 | bound: [[-2.6, 3.2],[-0.4, 3.6],[0.5, 8.3]]
4 | marching_cubes_bound: [[-2.6, 3.2],[-0.4, 3.6],[0.5, 8.3]]
5 |
6 | data:
7 | dataset: 'synthetic'
8 | datadir: Datasets/whiteroom
9 | output: output/Synthetic/w
10 | downsample: 1
11 |
12 | cam:
13 | near: 0
14 | far: 8
--------------------------------------------------------------------------------
/configs/benchmark_standard.yaml:
--------------------------------------------------------------------------------
1 | verbose: True
2 | test_param: False
3 | device: "cuda:0"
4 |
5 | rebut:
6 | loss: False #fs_loss variant
7 | sampling: False #ray_surface_samples variant
8 | keyframe: True
9 |
10 | utils:
11 | first_mesh: True
12 | mesh_resolution: 0.02 # should set 0.01 for good mesh
13 | mesh_bound_scale: 1.02
14 | mesh_render_color: False
15 | fig_freq: 100
16 | mesh_freq: 500
17 | training:
18 | rot_rep: 'quat' #'axis_angle'
19 |
20 | mapping:
21 | percent_pixels_save: 0.05
22 |
23 | tracking:
24 | ignore_edge_W: 20
25 | ignore_edge_H: 20
26 |
27 | ## Settings for hybrid ##
28 | hybrid: True #True
29 | hybrid_hash: True #implimet of hash+Tri; if False, then dense+Tri
30 |
31 | ## Settings for INR+Render##
32 | NeRFs:
33 | F: #only one: True
34 | choice_MLP: False
35 | choice_Dense_Grid: True
36 | choice_Sparse_Grid: False
37 | choice_Tri_Plane: False
38 | choice_TensoRF: False
39 | G: #only one: True
40 | sdf_approx: False
41 | sdf_style: True
42 | sdf_neus: False
43 | tricks:
44 | # senario coupling:
45 | #Cou.b. share_encoder:True, geo_app_connect_feat_dim: 0
46 | #Cou. share_encoder:True, geo_app_connect_feat_dim: 15
47 | # senario decoupling:
48 | #Dec. share_encoder: False
49 | share_encoder: True #False
50 | geo_app_connect_feat_dim: 0
51 |
52 | gamma: 'identity' #chose from['identity','blob','freq']
53 |
54 | map_lr: 0.01
55 | track_lr: 0.001
56 |
57 | keyframe_every: 5
58 | first_iters: 500 ##1000
59 | mapping_iters: 20 ##30
60 | tracking_iters: 20
61 | mapping_pix_samples: 2048 #4080
62 | tracking_pix_samples: 1024 ##2048
63 |
64 | ray_stratified_samples: 48
65 | ray_surface_samples: 12
66 |
67 | color_weight: 5
68 | depth_weight: 1 # 0.1
69 | sdf_fs_weight: 10
70 | sdf_tail_weight: 200
71 | sdf_center_weight: 50
72 |
73 | space_resolution:
74 | implicit_dim: 2 # vector length per vertex
75 | res_level: 2 #16 # resolution levels
76 |
77 | geo_block_size_fine: 0.02
78 | geo_block_size_coarse: 0.24
79 | app_block_size_fine: 0.02
80 | app_block_size_coarse: 0.24
81 | plane_block_size_coarse: 0.24 #0.5
82 | truncation: 0.1
--------------------------------------------------------------------------------
/environment.yml:
--------------------------------------------------------------------------------
1 | name: benchmark-release
2 | channels:
3 | - pytorch
4 | - conda-forge
5 | - defaults
6 | dependencies:
7 | - _libgcc_mutex=0.1=main
8 | - _openmp_mutex=5.1=1_gnu
9 | - blas=1.0=mkl
10 | - brotli=1.0.9=h5eee18b_7
11 | - brotli-bin=1.0.9=h5eee18b_7
12 | - bzip2=1.0.8=h7f98852_4
13 | - ca-certificates=2024.3.11=h06a4308_0
14 | - certifi=2020.6.20=pyhd3eb1b0_3
15 | - colorama=0.4.6=py37h06a4308_0
16 | - cudatoolkit=11.3.1=h9edb442_10
17 | - cycler=0.11.0=pyhd3eb1b0_0
18 | - ffmpeg=4.3=hf484d3e_0
19 | - freetype=2.10.4=h0708190_1
20 | - giflib=5.2.1=h5eee18b_3
21 | - gmp=6.2.1=h58526e2_0
22 | - gnutls=3.6.13=h85f3911_1
23 | - intel-openmp=2021.4.0=h06a4308_3561
24 | - jpeg=9e=h166bdaf_1
25 | - kiwisolver=1.4.4=py37h6a678d5_0
26 | - lame=3.100=h7f98852_1001
27 | - lcms2=2.12=hddcbb42_0
28 | - ld_impl_linux-64=2.38=h1181459_1
29 | - libbrotlicommon=1.0.9=h5eee18b_7
30 | - libbrotlidec=1.0.9=h5eee18b_7
31 | - libbrotlienc=1.0.9=h5eee18b_7
32 | - libffi=3.4.4=h6a678d5_0
33 | - libgcc-ng=11.2.0=h1234567_1
34 | - libgomp=11.2.0=h1234567_1
35 | - libiconv=1.17=h166bdaf_0
36 | - libpng=1.6.37=h21135ba_2
37 | - libspatialindex=1.9.3=h2531618_0
38 | - libstdcxx-ng=11.2.0=h1234567_1
39 | - libtiff=4.2.0=hf544144_3
40 | - libuv=1.43.0=h7f98852_0
41 | - libwebp=1.2.2=h55f646e_0
42 | - libwebp-base=1.2.2=h7f98852_1
43 | - lz4-c=1.9.3=h9c3ff4c_1
44 | - mkl=2021.4.0=h06a4308_640
45 | - mkl-service=2.4.0=py37h402132d_0
46 | - mkl_fft=1.3.1=py37h3e078e5_1
47 | - mkl_random=1.2.2=py37h219a48f_0
48 | - munkres=1.1.4=py_0
49 | - ncurses=6.4=h6a678d5_0
50 | - nettle=3.6=he412f7d_0
51 | - numpy-base=1.21.5=py37ha15fc14_3
52 | - olefile=0.46=pyh9f0ad1d_1
53 | - openh264=2.1.1=h780b84a_0
54 | - openjpeg=2.4.0=hb52868f_1
55 | - openssl=1.1.1w=h7f8727e_0
56 | - pip=22.3.1=py37h06a4308_0
57 | - python=3.7.16=h7a1cb2a_0
58 | - python_abi=3.7=2_cp37m
59 | - pytorch=1.10.1=py3.7_cuda11.3_cudnn8.2.0_0
60 | - pytorch-mutex=1.0=cuda
61 | - readline=8.2=h5eee18b_0
62 | - rtree=1.0.1=py37h06a4308_0
63 | - six=1.16.0=pyh6c4a22f_0
64 | - sqlite=3.41.2=h5eee18b_0
65 | - tk=8.6.12=h1ccaba5_0
66 | - torchaudio=0.10.1=py37_cu113
67 | - torchvision=0.11.2=py37_cu113
68 | - typing_extensions=4.3.0=py37h06a4308_0
69 | - wheel=0.38.4=py37h06a4308_0
70 | - xz=5.4.2=h5eee18b_0
71 | - zlib=1.2.13=h5eee18b_0
72 | - zstd=1.5.0=ha95c52a_0
73 | - pip:
74 | - absl-py==2.0.0
75 | - addict==2.4.0
76 | - aiofiles==22.1.0
77 | - aiosqlite==0.19.0
78 | - ansi2html==1.8.0
79 | - anyio==3.7.1
80 | - argon2-cffi==23.1.0
81 | - argon2-cffi-bindings==21.2.0
82 | - arrow==1.2.3
83 | - attrs==23.1.0
84 | - babel==2.14.0
85 | - backcall==0.2.0
86 | - beautifulsoup4==4.12.3
87 | - bleach==6.0.0
88 | - cached-property==1.5.2
89 | - cachetools==5.3.1
90 | - cffi==1.15.1
91 | - charset-normalizer==3.3.1
92 | - click==8.1.7
93 | - comm==0.1.4
94 | - configargparse==1.7
95 | - dash==2.14.0
96 | - dash-core-components==2.0.0
97 | - dash-html-components==2.0.0
98 | - dash-table==5.0.0
99 | - debugpy==1.7.0
100 | - decorator==5.1.1
101 | - defusedxml==0.7.1
102 | - deprecation==2.1.0
103 | - entrypoints==0.4
104 | - exceptiongroup==1.2.0
105 | - fastjsonschema==2.18.1
106 | - flask==2.2.5
107 | - fonttools==4.38.0
108 | - fqdn==1.5.1
109 | - freetype-py==2.4.0
110 | - fvcore==0.1.5.post20210915
111 | - google-auth==2.23.3
112 | - google-auth-oauthlib==0.4.6
113 | - grpcio==1.59.0
114 | - idna==3.4
115 | - imageio==2.31.2
116 | - importlib-metadata==6.7.0
117 | - importlib-resources==5.12.0
118 | - iopath==0.1.9
119 | - ipykernel==6.16.2
120 | - ipython==7.34.0
121 | - ipython-genutils==0.2.0
122 | - ipywidgets==8.1.1
123 | - isoduration==20.11.0
124 | - itsdangerous==2.1.2
125 | - jedi==0.19.1
126 | - jinja2==3.1.2
127 | - joblib==1.3.2
128 | - json5==0.9.16
129 | - jsonpointer==2.4
130 | - jsonschema==4.17.3
131 | - jupyter-client==7.4.9
132 | - jupyter-core==4.12.0
133 | - jupyter-events==0.6.3
134 | - jupyter-packaging==0.12.3
135 | - jupyter-server==1.24.0
136 | - jupyter-server-fileid==0.9.1
137 | - jupyter-server-ydoc==0.8.0
138 | - jupyter-ydoc==0.2.5
139 | - jupyterlab==3.6.7
140 | - jupyterlab-pygments==0.2.2
141 | - jupyterlab-server==2.24.0
142 | - jupyterlab-widgets==3.0.9
143 | - markdown==3.4.4
144 | - markupsafe==2.1.3
145 | - mathutils==2.81.2
146 | - matplotlib==3.5.3
147 | - matplotlib-inline==0.1.6
148 | - mistune==3.0.2
149 | - nbclassic==1.0.0
150 | - nbclient==0.7.4
151 | - nbconvert==7.6.0
152 | - nbformat==5.7.0
153 | - nest-asyncio==1.5.8
154 | - networkx==2.6.3
155 | - notebook==6.5.6
156 | - notebook-shim==0.2.4
157 | - numpy==1.21.6
158 | - numpymarchingcubes==0.0.1
159 | - oauthlib==3.2.2
160 | - open3d==0.15.2
161 | - opencv-contrib-python==4.6.0.66
162 | - packaging==23.1
163 | - pandas==1.3.5
164 | - pandocfilters==1.5.1
165 | - parso==0.8.3
166 | - pexpect==4.8.0
167 | - pickleshare==0.7.5
168 | - pillow==9.5.0
169 | - pkgutil-resolve-name==1.3.10
170 | - plotly==5.17.0
171 | - portalocker==1.4.0
172 | - prometheus-client==0.17.1
173 | - prompt-toolkit==3.0.39
174 | - protobuf==3.20.3
175 | - psutil==5.9.8
176 | - ptyprocess==0.7.0
177 | - pyasn1==0.5.0
178 | - pyasn1-modules==0.3.0
179 | - pycparser==2.21
180 | - pyglet==1.5.27
181 | - pygments==2.16.1
182 | - pyopengl==3.1.0
183 | - pyparsing==3.0.9
184 | - pyquaternion==0.9.9
185 | - pyrender==0.1.45
186 | - pyrsistent==0.19.3
187 | - python-dateutil==2.8.2
188 | - python-json-logger==2.0.7
189 | - pytorch3d==0.7.4
190 | - pytz==2023.3.post1
191 | - pywavelets==1.3.0
192 | - pyyaml==6.0
193 | - pyzmq==24.0.1
194 | - requests==2.31.0
195 | - requests-oauthlib==1.3.1
196 | - retrying==1.3.4
197 | - rfc3339-validator==0.1.4
198 | - rfc3986-validator==0.1.1
199 | - rsa==4.9
200 | - scikit-image==0.19.3
201 | - scikit-learn==1.0.2
202 | - scipy==1.7.3
203 | - send2trash==1.8.2
204 | - setuptools==58.2.0
205 | - sniffio==1.3.0
206 | - soupsieve==2.4.1
207 | - tabulate==0.9.0
208 | - tenacity==8.2.3
209 | - tensorboard==2.11.2
210 | - tensorboard-data-server==0.6.1
211 | - tensorboard-plugin-wit==1.8.1
212 | - termcolor==2.3.0
213 | - terminado==0.17.1
214 | - threadpoolctl==3.1.0
215 | - tifffile==2021.11.2
216 | - tinycss2==1.2.1
217 | - tinycudann==1.7
218 | - tomli==2.0.1
219 | - tomlkit==0.12.3
220 | - tornado==6.2
221 | - tqdm==4.65.0
222 | - traitlets==5.9.0
223 | - trimesh==3.21.5
224 | - typing-extensions==4.5.0
225 | - uri-template==1.3.0
226 | - urllib3==2.0.7
227 | - wcwidth==0.2.8
228 | - webcolors==1.13
229 | - webencodings==0.5.1
230 | - websocket-client==1.6.1
231 | - werkzeug==2.2.3
232 | - widgetsnbextension==4.0.9
233 | - y-py==0.6.2
234 | - yacs==0.1.8
235 | - ypy-websocket==0.8.4
236 | - zipp==3.15.0
237 | prefix: /hpc2hdd/home/tongyanhua/.conda/envs/benchmark-release
238 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/__init__.py:
--------------------------------------------------------------------------------
1 | from ._mcubes import marching_cubes
2 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/_mcubes.pyx:
--------------------------------------------------------------------------------
1 |
2 | # distutils: language = c++
3 | # cython: embedsignature = True
4 |
5 | # from libcpp.vector cimport vector
6 | import numpy as np
7 |
8 | # Define PY_ARRAY_UNIQUE_SYMBOL
9 | cdef extern from "pyarray_symbol.h":
10 | pass
11 |
12 | cimport numpy as np
13 |
14 | np.import_array()
15 |
16 | cdef extern from "pywrapper.h":
17 | cdef object c_marching_cubes "marching_cubes"(np.ndarray, double, double) except +
18 |
19 | def marching_cubes(np.ndarray volume, float isovalue, float truncation):
20 |
21 | verts, faces = c_marching_cubes(volume, isovalue, truncation)
22 | verts.shape = (-1, 3)
23 | faces.shape = (-1, 3)
24 | return verts, faces
25 |
26 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/marching_cubes.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 | #include
5 | #include
6 |
7 | #include "tables.h"
8 | #include "sparsegrid3.h"
9 | #include "marching_cubes.h"
10 |
11 | #define VOXELSIZE 1.0f
12 |
13 | struct vec3f {
14 | vec3f() {
15 | x = 0.0f;
16 | y = 0.0f;
17 | z = 0.0f;
18 | }
19 | vec3f(float x_, float y_, float z_) {
20 | x = x_;
21 | y = y_;
22 | z = z_;
23 | }
24 | inline vec3f operator+(const vec3f& other) const {
25 | return vec3f(x+other.x, y+other.y, z+other.z);
26 | }
27 | inline vec3f operator-(const vec3f& other) const {
28 | return vec3f(x-other.x, y-other.y, z-other.z);
29 | }
30 | inline vec3f operator*(float val) const {
31 | return vec3f(x*val, y*val, z*val);
32 | }
33 | inline void operator+=(const vec3f& other) {
34 | x += other.x;
35 | y += other.y;
36 | z += other.z;
37 | }
38 | static float distSq(const vec3f& v0, const vec3f& v1) {
39 | return ((v0.x-v1.x)*(v0.x-v1.x) + (v0.y-v1.y)*(v0.y-v1.y) + (v0.z-v1.z)*(v0.z-v1.z));
40 | }
41 | float x;
42 | float y;
43 | float z;
44 | };
45 | inline vec3f operator*(float s, const vec3f& v) {
46 | return v * s;
47 | }
48 | struct vec3uc {
49 | vec3uc() {
50 | x = 0;
51 | y = 0;
52 | z = 0;
53 | }
54 | vec3uc(unsigned char x_, unsigned char y_, unsigned char z_) {
55 | x = x_;
56 | y = y_;
57 | z = z_;
58 | }
59 | unsigned char x;
60 | unsigned char y;
61 | unsigned char z;
62 | };
63 |
64 | struct Triangle {
65 | vec3f v0;
66 | vec3f v1;
67 | vec3f v2;
68 | };
69 |
70 | void get_voxel(
71 | const vec3f& pos,
72 | const npy_accessor& tsdf_accessor,
73 | float truncation,
74 | float& d,
75 | int& w) {
76 | int x = (int)round(pos.x);
77 | int y = (int)round(pos.y);
78 | int z = (int)round(pos.z);
79 | if (z >= 0 && z < tsdf_accessor.size()[2] &&
80 | y >= 0 && y < tsdf_accessor.size()[1] &&
81 | x >= 0 && x < tsdf_accessor.size()[0]) {
82 | d = tsdf_accessor(x, y, z);
83 | if (d != -std::numeric_limits::infinity() && fabs(d) < truncation) w = 1;
84 | else w = 0;
85 | }
86 | else {
87 | d = -std::numeric_limits::infinity();
88 | w = 0;
89 | }
90 | }
91 |
92 | bool trilerp(
93 | const vec3f& pos,
94 | float& dist,
95 | const npy_accessor& tsdf_accessor,
96 | float truncation) {
97 | const float oSet = VOXELSIZE;
98 | const vec3f posDual = pos - vec3f(oSet / 2.0f, oSet / 2.0f, oSet / 2.0f);
99 | vec3f weight = vec3f(pos.x - (int)pos.x, pos.y - (int)pos.y, pos.z - (int)pos.z);
100 |
101 | dist = 0.0f;
102 | float d; int w;
103 | get_voxel(posDual + vec3f(0.0f, 0.0f, 0.0f), tsdf_accessor, truncation, d, w); if (w == 0) return false; dist += (1.0f - weight.x)*(1.0f - weight.y)*(1.0f - weight.z)*d;
104 | get_voxel(posDual + vec3f(oSet, 0.0f, 0.0f), tsdf_accessor, truncation, d, w); if (w == 0) return false; dist += weight.x *(1.0f - weight.y)*(1.0f - weight.z)*d;
105 | get_voxel(posDual + vec3f(0.0f, oSet, 0.0f), tsdf_accessor, truncation, d, w); if (w == 0) return false; dist += (1.0f - weight.x)* weight.y *(1.0f - weight.z)*d;
106 | get_voxel(posDual + vec3f(0.0f, 0.0f, oSet), tsdf_accessor, truncation, d, w); if (w == 0) return false; dist += (1.0f - weight.x)*(1.0f - weight.y)* weight.z *d;
107 | get_voxel(posDual + vec3f(oSet, oSet, 0.0f), tsdf_accessor, truncation, d, w); if (w == 0) return false; dist += weight.x * weight.y *(1.0f - weight.z)*d;
108 | get_voxel(posDual + vec3f(0.0f, oSet, oSet), tsdf_accessor, truncation, d, w); if (w == 0) return false; dist += (1.0f - weight.x)* weight.y * weight.z *d;
109 | get_voxel(posDual + vec3f(oSet, 0.0f, oSet), tsdf_accessor, truncation, d, w); if (w == 0) return false; dist += weight.x *(1.0f - weight.y)* weight.z *d;
110 | get_voxel(posDual + vec3f(oSet, oSet, oSet), tsdf_accessor, truncation, d, w); if (w == 0) return false; dist += weight.x * weight.y * weight.z *d;
111 |
112 | return true;
113 | }
114 |
115 | vec3f vertexInterp(float isolevel, const vec3f& p1, const vec3f& p2, float d1, float d2)
116 | {
117 | vec3f r1 = p1;
118 | vec3f r2 = p2;
119 | //printf("[interp] r1 = (%f, %f, %f), r2 = (%f, %f, %f) d1 = %f, d2 = %f, iso = %f\n", r1.x, r1.y, r1.z, r2.x, r2.y, r2.z, d1, d2, isolevel);
120 | //printf("%d, %d, %d || %f, %f, %f -> %f, %f, %f\n", fabs(isolevel - d1) < 0.00001f, fabs(isolevel - d2) < 0.00001f, fabs(d1 - d2) < 0.00001f, isolevel - d1, isolevel - d2, d1-d2, fabs(isolevel - d1), fabs(isolevel - d2), fabs(d1-d2));
121 |
122 | if (fabs(isolevel - d1) < 0.00001f) return r1;
123 | if (fabs(isolevel - d2) < 0.00001f) return r2;
124 | if (fabs(d1 - d2) < 0.00001f) return r1;
125 |
126 | float mu = (isolevel - d1) / (d2 - d1);
127 |
128 | vec3f res;
129 | res.x = p1.x + mu * (p2.x - p1.x); // Positions
130 | res.y = p1.y + mu * (p2.y - p1.y);
131 | res.z = p1.z + mu * (p2.z - p1.z);
132 |
133 | //printf("[interp] mu = %f, res = (%f, %f, %f) r1 = (%f, %f, %f), r2 = (%f, %f, %f)\n", mu, res.x, res.y, res.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z);
134 |
135 | return res;
136 | }
137 |
138 | void extract_isosurface_at_position(
139 | const vec3f& pos,
140 | const npy_accessor& tsdf_accessor,
141 | float truncation,
142 | float isolevel,
143 | float thresh,
144 | std::vector& results) {
145 | const float voxelsize = VOXELSIZE;
146 | const float P = voxelsize / 2.0f;
147 | const float M = -P;
148 |
149 | //const bool debugprint = (pos.z == 33 && pos.y == 56 && pos.x == 2) || (pos.z == 2 && pos.y == 56 && pos.x == 33);
150 |
151 | vec3f p000 = pos + vec3f(M, M, M); float dist000; bool valid000 = trilerp(p000, dist000, tsdf_accessor, truncation);
152 | vec3f p100 = pos + vec3f(P, M, M); float dist100; bool valid100 = trilerp(p100, dist100, tsdf_accessor, truncation);
153 | vec3f p010 = pos + vec3f(M, P, M); float dist010; bool valid010 = trilerp(p010, dist010, tsdf_accessor, truncation);
154 | vec3f p001 = pos + vec3f(M, M, P); float dist001; bool valid001 = trilerp(p001, dist001, tsdf_accessor, truncation);
155 | vec3f p110 = pos + vec3f(P, P, M); float dist110; bool valid110 = trilerp(p110, dist110, tsdf_accessor, truncation);
156 | vec3f p011 = pos + vec3f(M, P, P); float dist011; bool valid011 = trilerp(p011, dist011, tsdf_accessor, truncation);
157 | vec3f p101 = pos + vec3f(P, M, P); float dist101; bool valid101 = trilerp(p101, dist101, tsdf_accessor, truncation);
158 | vec3f p111 = pos + vec3f(P, P, P); float dist111; bool valid111 = trilerp(p111, dist111, tsdf_accessor, truncation);
159 | //if (debugprint) {
160 | // printf("[extract_isosurface_at_position] pos: %f, %f, %f\n", pos.x, pos.y, pos.z);
161 | // printf("\tp000 (%f, %f, %f) -> dist %f, color %d, %d, %d | valid %d\n", p000.x, p000.y, p000.z, dist000, (int)color000.x, (int)color000.y, (int)color000.z, (int)valid000);
162 | // printf("\tp100 (%f, %f, %f) -> dist %f, color %d, %d, %d | valid %d\n", p100.x, p100.y, p100.z, dist100, (int)color100.x, (int)color100.y, (int)color100.z, (int)valid100);
163 | // printf("\tp010 (%f, %f, %f) -> dist %f, color %d, %d, %d | valid %d\n", p010.x, p010.y, p010.z, dist010, (int)color010.x, (int)color010.y, (int)color010.z, (int)valid010);
164 | // printf("\tp001 (%f, %f, %f) -> dist %f, color %d, %d, %d | valid %d\n", p001.x, p001.y, p001.z, dist001, (int)color001.x, (int)color001.y, (int)color001.z, (int)valid001);
165 | // printf("\tp110 (%f, %f, %f) -> dist %f, color %d, %d, %d | valid %d\n", p110.x, p110.y, p110.z, dist110, (int)color110.x, (int)color110.y, (int)color110.z, (int)valid110);
166 | // printf("\tp011 (%f, %f, %f) -> dist %f, color %d, %d, %d | valid %d\n", p011.x, p011.y, p011.z, dist011, (int)color011.x, (int)color011.y, (int)color011.z, (int)valid011);
167 | // printf("\tp101 (%f, %f, %f) -> dist %f, color %d, %d, %d | valid %d\n", p101.x, p101.y, p101.z, dist101, (int)color101.x, (int)color101.y, (int)color101.z, (int)valid101);
168 | // printf("\tp111 (%f, %f, %f) -> dist %f, color %d, %d, %d | valid %d\n", p111.x, p111.y, p111.z, dist111, (int)color111.x, (int)color111.y, (int)color111.z, (int)valid111);
169 | //}
170 | if (!valid000 || !valid100 || !valid010 || !valid001 || !valid110 || !valid011 || !valid101 || !valid111) return;
171 |
172 | uint cubeindex = 0;
173 | if (dist010 < isolevel) cubeindex += 1;
174 | if (dist110 < isolevel) cubeindex += 2;
175 | if (dist100 < isolevel) cubeindex += 4;
176 | if (dist000 < isolevel) cubeindex += 8;
177 | if (dist011 < isolevel) cubeindex += 16;
178 | if (dist111 < isolevel) cubeindex += 32;
179 | if (dist101 < isolevel) cubeindex += 64;
180 | if (dist001 < isolevel) cubeindex += 128;
181 | const float thres = thresh;
182 | float distArray[] = { dist000, dist100, dist010, dist001, dist110, dist011, dist101, dist111 };
183 | //if (debugprint) {
184 | // printf("dists (%f, %f, %f, %f, %f, %f, %f, %f)\n", dist000, dist100, dist010, dist001, dist110, dist011, dist101, dist111);
185 | // printf("cubeindex %d\n", cubeindex);
186 | //}
187 | for (uint k = 0; k < 8; k++) {
188 | for (uint l = 0; l < 8; l++) {
189 | if (distArray[k] * distArray[l] < 0.0f) {
190 | if (fabs(distArray[k]) + fabs(distArray[l]) > thres) return;
191 | }
192 | else {
193 | if (fabs(distArray[k] - distArray[l]) > thres) return;
194 | }
195 | }
196 | }
197 | if (fabs(dist000) > thresh) return;
198 | if (fabs(dist100) > thresh) return;
199 | if (fabs(dist010) > thresh) return;
200 | if (fabs(dist001) > thresh) return;
201 | if (fabs(dist110) > thresh) return;
202 | if (fabs(dist011) > thresh) return;
203 | if (fabs(dist101) > thresh) return;
204 | if (fabs(dist111) > thresh) return;
205 |
206 | if (edgeTable[cubeindex] == 0 || edgeTable[cubeindex] == 255) return; // added by me edgeTable[cubeindex] == 255
207 |
208 | vec3uc c;
209 | {
210 | float d; int w;
211 | get_voxel(pos, tsdf_accessor, truncation, d, w);
212 | }
213 |
214 | vec3f vertlist[12];
215 | if (edgeTable[cubeindex] & 1) vertlist[0] = vertexInterp(isolevel, p010, p110, dist010, dist110);
216 | if (edgeTable[cubeindex] & 2) vertlist[1] = vertexInterp(isolevel, p110, p100, dist110, dist100);
217 | if (edgeTable[cubeindex] & 4) vertlist[2] = vertexInterp(isolevel, p100, p000, dist100, dist000);
218 | if (edgeTable[cubeindex] & 8) vertlist[3] = vertexInterp(isolevel, p000, p010, dist000, dist010);
219 | if (edgeTable[cubeindex] & 16) vertlist[4] = vertexInterp(isolevel, p011, p111, dist011, dist111);
220 | if (edgeTable[cubeindex] & 32) vertlist[5] = vertexInterp(isolevel, p111, p101, dist111, dist101);
221 | if (edgeTable[cubeindex] & 64) vertlist[6] = vertexInterp(isolevel, p101, p001, dist101, dist001);
222 | if (edgeTable[cubeindex] & 128) vertlist[7] = vertexInterp(isolevel, p001, p011, dist001, dist011);
223 | if (edgeTable[cubeindex] & 256) vertlist[8] = vertexInterp(isolevel, p010, p011, dist010, dist011);
224 | if (edgeTable[cubeindex] & 512) vertlist[9] = vertexInterp(isolevel, p110, p111, dist110, dist111);
225 | if (edgeTable[cubeindex] & 1024) vertlist[10] = vertexInterp(isolevel, p100, p101, dist100, dist101);
226 | if (edgeTable[cubeindex] & 2048) vertlist[11] = vertexInterp(isolevel, p000, p001, dist000, dist001);
227 |
228 | for (int i = 0; triTable[cubeindex][i] != -1; i += 3)
229 | {
230 | Triangle t;
231 | t.v0 = vertlist[triTable[cubeindex][i + 0]];
232 | t.v1 = vertlist[triTable[cubeindex][i + 1]];
233 | t.v2 = vertlist[triTable[cubeindex][i + 2]];
234 |
235 | //printf("triangle at (%f, %f, %f): (%f, %f, %f) (%f, %f, %f) (%f, %f, %f)\n", pos.x, pos.y, pos.z, t.v0.x, t.v0.y, t.v0.z, t.v1.x, t.v1.y, t.v1.z, t.v2.x, t.v2.y, t.v2.z);
236 | //printf("vertlist idxs: %d, %d, %d (%d, %d, %d)\n", triTable[cubeindex][i + 0], triTable[cubeindex][i + 1], triTable[cubeindex][i + 2], edgeTable[cubeindex] & 1, edgeTable[cubeindex] & 256, edgeTable[cubeindex] & 8);
237 | //getchar();
238 | results.push_back(t);
239 | }
240 | }
241 |
242 |
243 | // ----- MESH CLEANUP FUNCTIONS
244 | unsigned int remove_duplicate_faces(std::vector& faces)
245 | {
246 | struct vecHash {
247 | size_t operator()(const std::vector& v) const {
248 | //TODO larger prime number (64 bit) to match size_t
249 | const size_t p[] = {73856093, 19349669, 83492791};
250 | size_t res = 0;
251 | for (unsigned int i : v) {
252 | res = res ^ (size_t)i * p[i%3];
253 | }
254 | return res;
255 | //const size_t res = ((size_t)v.x * p0)^((size_t)v.y * p1)^((size_t)v.z * p2);
256 | }
257 | };
258 |
259 | size_t numFaces = faces.size();
260 | std::vector new_faces; new_faces.reserve(numFaces);
261 |
262 | std::unordered_set, vecHash> _set;
263 | for (size_t i = 0; i < numFaces; i++) {
264 | std::vector face = {(unsigned int)faces[i].x, (unsigned int)faces[i].y, (unsigned int)faces[i].z};
265 | std::sort(face.begin(), face.end());
266 | if (_set.find(face) == _set.end()) {
267 | //not found yet
268 | _set.insert(face);
269 | new_faces.push_back(faces[i]); //inserted the unsorted one
270 | }
271 | }
272 | if (faces.size() != new_faces.size()) {
273 | faces = new_faces;
274 | }
275 | //printf("Removed %d-%d=%d duplicate faces of %d\n", (int)numFaces, (int)new_faces.size(), (int)numFaces-(int)new_faces.size(), (int)numFaces);
276 |
277 | return (unsigned int)new_faces.size();
278 | }
279 | unsigned int remove_degenerate_faces(std::vector& faces)
280 | {
281 | std::vector new_faces;
282 |
283 | for (size_t i = 0; i < faces.size(); i++) {
284 | std::unordered_set _set(3);
285 | bool foundDuplicate = false;
286 | if (_set.find(faces[i].x) != _set.end()) { foundDuplicate = true; }
287 | else { _set.insert(faces[i].x); }
288 | if (!foundDuplicate && _set.find(faces[i].y) != _set.end()) { foundDuplicate = true; }
289 | else { _set.insert(faces[i].y); }
290 | if (!foundDuplicate && _set.find(faces[i].z) != _set.end()) { foundDuplicate = true; }
291 | else { _set.insert(faces[i].z); }
292 | if (!foundDuplicate) {
293 | new_faces.push_back(faces[i]);
294 | }
295 | }
296 | if (faces.size() != new_faces.size()) {
297 | faces = new_faces;
298 | }
299 |
300 | return (unsigned int)faces.size();
301 | }
302 | unsigned int hasNearestNeighbor( const vec3i& coord, SparseGrid3 > > &neighborQuery, const vec3f& v, float thresh )
303 | {
304 | float threshSq = thresh*thresh;
305 | for (int i = -1; i <= 1; i++) {
306 | for (int j = -1; j <= 1; j++) {
307 | for (int k = -1; k <= 1; k++) {
308 | vec3i c = coord + vec3i(i,j,k);
309 | if (neighborQuery.exists(c)) {
310 | for (const std::pair& n : neighborQuery[c]) {
311 | if (vec3f::distSq(v,n.first) < threshSq) {
312 | return n.second;
313 | }
314 | }
315 | }
316 | }
317 | }
318 | }
319 | return (unsigned int)-1;
320 | }
321 | unsigned int hasNearestNeighborApprox(const vec3i& coord, SparseGrid3 &neighborQuery) {
322 | for (int i = -1; i <= 1; i++) {
323 | for (int j = -1; j <= 1; j++) {
324 | for (int k = -1; k <= 1; k++) {
325 | vec3i c = coord + vec3i(i,j,k);
326 | if (neighborQuery.exists(c)) {
327 | return neighborQuery[c];
328 | }
329 | }
330 | }
331 | }
332 | return (unsigned int)-1;
333 | }
334 | int sgn(float val) {
335 | return (0.0f < val) - (val < 0.0f);
336 | }
337 | std::pair, std::vector> merge_close_vertices(const std::vector& meshTris, float thresh, bool approx)
338 | {
339 | // assumes voxelsize = 1
340 | assert(thresh > 0);
341 | unsigned int numV = (unsigned int)meshTris.size() * 3;
342 | std::vector vertices(numV);
343 | std::vector faces(meshTris.size());
344 | for (int i = 0; i < (int)meshTris.size(); i++) {
345 | vertices[3*i+0].x = meshTris[i].v0.x;
346 | vertices[3*i+0].y = meshTris[i].v0.y;
347 | vertices[3*i+0].z = meshTris[i].v0.z;
348 |
349 | vertices[3*i+1].x = meshTris[i].v1.x;
350 | vertices[3*i+1].y = meshTris[i].v1.y;
351 | vertices[3*i+1].z = meshTris[i].v1.z;
352 |
353 | vertices[3*i+2].x = meshTris[i].v2.x;
354 | vertices[3*i+2].y = meshTris[i].v2.y;
355 | vertices[3*i+2].z = meshTris[i].v2.z;
356 |
357 | faces[i].x = 3*i+0;
358 | faces[i].y = 3*i+1;
359 | faces[i].z = 3*i+2;
360 | }
361 |
362 | std::vector vertexLookUp; vertexLookUp.resize(numV);
363 | std::vector new_verts; new_verts.reserve(numV);
364 |
365 | unsigned int cnt = 0;
366 | if (approx) {
367 | SparseGrid3 neighborQuery(0.6f, numV*2);
368 | for (unsigned int v = 0; v < numV; v++) {
369 |
370 | const vec3f& vert = vertices[v];
371 | vec3i coord = vec3i(vert.x/thresh + 0.5f*sgn(vert.x), vert.y/thresh + 0.5f*sgn(vert.y), vert.z/thresh + 0.5f*sgn(vert.z));
372 | unsigned int nn = hasNearestNeighborApprox(coord, neighborQuery);
373 |
374 | if (nn == (unsigned int)-1) {
375 | neighborQuery[coord] = cnt;
376 | new_verts.push_back(vert);
377 | vertexLookUp[v] = cnt;
378 | cnt++;
379 | } else {
380 | vertexLookUp[v] = nn;
381 | }
382 | }
383 | } else {
384 | SparseGrid3 > > neighborQuery(0.6f, numV*2);
385 | for (unsigned int v = 0; v < numV; v++) {
386 |
387 | const vec3f& vert = vertices[v];
388 | vec3i coord = vec3i(vert.x/thresh + 0.5f*sgn(vert.x), vert.y/thresh + 0.5f*sgn(vert.y), vert.z/thresh + 0.5f*sgn(vert.z));
389 | unsigned int nn = hasNearestNeighbor(coord, neighborQuery, vert, thresh);
390 |
391 | if (nn == (unsigned int)-1) {
392 | neighborQuery[coord].push_back(std::make_pair(vert,cnt));
393 | new_verts.push_back(vert);
394 | vertexLookUp[v] = cnt;
395 | cnt++;
396 | } else {
397 | vertexLookUp[v] = nn;
398 | }
399 | }
400 | }
401 | // Update faces
402 | for (int i = 0; i < (int)faces.size(); i++) {
403 | faces[i].x = vertexLookUp[faces[i].x];
404 | faces[i].y = vertexLookUp[faces[i].y];
405 | faces[i].z = vertexLookUp[faces[i].z];
406 | }
407 |
408 | if (vertices.size() != new_verts.size()) {
409 | vertices = new_verts;
410 | }
411 |
412 | remove_degenerate_faces(faces);
413 | //printf("Merged %d-%d=%d of %d vertices\n", numV, cnt, numV-cnt, numV);
414 | return std::make_pair(vertices, faces);
415 | }
416 | // ----- MESH CLEANUP FUNCTIONS
417 |
418 | void run_marching_cubes_internal(
419 | const npy_accessor& tsdf_accessor,
420 | float isovalue,
421 | float truncation,
422 | float thresh,
423 | std::vector& results) {
424 | results.clear();
425 |
426 | for (int i = 0; i < (int)tsdf_accessor.size()[0]; i++) {
427 | for (int j = 0; j < (int)tsdf_accessor.size()[1]; j++) {
428 | for (int k = 0; k < (int)tsdf_accessor.size()[2]; k++) {
429 | extract_isosurface_at_position(vec3f(i, j, k), tsdf_accessor, truncation, isovalue, thresh, results);
430 | } // k
431 | } // j
432 | } // i
433 | //printf("#results = %d\n", (int)results.size());
434 | }
435 |
436 | void marching_cubes(const npy_accessor& tsdf_accessor, double isovalue, double truncation,
437 | std::vector& vertices, std::vector& polygons) {
438 |
439 | std::vector results;
440 | float thresh = 10.0f;
441 | run_marching_cubes_internal(tsdf_accessor, isovalue, truncation, thresh, results);
442 |
443 | // cleanup
444 | auto cleaned = merge_close_vertices(results, 0.00001f, true);
445 | remove_duplicate_faces(cleaned.second);
446 |
447 | vertices.resize(3 * cleaned.first.size());
448 | polygons.resize(3 * cleaned.second.size());
449 |
450 | for (int i = 0; i < (int)cleaned.first.size(); i++) {
451 | vertices[3 * i + 0] = cleaned.first[i].x;
452 | vertices[3 * i + 1] = cleaned.first[i].y;
453 | vertices[3 * i + 2] = cleaned.first[i].z;
454 | }
455 |
456 | for (int i = 0; i < (int)cleaned.second.size(); i++) {
457 | polygons[3 * i + 0] = cleaned.second[i].x;
458 | polygons[3 * i + 1] = cleaned.second[i].y;
459 | polygons[3 * i + 2] = cleaned.second[i].z;
460 | }
461 |
462 | }
463 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/marching_cubes.h:
--------------------------------------------------------------------------------
1 | #ifndef _MARCHING_CUBES_H
2 | #define _MARCHING_CUBES_H
3 |
4 | #include "pyarraymodule.h"
5 | #include
6 | #include
7 |
8 | struct npy_accessor {
9 | npy_accessor(PyArrayObject* arr, const std::array size) : m_arr(arr), m_size(size) {}
10 | const std::array& size() const {
11 | return m_size;
12 | }
13 | double operator()(long x, long y, long z) const {
14 | const npy_intp c[3] = {x, y, z};
15 | return PyArray_SafeGet(m_arr, c);
16 | }
17 |
18 | PyArrayObject* m_arr;
19 | const std::array m_size;
20 | };
21 |
22 | void marching_cubes(const npy_accessor& tsdf_accessor, double isovalue, double truncation,
23 | std::vector& vertices, std::vector& polygons);
24 |
25 | #endif // _MARCHING_CUBES_H
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/pyarray_symbol.h:
--------------------------------------------------------------------------------
1 |
2 | #define PY_ARRAY_UNIQUE_SYMBOL mcubes_PyArray_API
3 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/pyarraymodule.h:
--------------------------------------------------------------------------------
1 |
2 | #ifndef _EXTMODULE_H
3 | #define _EXTMODULE_H
4 |
5 | #include
6 | #include
7 |
8 | // #define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
9 | #define PY_ARRAY_UNIQUE_SYMBOL mcubes_PyArray_API
10 | #define NO_IMPORT_ARRAY
11 | #include "numpy/arrayobject.h"
12 |
13 | #include
14 |
15 | template
16 | struct numpy_typemap;
17 |
18 | #define define_numpy_type(ctype, dtype) \
19 | template<> \
20 | struct numpy_typemap \
21 | {static const int type = dtype;};
22 |
23 | define_numpy_type(bool, NPY_BOOL);
24 | define_numpy_type(char, NPY_BYTE);
25 | define_numpy_type(short, NPY_SHORT);
26 | define_numpy_type(int, NPY_INT);
27 | define_numpy_type(long, NPY_LONG);
28 | define_numpy_type(long long, NPY_LONGLONG);
29 | define_numpy_type(unsigned char, NPY_UBYTE);
30 | define_numpy_type(unsigned short, NPY_USHORT);
31 | define_numpy_type(unsigned int, NPY_UINT);
32 | define_numpy_type(unsigned long, NPY_ULONG);
33 | define_numpy_type(unsigned long long, NPY_ULONGLONG);
34 | define_numpy_type(float, NPY_FLOAT);
35 | define_numpy_type(double, NPY_DOUBLE);
36 | define_numpy_type(long double, NPY_LONGDOUBLE);
37 | define_numpy_type(std::complex, NPY_CFLOAT);
38 | define_numpy_type(std::complex, NPY_CDOUBLE);
39 | define_numpy_type(std::complex, NPY_CLONGDOUBLE);
40 |
41 | template
42 | T PyArray_SafeGet(const PyArrayObject* aobj, const npy_intp* indaux)
43 | {
44 | // HORROR.
45 | npy_intp* ind = const_cast(indaux);
46 | void* ptr = PyArray_GetPtr(const_cast(aobj), ind);
47 | switch(PyArray_TYPE(aobj))
48 | {
49 | case NPY_BOOL:
50 | return static_cast(*reinterpret_cast(ptr));
51 | case NPY_BYTE:
52 | return static_cast(*reinterpret_cast(ptr));
53 | case NPY_SHORT:
54 | return static_cast(*reinterpret_cast(ptr));
55 | case NPY_INT:
56 | return static_cast(*reinterpret_cast(ptr));
57 | case NPY_LONG:
58 | return static_cast(*reinterpret_cast(ptr));
59 | case NPY_LONGLONG:
60 | return static_cast(*reinterpret_cast(ptr));
61 | case NPY_UBYTE:
62 | return static_cast(*reinterpret_cast(ptr));
63 | case NPY_USHORT:
64 | return static_cast(*reinterpret_cast(ptr));
65 | case NPY_UINT:
66 | return static_cast(*reinterpret_cast(ptr));
67 | case NPY_ULONG:
68 | return static_cast(*reinterpret_cast(ptr));
69 | case NPY_ULONGLONG:
70 | return static_cast(*reinterpret_cast(ptr));
71 | case NPY_FLOAT:
72 | return static_cast(*reinterpret_cast(ptr));
73 | case NPY_DOUBLE:
74 | return static_cast(*reinterpret_cast(ptr));
75 | case NPY_LONGDOUBLE:
76 | return static_cast(*reinterpret_cast(ptr));
77 | default:
78 | throw std::runtime_error("data type not supported");
79 | }
80 | }
81 |
82 | template
83 | T PyArray_SafeSet(PyArrayObject* aobj, const npy_intp* indaux, const T& value)
84 | {
85 | // HORROR.
86 | npy_intp* ind = const_cast(indaux);
87 | void* ptr = PyArray_GetPtr(aobj, ind);
88 | switch(PyArray_TYPE(aobj))
89 | {
90 | case NPY_BOOL:
91 | *reinterpret_cast(ptr) = static_cast(value);
92 | break;
93 | case NPY_BYTE:
94 | *reinterpret_cast(ptr) = static_cast(value);
95 | break;
96 | case NPY_SHORT:
97 | *reinterpret_cast(ptr) = static_cast(value);
98 | break;
99 | case NPY_INT:
100 | *reinterpret_cast(ptr) = static_cast(value);
101 | break;
102 | case NPY_LONG:
103 | *reinterpret_cast(ptr) = static_cast(value);
104 | break;
105 | case NPY_LONGLONG:
106 | *reinterpret_cast(ptr) = static_cast(value);
107 | break;
108 | case NPY_UBYTE:
109 | *reinterpret_cast(ptr) = static_cast(value);
110 | break;
111 | case NPY_USHORT:
112 | *reinterpret_cast(ptr) = static_cast(value);
113 | break;
114 | case NPY_UINT:
115 | *reinterpret_cast(ptr) = static_cast(value);
116 | break;
117 | case NPY_ULONG:
118 | *reinterpret_cast(ptr) = static_cast(value);
119 | break;
120 | case NPY_ULONGLONG:
121 | *reinterpret_cast(ptr) = static_cast(value);
122 | break;
123 | case NPY_FLOAT:
124 | *reinterpret_cast(ptr) = static_cast(value);
125 | break;
126 | case NPY_DOUBLE:
127 | *reinterpret_cast(ptr) = static_cast(value);
128 | break;
129 | case NPY_LONGDOUBLE:
130 | *reinterpret_cast(ptr) = static_cast(value);
131 | break;
132 | default:
133 | throw std::runtime_error("data type not supported");
134 | }
135 | }
136 |
137 | #endif
138 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/pywrapper.cpp:
--------------------------------------------------------------------------------
1 |
2 | #include "pywrapper.h"
3 |
4 | #include "marching_cubes.h"
5 |
6 | #include
7 | #include
8 |
9 |
10 | PyObject* marching_cubes(PyArrayObject* arr, double isovalue, double truncation)
11 | {
12 | if(PyArray_NDIM(arr) != 3)
13 | throw std::runtime_error("Only three-dimensional arrays are supported.");
14 |
15 | // Prepare data.
16 | npy_intp* shape = PyArray_DIMS(arr);
17 | std::array lower{0, 0, 0};
18 | std::array upper{shape[0]-1, shape[1]-1, shape[2]-1};
19 | long numx = upper[0] - lower[0] + 1;
20 | long numy = upper[1] - lower[1] + 1;
21 | long numz = upper[2] - lower[2] + 1;
22 | std::vector vertices;
23 | std::vector polygons;
24 |
25 | // auto pyarray_to_cfunc = [&](long x, long y, long z) -> double {
26 | // const npy_intp c[3] = {x, y, z};
27 | // return PyArray_SafeGet(arr, c);
28 | // };
29 |
30 | npy_accessor tsdf_accessor(arr, {numx, numy, numz});
31 |
32 | // Marching cubes.
33 | marching_cubes(tsdf_accessor, isovalue, truncation, vertices, polygons);
34 |
35 | // Copy the result to two Python ndarrays.
36 | npy_intp size_vertices = vertices.size();
37 | npy_intp size_polygons = polygons.size();
38 | PyArrayObject* verticesarr = reinterpret_cast(PyArray_SimpleNew(1, &size_vertices, PyArray_DOUBLE));
39 | PyArrayObject* polygonsarr = reinterpret_cast(PyArray_SimpleNew(1, &size_polygons, PyArray_ULONG));
40 |
41 | std::vector::const_iterator it = vertices.begin();
42 | for(int i=0; it!=vertices.end(); ++i, ++it)
43 | *reinterpret_cast(PyArray_GETPTR1(verticesarr, i)) = *it;
44 | std::vector::const_iterator it2 = polygons.begin();
45 | for(int i=0; it2!=polygons.end(); ++i, ++it2)
46 | *reinterpret_cast(PyArray_GETPTR1(polygonsarr, i)) = *it2;
47 |
48 | PyObject* res = Py_BuildValue("(O,O)", verticesarr, polygonsarr);
49 | Py_XDECREF(verticesarr);
50 | Py_XDECREF(polygonsarr);
51 |
52 | return res;
53 | }
54 |
55 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/pywrapper.h:
--------------------------------------------------------------------------------
1 |
2 | #ifndef _PYWRAPPER_H
3 | #define _PYWRAPPER_H
4 |
5 | #include
6 | #include "pyarraymodule.h"
7 |
8 | #include
9 |
10 | PyObject* marching_cubes(PyArrayObject* arr, double isovalue, double truncation);
11 |
12 | #endif // _PYWRAPPER_H
13 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/sparsegrid3.h:
--------------------------------------------------------------------------------
1 |
2 | #include
3 | #include
4 | #include
5 |
6 | struct vec3i {
7 | vec3i() {
8 | x = 0;
9 | y = 0;
10 | z = 0;
11 | }
12 | vec3i(int x_, int y_, int z_) {
13 | x = x_;
14 | y = y_;
15 | z = z_;
16 | }
17 | inline vec3i operator+(const vec3i& other) const {
18 | return vec3i(x+other.x, y+other.y, z+other.z);
19 | }
20 | inline vec3i operator-(const vec3i& other) const {
21 | return vec3i(x-other.x, y-other.y, z-other.z);
22 | }
23 | inline bool operator==(const vec3i& other) const {
24 | if ((x == other.x) && (y == other.y) && (z == other.z))
25 | return true;
26 | return false;
27 | }
28 | int x;
29 | int y;
30 | int z;
31 | };
32 |
33 | namespace std {
34 |
35 | template <>
36 | struct hash : public std::unary_function {
37 | size_t operator()(const vec3i& v) const {
38 | //TODO larger prime number (64 bit) to match size_t
39 | const size_t p0 = 73856093;
40 | const size_t p1 = 19349669;
41 | const size_t p2 = 83492791;
42 | const size_t res = ((size_t)v.x * p0)^((size_t)v.y * p1)^((size_t)v.z * p2);
43 | return res;
44 | }
45 | };
46 |
47 | }
48 |
49 | template
50 | class SparseGrid3 {
51 | public:
52 | typedef typename std::unordered_map>::iterator iterator;
53 | typedef typename std::unordered_map>::const_iterator const_iterator;
54 | iterator begin() {return m_Data.begin();}
55 | iterator end() {return m_Data.end();}
56 | const_iterator begin() const {return m_Data.begin();}
57 | const_iterator end() const {return m_Data.end();}
58 |
59 | SparseGrid3(float maxLoadFactor = 0.6, size_t reserveBuckets = 64) {
60 | m_Data.reserve(reserveBuckets);
61 | m_Data.max_load_factor(maxLoadFactor);
62 | }
63 |
64 | size_t size() const {
65 | return m_Data.size();
66 | }
67 |
68 | void clear() {
69 | m_Data.clear();
70 | }
71 |
72 | bool exists(const vec3i& i) const {
73 | return (m_Data.find(i) != m_Data.end());
74 | }
75 |
76 | bool exists(int x, int y, int z) const {
77 | return exists(vec3i(x, y, z));
78 | }
79 |
80 | const T& operator()(const vec3i& i) const {
81 | return m_Data.find(i)->second;
82 | }
83 |
84 | //! if the element does not exist, it will be created with its default constructor
85 | T& operator()(const vec3i& i) {
86 | return m_Data[i];
87 | }
88 |
89 | const T& operator()(int x, int y, int z) const {
90 | return (*this)(vec3i(x,y,z));
91 | }
92 | T& operator()(int x, int y, int z) {
93 | return (*this)(vec3i(x,y,z));
94 | }
95 |
96 | const T& operator[](const vec3i& i) const {
97 | return (*this)(i);
98 | }
99 | T& operator[](const vec3i& i) {
100 | return (*this)(i);
101 | }
102 |
103 | protected:
104 | std::unordered_map> m_Data;
105 | };
106 |
107 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/marching_cubes/src/tables.h:
--------------------------------------------------------------------------------
1 |
2 | /////////////////////////////////////////////////////
3 | // tables
4 | /////////////////////////////////////////////////////
5 |
6 | // Polygonising a scalar field
7 | // Also known as: "3D Contouring", "Marching Cubes", "Surface Reconstruction"
8 | // Written by Paul Bourke
9 | // May 1994
10 | // http://paulbourke.net/geometry/polygonise/
11 |
12 |
13 | const static int edgeTable[256] = {
14 | 0x0, 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
15 | 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
16 | 0x190, 0x99, 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
17 | 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
18 | 0x230, 0x339, 0x33, 0x13a, 0x636, 0x73f, 0x435, 0x53c,
19 | 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
20 | 0x3a0, 0x2a9, 0x1a3, 0xaa, 0x7a6, 0x6af, 0x5a5, 0x4ac,
21 | 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
22 | 0x460, 0x569, 0x663, 0x76a, 0x66, 0x16f, 0x265, 0x36c,
23 | 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
24 | 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff, 0x3f5, 0x2fc,
25 | 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
26 | 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55, 0x15c,
27 | 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
28 | 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc,
29 | 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
30 | 0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,
31 | 0xcc, 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
32 | 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,
33 | 0x15c, 0x55, 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
34 | 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,
35 | 0x2fc, 0x3f5, 0xff, 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
36 | 0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,
37 | 0x36c, 0x265, 0x16f, 0x66, 0x76a, 0x663, 0x569, 0x460,
38 | 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,
39 | 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa, 0x1a3, 0x2a9, 0x3a0,
40 | 0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,
41 | 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33, 0x339, 0x230,
42 | 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,
43 | 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99, 0x190,
44 | 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,
45 | 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0 };
46 |
47 |
48 | const static int triTable[256][16] =
49 | { { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
50 | { 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
51 | { 0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
52 | { 1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
53 | { 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
54 | { 0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
55 | { 9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
56 | { 2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1 },
57 | { 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
58 | { 0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
59 | { 1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
60 | { 1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1 },
61 | { 3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
62 | { 0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1 },
63 | { 3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1 },
64 | { 9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
65 | { 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
66 | { 4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
67 | { 0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
68 | { 4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1 },
69 | { 1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
70 | { 3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1 },
71 | { 9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1 },
72 | { 2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1 },
73 | { 8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
74 | { 11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1 },
75 | { 9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1 },
76 | { 4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1 },
77 | { 3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1 },
78 | { 1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1 },
79 | { 4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1 },
80 | { 4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1 },
81 | { 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
82 | { 9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
83 | { 0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
84 | { 8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1 },
85 | { 1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
86 | { 3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1 },
87 | { 5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1 },
88 | { 2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1 },
89 | { 9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
90 | { 0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1 },
91 | { 0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1 },
92 | { 2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1 },
93 | { 10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1 },
94 | { 4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1 },
95 | { 5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1 },
96 | { 5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1 },
97 | { 9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
98 | { 9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1 },
99 | { 0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1 },
100 | { 1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
101 | { 9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1 },
102 | { 10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1 },
103 | { 8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1 },
104 | { 2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1 },
105 | { 7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1 },
106 | { 9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1 },
107 | { 2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1 },
108 | { 11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1 },
109 | { 9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1 },
110 | { 5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1 },
111 | { 11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1 },
112 | { 11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
113 | { 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
114 | { 0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
115 | { 9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
116 | { 1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1 },
117 | { 1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
118 | { 1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1 },
119 | { 9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1 },
120 | { 5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1 },
121 | { 2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
122 | { 11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1 },
123 | { 0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1 },
124 | { 5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1 },
125 | { 6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1 },
126 | { 0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1 },
127 | { 3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1 },
128 | { 6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1 },
129 | { 5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
130 | { 4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1 },
131 | { 1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1 },
132 | { 10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1 },
133 | { 6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1 },
134 | { 1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1 },
135 | { 8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1 },
136 | { 7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1 },
137 | { 3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1 },
138 | { 5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1 },
139 | { 0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1 },
140 | { 9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1 },
141 | { 8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1 },
142 | { 5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1 },
143 | { 0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1 },
144 | { 6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1 },
145 | { 10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
146 | { 4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1 },
147 | { 10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1 },
148 | { 8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1 },
149 | { 1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1 },
150 | { 3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1 },
151 | { 0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
152 | { 8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1 },
153 | { 10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1 },
154 | { 0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1 },
155 | { 3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1 },
156 | { 6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1 },
157 | { 9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1 },
158 | { 8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1 },
159 | { 3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1 },
160 | { 6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
161 | { 7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1 },
162 | { 0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1 },
163 | { 10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1 },
164 | { 10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1 },
165 | { 1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1 },
166 | { 2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1 },
167 | { 7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1 },
168 | { 7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
169 | { 2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1 },
170 | { 2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1 },
171 | { 1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1 },
172 | { 11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1 },
173 | { 8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1 },
174 | { 0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
175 | { 7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1 },
176 | { 7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
177 | { 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
178 | { 3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
179 | { 0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
180 | { 8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1 },
181 | { 10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
182 | { 1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1 },
183 | { 2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1 },
184 | { 6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1 },
185 | { 7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
186 | { 7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1 },
187 | { 2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1 },
188 | { 1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1 },
189 | { 10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1 },
190 | { 10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1 },
191 | { 0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1 },
192 | { 7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1 },
193 | { 6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
194 | { 3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1 },
195 | { 8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1 },
196 | { 9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1 },
197 | { 6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1 },
198 | { 1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1 },
199 | { 4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1 },
200 | { 10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1 },
201 | { 8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1 },
202 | { 0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
203 | { 1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1 },
204 | { 1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1 },
205 | { 8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1 },
206 | { 10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1 },
207 | { 4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1 },
208 | { 10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
209 | { 4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
210 | { 0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1 },
211 | { 5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1 },
212 | { 11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1 },
213 | { 9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1 },
214 | { 6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1 },
215 | { 7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1 },
216 | { 3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1 },
217 | { 7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1 },
218 | { 9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1 },
219 | { 3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1 },
220 | { 6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1 },
221 | { 9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1 },
222 | { 1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1 },
223 | { 4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1 },
224 | { 7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1 },
225 | { 6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1 },
226 | { 3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1 },
227 | { 0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1 },
228 | { 6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1 },
229 | { 1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1 },
230 | { 0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1 },
231 | { 11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1 },
232 | { 6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1 },
233 | { 5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1 },
234 | { 9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1 },
235 | { 1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1 },
236 | { 1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
237 | { 1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1 },
238 | { 10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1 },
239 | { 0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
240 | { 10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
241 | { 11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
242 | { 11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1 },
243 | { 5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1 },
244 | { 10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1 },
245 | { 11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1 },
246 | { 0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1 },
247 | { 9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1 },
248 | { 7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1 },
249 | { 2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1 },
250 | { 8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1 },
251 | { 9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1 },
252 | { 9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1 },
253 | { 1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
254 | { 0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1 },
255 | { 9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1 },
256 | { 9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
257 | { 5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1 },
258 | { 5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1 },
259 | { 0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1 },
260 | { 10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1 },
261 | { 2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1 },
262 | { 0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1 },
263 | { 0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1 },
264 | { 9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
265 | { 2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1 },
266 | { 5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1 },
267 | { 3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1 },
268 | { 5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1 },
269 | { 8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1 },
270 | { 0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
271 | { 8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1 },
272 | { 9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
273 | { 4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1 },
274 | { 0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1 },
275 | { 1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1 },
276 | { 3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1 },
277 | { 4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1 },
278 | { 9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1 },
279 | { 11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1 },
280 | { 11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1 },
281 | { 2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1 },
282 | { 9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1 },
283 | { 3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1 },
284 | { 1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
285 | { 4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1 },
286 | { 4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1 },
287 | { 4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
288 | { 4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
289 | { 9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
290 | { 3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1 },
291 | { 0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1 },
292 | { 3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
293 | { 1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1 },
294 | { 3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1 },
295 | { 0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
296 | { 3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
297 | { 2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1 },
298 | { 9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
299 | { 2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1 },
300 | { 1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
301 | { 1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
302 | { 0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
303 | { 0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 },
304 | { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 } };
305 |
--------------------------------------------------------------------------------
/external/NumpyMarchingCubes/setup.py:
--------------------------------------------------------------------------------
1 | # -*- encoding: utf-8 -*-
2 |
3 | from setuptools import setup
4 |
5 | from setuptools.extension import Extension
6 |
7 |
8 | class lazy_cythonize(list):
9 | """
10 | Lazy evaluate extension definition, to allow correct requirements install.
11 | """
12 |
13 | def __init__(self, callback):
14 | super(lazy_cythonize, self).__init__()
15 | self._list, self.callback = None, callback
16 |
17 | def c_list(self):
18 | if self._list is None:
19 | self._list = self.callback()
20 |
21 | return self._list
22 |
23 | def __iter__(self):
24 | for e in self.c_list():
25 | yield e
26 |
27 | def __getitem__(self, ii):
28 | return self.c_list()[ii]
29 |
30 | def __len__(self):
31 | return len(self.c_list())
32 |
33 |
34 | def extensions():
35 |
36 | from Cython.Build import cythonize
37 | import numpy
38 |
39 | numpy_include_dir = numpy.get_include()
40 |
41 | marching_cubes_module = Extension(
42 | "marching_cubes._mcubes",
43 | [
44 | "marching_cubes/src/_mcubes.pyx",
45 | "marching_cubes/src/pywrapper.cpp",
46 | "marching_cubes/src/marching_cubes.cpp"
47 | ],
48 | language="c++",
49 | extra_compile_args=['-std=c++11', '-Wall'],
50 | include_dirs=[numpy_include_dir],
51 | depends=[
52 | "marching_cubes/src/marching_cubes.h",
53 | "marching_cubes/src/pyarray_symbol.h",
54 | "marching_cubes/src/pyarraymodule.h",
55 | "marching_cubes/src/pywrapper.h"
56 | ],
57 | )
58 |
59 | return cythonize([marching_cubes_module])
60 |
61 | setup(
62 | name="NumpyMarchingCubes",
63 | version="0.0.1",
64 | description="Marching cubes for Python",
65 | author="Dejan Azinovic, Angela Dai, Justus Thies (PyMCubes: Pablo Márquez Neila)",
66 | url="",
67 | license="BSD 3-clause",
68 | long_description="""
69 | Marching cubes for Python
70 | """,
71 | classifiers=[
72 | "Development Status :: 5 - Production/Stable",
73 | "Environment :: Console",
74 | "Intended Audience :: Developers",
75 | "Intended Audience :: Science/Research",
76 | "License :: OSI Approved :: BSD License",
77 | "Natural Language :: English",
78 | "Operating System :: OS Independent",
79 | "Programming Language :: C++",
80 | "Programming Language :: Python",
81 | "Topic :: Multimedia :: Graphics :: 3D Modeling",
82 | "Topic :: Scientific/Engineering :: Image Recognition",
83 | ],
84 | packages=["marching_cubes"],
85 | ext_modules=lazy_cythonize(extensions),
86 | requires=['numpy', 'Cython', 'PyCollada'],
87 | setup_requires=['numpy', 'Cython']
88 | )
89 |
--------------------------------------------------------------------------------
/requirements.txt:
--------------------------------------------------------------------------------
1 | absl-py==2.0.0
2 | addict==2.4.0
3 | aiofiles==22.1.0
4 | aiosqlite==0.19.0
5 | ansi2html==1.8.0
6 | anyio==3.7.1
7 | argon2-cffi==23.1.0
8 | argon2-cffi-bindings==21.2.0
9 | arrow==1.2.3
10 | attrs==23.1.0
11 | Babel==2.14.0
12 | backcall==0.2.0
13 | beautifulsoup4==4.12.3
14 | bleach==6.0.0
15 | cached-property==1.5.2
16 | cachetools==5.3.1
17 | certifi==2020.6.20
18 | cffi==1.15.1
19 | charset-normalizer==3.3.1
20 | click==8.1.7
21 | colorama @ file:///croot/colorama_1672386526460/work
22 | comm==0.1.4
23 | ConfigArgParse==1.7
24 | cycler @ file:///tmp/build/80754af9/cycler_1637851556182/work
25 | dash==2.14.0
26 | dash-core-components==2.0.0
27 | dash-html-components==2.0.0
28 | dash-table==5.0.0
29 | debugpy==1.7.0
30 | decorator==5.1.1
31 | defusedxml==0.7.1
32 | deprecation==2.1.0
33 | entrypoints==0.4
34 | exceptiongroup==1.2.0
35 | fastjsonschema==2.18.1
36 | Flask==2.2.5
37 | fonttools==4.38.0
38 | fqdn==1.5.1
39 | freetype-py==2.4.0
40 | fvcore==0.1.5.post20210915
41 | google-auth==2.23.3
42 | google-auth-oauthlib==0.4.6
43 | grpcio==1.59.0
44 | idna==3.4
45 | imageio==2.31.2
46 | importlib-metadata==6.7.0
47 | importlib-resources==5.12.0
48 | iopath==0.1.9
49 | ipykernel==6.16.2
50 | ipython==7.34.0
51 | ipython-genutils==0.2.0
52 | ipywidgets==8.1.1
53 | isoduration==20.11.0
54 | itsdangerous==2.1.2
55 | jedi==0.19.1
56 | Jinja2==3.1.2
57 | joblib==1.3.2
58 | json5==0.9.16
59 | jsonpointer==2.4
60 | jsonschema==4.17.3
61 | jupyter-events==0.6.3
62 | jupyter-server==1.24.0
63 | jupyter-ydoc==0.2.5
64 | jupyter_client==7.4.9
65 | jupyter_core==4.12.0
66 | jupyter_packaging==0.12.3
67 | jupyter_server_fileid==0.9.1
68 | jupyter_server_ydoc==0.8.0
69 | jupyterlab==3.6.7
70 | jupyterlab-pygments==0.2.2
71 | jupyterlab-widgets==3.0.9
72 | jupyterlab_server==2.24.0
73 | kiwisolver @ file:///croot/kiwisolver_1672387140495/work
74 | Markdown==3.4.4
75 | MarkupSafe==2.1.3
76 | mathutils==2.81.2
77 | matplotlib==3.5.3
78 | matplotlib-inline==0.1.6
79 | mistune==3.0.2
80 | mkl-fft==1.3.1
81 | mkl-random==1.2.2
82 | mkl-service==2.4.0
83 | munkres==1.1.4
84 | nbclassic==1.0.0
85 | nbclient==0.7.4
86 | nbconvert==7.6.0
87 | nbformat==5.7.0
88 | nest-asyncio==1.5.8
89 | networkx==2.6.3
90 | notebook==6.5.6
91 | notebook_shim==0.2.4
92 | numpy==1.21.6
93 | NumpyMarchingCubes==0.0.1
94 | oauthlib==3.2.2
95 | olefile @ file:///home/conda/feedstock_root/build_artifacts/olefile_1602866521163/work
96 | open3d==0.15.2
97 | opencv-contrib-python==4.6.0.66
98 | packaging==23.1
99 | pandas==1.3.5
100 | pandocfilters==1.5.1
101 | parso==0.8.3
102 | pexpect==4.8.0
103 | pickleshare==0.7.5
104 | Pillow==9.5.0
105 | pkgutil_resolve_name==1.3.10
106 | plotly==5.17.0
107 | portalocker==1.4.0
108 | prometheus-client==0.17.1
109 | prompt-toolkit==3.0.39
110 | protobuf==3.20.3
111 | psutil==5.9.8
112 | ptyprocess==0.7.0
113 | pyasn1==0.5.0
114 | pyasn1-modules==0.3.0
115 | pycparser==2.21
116 | pyglet==1.5.27
117 | Pygments==2.16.1
118 | PyOpenGL==3.1.0
119 | pyparsing==3.0.9
120 | pyquaternion==0.9.9
121 | pyrender==0.1.45
122 | pyrsistent==0.19.3
123 | python-dateutil==2.8.2
124 | python-json-logger==2.0.7
125 | pytorch3d @ git+https://github.com/facebookresearch/pytorch3d.git@a8c70161a1c99c1878ae7cf312cf7907a84f01b0
126 | pytz==2023.3.post1
127 | PyWavelets==1.3.0
128 | PyYAML==6.0
129 | pyzmq==24.0.1
130 | requests==2.31.0
131 | requests-oauthlib==1.3.1
132 | retrying==1.3.4
133 | rfc3339-validator==0.1.4
134 | rfc3986-validator==0.1.1
135 | rsa==4.9
136 | Rtree @ file:///croot/rtree_1675157851263/work
137 | scikit-image==0.19.3
138 | scikit-learn==1.0.2
139 | scipy==1.7.3
140 | Send2Trash==1.8.2
141 | six @ file:///home/conda/feedstock_root/build_artifacts/six_1620240208055/work
142 | sniffio==1.3.0
143 | soupsieve==2.4.1
144 | tabulate==0.9.0
145 | tenacity==8.2.3
146 | tensorboard==2.11.2
147 | tensorboard-data-server==0.6.1
148 | tensorboard-plugin-wit==1.8.1
149 | termcolor==2.3.0
150 | terminado==0.17.1
151 | threadpoolctl==3.1.0
152 | tifffile==2021.11.2
153 | tinycss2==1.2.1
154 | tinycudann==1.7
155 | tomli==2.0.1
156 | tomlkit==0.12.3
157 | torch==1.10.1
158 | torchaudio==0.10.1
159 | torchvision==0.11.2
160 | tornado==6.2
161 | tqdm==4.65.0
162 | traitlets==5.9.0
163 | trimesh==3.21.5
164 | typing_extensions==4.5.0
165 | uri-template==1.3.0
166 | urllib3==2.0.7
167 | wcwidth==0.2.8
168 | webcolors==1.13
169 | webencodings==0.5.1
170 | websocket-client==1.6.1
171 | Werkzeug==2.2.3
172 | widgetsnbextension==4.0.9
173 | y-py==0.6.2
174 | yacs==0.1.8
175 | ypy-websocket==0.8.4
176 | zipp==3.15.0
177 |
--------------------------------------------------------------------------------
/run_benchmark.py:
--------------------------------------------------------------------------------
1 |
2 | # This file is modified from ESLAM
3 | import argparse
4 | import torch
5 | import numpy as np
6 | import random
7 | import os
8 | import json
9 | from torch.utils.tensorboard import SummaryWriter
10 | from src import config
11 | from src.standard_SLAM import standard_SLAM
12 |
13 | def _set_random_seed(seed) -> None:
14 | """Set randomness seed in torch and numpy"""
15 | np.random.seed(seed)
16 | random.seed(seed)
17 | torch.manual_seed(seed)
18 | torch.cuda.manual_seed(seed)
19 | # When running on the CuDNN backend, two further options must be set
20 | torch.backends.cudnn.deterministic = True
21 | torch.backends.cudnn.benchmark = False
22 | # Set a fixed value for the hash seed
23 | os.environ["PYTHONHASHSEED"] = str(seed)
24 | print(f"Random seed set as {seed}")
25 |
26 | def main():
27 | parser = argparse.ArgumentParser(
28 | description='Arguments for running standard benchmark SLAM.'
29 | )
30 | parser.add_argument('config', type=str, help='Path to config file.')
31 | parser.add_argument('--input_folder', type=str,
32 | help='input folder, this have higher priority, can overwrite the one in config file')
33 | parser.add_argument('--output', type=str,
34 | help='output folder, this have higher priority, can overwrite the one in config file')
35 | args = parser.parse_args()
36 |
37 | cfg = config.load_config(args.config, 'configs/benchmark_standard.yaml')
38 |
39 | save_path = cfg["data"]["output"]
40 | if not os.path.exists(save_path):
41 | os.makedirs(save_path)
42 | with open(os.path.join(save_path, 'config.json'),"w", encoding='utf-8') as f:
43 | f.write(json.dumps(cfg, indent=4))
44 |
45 | writer = SummaryWriter(save_path)
46 | _set_random_seed(919)
47 |
48 | nerf_slam = standard_SLAM(cfg, writer=writer)
49 |
50 | nerf_slam.run()
51 |
52 | if __name__ == '__main__':
53 | main()
54 |
--------------------------------------------------------------------------------
/src/NeRFs/decoder.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | import torch.nn.functional as F
4 | from src.utils.common_utils import normalize_3d_coordinate
5 |
6 |
7 | class Grid_Decoder(nn.Module):
8 | def __init__(self, device, bound, config, input_ch=3, pos_ch=3, geo_feat_dim=15, hidden_dim=32, num_layers=2,beta=None):
9 | super(Grid_Decoder, self).__init__()
10 | self.config = config
11 | self.bounding_box = bound
12 | self.input_ch = input_ch
13 | self.pos_ch = pos_ch
14 | self.hidden_dim = hidden_dim
15 | self.num_layers = num_layers
16 | if beta is not None:
17 | self.beta = beta
18 | self.coupling = config['NeRFs']['tricks']['share_encoder']
19 | self.geo_feat_dim = geo_feat_dim
20 | if (self.geo_feat_dim==0) and (self.coupling):
21 | self.coupling_base = True
22 | self.coupling = False
23 | else:
24 | self.coupling_base = False
25 |
26 | sdf_net = self.get_sdf_decoder()
27 | self.sdf_net = sdf_net.to(device)
28 | color_net = self.get_color_decoder()
29 | self.color_net = color_net.to(device)
30 |
31 | def get_sdf_decoder(self):
32 | sdf_net = []
33 | for l in range(self.num_layers):
34 | if l == 0:
35 | in_dim = self.input_ch
36 | else:
37 | in_dim = self.hidden_dim
38 | if l == self.num_layers - 1:
39 | if self.coupling:
40 | out_dim = 1 + self.geo_feat_dim # 1 sigma + 15 SH features for color
41 | else:
42 | out_dim = 1
43 | else:
44 | out_dim = self.hidden_dim
45 |
46 | sdf_net.append(nn.Linear(in_dim, out_dim, bias=False))
47 | if l != self.num_layers - 1:
48 | sdf_net.append(nn.ReLU(inplace=True))
49 |
50 | return nn.Sequential(*nn.ModuleList(sdf_net))
51 |
52 | def get_color_decoder(self):
53 | color_net = []
54 | for l in range(self.num_layers):
55 | if l == 0:
56 | if self.coupling:
57 | # only geo feature passed to color decoder
58 | in_dim = self.geo_feat_dim+self.pos_ch
59 | else:
60 | # its own color embeding
61 | in_dim = self.input_ch #+self.geo_feat_dim
62 | else:
63 | in_dim = self.hidden_dim
64 |
65 | if l == self.num_layers - 1:
66 | out_dim = 3 # 3 rgb
67 | else:
68 | out_dim = self.hidden_dim
69 |
70 | color_net.append(nn.Linear(in_dim, out_dim, bias=False))
71 | if l != self.num_layers - 1:
72 | color_net.append(nn.ReLU(inplace=True))
73 |
74 | return nn.Sequential(*nn.ModuleList(color_net))
75 |
76 | def forward(self, query_points, pos_embed_fn, geo_embed_fn, app_embed_fn = None):
77 |
78 | inputs_flat = torch.reshape(query_points, [-1, query_points.shape[-1]])
79 | inputs_flat = (inputs_flat - self.bounding_box[:, 0]) / (self.bounding_box[:, 1] - self.bounding_box[:, 0])
80 |
81 | # get feature embedding
82 | embed = geo_embed_fn(inputs_flat)
83 | embed_pos = pos_embed_fn(inputs_flat)
84 |
85 | if self.coupling:
86 | h = self.sdf_net(torch.cat([embed, embed_pos], dim=-1))
87 | sdf, geo_feat = h[...,:1], h[...,1:]
88 | rgb = self.color_net(torch.cat([geo_feat,embed_pos], dim=-1))
89 | elif self.coupling_base:
90 | sdf = self.sdf_net(torch.cat([embed, embed_pos], dim=-1))
91 | rgb = self.color_net(torch.cat([embed_pos, embed], dim=-1))
92 | elif (not self.coupling) and (not self.coupling_base):
93 | embed_color = app_embed_fn(inputs_flat)
94 | sdf = self.sdf_net(torch.cat([embed, embed_pos], dim=-1))
95 | rgb = self.color_net(torch.cat([embed_pos, embed_color], dim=-1))
96 |
97 | return torch.cat([rgb, sdf], -1)
98 |
99 | class Decomp_Decoders(nn.Module):
100 | """
101 | Decoders for SDF and RGB.
102 | Args:
103 | c_dim: feature dimensions
104 | hidden_size: hidden size of MLP
105 | truncation: truncation of SDF
106 | n_blocks: number of MLP blocks
107 | """
108 | def __init__(self, device, bound, config, pos_ch, c_dim=32, geo_feat_dim=15, hidden_size=32, n_blocks=2,beta=None):
109 | super().__init__()
110 | self.bound = bound
111 | self.device = device
112 | self.pos_ch = pos_ch
113 | self.c_dim = c_dim
114 | self.n_blocks = n_blocks
115 | if beta is not None:
116 | self.beta = beta
117 | self.coupling = config['NeRFs']['tricks']['share_encoder']
118 | self.F = 'tensorf' if config['NeRFs']['F']['choice_TensoRF'] else 'tri_plane'
119 | self.geo_feat_dim = geo_feat_dim
120 | if (self.geo_feat_dim==0) and (self.coupling):
121 | self.coupling_base = True
122 | self.coupling = False
123 | else:
124 | self.coupling_base = False
125 |
126 | if self.coupling:
127 | #SDF decoder
128 | linears = nn.ModuleList(
129 | [nn.Linear(c_dim, hidden_size)] +
130 | [nn.Linear(hidden_size, hidden_size) for i in range(n_blocks - 1)])
131 | output_linear = nn.Linear(hidden_size, 1+self.geo_feat_dim)
132 | #RGB decoder
133 | c_linears = nn.ModuleList(
134 | [nn.Linear(self.geo_feat_dim+self.pos_ch, hidden_size)] +
135 | [nn.Linear(hidden_size, hidden_size) for i in range(n_blocks - 1)])
136 | c_output_linear = nn.Linear(hidden_size, 3)
137 | else:
138 | #SDF decoder
139 | linears = nn.ModuleList(
140 | [nn.Linear(c_dim, hidden_size)] +
141 | [nn.Linear(hidden_size, hidden_size) for i in range(n_blocks - 1)])
142 | output_linear = nn.Linear(hidden_size, 1)
143 | #RGB decoder
144 | c_linears = nn.ModuleList(
145 | [nn.Linear(c_dim, hidden_size)] +
146 | [nn.Linear(hidden_size, hidden_size) for i in range(n_blocks - 1)])
147 | c_output_linear = nn.Linear(hidden_size, 3)
148 |
149 |
150 | self.linears = linears.to(self.device)
151 | self.output_linear = output_linear.to(self.device)
152 | self.c_linears = c_linears.to(self.device)
153 | self.c_output_linear = c_output_linear.to(self.device)
154 |
155 | def sample_decomp_feature(self, p_nor, planes_xy, planes_xz, planes_yz, lines_z = None, lines_y = None, lines_x = None):
156 | """
157 | Sample feature from planes
158 | Args:
159 | p_nor (tensor): normalized 3D coordinates
160 | planes_xy (list): xy planes
161 | planes_xz (list): xz planes
162 | planes_yz (list): yz planes
163 | Returns:
164 | feat (tensor): sampled features
165 | """
166 | vgrid = p_nor[None, :, None].to(torch.float32)
167 | if lines_z is not None:
168 | coord_line = torch.stack((p_nor[..., 2], p_nor[..., 1], p_nor[..., 0]))
169 | coord_line = torch.stack((torch.zeros_like(coord_line), coord_line), dim=-1).detach().view(3, -1, 1, 2).to(torch.float32)
170 |
171 | plane_feat = []
172 | line_feat = []
173 | for i in range(len(planes_xy)):
174 | xy = F.grid_sample(planes_xy[i].to(self.device), vgrid[..., [0, 1]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
175 | xz = F.grid_sample(planes_xz[i].to(self.device), vgrid[..., [0, 2]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
176 | yz = F.grid_sample(planes_yz[i].to(self.device), vgrid[..., [1, 2]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
177 | plane_feat.append(xy + xz + yz)
178 | if lines_x is not None:
179 | z = F.grid_sample(lines_z[i].to(self.device), coord_line[[0]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
180 | y = F.grid_sample(lines_y[i].to(self.device), coord_line[[1]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
181 | x = F.grid_sample(lines_x[i].to(self.device), coord_line[[2]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
182 | line_feat.append(z + y + x)
183 |
184 | plane_feat = torch.cat(plane_feat, dim=-1)
185 | line_feat = torch.cat(line_feat, dim=-1) if lines_x is not None else None
186 | if line_feat is not None:
187 | feat = plane_feat*line_feat
188 | else:
189 | feat = plane_feat
190 | return feat
191 |
192 | def get_raw_sdf(self, embed_pos, p_nor, all_planes, all_lines = None):
193 | """
194 | Get raw SDF
195 | Args:
196 | p_nor (tensor): normalized 3D coordinates
197 | all_planes (Tuple): all feature planes
198 | Returns:
199 | sdf (tensor): raw SDF
200 | """
201 | if all_lines is None:
202 | planes_xy, planes_xz, planes_yz = all_planes
203 | feat = self.sample_decomp_feature(p_nor, planes_xy, planes_xz, planes_yz)
204 | else:
205 | planes_xy, planes_xz, planes_yz = all_planes
206 | lines_z, lines_y, lines_x = all_lines
207 | feat = self.sample_decomp_feature(p_nor, planes_xy, planes_xz, planes_yz, lines_z, lines_y, lines_x)
208 |
209 | h = torch.cat([embed_pos.to(feat.dtype), feat], dim=-1) #feat
210 | for i, l in enumerate(self.linears):
211 | h = self.linears[i](h)
212 | h = F.relu(h, inplace=True)
213 | sdf_geo = torch.tanh(self.output_linear(h)).squeeze()
214 |
215 | return sdf_geo
216 |
217 | def get_raw_rgb(self, p_nor, embed_pos, all_planes = None, all_lines = None, geo_fea = None):
218 | """
219 | Get raw RGB
220 | Args:
221 | p_nor (tensor): normalized 3D coordinates
222 | all_planes (Tuple): all feature planes
223 | Returns:
224 | rgb (tensor): raw RGB
225 | """
226 |
227 | if self.F == 'tri_plane':
228 | if self.coupling:
229 | h = torch.cat([embed_pos.to(geo_fea.dtype), geo_fea], dim=-1) #geo_fea
230 | else:
231 | # if couling_base, inputed all_planes is geo_planes
232 | # if not couling_base, inputed all_planes is color_planes
233 | c_planes_xy, c_planes_xz, c_planes_yz = all_planes
234 | c_feat = self.sample_decomp_feature(p_nor, c_planes_xy, c_planes_xz, c_planes_yz)
235 | h = torch.cat([embed_pos.to(c_feat.dtype), c_feat], dim=-1) #c_feat
236 |
237 | elif self.F == 'tensorf':
238 | if self.coupling:
239 | h = torch.cat([embed_pos.to(geo_fea.dtype), geo_fea], dim=-1) #geo_fea
240 | else:
241 | c_planes_xy, c_planes_xz, c_planes_yz = all_planes
242 | c_lines_z, c_lines_y, c_lines_x = all_lines
243 | c_feat = self.sample_decomp_feature(p_nor, c_planes_xy, c_planes_xz, c_planes_yz, c_lines_z, c_lines_y, c_lines_x)
244 | h = torch.cat([embed_pos.to(c_feat.dtype), c_feat], dim=-1) #c_feat
245 |
246 | for i, l in enumerate(self.c_linears):
247 | h = self.c_linears[i](h)
248 | h = F.relu(h, inplace=True)
249 | rgb = self.c_output_linear(h) #torch.sigmoid(self.c_output_linear(h))
250 | return rgb
251 |
252 | def forward(self, p, pos_embed_fn, all_geo_planes, all_c_planes = None, all_geo_lines = None, all_c_lines = None):
253 | """
254 | Forward pass
255 | Args:
256 | p (tensor): 3D coordinates
257 | all_planes (Tuple): all feature planes
258 | Returns:
259 | raw (tensor): raw SDF and RGB
260 | """
261 |
262 | #p_shape = p.shape
263 | #p_nor = normalize_3d_coordinate(p.clone(), self.bound)
264 | inputs_flat = torch.reshape(p, [-1, p.shape[-1]])
265 | p_nor = (inputs_flat - self.bound[:, 0]) / (self.bound[:, 1] - self.bound[:, 0])
266 |
267 | embed_pos = pos_embed_fn(p_nor)
268 |
269 | sdf_geo = self.get_raw_sdf(p_nor, embed_pos, all_geo_planes, all_geo_lines)
270 |
271 | if self.coupling:
272 | #share encoder, but no connection between geo and app
273 | sdf, geo_feat = sdf_geo[...,:1], sdf_geo[...,1:]
274 | rgb = self.get_raw_rgb(p_nor, embed_pos, geo_fea = geo_feat)
275 | elif self.coupling_base:
276 | sdf = sdf_geo.unsqueeze(-1)
277 | rgb = self.get_raw_rgb(p_nor, embed_pos, all_planes = all_geo_planes, all_lines = all_geo_lines, geo_fea = None)
278 | elif (not self.coupling) and (not self.coupling_base):
279 | #separate encoder, then need color embeding
280 | sdf = sdf_geo.unsqueeze(-1)
281 | rgb = self.get_raw_rgb(p_nor, embed_pos, all_planes = all_c_planes, all_lines = all_c_lines, geo_fea = None)
282 |
283 | raw = torch.cat([rgb, sdf], dim=-1)
284 | #raw = raw.reshape(*p_shape[:-1], -1)
285 | return raw
286 |
287 |
288 | class Decoder(nn.Module):
289 | def __init__(self, device, bound, depth=4,width=256,in_dim=3,skips=[4],geo_feat_dim = None,coupling = False,beta=None):
290 | super().__init__()
291 | self.skips = skips
292 | self.coupling = coupling
293 | self.device = device
294 | self.bound = bound
295 | self.geo_feat_dim = geo_feat_dim
296 | if (self.geo_feat_dim==0) and (self.coupling):
297 | self.coupling_base = True
298 | self.coupling = False
299 | else:
300 | self.coupling_base = False
301 | width_p = 32
302 | if beta is not None:
303 | self.beta = beta
304 |
305 | pts_linears = nn.ModuleList(
306 | [nn.Linear(in_dim, width)] + [nn.Linear(width, width) if i not in self.skips else nn.Linear(width + in_dim, width) for i in range(depth-1)])
307 | self.pts_linears = pts_linears.to(self.device)
308 | if (not self.coupling) and (not self.coupling_base):
309 | pts_linears_c = nn.ModuleList(
310 | [nn.Linear(in_dim, width)] + [nn.Linear(width, width) if i not in self.skips else nn.Linear(width + in_dim, width) for i in range(depth-1)])
311 | self.pts_linears_c = pts_linears_c.to(self.device)
312 |
313 | if self.coupling:
314 | sdf_out = nn.Sequential(
315 | nn.Linear(width, width_p),#+in_dim, width),
316 | nn.ReLU(),
317 | nn.Linear(width_p, 1+self.geo_feat_dim))
318 | color_out = nn.Sequential(
319 | nn.Linear(self.geo_feat_dim+in_dim, width_p),#+in_dim, width),
320 | nn.ReLU(),
321 | nn.Linear(width_p, 3))#,nn.Sigmoid())
322 | else:
323 | sdf_out = nn.Sequential(
324 | nn.Linear(width, width_p),#+in_dim, width),
325 | nn.ReLU(),
326 | nn.Linear(width_p, 1))
327 | color_out = nn.Sequential(
328 | nn.Linear(width, width_p),#+in_dim, width),
329 | nn.ReLU(),
330 | nn.Linear(width_p, 3))#,nn.Sigmoid())
331 |
332 | self.sdf_out = sdf_out.to(self.device)
333 | self.color_out = color_out.to(self.device)
334 |
335 | #output_linear = nn.Linear(width, 4)
336 | #self.output_linear = output_linear.to(self.device)
337 |
338 | def get_values(self, x):
339 | h = x
340 | for i, l in enumerate(self.pts_linears):
341 | h = self.pts_linears[i](h)
342 | h = F.relu(h)
343 | if i in self.skips:
344 | h = torch.cat([x, h], -1)
345 |
346 | if (not self.coupling) and (not self.coupling_base):
347 | h_c = x
348 | for i, l in enumerate(self.pts_linears_c):
349 | h_c = self.pts_linears_c[i](h_c)
350 | h_c = F.relu(h_c)
351 | if i in self.skips:
352 | h_c = torch.cat([x, h_c], -1)
353 |
354 | if self.coupling:
355 | sdf_out = self.sdf_out(h)
356 | sdf = sdf_out[:, :1]
357 | sdf_feat = sdf_out[:, 1:]
358 |
359 | h = torch.cat([sdf_feat, x], dim=-1)#h=sdf_feat
360 | rgb = self.color_out(h)
361 | elif self.coupling_base:
362 | sdf = self.sdf_out(h)
363 | rgb = self.color_out(h)
364 | elif (not self.coupling) and (not self.coupling_base):
365 | #outputs = self.output_linear(h) #outputs[:, :3] = torch.sigmoid(outputs[:, :3])
366 | sdf = self.sdf_out(h)
367 | rgb = self.color_out(h_c)
368 |
369 | outputs = torch.cat([rgb, sdf], dim=-1)
370 | return outputs
371 |
372 | def forward(self, query_points, pos_embed_fn):
373 | #pointsf = query_points.reshape(-1, 3)
374 | inputs_flat = torch.reshape(query_points, [-1, query_points.shape[-1]])
375 | pointsf = (inputs_flat - self.bound[:, 0]) / (self.bound[:, 1] - self.bound[:, 0])
376 |
377 | x = pos_embed_fn(pointsf)
378 | outputs = self.get_values(x)
379 |
380 | return outputs
381 |
382 |
383 | def get_decoder(encoder, device ,bound, config, input_ch=32, pos_ch=3, geo_feat_dim=15, hidden_dim=32, num_layers=2, beta=None):
384 | if encoder == 'freq':
385 | coupling = config['NeRFs']['tricks']['share_encoder']
386 | decoder = Decoder(device = device,bound=bound,in_dim=input_ch,geo_feat_dim =geo_feat_dim, coupling = coupling, beta = beta)
387 | elif (encoder == 'dense') or (encoder == 'hash'):
388 | decoder = Grid_Decoder(bound=bound, config=config, input_ch=input_ch, pos_ch=pos_ch, geo_feat_dim=geo_feat_dim,
389 | hidden_dim=hidden_dim, num_layers=num_layers, device = device, beta = beta)
390 | elif (encoder == 'tri_plane') or (encoder == 'tensorf'):
391 | decoder = Decomp_Decoders(device = device, bound=bound, config=config, pos_ch=pos_ch, c_dim=input_ch, geo_feat_dim=geo_feat_dim,
392 | hidden_size=hidden_dim, n_blocks=num_layers, beta = beta)
393 | return decoder
--------------------------------------------------------------------------------
/src/NeRFs/decoder_hybrid.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | import torch.nn.functional as F
4 | from src.utils.common_utils import normalize_3d_coordinate
5 |
6 |
7 | class hybrid_Decoder(nn.Module):
8 | def __init__(self, device, bound, config, input_ch=3, pos_ch=3, geo_feat_dim=15, hidden_dim=32, num_layers=2,beta=None):
9 | super(hybrid_Decoder, self).__init__()
10 | self.device = device
11 | self.config = config
12 | self.dim = config['NeRFs']['space_resolution']['implicit_dim']
13 | self.bounding_box = bound
14 | self.input_ch = input_ch
15 | self.pos_ch = pos_ch
16 | self.hidden_dim = hidden_dim
17 | self.num_layers = num_layers
18 | if beta is not None:
19 | self.beta = beta
20 | self.coupling = config['NeRFs']['tricks']['share_encoder']
21 | self.geo_feat_dim = geo_feat_dim
22 | if (self.geo_feat_dim==0) and (self.coupling):
23 | self.coupling_base = True
24 | self.coupling = False
25 | else:
26 | self.coupling_base = False
27 |
28 | sdf_net = self.get_sdf_decoder()
29 | self.sdf_net = sdf_net.to(device)
30 | color_net = self.get_color_decoder()
31 | self.color_net = color_net.to(device)
32 |
33 | def get_sdf_decoder(self):
34 | sdf_net = []
35 | for l in range(self.num_layers):
36 | if l == 0:
37 | in_dim = self.input_ch
38 | else:
39 | in_dim = self.hidden_dim
40 | if l == self.num_layers - 1:
41 | if self.coupling:
42 | out_dim = 1 + self.geo_feat_dim # 1 sigma + 15 SH features for color
43 | else:
44 | out_dim = 1
45 | else:
46 | out_dim = self.hidden_dim
47 |
48 | sdf_net.append(nn.Linear(in_dim, out_dim, bias=False))
49 | if l != self.num_layers - 1:
50 | sdf_net.append(nn.ReLU(inplace=True))
51 |
52 | return nn.Sequential(*nn.ModuleList(sdf_net))
53 |
54 | def get_color_decoder(self):
55 | color_net = []
56 | for l in range(self.num_layers):
57 | if l == 0:
58 | in_dim = self.input_ch-self.dim
59 | #if self.coupling:
60 | # only geo feature passed to color decoder
61 | # in_dim = self.geo_feat_dim+self.pos_ch
62 | #else:
63 | # its own color embeding
64 | # in_dim = self.input_ch #+self.geo_feat_dim
65 | else:
66 | in_dim = self.hidden_dim
67 |
68 | if l == self.num_layers - 1:
69 | out_dim = 3 # 3 rgb
70 | else:
71 | out_dim = self.hidden_dim
72 |
73 | color_net.append(nn.Linear(in_dim, out_dim, bias=False))
74 | if l != self.num_layers - 1:
75 | color_net.append(nn.ReLU(inplace=True))
76 |
77 | return nn.Sequential(*nn.ModuleList(color_net))
78 |
79 | def sample_decomp_feature(self, p_nor, planes_xy, planes_xz, planes_yz, lines_z = None, lines_y = None, lines_x = None):
80 | """
81 | Sample feature from planes
82 | Args:
83 | p_nor (tensor): normalized 3D coordinates
84 | planes_xy (list): xy planes
85 | planes_xz (list): xz planes
86 | planes_yz (list): yz planes
87 | Returns:
88 | feat (tensor): sampled features
89 | """
90 | vgrid = p_nor[None, :, None].to(torch.float32)
91 | if lines_z is not None:
92 | coord_line = torch.stack((p_nor[..., 2], p_nor[..., 1], p_nor[..., 0]))
93 | coord_line = torch.stack((torch.zeros_like(coord_line), coord_line), dim=-1).detach().view(3, -1, 1, 2).to(torch.float32)
94 |
95 | plane_feat = []
96 | line_feat = []
97 | for i in range(len(planes_xy)):
98 | xy = F.grid_sample(planes_xy[i].to(self.device), vgrid[..., [0, 1]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
99 | xz = F.grid_sample(planes_xz[i].to(self.device), vgrid[..., [0, 2]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
100 | yz = F.grid_sample(planes_yz[i].to(self.device), vgrid[..., [1, 2]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
101 | plane_feat.append(xy + xz + yz)
102 | if lines_x is not None:
103 | z = F.grid_sample(lines_z[i].to(self.device), coord_line[[0]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
104 | y = F.grid_sample(lines_y[i].to(self.device), coord_line[[1]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
105 | x = F.grid_sample(lines_x[i].to(self.device), coord_line[[2]], padding_mode='border', align_corners=True, mode='bilinear').squeeze().transpose(0, 1)
106 | line_feat.append(z + y + x)
107 |
108 | plane_feat = torch.cat(plane_feat, dim=-1)
109 | line_feat = torch.cat(line_feat, dim=-1) if lines_x is not None else None
110 | if line_feat is not None:
111 | feat = plane_feat*line_feat
112 | else:
113 | feat = plane_feat
114 | return feat
115 |
116 |
117 | def forward(self, query_points, pos_embed_fn, all_geo_planes, geo_embed_fn, app_embed_fn = None):
118 |
119 | inputs_flat = torch.reshape(query_points, [-1, query_points.shape[-1]])
120 | inputs_flat = (inputs_flat - self.bounding_box[:, 0]) / (self.bounding_box[:, 1] - self.bounding_box[:, 0])
121 |
122 | # get feature embedding for gird
123 | embed = geo_embed_fn(inputs_flat)
124 | embed_pos = pos_embed_fn(inputs_flat)
125 |
126 | # get feature embedding for plane
127 | planes_xy, planes_xz, planes_yz = all_geo_planes
128 | feat = self.sample_decomp_feature(inputs_flat, planes_xy, planes_xz, planes_yz)
129 |
130 | #print('feat:', feat.shape)
131 | #print('embed:', embed.shape)
132 | #print('embed_pos:', embed_pos.shape)
133 | #print('inputs_flat', inputs_flat.shape)
134 |
135 | sdf = self.sdf_net(torch.cat([embed, feat, embed_pos], dim=-1))
136 | rgb = self.color_net(torch.cat([embed_pos, embed], dim=-1))
137 | return torch.cat([rgb, sdf], -1)
138 |
139 |
140 | def get_decoder(device ,bound, config, input_ch=32, pos_ch=3,
141 | geo_feat_dim=0, hidden_dim=32, num_layers=2, beta=None):
142 |
143 | decoder = hybrid_Decoder(bound=bound, config=config, input_ch=input_ch, pos_ch=pos_ch, geo_feat_dim=geo_feat_dim,
144 | hidden_dim=hidden_dim, num_layers=num_layers, device = device, beta = beta)
145 |
146 | return decoder
--------------------------------------------------------------------------------
/src/NeRFs/encoder.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import numpy as np
3 | import tinycudann as tcnn
4 | import torch.nn as nn
5 |
6 |
7 | class GaussianFourierFeatureTransform(torch.nn.Module):
8 | """
9 | Modified based on the implementation of Gaussian Fourier feature mapping.
10 |
11 | "Fourier Features Let Networks Learn High Frequency Functions in Low Dimensional Domains":
12 | https://arxiv.org/abs/2006.10739
13 | https://people.eecs.berkeley.edu/~bmild/fourfeat/index.html
14 |
15 | """
16 |
17 | def __init__(self, num_input_channels, mapping_size=93, scale=25, learnable=True):
18 | super().__init__()
19 |
20 | if learnable:
21 | self._B = nn.Parameter(torch.randn(
22 | (num_input_channels, mapping_size)) * scale)
23 | else:
24 | self._B = torch.randn((num_input_channels, mapping_size)) * scale
25 | def forward(self, x):
26 | x = x.squeeze(0)
27 | x = x.to(self._B.dtype)
28 | assert x.dim() == 2, 'Expected 2D input (got {}D input)'.format(x.dim())
29 | x = x @ self._B.to(x.device)
30 | return torch.sin(x)
31 |
32 |
33 | def get_encoder(encoding,
34 | res_level, implicit_dim,
35 | resolution_fine, resolution_coarse,
36 | log2_hashmap_size=19, input_dim=3):
37 | if 'dense' in encoding.lower():
38 | per_level_scale = np.exp2(np.log2( max(resolution_fine) / max(resolution_coarse)) / (res_level - 1))
39 | embed = tcnn.Encoding(
40 | n_input_dims=input_dim,
41 | encoding_config={
42 | "otype": "Grid",
43 | "type": "Dense",
44 | "n_levels": res_level,
45 | "n_features_per_level": implicit_dim,
46 | "base_resolution": max(resolution_coarse),
47 | "per_level_scale": per_level_scale,
48 | "interpolation": "Linear"},
49 | dtype=torch.float
50 | )
51 | out_dim = embed.n_output_dims
52 |
53 | elif 'hash' in encoding.lower():
54 | per_level_scale = np.exp2(np.log2( max(resolution_fine) / max(resolution_coarse)) / (res_level - 1))
55 | embed = tcnn.Encoding(
56 | n_input_dims=input_dim,
57 | encoding_config={
58 | "otype": 'HashGrid',
59 | "n_levels": res_level,
60 | "n_features_per_level": implicit_dim,
61 | "log2_hashmap_size": log2_hashmap_size,
62 | "base_resolution": max(resolution_coarse),
63 | "per_level_scale": per_level_scale},
64 | dtype=torch.float
65 | )
66 | out_dim = embed.n_output_dims
67 |
68 | elif 'tri_plane' or 'tensorf' in encoding.lower():
69 |
70 | per_level_scale_x = np.exp2(np.log2( resolution_fine[0] / resolution_coarse[0]) / (res_level - 1))
71 | per_level_scale_y = np.exp2(np.log2( resolution_fine[1] / resolution_coarse[1]) / (res_level - 1))
72 | per_level_scale_z = np.exp2(np.log2( resolution_fine[2] / resolution_coarse[2]) / (res_level - 1))
73 | grid_shape = [resolution_coarse[2], resolution_coarse[1], resolution_coarse[0]]
74 | grid_shape_level_scale = [per_level_scale_z, per_level_scale_y, per_level_scale_x]
75 | planes_xy, planes_xz, planes_yz = [], [], []
76 | if 'tensorf' in encoding.lower():
77 | lines_z, lines_y, lines_x = [], [], []
78 |
79 | for i in range(res_level):
80 | planes_xy.append(torch.empty([1, implicit_dim, *grid_shape[1:]]).normal_(mean=0, std=0.01))
81 | planes_xz.append(torch.empty([1, implicit_dim, grid_shape[0], grid_shape[2]]).normal_(mean=0, std=0.01))
82 | planes_yz.append(torch.empty([1, implicit_dim, *grid_shape[:2]]).normal_(mean=0, std=0.01))
83 |
84 | if 'tensorf' in encoding.lower():
85 | lines_z.append(torch.empty([1, implicit_dim, grid_shape[0],1]).normal_(mean=0, std=0.01))
86 | lines_y.append(torch.empty([1, implicit_dim, grid_shape[1],1]).normal_(mean=0, std=0.01))
87 | lines_x.append(torch.empty([1, implicit_dim, grid_shape[2],1]).normal_(mean=0, std=0.01))
88 |
89 | grid_shape = [int(grid_shape[0]*grid_shape_level_scale[0]),
90 | int(grid_shape[1]*grid_shape_level_scale[1]),
91 | int(grid_shape[2]*grid_shape_level_scale[2])]
92 |
93 | if 'tri_plane' in encoding.lower():
94 | embed = planes_xy, planes_xz, planes_yz
95 | if 'tensorf' in encoding.lower():
96 | embed = planes_xy, planes_xz, planes_yz, lines_z, lines_y, lines_x
97 |
98 | out_dim = None
99 |
100 | return embed, out_dim
101 |
102 |
103 | def get_pos_embed(embedding,input_dim=3, n_bins=16):
104 |
105 | if 'identity' in embedding.lower():
106 | embed = tcnn.Encoding(
107 | n_input_dims=input_dim,
108 | encoding_config={
109 | "otype": "Identity"
110 | },
111 | dtype=torch.float
112 | )
113 | out_dim = embed.n_output_dims
114 |
115 | elif 'blob' in embedding.lower():
116 | print('Use blob')
117 | embed = tcnn.Encoding(
118 | n_input_dims=input_dim,
119 | encoding_config={
120 | "otype": "OneBlob", #Component type.
121 | "n_bins": n_bins
122 | },
123 | dtype=torch.float
124 | )
125 | out_dim = embed.n_output_dims
126 |
127 | # Frequency encoding from iMAP
128 | elif 'freq' in embedding.lower():
129 | print('Use frequency')
130 | embed = GaussianFourierFeatureTransform(input_dim, mapping_size=93, scale=25)
131 | out_dim = 93
132 | return embed, out_dim
133 |
134 |
--------------------------------------------------------------------------------
/src/NeRFs/encoder_hybrid.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import numpy as np
3 | import tinycudann as tcnn
4 | import torch.nn as nn
5 |
6 |
7 | class GaussianFourierFeatureTransform(torch.nn.Module):
8 | """
9 | Modified based on the implementation of Gaussian Fourier feature mapping.
10 |
11 | "Fourier Features Let Networks Learn High Frequency Functions in Low Dimensional Domains":
12 | https://arxiv.org/abs/2006.10739
13 | https://people.eecs.berkeley.edu/~bmild/fourfeat/index.html
14 |
15 | """
16 |
17 | def __init__(self, num_input_channels, mapping_size=93, scale=25, learnable=True):
18 | super().__init__()
19 |
20 | if learnable:
21 | self._B = nn.Parameter(torch.randn(
22 | (num_input_channels, mapping_size)) * scale)
23 | else:
24 | self._B = torch.randn((num_input_channels, mapping_size)) * scale
25 | def forward(self, x):
26 | x = x.squeeze(0)
27 | x = x.to(self._B.dtype)
28 | assert x.dim() == 2, 'Expected 2D input (got {}D input)'.format(x.dim())
29 | x = x @ self._B.to(x.device)
30 | return torch.sin(x)
31 |
32 |
33 | def get_encoder(encoding,
34 | res_level, implicit_dim,
35 | resolution_fine, resolution_coarse,
36 | log2_hashmap_size=19, input_dim=3):
37 | if 'dense' in encoding.lower():
38 | per_level_scale = np.exp2(np.log2( max(resolution_fine) / max(resolution_coarse)) / (res_level - 1))
39 | if res_level==1:
40 | per_level_scale = 1
41 | embed = tcnn.Encoding(
42 | n_input_dims=input_dim,
43 | encoding_config={
44 | "otype": "Grid",
45 | "type": "Dense",
46 | "n_levels": res_level,
47 | "n_features_per_level": implicit_dim,
48 | "base_resolution": max(resolution_coarse),
49 | "per_level_scale": per_level_scale,
50 | "interpolation": "Linear"},
51 | dtype=torch.float
52 | )
53 | out_dim = embed.n_output_dims
54 |
55 | elif 'hash' in encoding.lower():
56 | per_level_scale = np.exp2(np.log2( max(resolution_fine) / max(resolution_coarse)) / (res_level - 1))
57 | embed = tcnn.Encoding(
58 | n_input_dims=input_dim,
59 | encoding_config={
60 | "otype": 'HashGrid',
61 | "n_levels": res_level,
62 | "n_features_per_level": implicit_dim,
63 | "log2_hashmap_size": log2_hashmap_size,
64 | "base_resolution": max(resolution_coarse),
65 | "per_level_scale": per_level_scale},
66 | dtype=torch.float
67 | )
68 | out_dim = embed.n_output_dims
69 |
70 | elif 'tri_plane' or 'tensorf' in encoding.lower():
71 |
72 | #per_level_scale_x = np.exp2(np.log2( resolution_fine[0] / resolution_coarse[0]) / (res_level - 1))
73 | #per_level_scale_y = np.exp2(np.log2( resolution_fine[1] / resolution_coarse[1]) / (res_level - 1))
74 | #per_level_scale_z = np.exp2(np.log2( resolution_fine[2] / resolution_coarse[2]) / (res_level - 1))
75 | grid_shape = [resolution_coarse[2], resolution_coarse[1], resolution_coarse[0]]
76 | #grid_shape_level_scale = [per_level_scale_z, per_level_scale_y, per_level_scale_x]
77 | planes_xy, planes_xz, planes_yz = [], [], []
78 | #if 'tensorf' in encoding.lower():
79 | # lines_z, lines_y, lines_x = [], [], []
80 |
81 | #for i in range(res_level):
82 | planes_xy.append(torch.empty([1, implicit_dim, *grid_shape[1:]]).normal_(mean=0, std=0.01))
83 | planes_xz.append(torch.empty([1, implicit_dim, grid_shape[0], grid_shape[2]]).normal_(mean=0, std=0.01))
84 | planes_yz.append(torch.empty([1, implicit_dim, *grid_shape[:2]]).normal_(mean=0, std=0.01))
85 |
86 | #if 'tensorf' in encoding.lower():
87 | # lines_z.append(torch.empty([1, implicit_dim, grid_shape[0],1]).normal_(mean=0, std=0.01))
88 | # lines_y.append(torch.empty([1, implicit_dim, grid_shape[1],1]).normal_(mean=0, std=0.01))
89 | # lines_x.append(torch.empty([1, implicit_dim, grid_shape[2],1]).normal_(mean=0, std=0.01))
90 |
91 | # grid_shape = [int(grid_shape[0]*grid_shape_level_scale[0]),
92 | # int(grid_shape[1]*grid_shape_level_scale[1]),
93 | # int(grid_shape[2]*grid_shape_level_scale[2])]
94 |
95 | if 'tri_plane' in encoding.lower():
96 | embed = planes_xy, planes_xz, planes_yz
97 | #if 'tensorf' in encoding.lower():
98 | # embed = planes_xy, planes_xz, planes_yz, lines_z, lines_y, lines_x
99 |
100 | out_dim = None
101 |
102 | return embed, out_dim
103 |
104 |
105 | def get_pos_embed(embedding,input_dim=3, n_bins=16):
106 |
107 | if 'identity' in embedding.lower():
108 | embed = tcnn.Encoding(
109 | n_input_dims=input_dim,
110 | encoding_config={
111 | "otype": "Identity"
112 | },
113 | dtype=torch.float
114 | )
115 | out_dim = embed.n_output_dims
116 |
117 | elif 'blob' in embedding.lower():
118 | print('Use blob')
119 | embed = tcnn.Encoding(
120 | n_input_dims=input_dim,
121 | encoding_config={
122 | "otype": "OneBlob", #Component type.
123 | "n_bins": n_bins
124 | },
125 | dtype=torch.float
126 | )
127 | out_dim = embed.n_output_dims
128 |
129 | # Frequency encoding from iMAP
130 | elif 'freq' in embedding.lower():
131 | print('Use frequency')
132 | embed = GaussianFourierFeatureTransform(input_dim, mapping_size=93, scale=25)
133 | out_dim = 93
134 | return embed, out_dim
135 |
136 |
--------------------------------------------------------------------------------
/src/NeRFs/keyframe_global.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import numpy as np
3 | import random
4 |
5 | class KeyFrameDatabase(object):
6 | def __init__(self, config, H, W, num_kf, num_rays_to_save, device) -> None:
7 | self.config = config
8 | self.keyframes = {}
9 | self.device = device
10 | self.rays = torch.zeros((num_kf, num_rays_to_save, 7))
11 | self.num_rays_to_save = num_rays_to_save
12 | self.frame_ids = None
13 | self.H = H
14 | self.W = W
15 |
16 |
17 | def __len__(self):
18 | return len(self.frame_ids)
19 |
20 | def get_length(self):
21 | return self.__len__()
22 |
23 | def sample_single_keyframe_rays(self, rays, option='random'):
24 | '''
25 | Sampling strategy for current keyframe rays
26 | '''
27 | if option == 'random':
28 | idxs = random.sample(range(0, self.H*self.W), self.num_rays_to_save)
29 | elif option == 'filter_depth':
30 | valid_depth_mask = (rays[..., -1] > 0.0) & (rays[..., -1] <= self.config["cam"]["depth_trunc"])
31 | rays_valid = rays[valid_depth_mask, :] # [n_valid, 7]
32 | num_valid = len(rays_valid)
33 | idxs = random.sample(range(0, num_valid), self.num_rays_to_save)
34 |
35 | else:
36 | raise NotImplementedError()
37 | rays = rays[:, idxs]
38 | return rays
39 |
40 | def attach_ids(self, frame_ids):
41 | '''
42 | Attach the frame ids to list
43 | '''
44 | if self.frame_ids is None:
45 | self.frame_ids = frame_ids
46 | else:
47 | self.frame_ids = torch.cat([self.frame_ids, frame_ids], dim=0)
48 |
49 | def add_keyframe(self, batch, filter_depth=False):
50 | '''
51 | Add keyframe rays to the keyframe database
52 | '''
53 | # batch direction (Bs=1, H*W, 3)
54 | rays = torch.cat([batch['direction'], batch['rgb'], batch['depth'][..., None]], dim=-1)
55 | rays = rays.reshape(1, -1, rays.shape[-1])
56 | if filter_depth:
57 | rays = self.sample_single_keyframe_rays(rays, 'filter_depth')
58 | else:
59 | rays = self.sample_single_keyframe_rays(rays)
60 |
61 | if not isinstance(batch['frame_id'], torch.Tensor):
62 | batch['frame_id'] = torch.tensor([batch['frame_id']])
63 |
64 | self.attach_ids(batch['frame_id'])
65 |
66 | # Store the rays
67 | self.rays[len(self.frame_ids)-1] = rays
68 |
69 | def sample_global_rays(self, bs):
70 | '''
71 | Sample rays from self.rays as well as frame_ids
72 | '''
73 | num_kf = self.__len__()
74 | idxs = torch.tensor(random.sample(range(num_kf * self.num_rays_to_save), bs))
75 | sample_rays = self.rays[:num_kf].reshape(-1, 7)[idxs]
76 |
77 | frame_ids = self.frame_ids[idxs//self.num_rays_to_save]
78 |
79 | return sample_rays, frame_ids
80 |
81 | def sample_global_keyframe(self, window_size, n_fixed=1):
82 | '''
83 | Sample keyframe globally
84 | Window size: limit the window size for keyframe
85 | n_fixed: sample the last n_fixed keyframes
86 | '''
87 | if window_size >= len(self.frame_ids):
88 | return self.rays[:len(self.frame_ids)], self.frame_ids
89 |
90 | current_num_kf = len(self.frame_ids)
91 | last_frame_ids = self.frame_ids[-n_fixed:]
92 |
93 | # Random sampling
94 | idx = random.sample(range(0, len(self.frame_ids) -n_fixed), window_size)
95 |
96 | # Include last n_fixed
97 | idx_rays = idx + list(range(current_num_kf-n_fixed, current_num_kf))
98 | select_rays = self.rays[idx_rays]
99 |
100 | return select_rays, \
101 | torch.cat([self.frame_ids[idx], last_frame_ids], dim=0)
102 |
103 | @torch.no_grad()
104 | def sample_overlap_keyframe(self, batch, frame_id, est_c2w_list, k_frame, n_samples=16, n_pixel=100, dataset=None):
105 | '''
106 | NICE-SLAM strategy for selecting overlapping keyframe from all previous frames
107 |
108 | batch: Information of current frame
109 | frame_id: id of current frame
110 | est_c2w_list: estimated c2w of all frames
111 | k_frame: num of keyframes for BA i.e. window size
112 | n_samples: num of sample points for each ray
113 | n_pixel: num of pixels for computing overlap
114 | '''
115 | c2w_est = est_c2w_list[frame_id]
116 |
117 | indices = torch.randint(dataset.H* dataset.W, (n_pixel,))
118 | rays_d_cam = batch['direction'].reshape(-1, 3)[indices].to(self.device)
119 | target_d = batch['depth'].reshape(-1, 1)[indices].repeat(1, n_samples).to(self.device)
120 | rays_d = torch.sum(rays_d_cam[..., None, :] * c2w_est[:3, :3], -1)
121 | rays_o = c2w_est[None, :3, -1].repeat(rays_d.shape[0], 1).to(self.device)
122 |
123 | t_vals = torch.linspace(0., 1., steps=n_samples).to(target_d)
124 | near = target_d*0.8
125 | far = target_d+0.5
126 | z_vals = near * (1.-t_vals) + far * (t_vals)
127 | pts = rays_o[..., None, :] + rays_d[..., None, :] * \
128 | z_vals[..., :, None] # [N_rays, N_samples, 3]
129 | pts_flat = pts.reshape(-1, 3).cpu().numpy()
130 |
131 | key_frame_list = []
132 |
133 | for i, frame_id in enumerate(self.frame_ids):
134 | frame_id = int(frame_id.item())
135 | c2w = est_c2w_list[frame_id].cpu().numpy()
136 | w2c = np.linalg.inv(c2w)
137 | ones = np.ones_like(pts_flat[:, 0]).reshape(-1, 1)
138 | pts_flat_homo = np.concatenate(
139 | [pts_flat, ones], axis=1).reshape(-1, 4, 1) # (N, 4)
140 | cam_cord_homo = w2c@pts_flat_homo # (N, 4, 1)=(4,4)*(N, 4, 1)
141 | cam_cord = cam_cord_homo[:, :3] # (N, 3, 1)
142 | K = np.array([[self.config['cam']['fx'], .0, self.config['cam']['cx']],
143 | [.0, self.config['cam']['fy'], self.config['cam']['cy']],
144 | [.0, .0, 1.0]]).reshape(3, 3)
145 | cam_cord[:, 0] *= -1
146 | uv = K@cam_cord
147 | z = uv[:, -1:]+1e-5
148 | uv = uv[:, :2]/z
149 | uv = uv.astype(np.float32)
150 | edge = 20
151 | mask = (uv[:, 0] < self.config['cam']['W']-edge)*(uv[:, 0] > edge) * \
152 | (uv[:, 1] < self.config['cam']['H']-edge)*(uv[:, 1] > edge)
153 | mask = mask & (z[:, :, 0] < 0)
154 | mask = mask.reshape(-1)
155 | percent_inside = mask.sum()/uv.shape[0]
156 | key_frame_list.append(
157 | {'id': frame_id, 'percent_inside': percent_inside, 'sample_id':i})
158 |
159 |
160 |
161 | key_frame_list = sorted(
162 | key_frame_list, key=lambda i: i['percent_inside'], reverse=True)
163 | selected_keyframe_list = [dic['sample_id']
164 | for dic in key_frame_list if dic['percent_inside'] > 0.00]
165 | selected_keyframe_list = list(np.random.permutation(
166 | np.array(selected_keyframe_list))[:k_frame])
167 |
168 | last_id = len(self.frame_ids) - 1
169 |
170 | if last_id not in selected_keyframe_list:
171 | selected_keyframe_list.append(last_id)
172 |
173 | return self.rays[selected_keyframe_list], selected_keyframe_list
--------------------------------------------------------------------------------
/src/config.py:
--------------------------------------------------------------------------------
1 | # This file is provided by ESLAM
2 |
3 | import yaml
4 |
5 | def load_config(path, default_path=None):
6 | """
7 | Loads config file.
8 |
9 | Args:
10 | path (str): path to config file.
11 | default_path (str, optional): whether to use default path. Defaults to None.
12 |
13 | Returns:
14 | cfg (dict): config dict.
15 |
16 | """
17 | # load configuration from file itself
18 | with open(path, 'r') as f:
19 | cfg_special = yaml.full_load(f)
20 |
21 | # check if we should inherit from a config
22 | inherit_from = cfg_special.get('inherit_from')
23 |
24 | # if yes, load this config first as default
25 | # if no, use the default_path
26 | if inherit_from is not None:
27 | cfg = load_config(inherit_from, default_path)
28 | elif default_path is not None:
29 | with open(default_path, 'r') as f:
30 | cfg = yaml.full_load(f)
31 | else:
32 | cfg = dict()
33 |
34 | # include main configuration
35 | update_recursive(cfg, cfg_special)
36 |
37 | return cfg
38 |
39 |
40 | def update_recursive(dict1, dict2):
41 | """
42 | Update two config dictionaries recursively.
43 |
44 | Args:
45 | dict1 (dict): first dictionary to be updated.
46 | dict2 (dict): second dictionary which entries should be used.
47 | """
48 | for k, v in dict2.items():
49 | if k not in dict1:
50 | dict1[k] = dict()
51 | if isinstance(v, dict):
52 | update_recursive(dict1[k], v)
53 | else:
54 | dict1[k] = v
--------------------------------------------------------------------------------
/src/datasets/dataset.py:
--------------------------------------------------------------------------------
1 |
2 | import glob
3 | import os
4 | import cv2
5 | import torch
6 | import torch.nn.functional as F
7 | import numpy as np
8 | from scipy.spatial.transform import Rotation
9 | from torch.utils.data import Dataset
10 | from src.datasets.dataset_utils import get_camera_rays, alphanum_key, as_intrinsics_matrix
11 |
12 |
13 | def get_dataset(config):
14 | '''
15 | Get the dataset class from the config file.
16 | '''
17 | if config['data']['dataset'] == 'replica':
18 | dataset = ReplicaDataset
19 |
20 | elif config['data']['dataset'] == 'scannet':
21 | dataset = ScannetDataset
22 |
23 | elif config['data']['dataset'] == 'synthetic':
24 | dataset = RGBDataset
25 |
26 | elif config['data']['dataset'] == 'tum':
27 | dataset = TUMDataset
28 |
29 |
30 | return dataset(config,
31 | config['data']['datadir'],
32 | trainskip=1,
33 | downsample_factor=1,
34 | sc_factor=1)
35 |
36 |
37 |
38 | class BaseDataset(Dataset):
39 | def __init__(self, cfg):
40 | self.png_depth_scale = cfg['cam']['png_depth_scale']
41 | self.H, self.W = cfg['cam']['H']//cfg['data']['downsample'],\
42 | cfg['cam']['W']//cfg['data']['downsample']
43 |
44 | self.fx, self.fy = cfg['cam']['fx']//cfg['data']['downsample'],\
45 | cfg['cam']['fy']//cfg['data']['downsample']
46 | self.cx, self.cy = cfg['cam']['cx']//cfg['data']['downsample'],\
47 | cfg['cam']['cy']//cfg['data']['downsample']
48 | self.distortion = np.array(
49 | cfg['cam']['distortion']) if 'distortion' in cfg['cam'] else None
50 | self.crop_size = cfg['cam']['crop_edge'] if 'crop_edge' in cfg['cam'] else 0
51 |
52 |
53 | self.ignore_w = cfg['tracking']['ignore_edge_W']
54 | self.ignore_h = cfg['tracking']['ignore_edge_H']
55 |
56 | self.total_pixels = (self.H - self.crop_size*2) * (self.W - self.crop_size*2)
57 | self.num_rays_to_save = int(self.total_pixels * cfg['mapping']['percent_pixels_save'])
58 | if cfg['rebut']['keyframe']:
59 | self.num_rays_to_save = int(self.total_pixels * 0.005) # down 10 times
60 |
61 |
62 | def __len__(self):
63 | raise NotImplementedError()
64 |
65 | def __getitem__(self, index):
66 | raise NotImplementedError()
67 |
68 |
69 | class ReplicaDataset(BaseDataset):
70 | def __init__(self, cfg, basedir, trainskip=1,
71 | downsample_factor=1, translation=0.0,
72 | sc_factor=1., crop=0):
73 | super(ReplicaDataset, self).__init__(cfg)
74 |
75 | self.basedir = basedir
76 | self.trainskip = trainskip
77 | self.downsample_factor = downsample_factor
78 | self.translation = translation
79 | self.sc_factor = sc_factor
80 | self.crop = crop
81 | self.img_files = sorted(glob.glob(f'{self.basedir}/results/frame*.jpg'))
82 | self.depth_paths = sorted(
83 | glob.glob(f'{self.basedir}/results/depth*.png'))
84 | self.load_poses(os.path.join(self.basedir, 'traj.txt'))
85 |
86 |
87 | self.rays_d = None
88 | self.tracking_mask = None
89 | self.frame_ids = range(0, len(self.img_files))
90 | self.num_frames = len(self.frame_ids)
91 |
92 | def __len__(self):
93 | return self.num_frames
94 |
95 |
96 | def __getitem__(self, index):
97 | color_path = self.img_files[index]
98 | depth_path = self.depth_paths[index]
99 |
100 | color_data = cv2.imread(color_path)
101 | if '.png' in depth_path:
102 | depth_data = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED)
103 | elif '.exr' in depth_path:
104 | raise NotImplementedError()
105 | if self.distortion is not None:
106 | raise NotImplementedError()
107 |
108 | color_data = cv2.cvtColor(color_data, cv2.COLOR_BGR2RGB)
109 | color_data = color_data / 255.
110 | depth_data = depth_data.astype(np.float32) / self.png_depth_scale * self.sc_factor
111 |
112 | H, W = depth_data.shape
113 | color_data = cv2.resize(color_data, (W, H))
114 |
115 | if self.downsample_factor > 1:
116 | H = H // self.downsample_factor
117 | W = W // self.downsample_factor
118 | color_data = cv2.resize(color_data, (W, H), interpolation=cv2.INTER_AREA)
119 | depth_data = cv2.resize(depth_data, (W, H), interpolation=cv2.INTER_NEAREST)
120 |
121 | if self.rays_d is None:
122 | self.rays_d = get_camera_rays(self.H, self.W, self.fx, self.fy, self.cx, self.cy)
123 |
124 | color_data = torch.from_numpy(color_data.astype(np.float32))
125 | depth_data = torch.from_numpy(depth_data.astype(np.float32))
126 |
127 | ret = {
128 | "frame_id": self.frame_ids[index],
129 | "c2w": self.poses[index],
130 | "rgb": color_data,
131 | "depth": depth_data,
132 | "direction": self.rays_d,
133 | }
134 |
135 | return ret
136 |
137 |
138 | def load_poses(self, path):
139 | self.poses = []
140 | with open(path, "r") as f:
141 | lines = f.readlines()
142 | for i in range(len(self.img_files)):
143 | line = lines[i]
144 | c2w = np.array(list(map(float, line.split()))).reshape(4, 4)
145 | c2w[:3, 1] *= -1
146 | c2w[:3, 2] *= -1
147 | c2w[:3, 3] *= self.sc_factor
148 | c2w = torch.from_numpy(c2w).float()
149 | self.poses.append(c2w)
150 |
151 |
152 | class ScannetDataset(BaseDataset):
153 | def __init__(self, cfg, basedir, trainskip=1,
154 | downsample_factor=1, translation=0.0,
155 | sc_factor=1., crop=0):
156 | super(ScannetDataset, self).__init__(cfg)
157 |
158 | self.config = cfg
159 | self.basedir = basedir
160 | self.trainskip = trainskip
161 | self.downsample_factor = downsample_factor
162 | self.translation = translation
163 | self.sc_factor = sc_factor
164 | self.crop = crop
165 | self.img_files = sorted(glob.glob(os.path.join(
166 | self.basedir, 'color', '*.jpg')), key=lambda x: int(os.path.basename(x)[:-4]))
167 | self.depth_paths = sorted(
168 | glob.glob(os.path.join(
169 | self.basedir, 'depth', '*.png')), key=lambda x: int(os.path.basename(x)[:-4]))
170 | self.load_poses(os.path.join(self.basedir, 'pose'))
171 |
172 | # self.depth_cleaner = cv2.rgbd.DepthCleaner_create(cv2.CV_32F, 5)
173 |
174 |
175 | self.rays_d = None
176 | self.frame_ids = range(0, len(self.img_files))
177 | self.num_frames = len(self.frame_ids)
178 |
179 | if self.config['cam']['crop_edge'] > 0:
180 | self.H -= self.config['cam']['crop_edge']*2
181 | self.W -= self.config['cam']['crop_edge']*2
182 | self.cx -= self.config['cam']['crop_edge']
183 | self.cy -= self.config['cam']['crop_edge']
184 |
185 | def __len__(self):
186 | return self.num_frames
187 |
188 | def __getitem__(self, index):
189 | color_path = self.img_files[index]
190 | depth_path = self.depth_paths[index]
191 |
192 | color_data = cv2.imread(color_path)
193 | if '.png' in depth_path:
194 | depth_data = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED)
195 | elif '.exr' in depth_path:
196 | raise NotImplementedError()
197 | if self.distortion is not None:
198 | raise NotImplementedError()
199 |
200 | color_data = cv2.cvtColor(color_data, cv2.COLOR_BGR2RGB)
201 | color_data = color_data / 255.
202 | depth_data = depth_data.astype(np.float32) / self.png_depth_scale * self.sc_factor
203 |
204 | H, W = depth_data.shape
205 | color_data = cv2.resize(color_data, (W, H))
206 |
207 | if self.downsample_factor > 1:
208 | H = H // self.downsample_factor
209 | W = W // self.downsample_factor
210 | self.fx = self.fx // self.downsample_factor
211 | self.fy = self.fy // self.downsample_factor
212 | color_data = cv2.resize(color_data, (W, H), interpolation=cv2.INTER_AREA)
213 | depth_data = cv2.resize(depth_data, (W, H), interpolation=cv2.INTER_NEAREST)
214 |
215 | edge = self.config['cam']['crop_edge']
216 | if edge > 0:
217 | # crop image edge, there are invalid value on the edge of the color image
218 | color_data = color_data[edge:-edge, edge:-edge]
219 | depth_data = depth_data[edge:-edge, edge:-edge]
220 |
221 | if self.rays_d is None:
222 | self.rays_d = get_camera_rays(self.H, self.W, self.fx, self.fy, self.cx, self.cy)
223 |
224 | color_data = torch.from_numpy(color_data.astype(np.float32))
225 | depth_data = torch.from_numpy(depth_data.astype(np.float32))
226 |
227 | ret = {
228 | "frame_id": self.frame_ids[index],
229 | "c2w": self.poses[index],
230 | "rgb": color_data,
231 | "depth": depth_data,
232 | "direction": self.rays_d
233 | }
234 |
235 | return ret
236 |
237 | def load_poses(self, path):
238 | self.poses = []
239 | pose_paths = sorted(glob.glob(os.path.join(path, '*.txt')), #
240 | key=lambda x: int(os.path.basename(x)[:-4]))
241 | for pose_path in pose_paths:
242 | with open(pose_path, "r") as f:
243 | lines = f.readlines()
244 | ls = []
245 | for line in lines:
246 | l = list(map(float, line.split(' ')))
247 | ls.append(l)
248 | c2w = np.array(ls).reshape(4, 4)
249 | c2w[:3, 1] *= -1
250 | c2w[:3, 2] *= -1
251 | c2w = torch.from_numpy(c2w).float()
252 | self.poses.append(c2w)
253 |
254 | class RGBDataset(BaseDataset):
255 | def __init__(self, cfg, basedir, trainskip=1,
256 | downsample_factor=1, translation=0.0,
257 | sc_factor=1., crop=0):
258 | super(RGBDataset, self).__init__(cfg)
259 |
260 | self.basedir = basedir
261 | self.trainskip = trainskip
262 | self.downsample_factor = downsample_factor
263 | self.translation = translation
264 | self.sc_factor = sc_factor
265 | self.crop = crop
266 | self.img_files = [os.path.join(self.basedir, 'images', f) for f in sorted(os.listdir(os.path.join(self.basedir, 'images')), key=alphanum_key) if f.endswith('png')]
267 | self.depth_paths = [os.path.join(self.basedir, 'depth_filtered', f) for f in sorted(os.listdir(os.path.join(self.basedir, 'depth_filtered')), key=alphanum_key) if f.endswith('png')]
268 | self.gt_depth_paths = [os.path.join(self.basedir, 'depth', f) for f in sorted(os.listdir(os.path.join(basedir, 'depth')), key=alphanum_key) if f.endswith('png')]
269 |
270 |
271 | self.all_poses, valid_poses = self.load_poses(os.path.join(self.basedir, 'trainval_poses.txt'))
272 | self.all_gt_poses, valid_gt_poses = self.load_poses(os.path.join(basedir, 'poses.txt'))
273 |
274 | init_pose = np.array(self.all_poses[0]).astype(np.float32)
275 | init_gt_pose = np.array(self.all_gt_poses[0]).astype(np.float32)
276 | self.align_matrix = init_gt_pose @ np.linalg.inv(init_pose)
277 |
278 | self.poses = []
279 | for pose in self.all_gt_poses:
280 | self.poses.append(torch.from_numpy(np.array(pose)).float())
281 |
282 |
283 | self.rays_d = None
284 | self.frame_ids = self.get_frame_ids()
285 | self.num_frames = len(self.frame_ids)
286 |
287 | def get_frame_ids(self):
288 | frame_ids = []
289 | num_frames = len(self.img_files)
290 | train_frame_ids = list(range(0, num_frames, self.trainskip))
291 |
292 | self.frame_ids = []
293 | for id in train_frame_ids:
294 | #if valid_poses[id]:
295 | frame_ids.append(id)
296 |
297 | return frame_ids
298 |
299 | def __len__(self):
300 | return self.num_frames
301 |
302 | def __getitem__(self, index):
303 | index = self.frame_ids[index]
304 | color_path = self.img_files[index]
305 | depth_path = self.depth_paths[index]
306 |
307 | color_data = cv2.imread(color_path)
308 | if '.png' in depth_path:
309 | depth_data = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED)
310 | elif '.exr' in depth_path:
311 | raise NotImplementedError()
312 | if self.distortion is not None:
313 | raise NotImplementedError()
314 |
315 | color_data = cv2.cvtColor(color_data, cv2.COLOR_BGR2RGB)
316 | color_data = color_data / 255.
317 | depth_data = depth_data.astype(np.float32) / self.png_depth_scale * self.sc_factor
318 |
319 | H, W = depth_data.shape
320 | color_data = cv2.resize(color_data, (W, H))
321 |
322 | if self.downsample_factor > 1:
323 | H = H // self.downsample_factor
324 | W = W // self.downsample_factor
325 | self.fx = self.fx // self.downsample_factor
326 | self.fy = self.fy // self.downsample_factor
327 | color_data = cv2.resize(color_data, (W, H), interpolation=cv2.INTER_AREA)
328 | depth_data = cv2.resize(depth_data, (W, H), interpolation=cv2.INTER_NEAREST)
329 |
330 | if self.rays_d is None:
331 | self.rays_d = get_camera_rays(H, W, self.fx, self.fy, self.cx, self.cy)
332 |
333 | color_data = torch.from_numpy(color_data.astype(np.float32))
334 | depth_data = torch.from_numpy(depth_data.astype(np.float32))
335 |
336 | ret = {
337 | "frame_id": index,
338 | "c2w": self.poses[index],
339 | "rgb": color_data,
340 | "depth": depth_data,
341 | "direction": self.rays_d
342 | }
343 |
344 | return ret
345 |
346 | def load_poses(self, path):
347 | file = open(path, "r")
348 | lines = file.readlines()
349 | file.close()
350 | poses = []
351 | valid = []
352 | lines_per_matrix = 4
353 | for i in range(0, len(lines), lines_per_matrix):
354 | if 'nan' in lines[i]:
355 | valid.append(False)
356 | poses.append(np.eye(4, 4, dtype=np.float32).tolist())
357 | else:
358 | valid.append(True)
359 | pose_floats = [[float(x) for x in line.split()] for line in lines[i:i+lines_per_matrix]]
360 | poses.append(pose_floats)
361 |
362 | return poses, valid
363 |
364 |
365 | class TUMDataset(BaseDataset):
366 | def __init__(self, cfg, basedir, align=True, trainskip=1,
367 | downsample_factor=1, translation=0.0,
368 | sc_factor=1., crop=0, load=True):
369 | super(TUMDataset, self).__init__(cfg)
370 |
371 | self.config = cfg
372 | self.basedir = basedir
373 | self.trainskip = trainskip
374 | self.downsample_factor = downsample_factor
375 | self.translation = translation
376 | self.sc_factor = sc_factor
377 | self.crop = crop
378 |
379 | self.color_paths, self.depth_paths, self.poses = self.loadtum(
380 | basedir, frame_rate=32)
381 |
382 | self.frame_ids = range(0, len(self.color_paths))
383 | self.num_frames = len(self.frame_ids)
384 |
385 | self.crop_size = cfg['cam']['crop_size'] if 'crop_size' in cfg['cam'] else None
386 |
387 | self.rays_d = None
388 | sx = self.crop_size[1] / self.W
389 | sy = self.crop_size[0] / self.H
390 | self.fx = sx*self.fx
391 | self.fy = sy*self.fy
392 | self.cx = sx*self.cx
393 | self.cy = sy*self.cy
394 | self.W = self.crop_size[1]
395 | self.H = self.crop_size[0]
396 |
397 | if self.config['cam']['crop_edge'] > 0:
398 | self.H -= self.config['cam']['crop_edge']*2
399 | self.W -= self.config['cam']['crop_edge']*2
400 | self.cx -= self.config['cam']['crop_edge']
401 | self.cy -= self.config['cam']['crop_edge']
402 |
403 | def pose_matrix_from_quaternion(self, pvec):
404 | """ convert 4x4 pose matrix to (t, q) """
405 | from scipy.spatial.transform import Rotation
406 |
407 | pose = np.eye(4)
408 | pose[:3, :3] = Rotation.from_quat(pvec[3:]).as_matrix()
409 | pose[:3, 3] = pvec[:3]
410 | return pose
411 |
412 | def associate_frames(self, tstamp_image, tstamp_depth, tstamp_pose, max_dt=0.08):
413 | """ pair images, depths, and poses """
414 | associations = []
415 | for i, t in enumerate(tstamp_image):
416 | if tstamp_pose is None:
417 | j = np.argmin(np.abs(tstamp_depth - t))
418 | if (np.abs(tstamp_depth[j] - t) < max_dt):
419 | associations.append((i, j))
420 |
421 | else:
422 | j = np.argmin(np.abs(tstamp_depth - t))
423 | k = np.argmin(np.abs(tstamp_pose - t))
424 |
425 | if (np.abs(tstamp_depth[j] - t) < max_dt) and \
426 | (np.abs(tstamp_pose[k] - t) < max_dt):
427 | associations.append((i, j, k))
428 |
429 | return associations
430 |
431 | def parse_list(self, filepath, skiprows=0):
432 | """ read list data """
433 | data = np.loadtxt(filepath, delimiter=' ',
434 | dtype=np.unicode_, skiprows=skiprows)
435 | return data
436 |
437 | def loadtum(self, datapath, frame_rate=-1):
438 | """ read video data in tum-rgbd format """
439 | if os.path.isfile(os.path.join(datapath, 'groundtruth.txt')):
440 | pose_list = os.path.join(datapath, 'groundtruth.txt')
441 | elif os.path.isfile(os.path.join(datapath, 'pose.txt')):
442 | pose_list = os.path.join(datapath, 'pose.txt')
443 |
444 | image_list = os.path.join(datapath, 'rgb.txt')
445 | depth_list = os.path.join(datapath, 'depth.txt')
446 |
447 | image_data = self.parse_list(image_list)
448 | depth_data = self.parse_list(depth_list)
449 | pose_data = self.parse_list(pose_list, skiprows=1)
450 | pose_vecs = pose_data[:, 1:].astype(np.float64)
451 |
452 | tstamp_image = image_data[:, 0].astype(np.float64)
453 | tstamp_depth = depth_data[:, 0].astype(np.float64)
454 | tstamp_pose = pose_data[:, 0].astype(np.float64)
455 | associations = self.associate_frames(
456 | tstamp_image, tstamp_depth, tstamp_pose)
457 |
458 | indicies = [0]
459 | for i in range(1, len(associations)):
460 | t0 = tstamp_image[associations[indicies[-1]][0]]
461 | t1 = tstamp_image[associations[i][0]]
462 | if t1 - t0 > 1.0 / frame_rate:
463 | indicies += [i]
464 |
465 | images, poses, depths, intrinsics = [], [], [], []
466 | inv_pose = None
467 | for ix in indicies:
468 | (i, j, k) = associations[ix]
469 | images += [os.path.join(datapath, image_data[i, 1])]
470 | depths += [os.path.join(datapath, depth_data[j, 1])]
471 | c2w = self.pose_matrix_from_quaternion(pose_vecs[k])
472 | # if inv_pose is None:
473 | # inv_pose = np.linalg.inv(c2w)
474 | # c2w = np.eye(4)
475 | # else:
476 | # c2w = inv_pose@c2w
477 | c2w[:3, 1] *= -1
478 | c2w[:3, 2] *= -1
479 | c2w = torch.from_numpy(c2w).float()
480 | poses += [c2w]
481 |
482 | return images, depths, poses
483 |
484 | def __len__(self):
485 | return self.num_frames
486 |
487 | def __getitem__(self, index):
488 | color_path = self.color_paths[index]
489 | depth_path = self.depth_paths[index]
490 |
491 | color_data = cv2.imread(color_path)
492 | if '.png' in depth_path:
493 | depth_data = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED)
494 | elif '.exr' in depth_path:
495 | raise NotImplementedError()
496 | if self.distortion is not None:
497 | K = as_intrinsics_matrix([self.config['cam']['fx'],
498 | self.config['cam']['fy'],
499 | self.config['cam']['cx'],
500 | self.config['cam']['cy']])
501 | color_data = cv2.undistort(color_data, K, self.distortion)
502 |
503 | color_data = cv2.cvtColor(color_data, cv2.COLOR_BGR2RGB)
504 | color_data = color_data / 255.
505 | depth_data = depth_data.astype(np.float32) / self.png_depth_scale * self.sc_factor
506 |
507 | H, W = depth_data.shape
508 | color_data = cv2.resize(color_data, (W, H))
509 |
510 | if self.downsample_factor > 1:
511 | H = H // self.downsample_factor
512 | W = W // self.downsample_factor
513 | self.fx = self.fx // self.downsample_factor
514 | self.fy = self.fy // self.downsample_factor
515 | color_data = cv2.resize(color_data, (W, H), interpolation=cv2.INTER_AREA)
516 | depth_data = cv2.resize(depth_data, (W, H), interpolation=cv2.INTER_NEAREST)
517 |
518 | if self.rays_d is None:
519 | self.rays_d =get_camera_rays(self.H, self.W, self.fx, self.fy, self.cx, self.cy)
520 |
521 |
522 | color_data = torch.from_numpy(color_data.astype(np.float32))
523 | depth_data = torch.from_numpy(depth_data.astype(np.float32))
524 |
525 | if self.crop_size is not None:
526 | # follow the pre-processing step in lietorch, actually is resize
527 | color_data = color_data.permute(2, 0, 1)
528 | color_data = F.interpolate(
529 | color_data[None], self.crop_size, mode='bilinear', align_corners=True)[0]
530 | depth_data = F.interpolate(
531 | depth_data[None, None], self.crop_size, mode='nearest')[0, 0]
532 | color_data = color_data.permute(1, 2, 0).contiguous()
533 |
534 | edge = self.config['cam']['crop_edge']
535 | if edge > 0:
536 | # crop image edge, there are invalid value on the edge of the color image
537 | color_data = color_data[edge:-edge, edge:-edge]
538 | depth_data = depth_data[edge:-edge, edge:-edge]
539 |
540 |
541 |
542 | ret = {
543 | "frame_id": self.frame_ids[index],
544 | "c2w": self.poses[index],
545 | "rgb": color_data,
546 | "depth": depth_data,
547 | "direction": self.rays_d
548 | }
549 | return ret
--------------------------------------------------------------------------------
/src/datasets/dataset_utils.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import re
3 | import numpy as np
4 |
5 |
6 | def as_intrinsics_matrix(intrinsics):
7 | """
8 | Get matrix representation of intrinsics.
9 |
10 | """
11 | K = np.eye(3)
12 | K[0, 0] = intrinsics[0]
13 | K[1, 1] = intrinsics[1]
14 | K[0, 2] = intrinsics[2]
15 | K[1, 2] = intrinsics[3]
16 | return K
17 |
18 | def alphanum_key(s):
19 | """ Turn a string into a list of string and number chunks.
20 | "z23a" -> ["z", 23, "a"]
21 | """
22 | return [int(x) if x.isdigit() else x for x in re.split('([0-9]+)', s)]
23 |
24 | def get_camera_rays(H, W, fx, fy=None, cx=None, cy=None, type='OpenGL'):
25 | """Get ray origins, directions from a pinhole camera."""
26 | # ----> i
27 | # |
28 | # |
29 | # X
30 | # j
31 | i, j = torch.meshgrid(torch.arange(W, dtype=torch.float32),
32 | torch.arange(H, dtype=torch.float32), indexing='xy')
33 |
34 | # View direction (X, Y, Lambda) / lambda
35 | # Move to the center of the screen
36 | # -------------
37 | # | y |
38 | # | | |
39 | # | .-- x |
40 | # | |
41 | # | |
42 | # -------------
43 |
44 | if cx is None:
45 | cx, cy = 0.5 * W, 0.5 * H
46 |
47 | if fy is None:
48 | fy = fx
49 | if type is 'OpenGL':
50 | dirs = torch.stack([(i - cx)/fx, -(j - cy)/fy, -torch.ones_like(i)], -1)
51 | elif type is 'OpenCV':
52 | dirs = torch.stack([(i - cx)/fx, (j - cy)/fy, torch.ones_like(i)], -1)
53 | else:
54 | raise NotImplementedError()
55 |
56 | rays_d = dirs
57 | return rays_d
--------------------------------------------------------------------------------
/src/tools/pose_eval.py:
--------------------------------------------------------------------------------
1 | # NICE-SLAM evaluation methods
2 | import argparse
3 | import os
4 | import numpy
5 | import torch
6 | import sys
7 | import numpy as np
8 | sys.path.append('.')
9 |
10 | def get_tensor_from_camera(RT, Tquad=False):
11 | """
12 | Convert transformation matrix to quaternion and translation.
13 |
14 | """
15 | gpu_id = -1
16 | if type(RT) == torch.Tensor:
17 | if RT.get_device() != -1:
18 | RT = RT.detach().cpu()
19 | gpu_id = RT.get_device()
20 | RT = RT.numpy()
21 |
22 | from mathutils import Matrix
23 | R, T = RT[:3, :3], RT[:3, 3]
24 | rot = Matrix(R)
25 | quad = rot.to_quaternion()
26 | if Tquad:
27 | tensor = np.concatenate([T, quad], 0)
28 | else:
29 | tensor = np.concatenate([quad, T], 0)
30 | tensor = torch.from_numpy(tensor).float()
31 | if gpu_id != -1:
32 | tensor = tensor.to(gpu_id)
33 | return tensor
34 |
35 | def associate(first_list, second_list, offset=0.0, max_difference=0.02):
36 | """
37 | Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim
38 | to find the closest match for every input tuple.
39 |
40 | Input:
41 | first_list -- first dictionary of (stamp,data) tuples
42 | second_list -- second dictionary of (stamp,data) tuples
43 | offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
44 | max_difference -- search radius for candidate generation
45 |
46 | Output:
47 | matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
48 |
49 | """
50 | first_keys = list(first_list.keys())
51 | second_keys = list(second_list.keys())
52 | potential_matches = [(abs(a - (b + offset)), a, b)
53 | for a in first_keys
54 | for b in second_keys
55 | if abs(a - (b + offset)) < max_difference]
56 | potential_matches.sort()
57 | matches = []
58 | for diff, a, b in potential_matches:
59 | if a in first_keys and b in second_keys:
60 | first_keys.remove(a)
61 | second_keys.remove(b)
62 | matches.append((a, b))
63 |
64 | matches.sort()
65 | return matches
66 |
67 | def align(model, data):
68 | """Align two trajectories using the method of Horn (closed-form).
69 |
70 | Input:
71 | model -- first trajectory (3xn)
72 | data -- second trajectory (3xn)
73 |
74 | Output:
75 | rot -- rotation matrix (3x3)
76 | trans -- translation vector (3x1)
77 | trans_error -- translational error per point (1xn)
78 |
79 | """
80 | numpy.set_printoptions(precision=3, suppress=True)
81 | model_zerocentered = model - model.mean(1)
82 | data_zerocentered = data - data.mean(1)
83 |
84 | W = numpy.zeros((3, 3))
85 | for column in range(model.shape[1]):
86 | W += numpy.outer(model_zerocentered[:,
87 | column], data_zerocentered[:, column])
88 | U, d, Vh = numpy.linalg.linalg.svd(W.transpose())
89 | S = numpy.matrix(numpy.identity(3))
90 | if(numpy.linalg.det(U) * numpy.linalg.det(Vh) < 0):
91 | S[2, 2] = -1
92 | rot = U*S*Vh
93 | trans = data.mean(1) - rot * model.mean(1)
94 |
95 | model_aligned = rot * model + trans
96 | alignment_error = model_aligned - data
97 |
98 | trans_error = numpy.sqrt(numpy.sum(numpy.multiply(
99 | alignment_error, alignment_error), 0)).A[0]
100 |
101 | return rot, trans, trans_error
102 |
103 | def plot_traj(ax, stamps, traj, style, color, label):
104 | """
105 | Plot a trajectory using matplotlib.
106 |
107 | Input:
108 | ax -- the plot
109 | stamps -- time stamps (1xn)
110 | traj -- trajectory (3xn)
111 | style -- line style
112 | color -- line color
113 | label -- plot legend
114 |
115 | """
116 | stamps.sort()
117 | interval = numpy.median([s-t for s, t in zip(stamps[1:], stamps[:-1])])
118 | x = []
119 | y = []
120 | last = stamps[0]
121 | for i in range(len(stamps)):
122 | if stamps[i]-last < 2*interval:
123 | x.append(traj[i][0])
124 | y.append(traj[i][1])
125 | elif len(x) > 0:
126 | ax.plot(x, y, style, color=color, label=label)
127 | label = ""
128 | x = []
129 | y = []
130 | last = stamps[i]
131 | if len(x) > 0:
132 | ax.plot(x, y, style, color=color, label=label)
133 |
134 | def evaluate_ate(first_list, second_list, plot="", _args="",frame_id=0,final_flag = False):
135 | # parse command line
136 | parser = argparse.ArgumentParser(
137 | description='This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.')
138 | # parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
139 | # parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
140 | parser.add_argument(
141 | '--offset', help='time offset added to the timestamps of the second file (default: 0.0)', default=0.0)
142 | parser.add_argument(
143 | '--scale', help='scaling factor for the second trajectory (default: 1.0)', default=1.0)
144 | parser.add_argument(
145 | '--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)', default=0.02)
146 | parser.add_argument(
147 | '--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
148 | parser.add_argument('--save_associations',
149 | help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
150 | parser.add_argument(
151 | '--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
152 | parser.add_argument(
153 | '--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true')
154 | args = parser.parse_args(_args)
155 | args.plot = plot
156 | # first_list = associate.read_file_list(args.first_file)
157 | # second_list = associate.read_file_list(args.second_file)
158 |
159 | matches = associate(first_list, second_list, float(
160 | args.offset), float(args.max_difference))
161 | if len(matches) < 2 and len(first_list) > 5:
162 | raise ValueError(
163 | "Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! \
164 | Did you choose the correct sequence?")
165 |
166 | first_xyz = numpy.matrix(
167 | [[float(value) for value in first_list[a][0:3]] for a, b in matches]).transpose()
168 | second_xyz = numpy.matrix([[float(value)*float(args.scale)
169 | for value in second_list[b][0:3]] for a, b in matches]).transpose()
170 |
171 | rot, trans, trans_error = align(second_xyz, first_xyz)
172 |
173 | second_xyz_aligned = rot * second_xyz + trans
174 |
175 | first_stamps = list(first_list.keys())
176 | first_stamps.sort()
177 | first_xyz_full = numpy.matrix(
178 | [[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
179 |
180 | second_stamps = list(second_list.keys())
181 | second_stamps.sort()
182 | second_xyz_full = numpy.matrix([[float(value)*float(args.scale)
183 | for value in second_list[b][0:3]] for b in second_stamps]).transpose()
184 | second_xyz_full_aligned = rot * second_xyz_full + trans
185 |
186 | if args.verbose:
187 | print("compared_pose_pairs %d pairs" % (len(trans_error)))
188 |
189 | print("absolute_translational_error.rmse %f m" % numpy.sqrt(
190 | numpy.dot(trans_error, trans_error) / len(trans_error)))
191 | print("absolute_translational_error.mean %f m" %
192 | numpy.mean(trans_error))
193 | print("absolute_translational_error.median %f m" %
194 | numpy.median(trans_error))
195 | print("absolute_translational_error.std %f m" % numpy.std(trans_error))
196 | print("absolute_translational_error.min %f m" % numpy.min(trans_error))
197 | print("absolute_translational_error.max %f m" % numpy.max(trans_error))
198 |
199 | if args.save_associations:
200 | file = open(args.save_associations, "w")
201 | file.write("\n".join(["%f %f %f %f %f %f %f %f" % (a, x1, y1, z1, b, x2, y2, z2) for (
202 | a, b), (x1, y1, z1), (x2, y2, z2) in zip(matches, first_xyz.transpose().A, second_xyz_aligned.transpose().A)]))
203 | file.close()
204 |
205 | if args.save:
206 | file = open(args.save, "w")
207 | file.write("\n".join(["%f " % stamp+" ".join(["%f" % d for d in line])
208 | for stamp, line in zip(second_stamps, second_xyz_full_aligned.transpose().A)]))
209 | file.close()
210 |
211 | if (args.plot and frame_id%500==0 and frame_id!=0) or (final_flag == True):
212 | import matplotlib
213 | matplotlib.use('Agg')
214 | import matplotlib.pylab as pylab
215 | import matplotlib.pyplot as plt
216 | from matplotlib.patches import Ellipse
217 | fig = plt.figure()
218 | ax = fig.add_subplot(111)
219 | ATE = numpy.sqrt(
220 | numpy.dot(trans_error, trans_error) / len(trans_error))
221 | png_name = os.path.basename(args.plot)
222 | ax.set_title(f'len:{len(trans_error)} ATE RMSE:{ATE} {png_name[:-3]}')
223 | plot_traj(ax, first_stamps, first_xyz_full.transpose().A,
224 | '-', "black", "ground truth")
225 | plot_traj(ax, second_stamps, second_xyz_full_aligned.transpose(
226 | ).A, '-', "blue", "estimated")
227 |
228 | label = "difference"
229 | for (a, b), (x1, y1, z1), (x2, y2, z2) in zip(matches, first_xyz.transpose().A, second_xyz_aligned.transpose().A):
230 | # ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
231 | label = ""
232 | ax.legend()
233 | ax.set_xlabel('x [m]')
234 | ax.set_ylabel('y [m]')
235 | plt.savefig(args.plot, dpi=90)
236 |
237 | return {
238 | "compared_pose_pairs": (len(trans_error)),
239 | "absolute_translational_error.rmse": numpy.sqrt(numpy.dot(trans_error, trans_error) / len(trans_error)),
240 | "absolute_translational_error.mean": numpy.mean(trans_error),
241 | "absolute_translational_error.median": numpy.median(trans_error),
242 | "absolute_translational_error.std": numpy.std(trans_error),
243 | "absolute_translational_error.min": numpy.min(trans_error),
244 | "absolute_translational_error.max": numpy.max(trans_error),
245 | }
246 |
247 | def evaluate(poses_gt, poses_est, plot, frame_id=0,final_flag = False):
248 |
249 | poses_gt = poses_gt.cpu().numpy()
250 | poses_est = poses_est.cpu().numpy()
251 |
252 | N = poses_gt.shape[0]
253 | poses_gt = dict([(i, poses_gt[i]) for i in range(N)])
254 | poses_est = dict([(i, poses_est[i]) for i in range(N)])
255 |
256 | results = evaluate_ate(poses_gt, poses_est, plot, frame_id=frame_id,final_flag = final_flag)
257 | return results
258 |
259 | def convert_poses(c2w_list, N, scale, gt=True):
260 | poses = []
261 | mask = torch.ones(N).bool()
262 | for idx in range(0, N):
263 | if gt:
264 | # some frame have `nan` or `inf` in gt pose of ScanNet,
265 | # but our system have estimated camera pose for all frames
266 | # therefore, when calculating the pose error, we need to mask out invalid pose
267 | if torch.isinf(c2w_list[idx]).any():
268 | mask[idx] = 0
269 | continue
270 | if torch.isnan(c2w_list[idx]).any():
271 | mask[idx] = 0
272 | continue
273 | c2w_list[idx][:3, 3] /= scale
274 | poses.append(get_tensor_from_camera(c2w_list[idx], Tquad=True))
275 | poses = torch.stack(poses)
276 | return poses, mask
277 |
278 | def pose_evaluation(poses_gt, poses_est, scale, path_to_save, i, final_flag = False,img='pose', name='output.txt'):
279 | N = len(poses_est)
280 | poses_gt, mask = convert_poses(poses_gt, N, scale)
281 | poses_est, _ = convert_poses(poses_est, N, scale)
282 | poses_est = poses_est[mask]
283 | plt_path = os.path.join(path_to_save, '{}_{}.png'.format(img, i))
284 |
285 | results = evaluate(poses_gt, poses_est, plot=plt_path, frame_id=i, final_flag = final_flag)
286 | results['Name'] = i
287 | print(results, file=open(os.path.join(path_to_save, name), "a"))
288 | return results
--------------------------------------------------------------------------------
/src/utils/common_utils.py:
--------------------------------------------------------------------------------
1 |
2 |
3 | import torch
4 |
5 | def mse2psnr(x):
6 | '''
7 | MSE to PSNR
8 | '''
9 | return -10. * torch.log(x) / torch.log(torch.Tensor([10.])).to(x)
10 |
11 | def normalize_3d_coordinate(p, bound):
12 | """
13 | Normalize 3d coordinate to [-1, 1] range.
14 | Args:
15 | p: (N, 3) 3d coordinate
16 | bound: (3, 2) min and max of each dimension
17 | Returns:
18 | (N, 3) normalized 3d coordinate
19 |
20 | """
21 | p = p.reshape(-1, 3)
22 | p[:, 0] = ((p[:, 0]-bound[0, 0])/(bound[0, 1]-bound[0, 0]))*2-1.0
23 | p[:, 1] = ((p[:, 1]-bound[1, 0])/(bound[1, 1]-bound[1, 0]))*2-1.0
24 | p[:, 2] = ((p[:, 2]-bound[2, 0])/(bound[2, 1]-bound[2, 0]))*2-1.0
25 | return p
26 |
27 | def intersect_with_sphere(center, ray_unit, radius=1.0):
28 | ctc = (center * center).sum(dim=-1, keepdim=True) # [...,1]
29 | ctv = (center * ray_unit).sum(dim=-1, keepdim=True) # [...,1]
30 | b2_minus_4ac = ctv ** 2 - (ctc - radius ** 2)
31 | dist_near = -ctv - b2_minus_4ac.sqrt()
32 | dist_far = -ctv + b2_minus_4ac.sqrt()
33 | return dist_near, dist_far
--------------------------------------------------------------------------------
/src/utils/mesh_utils_eslam.py:
--------------------------------------------------------------------------------
1 | # This file is a modified version of https://github.com/idiap/ESLAM/blob/main/src/utils/Mesher.py
2 | # which is covered by the following copyright and permission notice:
3 | # This file is a part of ESLAM.
4 | #
5 | # ESLAM is a NeRF-based SLAM system. It utilizes Neural Radiance Fields (NeRF)
6 | # to perform Simultaneous Localization and Mapping (SLAM) in real-time.
7 | # This software is the implementation of the paper "ESLAM: Efficient Dense SLAM
8 | # System Based on Hybrid Representation of Signed Distance Fields" by
9 | # Mohammad Mahdi Johari, Camilla Carta, and Francois Fleuret.
10 | #
11 | # Copyright 2023 ams-OSRAM AG
12 | #
13 | # Author: Mohammad Mahdi Johari
14 | #
15 | # Licensed under the Apache License, Version 2.0 (the "License");
16 | # you may not use this file except in compliance with the License.
17 | # You may obtain a copy of the License at
18 | #
19 | # http://www.apache.org/licenses/LICENSE-2.0
20 | #
21 | # Unless required by applicable law or agreed to in writing, software
22 | # distributed under the License is distributed on an "AS IS" BASIS,
23 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
24 | # See the License for the specific language governing permissions and
25 | # limitations under the License.
26 |
27 | import numpy as np
28 | import open3d as o3d
29 | import skimage
30 | import torch
31 | import trimesh
32 | from packaging import version
33 |
34 | class Mesher(object):
35 | """
36 | Mesher class.
37 | """
38 | def __init__(self, config, bound, marching_cube_bound, dataset, NeRF, points_batch_size=500000, ray_batch_size=100000):
39 | self.points_batch_size = points_batch_size
40 | self.ray_batch_size = ray_batch_size
41 | self.marching_cubes_bound = marching_cube_bound
42 | self.bound = bound
43 | self.scale = 1
44 | self.level_set = 0
45 |
46 | self.resolution = config['utils']['mesh_resolution']
47 | self.mesh_bound_scale = config['utils']['mesh_bound_scale']
48 | self.verbose = config['verbose']
49 |
50 | self.H, self.W, self.fx, self.fy, self.cx, self.cy = dataset.H, dataset.W, dataset.fx, dataset.fy, dataset.cx, dataset.cy
51 |
52 | def get_bound_from_frames(self, keyframe_dict, scale=1):
53 | """
54 | Get the scene bound (convex hull),
55 | using sparse estimated camera poses and corresponding depth images.
56 |
57 | Args:
58 | keyframe_dict (list): list of keyframe info dictionary.
59 | scale (float): scene scale.
60 |
61 | Returns:
62 | return_mesh (trimesh.Trimesh): the convex hull.
63 | """
64 |
65 | H, W, fx, fy, cx, cy = self.H, self.W, self.fx, self.fy, self.cx, self.cy
66 |
67 | if version.parse(o3d.__version__) >= version.parse('0.13.0'):
68 | # for new version as provided in environment.yaml
69 | volume = o3d.pipelines.integration.ScalableTSDFVolume(
70 | voxel_length=4.0 * scale / 512.0,
71 | sdf_trunc=0.04 * scale,
72 | color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8)
73 | else:
74 | # for lower version
75 | volume = o3d.integration.ScalableTSDFVolume(
76 | voxel_length=4.0 * scale / 512.0,
77 | sdf_trunc=0.04 * scale,
78 | color_type=o3d.integration.TSDFVolumeColorType.RGB8)
79 | cam_points = []
80 | for keyframe in keyframe_dict:
81 | c2w = keyframe['est_c2w'].cpu().numpy()
82 | # convert to open3d camera pose
83 | c2w[:3, 1] *= -1.0
84 | c2w[:3, 2] *= -1.0
85 | w2c = np.linalg.inv(c2w)
86 | cam_points.append(c2w[:3, 3])
87 | depth = keyframe['depth'].cpu().numpy()
88 | color = keyframe['color'].cpu().numpy()
89 |
90 | depth = o3d.geometry.Image(depth.astype(np.float32))
91 | color = o3d.geometry.Image(np.array(
92 | (color * 255).astype(np.uint8)))
93 |
94 | intrinsic = o3d.camera.PinholeCameraIntrinsic(W, H, fx, fy, cx, cy)
95 | rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
96 | color,
97 | depth,
98 | depth_scale=1,
99 | depth_trunc=1000,
100 | convert_rgb_to_intensity=False)
101 | volume.integrate(rgbd, intrinsic, w2c)
102 |
103 | cam_points = np.stack(cam_points, axis=0)
104 | mesh = volume.extract_triangle_mesh()
105 | mesh_points = np.array(mesh.vertices)
106 | points = np.concatenate([cam_points, mesh_points], axis=0)
107 | o3d_pc = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(points))
108 | mesh, _ = o3d_pc.compute_convex_hull()
109 | mesh.compute_vertex_normals()
110 | if version.parse(o3d.__version__) >= version.parse('0.13.0'):
111 | mesh = mesh.scale(self.mesh_bound_scale, mesh.get_center())
112 | else:
113 | mesh = mesh.scale(self.mesh_bound_scale, center=True)
114 | points = np.array(mesh.vertices)
115 | faces = np.array(mesh.triangles)
116 | return_mesh = trimesh.Trimesh(vertices=points, faces=faces)
117 | return return_mesh
118 |
119 | def eval_points(self, p, NeRF):
120 | """
121 | Evaluates the TSDF and/or color value for the points.
122 | Args:
123 | p (torch.Tensor): points to be evaluated, shape (N, 3).
124 | all_planes (Tuple): all feature planes.
125 | decoders (torch.nn.Module): decoders for TSDF and color.
126 | Returns:
127 | ret (torch.Tensor): the evaluation result, shape (N, 4).
128 | """
129 |
130 | p_split = torch.split(p, self.points_batch_size)
131 | bound = self.bound
132 | rets = []
133 | for pi in p_split:
134 | # mask for points out of bound
135 | mask_x = (pi[:, 0] < bound[0][1]) & (pi[:, 0] > bound[0][0])
136 | mask_y = (pi[:, 1] < bound[1][1]) & (pi[:, 1] > bound[1][0])
137 | mask_z = (pi[:, 2] < bound[2][1]) & (pi[:, 2] > bound[2][0])
138 | mask = mask_x & mask_y & mask_z
139 |
140 | ret = decoders(pi, all_planes=all_planes)
141 |
142 | ret[~mask, -1] = -1
143 | rets.append(ret)
144 |
145 | ret = torch.cat(rets, dim=0)
146 | return ret
147 |
148 | def get_grid_uniform(self, resolution):
149 | """
150 | Get query point coordinates for marching cubes.
151 |
152 | Args:
153 | resolution (int): marching cubes resolution.
154 |
155 | Returns:
156 | (dict): points coordinates and sampled coordinates for each axis.
157 | """
158 | bound = self.marching_cubes_bound
159 |
160 | padding = 0.05
161 |
162 | nsteps_x = ((bound[0][1] - bound[0][0] + 2 * padding) / resolution).round().int().item()
163 | x = np.linspace(bound[0][0] - padding, bound[0][1] + padding, nsteps_x)
164 |
165 | nsteps_y = ((bound[1][1] - bound[1][0] + 2 * padding) / resolution).round().int().item()
166 | y = np.linspace(bound[1][0] - padding, bound[1][1] + padding, nsteps_y)
167 |
168 | nsteps_z = ((bound[2][1] - bound[2][0] + 2 * padding) / resolution).round().int().item()
169 | z = np.linspace(bound[2][0] - padding, bound[2][1] + padding, nsteps_z)
170 |
171 | x_t, y_t, z_t = torch.from_numpy(x).float(), torch.from_numpy(y).float(), torch.from_numpy(z).float()
172 | grid_x, grid_y, grid_z = torch.meshgrid(x_t, y_t, z_t, indexing='xy')
173 | grid_points_t = torch.stack([grid_x.reshape(-1), grid_y.reshape(-1), grid_z.reshape(-1)], dim=1)
174 |
175 | return {"grid_points": grid_points_t, "xyz": [x, y, z]}
176 |
177 | def get_mesh(self, mesh_out_file, NeRF, keyframe_dict, device='cuda:0', color=True):
178 | """
179 | Get mesh from keyframes and feature planes and save to file.
180 | Args:
181 | mesh_out_file (str): output mesh file.
182 | all_planes (Tuple): all feature planes.
183 | decoders (torch.nn.Module): decoders for TSDF and color.
184 | keyframe_dict (dict): keyframe dictionary.
185 | device (str): device to run the model.
186 | color (bool): whether to use color.
187 | Returns:
188 | None
189 | """
190 |
191 | with torch.no_grad():
192 | grid = self.get_grid_uniform(self.resolution)
193 | points = grid['grid_points']
194 | mesh_bound = self.get_bound_from_frames(keyframe_dict, self.scale)
195 |
196 | z = []
197 | mask = []
198 | for i, pnts in enumerate(torch.split(points, self.points_batch_size, dim=0)):
199 | mask.append(mesh_bound.contains(pnts.cpu().numpy()))
200 | mask = np.concatenate(mask, axis=0)
201 |
202 | for i, pnts in enumerate(torch.split(points, self.points_batch_size, dim=0)):
203 | z.append(self.eval_points(pnts.to(device), NeRF).cpu().numpy()[:, -1])
204 | z = np.concatenate(z, axis=0)
205 | z[~mask] = -1
206 |
207 | try:
208 | if version.parse(
209 | skimage.__version__) > version.parse('0.15.0'):
210 | # for new version as provided in environment.yaml
211 | verts, faces, normals, values = skimage.measure.marching_cubes(
212 | volume=z.reshape(
213 | grid['xyz'][1].shape[0], grid['xyz'][0].shape[0],
214 | grid['xyz'][2].shape[0]).transpose([1, 0, 2]),
215 | level=self.level_set,
216 | spacing=(grid['xyz'][0][2] - grid['xyz'][0][1],
217 | grid['xyz'][1][2] - grid['xyz'][1][1],
218 | grid['xyz'][2][2] - grid['xyz'][2][1]))
219 | else:
220 | # for lower version
221 | verts, faces, normals, values = skimage.measure.marching_cubes_lewiner(
222 | volume=z.reshape(
223 | grid['xyz'][1].shape[0], grid['xyz'][0].shape[0],
224 | grid['xyz'][2].shape[0]).transpose([1, 0, 2]),
225 | level=self.level_set,
226 | spacing=(grid['xyz'][0][2] - grid['xyz'][0][1],
227 | grid['xyz'][1][2] - grid['xyz'][1][1],
228 | grid['xyz'][2][2] - grid['xyz'][2][1]))
229 | except:
230 | print('marching_cubes error. Possibly no surface extracted from the level set.')
231 | return
232 |
233 | # convert back to world coordinates
234 | vertices = verts + np.array([grid['xyz'][0][0], grid['xyz'][1][0], grid['xyz'][2][0]])
235 |
236 | if color:
237 | # color is extracted by passing the coordinates of mesh vertices through the network
238 | points = torch.from_numpy(vertices)
239 | z = []
240 | for i, pnts in enumerate(torch.split(points, self.points_batch_size, dim=0)):
241 | z_color = self.eval_points(pnts.to(device).float(),NeRF).cpu()[..., :3]
242 | z.append(z_color)
243 | z = torch.cat(z, dim=0)
244 | vertex_colors = z.numpy()
245 | else:
246 | vertex_colors = None
247 |
248 | vertices /= self.scale
249 | mesh = trimesh.Trimesh(vertices, faces, vertex_colors=vertex_colors)
250 | mesh.export(mesh_out_file)
251 | if self.verbose:
252 | print('Saved mesh at', mesh_out_file)
--------------------------------------------------------------------------------
/src/utils/mesher_utils.py:
--------------------------------------------------------------------------------
1 | import os
2 | import torch
3 | import numpy as np
4 | import trimesh
5 | import marching_cubes as mcubes
6 | from matplotlib import pyplot as plt
7 |
8 |
9 | #### GO-Surf ####
10 | def coordinates(voxel_dim, device: torch.device, flatten=True):
11 | if type(voxel_dim) is int:
12 | nx = ny = nz = voxel_dim
13 | else:
14 | nx, ny, nz = voxel_dim[0], voxel_dim[1], voxel_dim[2]
15 | x = torch.arange(0, nx, dtype=torch.long, device=device)
16 | y = torch.arange(0, ny, dtype=torch.long, device=device)
17 | z = torch.arange(0, nz, dtype=torch.long, device=device)
18 | x, y, z = torch.meshgrid(x, y, z, indexing="ij")
19 |
20 | if not flatten:
21 | return torch.stack([x, y, z], dim=-1)
22 |
23 | return torch.stack((x.flatten(), y.flatten(), z.flatten()))
24 | #### ####
25 |
26 | def getVoxels(x_max, x_min, y_max, y_min, z_max, z_min, voxel_size=None, resolution=None):
27 |
28 | if not isinstance(x_max, float):
29 | x_max = float(x_max)
30 | x_min = float(x_min)
31 | y_max = float(y_max)
32 | y_min = float(y_min)
33 | z_max = float(z_max)
34 | z_min = float(z_min)
35 |
36 | if voxel_size is not None:
37 | Nx = abs(round((x_max - x_min) / voxel_size + 0.0005))
38 | Ny = abs(round((y_max - y_min) / voxel_size + 0.0005))
39 | Nz = abs(round((z_max - z_min) / voxel_size + 0.0005))
40 |
41 | tx = torch.linspace(x_min, x_max, Nx + 1)
42 | ty = torch.linspace(y_min, y_max, Ny + 1)
43 | tz = torch.linspace(z_min, z_max, Nz + 1)
44 | else:
45 | tx = torch.linspace(x_min, x_max, resolution)
46 | ty = torch.linspace(y_min, y_max,resolution)
47 | tz = torch.linspace(z_min, z_max, resolution)
48 |
49 |
50 | return tx, ty, tz
51 |
52 | def get_batch_query_fn(query_fn, num_args=1, device=None):
53 |
54 | if num_args == 1:
55 | fn = lambda f, i0, i1: query_fn(f[i0:i1, None, :].to(device))
56 | else:
57 | fn = lambda f, f1, i0, i1: query_fn(f[i0:i1, None, :].to(device), f1[i0:i1, :].to(device))
58 |
59 |
60 | return fn
61 |
62 | #### NeuralRGBD ####
63 | @torch.no_grad()
64 | def extract_mesh(NeRF, config, bounding_box, marching_cube_bound=None, voxel_size=None,
65 | resolution=None, isolevel=0.0 , mesh_savepath='', device=None):
66 | '''
67 | Extracts mesh from the scene model using marching cubes (Adapted from NeuralRGBD)
68 | '''
69 | query_fn = NeRF.query_sdf
70 | color_func = NeRF.query_color
71 |
72 | # Query network on dense 3d grid of points
73 | if marching_cube_bound is None:
74 | marching_cube_bound = bounding_box
75 |
76 | x_min, y_min, z_min = marching_cube_bound[:, 0]
77 | x_max, y_max, z_max = marching_cube_bound[:, 1]
78 |
79 | #voxel_size=None
80 | tx, ty, tz = getVoxels(x_max, x_min, y_max, y_min, z_max, z_min, voxel_size, resolution)
81 | query_pts = torch.stack(torch.meshgrid(tx, ty, tz, indexing='ij'), -1).to(torch.float32)
82 |
83 | sh = query_pts.shape
84 |
85 | #flat = query_pts.reshape([-1, 3])
86 | #bounding_box_cpu = bounding_box.cpu()
87 | #if config['grid']['tcnn_encoding']:
88 | # flat = (flat - bounding_box_cpu[:, 0]) / (bounding_box_cpu[:, 1] - bounding_box_cpu[:, 0])
89 | #fn = get_batch_query_fn(query_fn, device=bounding_box.device)
90 | chunk = 10000 #1024 * 64 #10000
91 | #raw = [fn(flat, i, i + chunk).cpu().data.numpy() for i in range(0, flat.shape[0], chunk)]
92 |
93 | pts = query_pts.reshape(sh[0]*sh[1],sh[2], 3)
94 |
95 | raw_list = []
96 | for i in range(0, pts.shape[0],chunk):
97 | pts_chunk = pts[i : i + chunk]
98 | #raw = query_fn(pts.to(torch.float).to(device)).cpu().data.numpy()
99 | raw = query_fn(pts_chunk.to(torch.float).to(device)).cpu().data.numpy()
100 | raw_list.append(raw)
101 | raw = np.concatenate(raw_list, 0).astype(np.float32)
102 |
103 | raw = np.reshape(raw, list(sh[:-1]) + [-1])
104 |
105 | print('Running Marching Cubes')
106 | vertices, triangles = mcubes.marching_cubes(raw.squeeze(), isolevel, truncation=3)
107 | print('done', vertices.shape, triangles.shape)
108 |
109 | # normalize vertex positions
110 | vertices[:, :3] /= np.array([[tx.shape[0] - 1, ty.shape[0] - 1, tz.shape[0] - 1]])
111 |
112 | if len(list(vertices)) == 0:
113 | print('No vertices found')
114 | return
115 |
116 | # Rescale and translate
117 | tx = tx.cpu().data.numpy()
118 | ty = ty.cpu().data.numpy()
119 | tz = tz.cpu().data.numpy()
120 |
121 | scale = np.array([tx[-1] - tx[0], ty[-1] - ty[0], tz[-1] - tz[0]])
122 | offset = np.array([tx[0], ty[0], tz[0]])
123 | vertices[:, :3] = scale[np.newaxis, :] * vertices[:, :3] + offset
124 |
125 | # Transform to metric units
126 | # vertices[:, :3] = vertices[:, :3]
127 |
128 |
129 | if color_func is not None and not config['utils']['mesh_render_color']:
130 | # yes we don't want to render color
131 | #if config['grid']['tcnn_encoding']:
132 | # vert_flat = (torch.from_numpy(vertices).to(bounding_box) - bounding_box[:, 0]) / (bounding_box[:, 1] - bounding_box[:, 0])
133 | vert_flat = torch.from_numpy(vertices).to(device)
134 | sh = vert_flat.shape
135 |
136 | #fn_color = get_batch_query_fn(color_func, 1)
137 |
138 | #chunk = 1024 * 64
139 | #raw = [fn_color(vert_flat, i, i + chunk).cpu().data.numpy() for i in range(0, vert_flat.shape[0], chunk)]
140 |
141 | raw_color_list = []
142 | for i in range(0, vert_flat.shape[0],chunk):
143 | vert_flat_chunk = vert_flat[i : i + chunk]
144 | #raw_color = color_func(vert_flat.unsqueeze(1).to(torch.float))
145 | raw_color = color_func(vert_flat_chunk.unsqueeze(1).to(torch.float)).cpu().data.numpy()
146 | raw_color_list.append(raw_color)
147 |
148 | raw_color = np.concatenate(raw_color_list, 0).astype(np.float32)
149 | color = np.reshape(raw_color, list(sh[:-1]) + [-1])
150 | mesh = trimesh.Trimesh(vertices, triangles, process=False, vertex_colors=color)
151 |
152 | elif color_func is not None and config['utils']['mesh_render_color'] and False:
153 | print('rendering surface color')
154 | chunk = 1024 * 64
155 | mesh = trimesh.Trimesh(vertices, triangles, process=False)
156 | vertex_normals = torch.from_numpy(mesh.vertex_normals)
157 | #fn_color = get_batch_query_fn(color_func, 2, device=bounding_box.device)
158 |
159 | raw_color = color_func(vert_flat.unsqueeze(1).to(torch.float))
160 |
161 | #raw = [fn_color(torch.from_numpy(vertices), vertex_normals, i, i + chunk).cpu().data.numpy() for i in range(0, vertices.shape[0], chunk)]
162 |
163 | sh = vertex_normals.shape
164 |
165 | raw = np.concatenate(raw, 0).astype(np.float32)
166 | color = np.reshape(raw, list(sh[:-1]) + [-1])
167 | mesh = trimesh.Trimesh(vertices, triangles, process=False, vertex_colors=color)
168 |
169 | else:
170 | # Create mesh
171 | mesh = trimesh.Trimesh(vertices, triangles, process=False)
172 |
173 | os.makedirs(os.path.split(mesh_savepath)[0], exist_ok=True)
174 | mesh.export(mesh_savepath)
175 |
176 | print('Mesh saved')
177 | return mesh
178 | #### ####
179 |
180 | #### SimpleRecon ####
181 | def colormap_image(
182 | image_1hw,
183 | mask_1hw=None,
184 | invalid_color=(0.0, 0, 0.0),
185 | flip=True,
186 | vmin=None,
187 | vmax=None,
188 | return_vminvmax=False,
189 | colormap="turbo",
190 | ):
191 | """
192 | Colormaps a one channel tensor using a matplotlib colormap.
193 | Args:
194 | image_1hw: the tensor to colomap.
195 | mask_1hw: an optional float mask where 1.0 donates valid pixels.
196 | colormap: the colormap to use. Default is turbo.
197 | invalid_color: the color to use for invalid pixels.
198 | flip: should we flip the colormap? True by default.
199 | vmin: if provided uses this as the minimum when normalizing the tensor.
200 | vmax: if provided uses this as the maximum when normalizing the tensor.
201 | When either of vmin or vmax are None, they are computed from the
202 | tensor.
203 | return_vminvmax: when true, returns vmin and vmax.
204 | Returns:
205 | image_cm_3hw: image of the colormapped tensor.
206 | vmin, vmax: returned when return_vminvmax is true.
207 | """
208 | valid_vals = image_1hw if mask_1hw is None else image_1hw[mask_1hw.bool()]
209 | if vmin is None:
210 | vmin = valid_vals.min()
211 | if vmax is None:
212 | vmax = valid_vals.max()
213 |
214 | cmap = torch.Tensor(
215 | plt.cm.get_cmap(colormap)(
216 | torch.linspace(0, 1, 256)
217 | )[:, :3]
218 | ).to(image_1hw.device)
219 | if flip:
220 | cmap = torch.flip(cmap, (0,))
221 |
222 | h, w = image_1hw.shape[1:]
223 |
224 | image_norm_1hw = (image_1hw - vmin) / (vmax - vmin)
225 | image_int_1hw = (torch.clamp(image_norm_1hw * 255, 0, 255)).byte().long()
226 |
227 | image_cm_3hw = cmap[image_int_1hw.flatten(start_dim=1)
228 | ].permute([0, 2, 1]).view([-1, h, w])
229 |
230 | if mask_1hw is not None:
231 | invalid_color = torch.Tensor(invalid_color).view(3, 1, 1).to(image_1hw.device)
232 | image_cm_3hw = image_cm_3hw * mask_1hw + invalid_color * (1 - mask_1hw)
233 |
234 | if return_vminvmax:
235 | return image_cm_3hw, vmin, vmax
236 | else:
237 | return image_cm_3hw
238 |
239 |
240 |
241 |
242 |
243 |
--------------------------------------------------------------------------------
/src/utils/pose_eval_utils.py:
--------------------------------------------------------------------------------
1 | # NICE-SLAM evaluation methods
2 | import argparse
3 | import os
4 | import numpy
5 | import torch
6 | import sys
7 | import numpy as np
8 | sys.path.append('.')
9 |
10 | def get_tensor_from_camera(RT, Tquad=False):
11 | """
12 | Convert transformation matrix to quaternion and translation.
13 |
14 | """
15 | gpu_id = -1
16 | if type(RT) == torch.Tensor:
17 | if RT.get_device() != -1:
18 | RT = RT.detach().cpu()
19 | gpu_id = RT.get_device()
20 | RT = RT.numpy()
21 |
22 | from mathutils import Matrix
23 | R, T = RT[:3, :3], RT[:3, 3]
24 | rot = Matrix(R)
25 | quad = rot.to_quaternion()
26 | if Tquad:
27 | tensor = np.concatenate([T, quad], 0)
28 | else:
29 | tensor = np.concatenate([quad, T], 0)
30 | tensor = torch.from_numpy(tensor).float()
31 | if gpu_id != -1:
32 | tensor = tensor.to(gpu_id)
33 | return tensor
34 |
35 | def associate(first_list, second_list, offset=0.0, max_difference=0.02):
36 | """
37 | Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim
38 | to find the closest match for every input tuple.
39 |
40 | Input:
41 | first_list -- first dictionary of (stamp,data) tuples
42 | second_list -- second dictionary of (stamp,data) tuples
43 | offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
44 | max_difference -- search radius for candidate generation
45 |
46 | Output:
47 | matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
48 |
49 | """
50 | first_keys = list(first_list.keys())
51 | second_keys = list(second_list.keys())
52 | potential_matches = [(abs(a - (b + offset)), a, b)
53 | for a in first_keys
54 | for b in second_keys
55 | if abs(a - (b + offset)) < max_difference]
56 | potential_matches.sort()
57 | matches = []
58 | for diff, a, b in potential_matches:
59 | if a in first_keys and b in second_keys:
60 | first_keys.remove(a)
61 | second_keys.remove(b)
62 | matches.append((a, b))
63 |
64 | matches.sort()
65 | return matches
66 |
67 | def align(model, data):
68 | """Align two trajectories using the method of Horn (closed-form).
69 |
70 | Input:
71 | model -- first trajectory (3xn)
72 | data -- second trajectory (3xn)
73 |
74 | Output:
75 | rot -- rotation matrix (3x3)
76 | trans -- translation vector (3x1)
77 | trans_error -- translational error per point (1xn)
78 |
79 | """
80 | numpy.set_printoptions(precision=3, suppress=True)
81 | model_zerocentered = model - model.mean(1)
82 | data_zerocentered = data - data.mean(1)
83 |
84 | W = numpy.zeros((3, 3))
85 | for column in range(model.shape[1]):
86 | W += numpy.outer(model_zerocentered[:,
87 | column], data_zerocentered[:, column])
88 | U, d, Vh = numpy.linalg.linalg.svd(W.transpose())
89 | S = numpy.matrix(numpy.identity(3))
90 | if(numpy.linalg.det(U) * numpy.linalg.det(Vh) < 0):
91 | S[2, 2] = -1
92 | rot = U*S*Vh
93 | trans = data.mean(1) - rot * model.mean(1)
94 |
95 | model_aligned = rot * model + trans
96 | alignment_error = model_aligned - data
97 |
98 | trans_error = numpy.sqrt(numpy.sum(numpy.multiply(
99 | alignment_error, alignment_error), 0)).A[0]
100 |
101 | return rot, trans, trans_error
102 |
103 | def plot_traj(ax, stamps, traj, style, color, label):
104 | """
105 | Plot a trajectory using matplotlib.
106 |
107 | Input:
108 | ax -- the plot
109 | stamps -- time stamps (1xn)
110 | traj -- trajectory (3xn)
111 | style -- line style
112 | color -- line color
113 | label -- plot legend
114 |
115 | """
116 | stamps.sort()
117 | interval = numpy.median([s-t for s, t in zip(stamps[1:], stamps[:-1])])
118 | x = []
119 | y = []
120 | last = stamps[0]
121 | for i in range(len(stamps)):
122 | if stamps[i]-last < 2*interval:
123 | x.append(traj[i][0])
124 | y.append(traj[i][1])
125 | elif len(x) > 0:
126 | ax.plot(x, y, style, color=color, label=label)
127 | label = ""
128 | x = []
129 | y = []
130 | last = stamps[i]
131 | if len(x) > 0:
132 | ax.plot(x, y, style, color=color, label=label)
133 |
134 | def evaluate_ate(first_list, second_list, plot="", _args="",frame_id=0,final_flag = False):
135 | # parse command line
136 | parser = argparse.ArgumentParser(
137 | description='This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.')
138 | # parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
139 | # parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
140 | parser.add_argument(
141 | '--offset', help='time offset added to the timestamps of the second file (default: 0.0)', default=0.0)
142 | parser.add_argument(
143 | '--scale', help='scaling factor for the second trajectory (default: 1.0)', default=1.0)
144 | parser.add_argument(
145 | '--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)', default=0.02)
146 | parser.add_argument(
147 | '--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
148 | parser.add_argument('--save_associations',
149 | help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
150 | parser.add_argument(
151 | '--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
152 | parser.add_argument(
153 | '--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true')
154 | args = parser.parse_args(_args)
155 | args.plot = plot
156 | # first_list = associate.read_file_list(args.first_file)
157 | # second_list = associate.read_file_list(args.second_file)
158 |
159 | matches = associate(first_list, second_list, float(
160 | args.offset), float(args.max_difference))
161 | if len(matches) < 2 and len(first_list) > 5:
162 | raise ValueError(
163 | "Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! \
164 | Did you choose the correct sequence?")
165 |
166 | first_xyz = numpy.matrix(
167 | [[float(value) for value in first_list[a][0:3]] for a, b in matches]).transpose()
168 | second_xyz = numpy.matrix([[float(value)*float(args.scale)
169 | for value in second_list[b][0:3]] for a, b in matches]).transpose()
170 |
171 | rot, trans, trans_error = align(second_xyz, first_xyz)
172 |
173 | second_xyz_aligned = rot * second_xyz + trans
174 |
175 | first_stamps = list(first_list.keys())
176 | first_stamps.sort()
177 | first_xyz_full = numpy.matrix(
178 | [[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
179 |
180 | second_stamps = list(second_list.keys())
181 | second_stamps.sort()
182 | second_xyz_full = numpy.matrix([[float(value)*float(args.scale)
183 | for value in second_list[b][0:3]] for b in second_stamps]).transpose()
184 | second_xyz_full_aligned = rot * second_xyz_full + trans
185 |
186 | if args.verbose:
187 | print("compared_pose_pairs %d pairs" % (len(trans_error)))
188 |
189 | print("absolute_translational_error.rmse %f m" % numpy.sqrt(
190 | numpy.dot(trans_error, trans_error) / len(trans_error)))
191 | print("absolute_translational_error.mean %f m" %
192 | numpy.mean(trans_error))
193 | print("absolute_translational_error.median %f m" %
194 | numpy.median(trans_error))
195 | print("absolute_translational_error.std %f m" % numpy.std(trans_error))
196 | print("absolute_translational_error.min %f m" % numpy.min(trans_error))
197 | print("absolute_translational_error.max %f m" % numpy.max(trans_error))
198 |
199 | if args.save_associations:
200 | file = open(args.save_associations, "w")
201 | file.write("\n".join(["%f %f %f %f %f %f %f %f" % (a, x1, y1, z1, b, x2, y2, z2) for (
202 | a, b), (x1, y1, z1), (x2, y2, z2) in zip(matches, first_xyz.transpose().A, second_xyz_aligned.transpose().A)]))
203 | file.close()
204 |
205 | if args.save:
206 | file = open(args.save, "w")
207 | file.write("\n".join(["%f " % stamp+" ".join(["%f" % d for d in line])
208 | for stamp, line in zip(second_stamps, second_xyz_full_aligned.transpose().A)]))
209 | file.close()
210 |
211 | if (args.plot and frame_id%50==0 and frame_id!=0) or (final_flag == True):
212 | import matplotlib
213 | matplotlib.use('Agg')
214 | import matplotlib.pylab as pylab
215 | import matplotlib.pyplot as plt
216 | from matplotlib.patches import Ellipse
217 | fig = plt.figure()
218 | ax = fig.add_subplot(111)
219 | ATE = numpy.sqrt(
220 | numpy.dot(trans_error, trans_error) / len(trans_error))
221 | png_name = os.path.basename(args.plot)
222 | ax.set_title(f'len:{len(trans_error)} ATE RMSE:{ATE} {png_name[:-3]}')
223 | plot_traj(ax, first_stamps, first_xyz_full.transpose().A,
224 | '-', "black", "ground truth")
225 | plot_traj(ax, second_stamps, second_xyz_full_aligned.transpose(
226 | ).A, '-', "blue", "estimated")
227 |
228 | label = "difference"
229 | for (a, b), (x1, y1, z1), (x2, y2, z2) in zip(matches, first_xyz.transpose().A, second_xyz_aligned.transpose().A):
230 | # ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
231 | label = ""
232 | ax.legend()
233 | ax.set_xlabel('x [m]')
234 | ax.set_ylabel('y [m]')
235 | plt.savefig(args.plot, dpi=90)
236 |
237 | return {
238 | "compared_pose_pairs": (len(trans_error)),
239 | "absolute_translational_error.rmse": numpy.sqrt(numpy.dot(trans_error, trans_error) / len(trans_error)),
240 | "absolute_translational_error.mean": numpy.mean(trans_error),
241 | "absolute_translational_error.median": numpy.median(trans_error),
242 | "absolute_translational_error.std": numpy.std(trans_error),
243 | "absolute_translational_error.min": numpy.min(trans_error),
244 | "absolute_translational_error.max": numpy.max(trans_error),
245 | }
246 |
247 | def evaluate(poses_gt, poses_est, plot, frame_id=0,final_flag = False):
248 |
249 | poses_gt = poses_gt.cpu().numpy()
250 | poses_est = poses_est.cpu().numpy()
251 |
252 | N = poses_gt.shape[0]
253 | poses_gt = dict([(i, poses_gt[i]) for i in range(N)])
254 | poses_est = dict([(i, poses_est[i]) for i in range(N)])
255 |
256 | results = evaluate_ate(poses_gt, poses_est, plot, frame_id=frame_id,final_flag = final_flag)
257 | return results
258 |
259 | def convert_poses(c2w_list, N, scale, gt=True):
260 | poses = []
261 | mask = torch.ones(N).bool()
262 | for idx in range(0, N):
263 | if gt:
264 | # some frame have `nan` or `inf` in gt pose of ScanNet,
265 | # but our system have estimated camera pose for all frames
266 | # therefore, when calculating the pose error, we need to mask out invalid pose
267 | if torch.isinf(c2w_list[idx]).any():
268 | mask[idx] = 0
269 | continue
270 | if torch.isnan(c2w_list[idx]).any():
271 | mask[idx] = 0
272 | continue
273 | c2w_list[idx][:3, 3] /= scale
274 | poses.append(get_tensor_from_camera(c2w_list[idx], Tquad=True))
275 | poses = torch.stack(poses)
276 | return poses, mask
277 |
278 | def pose_evaluation(poses_gt, poses_est, scale, path_to_save, i, final_flag = False,img='pose', name='output.txt'):
279 | N = len(poses_est)
280 | poses_gt, mask = convert_poses(poses_gt, N, scale)
281 | poses_est, _ = convert_poses(poses_est, N, scale)
282 | poses_est = poses_est[mask]
283 | plt_path = os.path.join(path_to_save, '{}_{}.png'.format(img, i))
284 |
285 | results = evaluate(poses_gt, poses_est, plot=plt_path, frame_id=i, final_flag = final_flag)
286 | results['Name'] = i
287 | print(results, file=open(os.path.join(path_to_save, name), "a"))
288 | return results
--------------------------------------------------------------------------------
/src/utils/pose_utils.py:
--------------------------------------------------------------------------------
1 | import torch
2 | from pytorch3d.transforms import matrix_to_quaternion, quaternion_to_matrix, rotation_6d_to_matrix, quaternion_to_axis_angle
3 |
4 | # TODO: Identity would cause the problem...
5 | def axis_angle_to_matrix(data):
6 | batch_dims = data.shape[:-1]
7 |
8 | theta = torch.norm(data, dim=-1, keepdim=True)
9 | omega = data / theta
10 |
11 | omega1 = omega[...,0:1]
12 | omega2 = omega[...,1:2]
13 | omega3 = omega[...,2:3]
14 | zeros = torch.zeros_like(omega1)
15 |
16 | K = torch.concat([torch.concat([zeros, -omega3, omega2], dim=-1)[...,None,:],
17 | torch.concat([omega3, zeros, -omega1], dim=-1)[...,None,:],
18 | torch.concat([-omega2, omega1, zeros], dim=-1)[...,None,:]], dim=-2)
19 | I = torch.eye(3).expand(*batch_dims,3,3).to(data)
20 |
21 | return I + torch.sin(theta).unsqueeze(-1) * K + (1. - torch.cos(theta).unsqueeze(-1)) * (K @ K)
22 |
23 | def matrix_to_axis_angle(rot):
24 | """
25 | :param rot: [N, 3, 3]
26 | :return:
27 | """
28 | return quaternion_to_axis_angle(matrix_to_quaternion(rot))
29 |
30 | def at_to_transform_matrix(rot, trans):
31 | """
32 | :param rot: axis-angle [bs, 3]
33 | :param trans: translation vector[bs, 3]
34 | :return: transformation matrix [b, 4, 4]
35 | """
36 | bs = rot.shape[0]
37 | T = torch.eye(4).to(rot)[None, ...].repeat(bs, 1, 1)
38 | R = axis_angle_to_matrix(rot)
39 | T[:, :3, :3] = R
40 | T[:, :3, 3] = trans
41 | return T
42 |
43 | def qt_to_transform_matrix(rot, trans):
44 | """
45 | :param rot: axis-angle [bs, 3]
46 | :param trans: translation vector[bs, 3]
47 | :return: transformation matrix [b, 4, 4]
48 | """
49 | bs = rot.shape[0]
50 | T = torch.eye(4).to(rot)[None, ...].repeat(bs, 1, 1)
51 | R = quaternion_to_matrix(rot)
52 | T[:, :3, :3] = R
53 | T[:, :3, 3] = trans
54 | return T
55 |
56 | def six_t_to_transform_matrix(rot, trans):
57 | """
58 | :param rot: 6d rotation [bs, 6]
59 | :param trans: translation vector[bs, 3]
60 | :return: transformation matrix [b, 4, 4]
61 | """
62 | bs = rot.shape[0]
63 | T = torch.eye(4).to(rot)[None, ...].repeat(bs, 1, 1)
64 | R = rotation_6d_to_matrix(rot)
65 | T[:, :3, :3] = R
66 | T[:, :3, 3] = trans
67 | return
--------------------------------------------------------------------------------
/src/utils/render_utils.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn.functional as F
3 | from torch.cuda.amp import autocast
4 |
5 |
6 | def alpha_compositing_weights(alphas):
7 | """Alpha compositing of (sampled) MPIs given their RGBs and alphas.
8 | Args:
9 | alphas (tensor [batch,ray,samples]): The predicted opacity values.
10 | Returns:
11 | weights (tensor [batch,ray,samples,1]): The predicted weight of each MPI (in [0,1]).
12 | """
13 | alphas_front = torch.cat([torch.zeros_like(alphas[..., :1]),
14 | alphas[..., :-1]], dim=2) # [B,R,N]
15 | with autocast(enabled=False): # TODO: may be unstable in some cases.
16 | visibility = (1 - alphas_front).cumprod(dim=2) # [B,R,N]
17 | weights = (alphas * visibility)[..., None] # [B,R,N,1]
18 | return weights
19 |
20 |
21 | def composite(quantities, weights):
22 | """Composite the samples to render the RGB/depth/opacity of the corresponding pixels.
23 | Args:
24 | quantities (tensor [batch,ray,samples,k]): The quantity to be weighted summed.
25 | weights (tensor [batch,ray,samples,1]): The predicted weight of each sampled point along the ray.
26 | Returns:
27 | quantity (tensor [batch,ray,k]): The expected (rendered) quantity.
28 | """
29 | # Integrate RGB and depth weighted by probability.
30 | quantity = (quantities * weights).sum(dim=2) # [B,R,K]
31 | return quantity
32 |
33 |
34 | def compute_loss(prediction, target, loss_type='l2'):
35 | '''
36 | Params:
37 | prediction: torch.Tensor, (Bs, N_samples)
38 | target: torch.Tensor, (Bs, N_samples)
39 | loss_type: str
40 | Return:
41 | loss: torch.Tensor, (1,)
42 | '''
43 |
44 | if loss_type == 'l2':
45 | return F.mse_loss(prediction, target)
46 | elif loss_type == 'l1':
47 | return F.l1_loss(prediction, target)
48 |
49 | raise Exception('Unsupported loss type')
--------------------------------------------------------------------------------
/src/utils/vis_utils.py:
--------------------------------------------------------------------------------
1 | from matplotlib import pyplot as plt
2 | import numpy as np
3 | import torch
4 |
5 | def get_rays(H, W, fx, fy, cx, cy, c2w, device):
6 | """
7 | Get rays for a whole image.
8 |
9 | """
10 | if isinstance(c2w, np.ndarray):
11 | c2w = torch.from_numpy(c2w)
12 | # pytorch's meshgrid has indexing='ij'
13 | i, j = torch.meshgrid(torch.linspace(0, W-1, W), torch.linspace(0, H-1, H))
14 | i = i.t() # transpose
15 | j = j.t()
16 | dirs = torch.stack(
17 | [(i-cx)/fx, -(j-cy)/fy, -torch.ones_like(i)], -1).to(device)
18 | dirs = dirs.reshape(H, W, 1, 3)
19 | # Rotate ray directions from camera frame to the world frame
20 | # dot product, equals to: [c2w.dot(dir) for dir in dirs]
21 | rays_d = torch.sum(dirs * c2w[:3, :3], -1)
22 | rays_o = c2w[:3, -1].expand(rays_d.shape)
23 | return rays_o, rays_d
24 |
25 |
26 | def plot_depth_color(NeRF, c2w,H ,W ,fx ,fy ,cx ,cy ,gt_depth,gt_color,output,opt_iter,device):
27 | '''
28 | gt_depth, gt_color, cur_c2ws are stream of tensors
29 | idx is to index them
30 | '''
31 | with torch.no_grad():
32 | rays_o ,rays_d = get_rays(H, W, fx, fy, cx, cy, c2w, device)
33 | rays_o = torch.reshape(rays_o, [-1, rays_o.shape[-1]])
34 | rays_d = torch.reshape(rays_d, [-1, rays_d.shape[-1]])
35 | target_d = torch.reshape(gt_depth, [-1, 1])
36 | ray_batch_size = 10000 #0
37 |
38 | depth_list = []
39 | color_list = []
40 | for i in range(0, rays_d.shape[0], ray_batch_size):
41 | rays_d_batch = rays_d[i : i + ray_batch_size]
42 | rays_o_batch = rays_o[i : i + ray_batch_size]
43 | target_d_batch = (
44 | target_d[i : i + ray_batch_size] if target_d is not None else None
45 | )
46 |
47 | color, depth = NeRF.render_rays(rays_o_batch, rays_d_batch,
48 | target_d = target_d_batch,
49 | render_img=True)
50 |
51 | depth_list.append(depth.double())
52 | color_list.append(color)
53 |
54 | depth = torch.cat(depth_list, dim=0)
55 | color = torch.cat(color_list, dim=0)
56 |
57 | depth_np = depth.reshape(H, W).detach().cpu().numpy()
58 | color_np = color.reshape(H, W, 3).detach().cpu().numpy()
59 | gt_depth_np = gt_depth.detach().cpu().numpy()
60 | gt_color_np = gt_color.detach().cpu().numpy()
61 | depth_residual = np.abs(gt_depth_np - depth_np)
62 | depth_residual[gt_depth_np == 0.0] = 0.0
63 | color_residual = np.abs(gt_color_np - color_np)
64 | color_residual[gt_depth_np == 0.0] = 0.0
65 |
66 | fig, axs = plt.subplots(2, 3)
67 | fig.tight_layout()
68 | max_depth = np.max(gt_depth_np)
69 | max_depth = 2 if max_depth == 0 else max_depth
70 | axs[0, 0].imshow(gt_depth_np, cmap="plasma", vmin=0, vmax=max_depth)
71 | axs[0, 0].set_title("Input Depth")
72 | axs[0, 0].set_xticks([])
73 | axs[0, 0].set_yticks([])
74 | axs[0, 1].imshow(depth_np, cmap="plasma", vmin=0, vmax=max_depth)
75 | axs[0, 1].set_title("Generated Depth")
76 | axs[0, 1].set_xticks([])
77 | axs[0, 1].set_yticks([])
78 | axs[0, 2].imshow(depth_residual, cmap="plasma", vmin=0, vmax=max_depth)
79 | axs[0, 2].set_title("Depth Residual")
80 | axs[0, 2].set_xticks([])
81 | axs[0, 2].set_yticks([])
82 | gt_color_np = np.clip(gt_color_np, 0, 1)
83 | color_np = np.clip(color_np, 0, 1)
84 | color_residual = np.clip(color_residual, 0, 1)
85 | axs[1, 0].imshow(gt_color_np, cmap="plasma")
86 | axs[1, 0].set_title("Input RGB")
87 | axs[1, 0].set_xticks([])
88 | axs[1, 0].set_yticks([])
89 | axs[1, 1].imshow(color_np, cmap="plasma")
90 | axs[1, 1].set_title("Generated RGB")
91 | axs[1, 1].set_xticks([])
92 | axs[1, 1].set_yticks([])
93 | axs[1, 2].imshow(color_residual, cmap="plasma")
94 | axs[1, 2].set_title("RGB Residual")
95 | axs[1, 2].set_xticks([])
96 | axs[1, 2].set_yticks([])
97 | plt.subplots_adjust(wspace=0, hspace=0)
98 | plt.savefig(
99 | f"{output}/{opt_iter:04d}.jpg",
100 | bbox_inches="tight",
101 | pad_inches=0.2,
102 | )
103 | plt.clf()
104 | torch.cuda.empty_cache()
105 |
106 | def render_img(self, c2w, device, gt_depth=None, **kwargs):
107 | """
108 | Renders out depth, uncertainty, and color images.
109 |
110 | Args:
111 | c (dict): feature grids.
112 | decoders (nn.module): decoders.
113 | c2w (tensor): camera to world matrix of current frame.
114 | device (str): device name to compute on.
115 | stage (str): query stage.
116 | gt_depth (tensor, optional): sensor depth image. Defaults to None.
117 |
118 | Returns:
119 | depth (tensor, H*W): rendered depth image.
120 | uncertainty (tensor, H*W): rendered uncertainty image.
121 | color (tensor, H*W*3): rendered color image.
122 | """
123 | with torch.no_grad():
124 | H = self.H
125 | W = self.W
126 | rays_o, rays_d = get_rays(
127 | H, W, self.fx, self.fy, self.cx, self.cy, c2w, device
128 | )
129 | rays_o = rays_o.reshape(-1, 3)
130 | rays_d = rays_d.reshape(-1, 3)
131 |
132 | depth_list = []
133 | uncertainty_list = []
134 | color_list = []
135 |
136 | ray_batch_size = self.ray_batch_size
137 | if gt_depth is not None:
138 | gt_depth = gt_depth.reshape(-1)
139 |
140 | if "render_size" in kwargs:
141 | skip = rays_d.shape[0] // kwargs["render_size"]
142 | kwargs["skip"] = skip
143 |
144 | if "skip" in kwargs:
145 | rays_d = rays_d[:: kwargs["skip"]]
146 | rays_o = rays_o[:: kwargs["skip"]]
147 |
148 | for i in range(0, rays_d.shape[0], ray_batch_size):
149 | rays_d_batch = rays_d[i : i + ray_batch_size]
150 | rays_o_batch = rays_o[i : i + ray_batch_size]
151 | gt_depth_batch = (
152 | gt_depth[i : i + ray_batch_size] if gt_depth is not None else None
153 | )
154 | ret = self.render_batch_ray(
155 | rays_d_batch,
156 | rays_o_batch,
157 | device,
158 | gt_depth=gt_depth_batch,
159 | **kwargs
160 | )
161 |
162 | depth, uncertainty, color, extra_ret, density = ret
163 | depth_list.append(depth.double())
164 | uncertainty_list.append(uncertainty.double())
165 | color_list.append(color)
166 |
167 | depth = torch.cat(depth_list, dim=0)
168 | uncertainty = torch.cat(uncertainty_list, dim=0)
169 | color = torch.cat(color_list, dim=0)
170 |
171 | if "skip" not in kwargs or kwargs["skip"] == 1:
172 | depth = depth.reshape(H, W)
173 | uncertainty = uncertainty.reshape(H, W)
174 | color = color.reshape(H, W, 3)
175 | return depth, uncertainty, color
--------------------------------------------------------------------------------