├── LICENSE
├── README.md
├── assets
├── pipeline_neuralImplicit.png
└── twobranch.png
├── configs
├── README.md
├── ifr-fusion-lr-kt0.yaml
├── ifr-fusion-lr-kt1.yaml
├── ifr-fusion-lr-kt2.yaml
├── ifr-fusion-lr-kt3.yaml
├── ifr-fusion-replica.yaml
├── test_kitti.yaml
└── train.yaml
├── data_generator.py
├── dataset
├── kitti_dataset.py
├── production
│ ├── __init__.py
│ └── icl_nuim.py
└── training
│ ├── lif_dataset.py
│ ├── main_generator.py
│ ├── shapenet_model.py
│ ├── shapenet_valid_list.json
│ └── simple_shape.py
├── dataset_ptam.py
├── exp_transform.py
├── ifr
├── ifr_exp_transform.py
└── ifr_main.py
├── im2mesh
├── __init__.py
├── checkpoints.py
├── common.py
├── config.py
├── data
│ ├── __init__.py
│ ├── core.py
│ ├── fields.py
│ ├── real.py
│ └── transforms.py
├── dmc
│ ├── __init__.py
│ ├── config.py
│ ├── generation.py
│ ├── models
│ │ ├── __init__.py
│ │ ├── decoder.py
│ │ └── encoder.py
│ ├── ops
│ │ ├── __init__.py
│ │ ├── cpp_modules
│ │ │ ├── old
│ │ │ │ ├── commons.cpp
│ │ │ │ ├── commons.h
│ │ │ │ ├── pred_to_mesh.cpp
│ │ │ │ └── pred_to_mesh.h
│ │ │ ├── pred_to_mesh_.cpp
│ │ │ └── setup.py
│ │ ├── curvature_constraint.py
│ │ ├── grid_pooling.py
│ │ ├── occupancy_connectivity.py
│ │ ├── occupancy_to_topology.py
│ │ ├── point_triangle_distance.py
│ │ ├── setup.py
│ │ ├── src
│ │ │ ├── curvature_constraint_kernel.cu
│ │ │ ├── extension.cpp
│ │ │ ├── grid_pooling_kernel.cu
│ │ │ ├── kernels.h
│ │ │ ├── occupancy_connectivity_kernel.cu
│ │ │ ├── occupancy_to_topology_kernel.cu
│ │ │ └── point_triangle_distance_kernel.cu
│ │ ├── table.py
│ │ └── tests
│ │ │ ├── loss_autograd.py
│ │ │ ├── test_curvature.py
│ │ │ ├── test_distance.py
│ │ │ ├── test_gridpooling.py
│ │ │ ├── test_occupancy_connectivity.py
│ │ │ ├── test_occupancy_connectivity_yiyi.py
│ │ │ └── test_occupancy_to_topology.py
│ ├── training.py
│ └── utils
│ │ ├── __init__.py
│ │ ├── config.py
│ │ ├── pointTriangleDistance.py
│ │ ├── pred2mesh.py
│ │ ├── util.py
│ │ └── visualize.py
├── encoder
│ ├── __init__.py
│ ├── conv.py
│ ├── pix2mesh_cond.py
│ ├── pointnet.py
│ ├── psgn_cond.py
│ ├── r2n2.py
│ ├── vnn.py
│ ├── vnn2.py
│ ├── vnn_tnet.py
│ └── voxels.py
├── eval.py
├── layers.py
├── layers_equi.py
├── onet
│ ├── __init__.py
│ ├── config.py
│ ├── generation.py
│ ├── models
│ │ ├── __init__.py
│ │ ├── decoder.py
│ │ ├── decoder_inner.py
│ │ ├── encoder_latent.py
│ │ └── legacy.py
│ └── training.py
├── pix2mesh
│ ├── __init__.py
│ ├── config.py
│ ├── ellipsoid
│ │ ├── face1.obj
│ │ ├── face2.obj
│ │ ├── face3.obj
│ │ └── info_ellipsoid.dat
│ ├── generation.py
│ ├── layers.py
│ ├── models
│ │ ├── __init__.py
│ │ └── decoder.py
│ └── training.py
├── preprocess.py
├── psgn
│ ├── __init__.py
│ ├── config.py
│ ├── generation.py
│ ├── models
│ │ ├── __init__.py
│ │ ├── decoder.py
│ │ └── psgn_2branch.py
│ └── training.py
├── r2n2
│ ├── __init__.py
│ ├── config.py
│ ├── generation.py
│ ├── models
│ │ ├── __init__.py
│ │ └── decoder.py
│ └── training.py
├── training.py
├── utils
│ ├── __init__.py
│ ├── binvox_rw.py
│ ├── icp.py
│ ├── io.py
│ ├── libkdtree
│ │ ├── .gitignore
│ │ ├── LICENSE.txt
│ │ ├── MANIFEST.in
│ │ ├── README
│ │ ├── README.rst
│ │ ├── __init__.py
│ │ ├── pykdtree
│ │ │ ├── __init__.py
│ │ │ ├── _kdtree_core.c
│ │ │ ├── _kdtree_core.c.mako
│ │ │ ├── kdtree.c
│ │ │ ├── kdtree.pyx
│ │ │ ├── render_template.py
│ │ │ └── test_tree.py
│ │ └── setup.cfg
│ ├── libmcubes
│ │ ├── .gitignore
│ │ ├── LICENSE
│ │ ├── README.rst
│ │ ├── __init__.py
│ │ ├── exporter.py
│ │ ├── marchingcubes.cpp
│ │ ├── marchingcubes.h
│ │ ├── mcubes.pyx
│ │ ├── pyarray_symbol.h
│ │ ├── pyarraymodule.h
│ │ ├── pywrapper.cpp
│ │ └── pywrapper.h
│ ├── libmesh
│ │ ├── .gitignore
│ │ ├── __init__.py
│ │ ├── inside_mesh.py
│ │ └── triangle_hash.pyx
│ ├── libmise
│ │ ├── .gitignore
│ │ ├── __init__.py
│ │ ├── mise.pyx
│ │ └── test.py
│ ├── libsimplify
│ │ ├── Simplify.h
│ │ ├── __init__.py
│ │ ├── simplify_mesh.pyx
│ │ └── test.py
│ ├── libvoxelize
│ │ ├── .gitignore
│ │ ├── __init__.py
│ │ ├── tribox2.h
│ │ └── voxelize.pyx
│ ├── mesh.py
│ ├── visualize.py
│ └── voxels.py
└── vnn_onet
│ ├── __init__.py
│ ├── config.py
│ ├── generation.py
│ ├── models
│ ├── __init__.py
│ ├── decoder.py
│ ├── decoder_inner.py
│ ├── encoder_latent.py
│ └── legacy.py
│ └── training.py
├── network
├── criterion.py
├── di_decoder.py
├── di_encoder.py
├── di_vnn_encoder.py
├── utility.py
└── vnn.py
├── network_trainer.py
├── pose_fmt.py
├── pose_fmt_kitti.py
├── sampler_cuda
├── CMakeLists.txt
├── PreprocessMesh.cu
├── ShaderProgram.cpp
├── Utils.cu
└── Utils.h
├── system
├── ext
│ ├── __init__.py
│ ├── imgproc
│ │ ├── common.cuh
│ │ ├── imgproc.cpp
│ │ ├── imgproc.cu
│ │ └── photometric.cu
│ ├── indexing
│ │ ├── indexing.cpp
│ │ └── indexing.cu
│ ├── marching_cubes
│ │ ├── mc.cpp
│ │ ├── mc_data.cuh
│ │ └── mc_interp_kernel.cu
│ └── pcproc
│ │ ├── cuda_kdtree.cu
│ │ ├── cuda_kdtree.cuh
│ │ ├── cutil_math.h
│ │ ├── pcproc.cpp
│ │ └── pcproc.cu
├── map.py
├── map_dictindexer.py
├── map_outer_J.py
└── tracker.py
├── trainer
├── lr_schedule.py
└── main.py
├── transform
├── transform.py
└── utils.py
├── treasure
├── encoder_600.pth.tar
├── hyper.json
└── model_600.pth.tar
└── utils
├── coding.py
├── draw.py
├── exp_util.py
├── gradient.py
├── index.py
├── motion_util.py
├── pt_util.py
└── vis_util.py
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2022 Yijun Yuan
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/assets/pipeline_neuralImplicit.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Jarrome/IMT_Mapping/974547684a4474be4c281a9380d4a15150f4bf48/assets/pipeline_neuralImplicit.png
--------------------------------------------------------------------------------
/assets/twobranch.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Jarrome/IMT_Mapping/974547684a4474be4c281a9380d4a15150f4bf48/assets/twobranch.png
--------------------------------------------------------------------------------
/configs/README.md:
--------------------------------------------------------------------------------
1 | ## How to configure
2 |
3 | For example, ```./config/ifr-fusion-lr-kt0.yaml``` contains several settings:
4 |
5 | ```
6 | dataset_type: "TUM" # for ICL-NUIM 's TUM format
7 | scene: "0n" # scene 0 with noise
8 | use_gt: False # not use groundtruth trajectory
9 | pose_folder: "./treasure/orbslam2_record/lrkt0n/" # the predicted pose stream from orbslam2
10 | outdir: "./res/lrkt0n_ours/" # where we stores the intermediary mesh
11 | calib: [481.2, 480.0, 319.50, 239.50, 5000.0] # calibration of images
12 |
13 | sequence_kwargs: # this is called with within sequence namespace
14 | path: "./data/ICL_NUIM/lr_kt0n/"
15 | start_frame: 0
16 | end_frame: -1 # Run all frames
17 | first_tq: [-1.4, 1.5, 1.5, 0.0, -1.0, 0.0, 0.0] # Starting pose
18 |
19 | # Network parameters
20 | training_hypers: "./treasure/hyper.json"
21 | using_epoch: 600
22 |
23 | # Enable visualization
24 | vis: True
25 | resolution: 4
26 |
27 | # meshing
28 | max_n_triangles: 4e6
29 | max_std: 0.15 # 0.06
30 |
31 | # These two define the range of depth observations to be cropped. Unit is meter.
32 | depth_cut_min: 0.5
33 | depth_cut_max: 5.0
34 |
35 | # not exactly used in code, please follow real implementation
36 | meshing_interval: 20
37 | integrate_interval: 20
38 |
39 | # Mapping parameters
40 | mapping:
41 | # Bound of the scene to be reconstructed
42 | bound_min: [-3.5, -0.5, -2.5]
43 | bound_max: [4.5, 3.5, 5.5]
44 | voxel_size: 0.1
45 | # Prune observations if detected as noise.
46 | prune_min_vox_obs: 16
47 | ignore_count_th: 16.0
48 | encoder_count_th: 600.0
49 | ```
50 |
--------------------------------------------------------------------------------
/configs/ifr-fusion-lr-kt0.yaml:
--------------------------------------------------------------------------------
1 | # Sequence parameters
2 | sequence_type: "icl_nuim.ICLNUIMSequence"
3 |
4 | dataset_type: "TUM"
5 | scene: "0n"
6 | use_gt: False
7 | pose_folder: "./treasure/orbslam2_record/lrkt0n/"
8 | outdir: "./res/lrkt0n_ours/"
9 |
10 | calib: [481.2, 480.0, 319.50, 239.50, 5000.0]
11 |
12 | sequence_kwargs:
13 | path: "./data/ICL_NUIM/lr_kt0n/"
14 | start_frame: 0
15 | end_frame: -1 # Run all frames
16 | first_tq: [-1.4, 1.5, 1.5, 0.0, -1.0, 0.0, 0.0] # Starting pose
17 |
18 | # Network parameters (network structure, etc. will be inherited from the training config)
19 | training_hypers: "./treasure/hyper.json"
20 | using_epoch: 600
21 |
22 | # Separate tracking and meshing.
23 | run_async: false
24 | # Enable visualization
25 | vis: True
26 | resolution: 4
27 |
28 | # meshing
29 | max_n_triangles: 4e6
30 | max_std: 0.15 # 0.06
31 |
32 |
33 | # These two define the range of depth observations to be cropped. Unit is meter.
34 | depth_cut_min: 0.5
35 | depth_cut_max: 5.0
36 |
37 | meshing_interval: 20
38 | integrate_interval: 20
39 |
40 | # Mapping parameters
41 | mapping:
42 | # Bound of the scene to be reconstructed
43 | bound_min: [-3.5, -0.5, -2.5]
44 | bound_max: [4.5, 3.5, 5.5]
45 | voxel_size: 0.1
46 | # Prune observations if detected as noise.
47 | prune_min_vox_obs: 16
48 | ignore_count_th: 16.0
49 | encoder_count_th: 600.0
50 |
51 | # Tracking parameters
52 | tracking:
53 | # An array defining how the camera pose is optimized.
54 | # Each element is a dictionary:
55 | # For example {"n": 2, "type": [['sdf'], ['rgb', 1]]} means to optimize the summation of sdf term and rgb term
56 | # at the 1st level pyramid for 2 iterations.
57 | iter_config:
58 | - {"n": 10, "type": [['rgb', 2]]}
59 | - {"n": 10, "type": [['sdf'], ['rgb', 1]]}
60 | - {"n": 50, "type": [['sdf'], ['rgb', 0]]}
61 | sdf:
62 | robust_kernel: "huber"
63 | robust_k: 5.0
64 | subsample: 0.5
65 | rgb:
66 | weight: 500.0
67 | robust_kernel: null
68 | robust_k: 0.01
69 | min_grad_scale: 0.0
70 | max_depth_delta: 0.2
71 |
--------------------------------------------------------------------------------
/configs/ifr-fusion-lr-kt1.yaml:
--------------------------------------------------------------------------------
1 | # Sequence parameters
2 | sequence_type: "icl_nuim.ICLNUIMSequence"
3 |
4 | dataset_type: "TUM"
5 | scene: "1n"
6 | use_gt: False
7 | pose_folder: "./treasure/orbslam2_record/lrkt1n/"
8 | outdir: "./res/lrkt1n_ours/"
9 |
10 | calib: [481.2, 480.0, 319.50, 239.50, 5000.0]
11 |
12 | sequence_kwargs:
13 | path: "./data/ICL_NUIM/lr_kt1n/"
14 | start_frame: 0
15 | end_frame: -1
16 | first_tq: [-0.1, 1.3, 0.1, 0.0, -1.0, 0.0, 0.0]
17 |
18 | # Network parameters (network structure, etc. will be inherited from the training config)
19 | training_hypers: "./treasure/hyper.json"
20 | using_epoch: 600
21 |
22 | # Separate tracking and meshing.
23 | run_async: false
24 | # Enable visualization
25 | vis: True
26 | resolution: 4
27 |
28 |
29 | # meshing
30 | max_n_triangles: 4e6
31 | max_std: 0.15 # 0.06
32 |
33 |
34 | depth_cut_min: 0.5
35 | depth_cut_max: 3.0
36 |
37 | meshing_interval: 20
38 | integrate_interval: 20
39 |
40 | # Mapping parameters (See README.md for details)
41 | mapping:
42 | bound_min: [-3.5, -0.5, -2.5]
43 | bound_max: [4.5, 3.5, 5.5]
44 | voxel_size: 0.1
45 | prune_min_vox_obs: 16
46 | ignore_count_th: 16.0
47 | encoder_count_th: 600.0
48 |
49 | # Tracking parameters (See README.md for details)
50 | tracking:
51 | iter_config:
52 | - {"n": 10, "type": [['rgb', 2]]}
53 | - {"n": 10, "type": [['sdf'], ['rgb', 1]]}
54 | - {"n": 50, "type": [['sdf'], ['rgb', 0]]}
55 | sdf:
56 | robust_kernel: "huber"
57 | robust_k: 5.0
58 | subsample: 0.5
59 | rgb:
60 | weight: 500.0
61 | robust_kernel: null
62 | robust_k: 0.01
63 | min_grad_scale: 0.0
64 | max_depth_delta: 0.2
65 |
66 |
--------------------------------------------------------------------------------
/configs/ifr-fusion-lr-kt2.yaml:
--------------------------------------------------------------------------------
1 | # Sequence parameters
2 | sequence_type: "icl_nuim.ICLNUIMSequence"
3 |
4 | dataset_type: "TUM"
5 | scene: "2n"
6 | use_gt: False
7 | pose_folder: "./treasure/orbslam2_record/lrkt2n/"
8 | outdir: "./res/lrkt2n_ours/"
9 |
10 | calib: [481.2, 480.0, 319.50, 239.50, 5000.0]
11 |
12 |
13 | sequence_kwargs:
14 | path: "./data/ICL_NUIM/lr_kt2n/"
15 | start_frame: 0
16 | end_frame: -1 # Run all frames
17 | first_tq: [-0.7, 1.3, 1.5, 0.0, -1.0, 0.0, 0.0] # Starting pose
18 |
19 | # Network parameters (network structure, etc. will be inherited from the training config)
20 | training_hypers: "./treasure/hyper.json"
21 | using_epoch: 600
22 |
23 | # Separate tracking and meshing.
24 | run_async: false
25 | # Enable visualization
26 | vis: True
27 | resolution: 4
28 |
29 | # meshing
30 | max_n_triangles: 4e6
31 | max_std: 0.15 # 0.06
32 |
33 |
34 | # These two define the range of depth observations to be cropped. Unit is meter.
35 | depth_cut_min: 0.5
36 | depth_cut_max: 3.0
37 |
38 | meshing_interval: 20
39 | integrate_interval: 20
40 |
41 | # Mapping parameters
42 | mapping:
43 | # Bound of the scene to be reconstructed
44 | bound_min: [-3.5, -0.5, -2.5]
45 | bound_max: [4.5, 3.5, 5.5]
46 | voxel_size: 0.1
47 | # Prune observations if detected as noise.
48 | prune_min_vox_obs: 16
49 | ignore_count_th: 16.0
50 | encoder_count_th: 600.0
51 |
52 | # Tracking parameters
53 | tracking:
54 | # An array defining how the camera pose is optimized.
55 | # Each element is a dictionary:
56 | # For example {"n": 2, "type": [['sdf'], ['rgb', 1]]} means to optimize the summation of sdf term and rgb term
57 | # at the 1st level pyramid for 2 iterations.
58 | iter_config:
59 | - {"n": 10, "type": [['sdf'], ['rgb', 1]]}
60 | - {"n": 50, "type": [['sdf'], ['rgb', 0]]}
61 | sdf:
62 | robust_kernel: "huber"
63 | robust_k: 5.0
64 | subsample: 0.5
65 | rgb:
66 | weight: 50.0
67 | robust_kernel: null
68 | robust_k: 0.01
69 | min_grad_scale: 0.0
70 | max_depth_delta: 0.2
71 |
--------------------------------------------------------------------------------
/configs/ifr-fusion-lr-kt3.yaml:
--------------------------------------------------------------------------------
1 | # Sequence parameters
2 | sequence_type: "icl_nuim.ICLNUIMSequence"
3 |
4 | dataset_type: "TUM"
5 | scene: "3n"
6 | use_gt: False
7 | pose_folder: "./treasure/orbslam2_record/lrkt3n/"
8 | outdir: "./res/lrkt3n_ours/"
9 |
10 | calib: [481.2, 480.0, 319.50, 239.50, 5000.0]
11 |
12 | sequence_kwargs:
13 | path: "./data/ICL_NUIM/lr_kt3n/"
14 | start_frame: 0
15 | end_frame: -1 # Run all frames
16 | first_tq: [-1.2, 1.3, 1.0, 0.0, -1.0, 0.0, 0.0] # Starting pose
17 |
18 | # Network parameters (network structure, etc. will be inherited from the training config)
19 | training_hypers: "./treasure/hyper.json"
20 | using_epoch: 600
21 |
22 | # Separate tracking and meshing.
23 | run_async: false
24 | # Enable visualization
25 | vis: True
26 | resolution: 4
27 |
28 | # meshing
29 | max_n_triangles: 4e6
30 | max_std: 0.15 # 0.06
31 |
32 |
33 | # These two define the range of depth observations to be cropped. Unit is meter.
34 | depth_cut_min: 0.5
35 | depth_cut_max: 5.0
36 |
37 | meshing_interval: 20
38 | integrate_interval: 20
39 |
40 | # Mapping parameters
41 | mapping:
42 | # Bound of the scene to be reconstructed
43 | bound_min: [-3.5, -0.5, -2.5]
44 | bound_max: [4.5, 3.5, 5.5]
45 | voxel_size: 0.1
46 | # Prune observations if detected as noise.
47 | prune_min_vox_obs: 16
48 | ignore_count_th: 16.0
49 | encoder_count_th: 600.0
50 |
51 | # Tracking parameters
52 | tracking:
53 | # An array defining how the camera pose is optimized.
54 | # Each element is a dictionary:
55 | # For example {"n": 2, "type": [['sdf'], ['rgb', 1]]} means to optimize the summation of sdf term and rgb term
56 | # at the 1st level pyramid for 2 iterations.
57 | iter_config:
58 | - {"n": 10, "type": [['sdf'], ['rgb', 1]]}
59 | - {"n": 50, "type": [['sdf'], ['rgb', 0]]}
60 | sdf:
61 | robust_kernel: "huber"
62 | robust_k: 5.0
63 | subsample: 0.5
64 | rgb:
65 | weight: 50.0
66 | robust_kernel: null
67 | robust_k: 0.01
68 | min_grad_scale: 0.0
69 | max_depth_delta: 0.2
70 |
--------------------------------------------------------------------------------
/configs/ifr-fusion-replica.yaml:
--------------------------------------------------------------------------------
1 | # Sequence parameters
2 | sequence_type: "icl_nuim.ICLNUIMSequence"
3 |
4 | dataset_type: "replica"
5 | scene: "office0"
6 | use_gt: False
7 | pose_folder: "./treasure/orbslam2_record/imap_office0/"
8 | outdir: "./res/replica_office0_ours/"
9 |
10 | calib: [600., 600., 599.5, 339.5, 6553.5]
11 |
12 | sequence_kwargs:
13 | path: "./data/replica/office0"
14 | start_frame: 0
15 | end_frame: -1 # Run all frames
16 | #first_tq: [-1.2, 1.3, 1.0, 0.0, -1.0, 0.0, 0.0] # Starting pose
17 | first_tq: [0, 0, 0.0, 0.0, -1.0, 0.0, 0.0]
18 |
19 | # Network parameters (network structure, etc. will be inherited from the training config)
20 | training_hypers: "./treasure/hyper.json"
21 | using_epoch: 600
22 |
23 | # Separate tracking and meshing.
24 | run_async: false
25 | # Enable visualization
26 | vis: false
27 | resolution: 3
28 |
29 | # These two define the range of depth observations to be cropped. Unit is meter.
30 | depth_cut_min: 0.5
31 | depth_cut_max: 5.0
32 |
33 | meshing_interval: 20
34 | integrate_interval: 20
35 |
36 | # Mapping parameters
37 | mapping:
38 | # Bound of the scene to be reconstructed
39 | bound_min: [-10.5, -5.5, -10.]
40 | bound_max: [10.5, 5.5, 10.5]
41 |
42 | voxel_size: 0.1
43 | # Prune observations if detected as noise.
44 | prune_min_vox_obs: 16
45 | ignore_count_th: 16.0
46 | encoder_count_th: 600.0
47 |
48 | # Tracking parameters
49 | tracking:
50 | # An array defining how the camera pose is optimized.
51 | # Each element is a dictionary:
52 | # For example {"n": 2, "type": [['sdf'], ['rgb', 1]]} means to optimize the summation of sdf term and rgb term
53 | # at the 1st level pyramid for 2 iterations.
54 | iter_config:
55 | - {"n": 10, "type": [['sdf'], ['rgb', 1]]}
56 | - {"n": 50, "type": [['sdf'], ['rgb', 0]]}
57 | sdf:
58 | robust_kernel: "huber"
59 | robust_k: 5.0
60 | subsample: 0.5
61 | rgb:
62 | weight: 50.0
63 | robust_kernel: null
64 | robust_k: 0.01
65 | min_grad_scale: 0.0
66 | max_depth_delta: 0.2
67 |
--------------------------------------------------------------------------------
/configs/test_kitti.yaml:
--------------------------------------------------------------------------------
1 | # data
2 | basedir: "./data/kitti_odometry/dataset/sequences"
3 | sequence: 0
4 |
5 | # Network parameters (network structure, etc. will be inherited from the training config)
6 | training_hypers: "./treasure/hyper.json"
7 | using_epoch: 600
8 |
9 | # Separate tracking and meshing.
10 | run_async: false
11 | # Enable visualization
12 | vis: false
13 | resolution: 4
14 |
15 | # These two define the range of depth observations to be cropped. Unit is meter.
16 | depth_cut_min: 0.5
17 | depth_cut_max: 5.0
18 |
19 | meshing_interval: 20
20 | integrate_interval: 5
21 |
22 | # Mapping parameters
23 | mapping:
24 | # Bound of the scene to be reconstructed
25 | bound_min: [-200., -800., -100.]
26 | bound_max: [800., 800., 200.]
27 | voxel_size: 4.
28 | # Prune observations if detected as noise.
29 | prune_min_vox_obs: 0
30 | ignore_count_th: 100
31 | encoder_count_th: 600.0
32 |
33 | vis: False
34 |
35 |
36 | # Tracking parameters
37 | tracking:
38 | # An array defining how the camera pose is optimized.
39 | # Each element is a dictionary:
40 | # For example {"n": 2, "type": [['sdf'], ['rgb', 1]]} means to optimize the summation of sdf term and rgb term
41 | # at the 1st level pyramid for 2 iterations.
42 | iter_config:
43 | - {"n": 10, "type": [['sdf']]}
44 | - {"n": 50, "type": [['sdf']]}
45 | sdf:
46 | robust_kernel: "huber"
47 | robust_k: 5.0
48 | subsample: 0.5
49 |
--------------------------------------------------------------------------------
/configs/train.yaml:
--------------------------------------------------------------------------------
1 | run_name: "default"
2 |
3 | num_epochs: 600
4 | batch_size: 64
5 | batch_split: 1
6 | samples_per_lif: 4096
7 | min_context_points: 16
8 |
9 | lr_schedule:
10 | # For decoder parameters
11 | - { "Type" : "Step", "Initial" : 0.001, "Interval" : 80, "Factor" : 0.4 }
12 | # For encoder parameters
13 | - { "Type" : "Step", "Initial" : 0.001, "Interval" : 80, "Factor" : 0.4 }
14 |
15 | # Dataset.
16 | train_set:
17 | - { "data_path": "../di-datasets/shapenet_plivoxs", "augment_rotation": 'Y', "num_surface_sample": 128, "augment_noise": [0.025, 40.0] }
18 |
19 | # Code specification
20 | code_bound: null
21 | code_length: 9
22 |
23 | # Decoder specification
24 | network_name: "di_decoder"
25 | network_specs:
26 | dims: [ 128, 128, 128, 128 ]
27 | dropout: [0, 1, 2, 3, 4, 5]
28 | dropout_prob: 0.2
29 | norm_layers: [0, 1, 2, 3, 4, 5]
30 | latent_in: [3]
31 | weight_norm: true
32 |
33 | # Encoder specification
34 | encoder_name: "di_vnn_encoder"
35 | encoder_specs:
36 | per_point_feat: [ 6, 32, 64, 256 ]
37 | bn: {"class": "BatchNorm"}
38 |
39 | # Snapshots saving parameters
40 | snapshot_frequency: 100
41 | additional_snapshots: [50]
42 |
43 | # SDF samples
44 | training_loss:
45 | types: [ "neg_log_likelihood", "reg_loss" ]
46 | enforce_minmax: true
47 | clamping_distance: 0.2
48 | code_reg_lambda: 1.0e-2
49 |
--------------------------------------------------------------------------------
/data_generator.py:
--------------------------------------------------------------------------------
1 | dataset/training/main_generator.py
--------------------------------------------------------------------------------
/dataset/production/__init__.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 |
3 |
4 | class FrameIntrinsic:
5 | def __init__(self, fx, fy, cx, cy, dscale):
6 | self.cx = cx
7 | self.cy = cy
8 | self.fx = fx
9 | self.fy = fy
10 | self.dscale = dscale
11 |
12 | def to_K(self):
13 | return np.asarray([
14 | [self.fx, 0.0, self.cx],
15 | [0.0, self.fy, self.cy],
16 | [0.0, 0.0, 1.0]
17 | ])
18 |
19 |
20 | class FrameData:
21 | def __init__(self):
22 | self.rgb = None
23 | self.depth = None
24 | self.gt_pose = None
25 | self.calib = None
26 |
27 |
28 | class RGBDSequence:
29 | def __init__(self):
30 | self.frame_id = 0
31 |
32 | def __iter__(self):
33 | return self
34 |
35 | def __len__(self):
36 | raise NotImplementedError
37 |
38 | def __next__(self):
39 | raise NotImplementedError
40 |
--------------------------------------------------------------------------------
/dataset/production/icl_nuim.py:
--------------------------------------------------------------------------------
1 | import cv2
2 | import os
3 | import torch
4 | from dataset.production import *
5 | from pyquaternion import Quaternion
6 | from pathlib import Path
7 | from utils import motion_util
8 | import pdb
9 |
10 |
11 | class ICLNUIMSequence(RGBDSequence):
12 | def __init__(self, path: str, start_frame: int = 0, end_frame: int = -1, first_tq: list = None, load_gt: bool = False):
13 | super().__init__()
14 | self.path = Path(path)
15 | self.color_names = sorted([f"rgb/{t}" for t in os.listdir(self.path / "rgb")], key=lambda t: int(t[4:].split(".")[0]))
16 | self.depth_names = [f"depth/{t}.png" for t in range(len(self.color_names))]
17 | self.calib = [481.2, 480.0, 319.50, 239.50, 5000.0]
18 | #self.calib = [525.0, 525.0, 319.5, 239.5, 5000]
19 | if first_tq is not None:
20 | self.first_iso = motion_util.Isometry(q=Quaternion(array=first_tq[3:]), t=np.array(first_tq[:3]))
21 | else:
22 | self.first_iso = motion_util.Isometry(q=Quaternion(array=[0.0, -1.0, 0.0, 0.0]))
23 |
24 | if end_frame == -1:
25 | end_frame = len(self.color_names)
26 |
27 | self.color_names = self.color_names[start_frame:end_frame]
28 | self.depth_names = self.depth_names[start_frame:end_frame]
29 |
30 | if load_gt:
31 | gt_traj_path = (list(self.path.glob("*.freiburg")) + list(self.path.glob("groundtruth.txt")))[0]
32 | self.gt_trajectory = self._parse_traj_file(gt_traj_path)
33 | self.gt_trajectory = self.gt_trajectory[start_frame:end_frame]
34 | change_iso = self.first_iso.dot(self.gt_trajectory[0].inv())
35 | self.gt_trajectory = [change_iso.dot(t) for t in self.gt_trajectory]
36 | assert len(self.gt_trajectory) == len(self.color_names)
37 | else:
38 | self.gt_trajectory = None
39 |
40 | def _parse_traj_file(self, traj_path):
41 | camera_ext = {}
42 | traj_data = np.genfromtxt(traj_path)
43 | cano_quat = motion_util.Isometry(q=Quaternion(axis=[0.0, 0.0, 1.0], degrees=180.0))
44 | for cur_p in traj_data:
45 | cur_q = Quaternion(imaginary=cur_p[4:7], real=cur_p[-1]).rotation_matrix
46 | cur_t = cur_p[1:4]
47 | cur_q[1] = -cur_q[1]
48 | cur_q[:, 1] = -cur_q[:, 1]
49 | cur_t[1] = -cur_t[1]
50 | cur_iso = motion_util.Isometry(q=Quaternion(matrix=cur_q), t=cur_t)
51 | camera_ext[cur_p[0]] = cano_quat.dot(cur_iso)
52 | camera_ext[0] = camera_ext[1]
53 | return [camera_ext[t] for t in range(len(camera_ext))]
54 |
55 | def __len__(self):
56 | return len(self.color_names)
57 |
58 | def __next__(self):
59 | if self.frame_id >= len(self):
60 | raise StopIteration
61 |
62 | depth_img_path = self.path / self.depth_names[self.frame_id]
63 | rgb_img_path = self.path / self.color_names[self.frame_id]
64 |
65 | # Convert depth image into point cloud.
66 | frame_data = FrameData()
67 | depth_data = cv2.imread(str(depth_img_path), cv2.IMREAD_UNCHANGED)
68 | frame_data.depth_cv2 = depth_data
69 | depth_data = torch.from_numpy(depth_data.astype(np.float32)).cuda() / self.calib[4]
70 |
71 | rgb_data = cv2.imread(str(rgb_img_path))
72 | rgb_data = cv2.cvtColor(rgb_data, cv2.COLOR_BGR2RGB)
73 | frame_data.rgb_cv2 = rgb_data
74 | rgb_data = torch.from_numpy(rgb_data).cuda().float() / 255.
75 |
76 | frame_data.gt_pose = self.gt_trajectory[self.frame_id] if self.gt_trajectory is not None else None
77 | frame_data.calib = FrameIntrinsic(self.calib[0], self.calib[1], self.calib[2], self.calib[3], self.calib[4])
78 | frame_data.depth = depth_data
79 | frame_data.rgb = rgb_data
80 |
81 | self.frame_id += 1
82 | return frame_data
83 |
--------------------------------------------------------------------------------
/dataset/training/shapenet_model.py:
--------------------------------------------------------------------------------
1 | import os
2 | import math
3 | import json
4 | import random
5 | import logging
6 | import numpy as np
7 | from utils import motion_util
8 | from pathlib import Path
9 |
10 |
11 | class ShapeNetGenerator:
12 | """
13 | Use ShapeNet core to generate data.
14 | """
15 | VALID_LIST_PATH = Path(__file__).parent / "shapenet_valid_list.json"
16 |
17 | def __init__(self, shapenet_path, categories, shapes_per_category, scale):
18 | self.categories = categories
19 | self.shapes_per_category = shapes_per_category
20 | self.scale = scale
21 |
22 | # Sample objects
23 | self.data_sources = []
24 | self.data_scales = []
25 | with self.VALID_LIST_PATH.open("r") as f:
26 | valid_list_data = json.load(f)
27 |
28 | for category_name, category_shape_count, category_scale in zip(self.categories, self.shapes_per_category, self.scale):
29 | category_path = Path(shapenet_path) / category_name
30 | if category_name in valid_list_data["ShapeNetV2"].keys():
31 | logging.info(f"Category {category_name} is found in plist file")
32 | sampled_objects = valid_list_data["ShapeNetV2"][category_name]
33 | else:
34 | logging.info(f"Category {category_name} is not found in plist file")
35 | sampled_objects = os.listdir(category_path)
36 | if category_shape_count != -1:
37 | sampled_objects = random.sample(sampled_objects, category_shape_count)
38 | self.data_sources += [category_path / s for s in sampled_objects]
39 | self.data_scales += [category_scale for _ in sampled_objects]
40 |
41 | def __len__(self):
42 | return len(self.data_sources)
43 |
44 | @staticmethod
45 | def _equidist_point_on_sphere(samples):
46 | points = []
47 | phi = math.pi * (3. - math.sqrt(5.))
48 |
49 | for i in range(samples):
50 | y = 1 - (i / float(samples - 1)) * 2
51 | radius = math.sqrt(1 - y * y)
52 | theta = phi * i
53 |
54 | x = math.cos(theta) * radius
55 | z = math.sin(theta) * radius
56 | points.append((x, y, z))
57 |
58 | return np.asarray(points)
59 |
60 | def get_source(self, data_id):
61 | return str(self.data_sources[data_id])
62 |
63 | def __getitem__(self, idx):
64 | data_source = self.data_sources[idx]
65 | data_scale = self.data_scales[idx]
66 | obj_path = data_source / "models" / "model_normalized.obj"
67 |
68 | vp_camera = self._equidist_point_on_sphere(300)
69 | camera_ext = []
70 | for camera_i in range(vp_camera.shape[0]):
71 | iso = motion_util.Isometry.look_at(vp_camera[camera_i], np.zeros(3,))
72 | camera_ext.append(iso)
73 | camera_int = [0.8, 0.0, 2.5] # (window-size-half, z-min, z-max) under ortho-proj.
74 |
75 | return str(obj_path), [camera_int, camera_ext], None, data_scale
76 |
77 | def clean(self, data_id):
78 | pass
79 |
--------------------------------------------------------------------------------
/exp_transform.py:
--------------------------------------------------------------------------------
1 | import os
2 | import numpy as np
3 |
4 | import time
5 | from itertools import chain
6 | from collections import defaultdict
7 | from scipy.spatial.transform import Rotation as R
8 |
9 | import ifr.ifr_exp_transform as ifr
10 | from ifr.ifr_exp_transform import vis_param
11 |
12 | import glob
13 | import pdb
14 |
15 |
16 |
17 | from pyquaternion import Quaternion
18 |
19 | # namespace
20 | args = vis_param.args
21 | sequence = vis_param.sequence
22 | # scene name
23 | scene = args.scene
24 | # where we store the incremental result for demonstration
25 | args.outdir_transform = args.outdir[:-1]+'_transform'
26 | os.makedirs(args.outdir_transform,exist_ok=True)
27 |
28 |
29 |
30 | if __name__ == '__main__':
31 | import os
32 | import sys
33 |
34 | from dataset_ptam import TUMRGBDDataset, ICLNUIMDataset
35 |
36 |
37 | args.dataset = args.dataset_type
38 |
39 | if 'tum' in args.dataset.lower():
40 | dataset = TUMRGBDDataset(sequence.path)
41 | else:
42 | assert "Not supported data type"
43 |
44 | '''
45 | load gt traj to check correctness
46 | '''
47 | GT = True
48 | if GT:
49 | gt_traj = np.genfromtxt(str(sequence.path)+'/livingRoom'+scene+'.gt.freiburg')
50 | gt_poses = []
51 |
52 |
53 |
54 | durations = []
55 | data_i = 0
56 | #for i in range(len(dataset))[:]:
57 | kf_idx = []
58 | def run_algo(vis):
59 | global data_i
60 | i = data_i#data_next()
61 | data_i += 1
62 |
63 |
64 | if i % 20 == 0:#
65 | is_keyframe = True
66 | kf_idx.append(i)
67 | else:
68 | is_keyframe = False
69 | if dataset.timestamps is None:
70 | timestamp = i / 20.
71 | else:
72 | timestamp = dataset.timestamps[i]
73 |
74 | time_start = time.time()
75 |
76 | # 0. check if current keyframe
77 | if is_keyframe:
78 | #gt_pose = g2o.Isometry3d(g2o.Quaternion(gt_traj[i,-1],gt_traj[i,4],gt_traj[i,5],gt_traj[i,6]), gt_traj[i,1:4])
79 | gt_pose = gt_traj[i,:] if GT else None
80 | # 1. prepare current frame to get torch frame_data
81 | frame_data = (dataset.rgb[i],dataset.depth[i])
82 |
83 | # 2. get all the poses of keyframe
84 | new_poses = []
85 | if not GT:
86 | assert(False)
87 | poses = read_elasticfusion_file(i, kf_idx)
88 | new_poses= poses
89 | else:
90 | gt_poses.append(gt_pose)
91 | new_poses = gt_poses
92 |
93 | # 3.2 if some pose changed, update map
94 | ifr.refresh(frame_data, new_poses, frame_id = i, vis=vis, ptam_p = not GT)
95 | else:
96 | return
97 |
98 |
99 | duration = time.time() - time_start
100 | durations.append(duration)
101 | print('duration', duration)
102 | print()
103 | print()
104 |
105 | if ifr.engine:
106 | ifr.engine.register_animation_callback(callback_func = run_algo)
107 | vis_ph = ifr.vis_util.wireframe_bbox([-4., -4., -4.], [4., 4., 4.])
108 | ifr.engine.add_geometry(vis_ph)
109 | ifr.engine.remove_geometry(vis_ph, reset_bounding_box=False)
110 | ifr.engine.run()
111 | ifr.engine.destroy_window()
112 | else:
113 | try:
114 | while True:
115 | run_algo(None)
116 | except Exception as e:
117 | print(e)
118 |
119 |
120 |
--------------------------------------------------------------------------------
/im2mesh/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Jarrome/IMT_Mapping/974547684a4474be4c281a9380d4a15150f4bf48/im2mesh/__init__.py
--------------------------------------------------------------------------------
/im2mesh/checkpoints.py:
--------------------------------------------------------------------------------
1 | import os
2 | import urllib
3 | import torch
4 | from torch.utils import model_zoo
5 |
6 |
7 | class CheckpointIO(object):
8 | ''' CheckpointIO class.
9 |
10 | It handles saving and loading checkpoints.
11 |
12 | Args:
13 | checkpoint_dir (str): path where checkpoints are saved
14 | '''
15 | def __init__(self, checkpoint_dir='./chkpts', **kwargs):
16 | self.module_dict = kwargs
17 | self.checkpoint_dir = checkpoint_dir
18 | if not os.path.exists(checkpoint_dir):
19 | os.makedirs(checkpoint_dir)
20 |
21 | def register_modules(self, **kwargs):
22 | ''' Registers modules in current module dictionary.
23 | '''
24 | self.module_dict.update(kwargs)
25 |
26 | def save(self, filename, **kwargs):
27 | ''' Saves the current module dictionary.
28 |
29 | Args:
30 | filename (str): name of output file
31 | '''
32 | if not os.path.isabs(filename):
33 | filename = os.path.join(self.checkpoint_dir, filename)
34 |
35 | outdict = kwargs
36 | for k, v in self.module_dict.items():
37 | outdict[k] = v.state_dict()
38 | torch.save(outdict, filename)
39 |
40 | def load(self, filename):
41 | '''Loads a module dictionary from local file or url.
42 |
43 | Args:
44 | filename (str): name of saved module dictionary
45 | '''
46 | if is_url(filename):
47 | return self.load_url(filename)
48 | else:
49 | return self.load_file(filename)
50 |
51 | def load_file(self, filename):
52 | '''Loads a module dictionary from file.
53 |
54 | Args:
55 | filename (str): name of saved module dictionary
56 | '''
57 |
58 | if not os.path.isabs(filename):
59 | filename = os.path.join(self.checkpoint_dir, filename)
60 |
61 | if os.path.exists(filename):
62 | print(filename)
63 | print('=> Loading checkpoint from local file...')
64 | state_dict = torch.load(filename)
65 | scalars = self.parse_state_dict(state_dict)
66 | return scalars
67 | else:
68 | raise FileExistsError
69 |
70 | def load_url(self, url):
71 | '''Load a module dictionary from url.
72 |
73 | Args:
74 | url (str): url to saved model
75 | '''
76 | print(url)
77 | print('=> Loading checkpoint from url...')
78 | state_dict = model_zoo.load_url(url, progress=True)
79 | scalars = self.parse_state_dict(state_dict)
80 | return scalars
81 |
82 | def parse_state_dict(self, state_dict):
83 | '''Parse state_dict of model and return scalars.
84 |
85 | Args:
86 | state_dict (dict): State dict of model
87 | '''
88 |
89 | for k, v in self.module_dict.items():
90 | if k in state_dict:
91 | v.load_state_dict(state_dict[k])
92 | else:
93 | print('Warning: Could not find %s in checkpoint!' % k)
94 | scalars = {k: v for k, v in state_dict.items()
95 | if k not in self.module_dict}
96 | return scalars
97 |
98 | def is_url(url):
99 | scheme = urllib.parse.urlparse(url).scheme
100 | return scheme in ('http', 'https')
--------------------------------------------------------------------------------
/im2mesh/data/__init__.py:
--------------------------------------------------------------------------------
1 |
2 | from im2mesh.data.core import (
3 | Shapes3dDataset, collate_remove_none, worker_init_fn
4 | )
5 | from im2mesh.data.fields import (
6 | IndexField, CategoryField, ImagesField, PointsField,
7 | VoxelsField, PointCloudField, MeshField,
8 | )
9 | from im2mesh.data.transforms import (
10 | PointcloudNoise, SubsamplePointcloud,
11 | SubsamplePoints
12 | )
13 | from im2mesh.data.real import (
14 | KittiDataset, OnlineProductDataset,
15 | ImageDataset,
16 | )
17 |
18 |
19 | __all__ = [
20 | # Core
21 | Shapes3dDataset,
22 | collate_remove_none,
23 | worker_init_fn,
24 | # Fields
25 | IndexField,
26 | CategoryField,
27 | ImagesField,
28 | PointsField,
29 | VoxelsField,
30 | PointCloudField,
31 | MeshField,
32 | # Transforms
33 | PointcloudNoise,
34 | SubsamplePointcloud,
35 | SubsamplePoints,
36 | # Real Data
37 | KittiDataset,
38 | OnlineProductDataset,
39 | ImageDataset,
40 | ]
41 |
--------------------------------------------------------------------------------
/im2mesh/dmc/__init__.py:
--------------------------------------------------------------------------------
1 | from im2mesh.dmc import (
2 | config, generation, training, models
3 | )
4 |
5 | __all__ = [
6 | config, generation, training, models
7 | ]
8 |
--------------------------------------------------------------------------------
/im2mesh/dmc/config.py:
--------------------------------------------------------------------------------
1 | import os
2 | from im2mesh.dmc import models, training, generation
3 | from im2mesh import data
4 |
5 |
6 | def get_model(cfg, device=None, **kwargs):
7 | encoder = cfg['model']['encoder']
8 | decoder = cfg['model']['decoder']
9 | c_dim = cfg['model']['c_dim']
10 | encoder_kwargs = cfg['model']['encoder_kwargs']
11 | decoder_kwargs = cfg['model']['decoder_kwargs']
12 |
13 | encoder = models.encoder_dict[encoder](
14 | **encoder_kwargs
15 | )
16 |
17 | decoder = models.decoder_dict[decoder](
18 | **decoder_kwargs
19 | )
20 |
21 | model = models.DMC(decoder, encoder)
22 | model = model.to(device)
23 | return model
24 |
25 |
26 | def get_trainer(model, optimizer, cfg, device, **kwargs):
27 | input_type = cfg['data']['input_type']
28 | out_dir = cfg['training']['out_dir']
29 | vis_dir = os.path.join(out_dir, 'vis')
30 | num_voxels = cfg['model']['num_voxels']
31 | weight_prior = cfg['model']['dmc_weight_prior']
32 |
33 | trainer = training.Trainer(
34 | model, optimizer, device=device, input_type=input_type,
35 | vis_dir=vis_dir, num_voxels=num_voxels,
36 | weight_prior=weight_prior,
37 | )
38 | return trainer
39 |
40 |
41 | def get_generator(model, cfg, device, **kwargs):
42 | num_voxels = cfg['model']['num_voxels']
43 |
44 | generator = generation.Generator3D(
45 | model, device=device, num_voxels=num_voxels
46 | )
47 | return generator
48 |
49 |
50 | def get_data_fields(split, cfg, **kwargs):
51 | with_transforms = cfg['data']['with_transforms']
52 | # TODO: put this into config
53 | pointcloud_n = 3000
54 | pointcloud_transform = data.SubsamplePointcloud(pointcloud_n)
55 |
56 | fields = {}
57 | fields['pointcloud'] = data.PointCloudField(
58 | cfg['data']['pointcloud_file'], pointcloud_transform,
59 | with_transforms=with_transforms
60 | )
61 |
62 | return fields
63 |
--------------------------------------------------------------------------------
/im2mesh/dmc/generation.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import numpy as np
3 | import trimesh
4 | from im2mesh.dmc.utils.pred2mesh import pred_to_mesh_max
5 | from im2mesh.dmc.ops.occupancy_to_topology import OccupancyToTopology
6 | from im2mesh.dmc.ops.table import get_accept_topology
7 |
8 |
9 | class Generator3D(object):
10 | def __init__(self, model, device=None, num_voxels=32):
11 | self.model = model.to(device)
12 | self.device = device
13 | self.num_voxels = num_voxels
14 | self.vis_topology = torch.LongTensor(get_accept_topology(4))
15 |
16 | def generate_mesh(self, data):
17 | self.model.eval()
18 | device = self.device
19 |
20 | inputs = data.get('inputs', torch.empty(1, 0)).to(device)
21 |
22 | inputs = self.num_voxels * (inputs / 1.2 + 0.5)
23 |
24 | with torch.no_grad():
25 | offset, topology, occupancy = self.model(inputs)
26 |
27 | offset = offset.squeeze()
28 | topology = topology.squeeze()
29 | topology = topology[:, self.vis_topology]
30 |
31 | vertices, faces = pred_to_mesh_max(offset, topology)
32 | faces = faces.astype(np.int64)
33 |
34 | vertices = 1.2 * (vertices / self.num_voxels - 0.5)
35 | mesh = trimesh.Trimesh(vertices=vertices, faces=faces, process=False)
36 | return mesh
37 |
38 |
39 |
--------------------------------------------------------------------------------
/im2mesh/dmc/models/__init__.py:
--------------------------------------------------------------------------------
1 | import torch.nn as nn
2 | from im2mesh.dmc.models import encoder, decoder
3 |
4 |
5 | decoder_dict = {
6 | 'unet': decoder.UNetDecoder
7 | }
8 |
9 | encoder_dict = {
10 | 'pointnet_local': encoder.PointNetLocal,
11 | }
12 |
13 | class DMC(nn.Module):
14 | def __init__(self, decoder, encoder):
15 | super().__init__()
16 | self.decoder = decoder
17 | self.encoder = encoder
18 |
19 | def forward(self, x):
20 | c = self.encoder(x)
21 | offset, topology, occupancy = self.decoder(c)
22 |
23 | return offset, topology, occupancy
24 |
--------------------------------------------------------------------------------
/im2mesh/dmc/models/encoder.py:
--------------------------------------------------------------------------------
1 | import torch.nn as nn
2 | import torch
3 |
4 | from im2mesh.dmc.ops.grid_pooling import GridPooling
5 |
6 |
7 | class PointNetLocal(nn.Module):
8 | ''' Point Net Local Conditional Network from the Deep Marching Cubes paper.
9 |
10 | It applies two fully connected layers to the input points (dim 3) in a
11 | 1D Convolutional Layer fashion to avoid to specify the number of
12 | incoming points
13 | '''
14 | def __init__(self, c_dim=256, out_dim=16, cell_W=16, cell_H=16, cell_D=16):
15 | super().__init__()
16 | self.cell_W = cell_W
17 | self.cell_H = cell_H
18 | self.cell_D = cell_D
19 |
20 | # TODO change gridpooling input to be compatible to single values of W H D
21 | self.gridshape = torch.cuda.LongTensor([cell_W, cell_H, cell_D])
22 | actvn = nn.ReLU()
23 | self.grid_pool = GridPooling(self.gridshape)
24 | self.conv1 = nn.Sequential(
25 | nn.Conv1d(3, c_dim, 1), actvn
26 | )
27 | #self.conv2 = nn.Sequential(
28 | # nn.Conv1d(c_dim, out_dim, 1), actvn
29 | #)
30 | self.conv2 = nn.Conv1d(c_dim, out_dim, 1)
31 |
32 | def forward(self, x):
33 | pts = x
34 | feats = x.transpose(1, 2) # b_size x 3 x num_points
35 | feats = self.conv1(feats) # b_size x c_dim x num_points
36 | feats = self.conv2(feats) # b_size x out_dim x num_points
37 | feats = feats.transpose(1, 2) # b_size x num_points x out_dim
38 |
39 | out = self.point_to_cell(pts, feats, self.cell_W, self.cell_H, self.cell_D)
40 | return out
41 |
42 | def point_to_cell(self, pts, feat, W, H, D, expand=1):
43 | """ perform maxpool on points in every cell set zero vector if cell is
44 | empty if expand=1 then return (N+1)x(N+1)x(N+1), for dmc xpand=0 then
45 | return NxNxN, for occupancy/sdf baselines
46 | """
47 | batchsize = feat.size()[0]
48 | C = feat.size()[2]
49 |
50 | feat_cell = []
51 | # grid_shape = torch.LongTensor([W, H, D])
52 | for k in range(batchsize):
53 | feat_cell.append(self.grid_pool(feat[k, :, :], pts[k, :, :]))
54 |
55 | feat_cell = torch.stack(feat_cell, dim=0)
56 |
57 | # TODO check if this view is compatible to output of grid pool
58 | feat_cell = torch.transpose(feat_cell, 1, 2).contiguous().view(
59 | -1, C, W, H, D)
60 | if expand == 0:
61 | return feat_cell
62 |
63 | # expand to (W+1)x(H+1)
64 | curr_size = feat_cell.size()
65 | feat_cell_exp = torch.zeros(
66 | curr_size[0], curr_size[1], curr_size[2]+1, curr_size[3]+1,
67 | curr_size[4]+1).to(pts.device)
68 | feat_cell_exp[:, :, :-1, :-1, :-1] = feat_cell
69 | return feat_cell_exp
70 |
--------------------------------------------------------------------------------
/im2mesh/dmc/ops/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Jarrome/IMT_Mapping/974547684a4474be4c281a9380d4a15150f4bf48/im2mesh/dmc/ops/__init__.py
--------------------------------------------------------------------------------
/im2mesh/dmc/ops/cpp_modules/old/commons.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include "commons.h"
3 |
4 | /**
5 | * convert vertex displacement field to vertices locations
6 | * params:
7 | * offset vertex displacement field, 3xWxHxD
8 | * x indice of a cell in the full grid
9 | * y indice of a cell in the full grid
10 | * z indice of a cell in the full grid
11 | * return:
12 | * vertices the location of 12 vertices for the specific cell, 3x12
13 | *
14 | */
15 | THFloatTensor* offset_to_vertices(THFloatTensor *offset, int x, int y, int z){
16 | THFloatTensor *vertices = THFloatTensor_newWithSize2d(3, 12);
17 |
18 | // #0
19 | THFloatTensor_set2d(vertices, 0, 0, 0.5-THFloatTensor_get4d(offset, 0, x+1, y+1, z ));
20 | THFloatTensor_set2d(vertices, 1, 0, 1.0);
21 | THFloatTensor_set2d(vertices, 2, 0, 0.0);
22 | // #1
23 | THFloatTensor_set2d(vertices, 0, 1, 1.0);
24 | THFloatTensor_set2d(vertices, 1, 1, 0.5-THFloatTensor_get4d(offset, 1, x+1, y+1, z ));
25 | THFloatTensor_set2d(vertices, 2, 1, 0.0);
26 | // #2
27 | THFloatTensor_set2d(vertices, 0, 2, 0.5-THFloatTensor_get4d(offset, 0, x+1, y , z ));
28 | THFloatTensor_set2d(vertices, 1, 2, 0.0);
29 | THFloatTensor_set2d(vertices, 2, 2, 0.0);
30 | // #3
31 | THFloatTensor_set2d(vertices, 0, 3, 0.0);
32 | THFloatTensor_set2d(vertices, 1, 3, 0.5-THFloatTensor_get4d(offset, 1, x , y+1, z ));
33 | THFloatTensor_set2d(vertices, 2, 3, 0.0);
34 |
35 | // #4
36 | THFloatTensor_set2d(vertices, 0, 4, 0.5-THFloatTensor_get4d(offset, 0, x+1, y+1, z+1));
37 | THFloatTensor_set2d(vertices, 1, 4, 1.0);
38 | THFloatTensor_set2d(vertices, 2, 4, 1.0);
39 | // #5
40 | THFloatTensor_set2d(vertices, 0, 5, 1.0);
41 | THFloatTensor_set2d(vertices, 1, 5, 0.5-THFloatTensor_get4d(offset, 1, x+1, y+1, z+1));
42 | THFloatTensor_set2d(vertices, 2, 5, 1.0);
43 | // #6
44 | THFloatTensor_set2d(vertices, 0, 6, 0.5-THFloatTensor_get4d(offset, 0, x+1, y , z+1));
45 | THFloatTensor_set2d(vertices, 1, 6, 0.0);
46 | THFloatTensor_set2d(vertices, 2, 6, 1.0);
47 | // #7
48 | THFloatTensor_set2d(vertices, 0, 7, 0.0);
49 | THFloatTensor_set2d(vertices, 1, 7, 0.5-THFloatTensor_get4d(offset, 1, x , y+1, z+1));
50 | THFloatTensor_set2d(vertices, 2, 7, 1.0);
51 |
52 | // #8
53 | THFloatTensor_set2d(vertices, 0, 8, 0.0);
54 | THFloatTensor_set2d(vertices, 1, 8, 1.0);
55 | THFloatTensor_set2d(vertices, 2, 8, 0.5-THFloatTensor_get4d(offset, 2, x , y+1, z+1));
56 | // #9
57 | THFloatTensor_set2d(vertices, 0, 9, 1.0);
58 | THFloatTensor_set2d(vertices, 1, 9, 1.0);
59 | THFloatTensor_set2d(vertices, 2, 9, 0.5-THFloatTensor_get4d(offset, 2, x+1, y+1, z+1));
60 | // #10
61 | THFloatTensor_set2d(vertices, 0, 10, 1.0);
62 | THFloatTensor_set2d(vertices, 1, 10, 0.0);
63 | THFloatTensor_set2d(vertices, 2, 10, 0.5-THFloatTensor_get4d(offset, 2, x+1, y , z+1));
64 | // #11
65 | THFloatTensor_set2d(vertices, 0, 11, 0.0);
66 | THFloatTensor_set2d(vertices, 1, 11, 0.0);
67 | THFloatTensor_set2d(vertices, 2, 11, 0.5-THFloatTensor_get4d(offset, 2, x , y , z+1));
68 | return vertices;
69 | }
70 |
71 | /**
72 | * get points in a specific cell
73 | * params:
74 | * points all points in the grid, Nx3
75 | * i the offset of the specific cell
76 | * j the offset of the specific cell
77 | * k the offset of the specific cell
78 | * return:
79 | * indices a binary 1D tensor indicating if a point is in a specific cell or not, N
80 | *
81 | */
82 | THLongTensor* points_in_grid(THFloatTensor *points, float i, float j, float k){
83 | int N=THFloatTensor_size(points, 0);
84 | THLongTensor *indices = THLongTensor_new();
85 |
86 | THByteTensor *mask = THByteTensor_newWithSize1d(N);
87 | THByteTensor_zero(mask);
88 | for (int p=0; p= i && THFloatTensor_get2d(points, p, 0) < i+1 &&
90 | THFloatTensor_get2d(points, p, 1) >= j && THFloatTensor_get2d(points, p, 1) < j+1 &&
91 | THFloatTensor_get2d(points, p, 2) >= k && THFloatTensor_get2d(points, p, 2) < k+1)
92 | THByteTensor_set1d(mask, p, 1);
93 | }
94 |
95 | THByteTensor_nonzero(indices, mask);
96 |
97 | THLongTensor_squeeze(indices, indices);
98 | THByteTensor_free(mask);
99 | return indices;
100 | }
101 |
--------------------------------------------------------------------------------
/im2mesh/dmc/ops/cpp_modules/old/pred_to_mesh.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 | #include "commons.h"
5 |
6 |
7 | // considered all topologies with 4 triangles during visualization
8 | static int visTopology[2][140]={{0, 1, 2, 3, 4, 6, 7, 8, 9, 11, 12, 13, 14, 15, 16, 17, 19, 23, 25, 27, 29, 31, 32, 34, 35, 38, 39, 43, 46, 47, 48, 49, 50, 51, 54, 55, 57, 59, 63, 64, 68, 70, 71, 76, 77, 78, 79, 95, 96, 98, 99, 100, 102, 103, 108, 110, 111, 112, 113, 114, 115, 116, 118, 119, 123, 126, 127, 128, 136, 137, 139, 140, 141, 142, 143, 144, 145, 147, 152, 153, 155, 156, 157, 159, 175, 176, 177, 178, 179, 183, 184, 185, 187, 189, 191, 192, 196, 198, 200, 201, 204, 205, 206, 207, 208, 209, 212, 216, 217, 219, 220, 221, 222, 223, 224, 226, 228, 230, 231, 232, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255},
9 | {0, 1, 1, 2, 1, 2, 3, 1, 2, 3, 2, 3, 3, 2, 1, 2, 3, 4, 3, 4, 4, 3, 1, 2, 3, 3, 4, 4, 4, 3, 2, 3, 3, 2, 4, 3, 4, 3, 2, 1, 2, 3, 4, 3, 4, 4, 3, 4, 2, 3, 4, 3, 2, 3, 4, 3, 2, 3, 4, 4, 3, 4, 3, 2, 4, 4, 1, 1, 2, 3, 4, 3, 4, 4, 3, 2, 3, 4, 3, 2, 3, 4, 3, 2, 4, 3, 4, 4, 3, 4, 4, 3, 2, 4, 1, 2, 3, 4, 3, 4, 2, 3, 3, 2, 3, 4, 4, 4, 3, 4, 3, 2, 4, 1, 3, 4, 4, 3, 4, 4, 3, 4, 2, 1, 2, 3, 3, 2, 3, 4, 2, 1, 3, 2, 4, 1, 2, 1, 1, 0}};
10 |
11 | /**
12 | * convert the topology probability and vertex displacement field to a mesh by
13 | * selecting the topology with maximum probability in every cell
14 | * params:
15 | * offset vertex displacement field
16 | * topology topology probabilities
17 | * vertices_all vertices locations for all triangles in topologies with maximum probabilities
18 | * note there might be duplications and the unique vertices will be extracted afterwards
19 | * faces_all faces represented by the indices in vertices_all
20 | * vertice_number record the number of vertices as we initialzed the vertices_all with a fixed length
21 | * face_number record the number of faces as we initialized the faces_all with a fixed length
22 | */
23 | int pred_to_mesh(THFloatTensor offset, THLongTensor *topology, THFloatTensor *vertices_all, THFloatTensor *faces_all, THLongTensor *vertice_number, THLongTensor *face_number){
24 | // data format check
25 | if (THFloatTensor_nDimension(offset)!=4 || THLongTensor_nDimension(topology)!=3 ){
26 | printf("Invalid nDimension!\n");
27 | printf("Expected 4, 3, received %d, %d \n", THFloatTensor_nDimension(offset), THLongTensor_nDimension(topology));
28 | return 0;
29 | }
30 | int W,H,D;
31 | W = THFloatTensor_size(offset,1)-1;
32 | H = THFloatTensor_size(offset,2)-1;
33 | D = THFloatTensor_size(offset,3)-1;
34 |
35 | int vertice_cnt=0;
36 | int face_cnt=0;
37 |
38 | for (int i=0; i
2 | #include
3 | #include
4 | #include
5 | #include
6 | __constant__ float grid_size=1.0;
7 | namespace{
8 | /**
9 | * perform max-pooling within the cells
10 | * parallel over each cell and each feature dimension
11 | */
12 | template
13 | __global__ void grid_pooling_kernel(
14 | const scalar_t *point,
15 | const scalar_t *feat_points,
16 | scalar_t *feat_cell,
17 | int *indices,
18 | const int n ){
19 | // cell indices
20 | int i = blockIdx.x;
21 | int j = blockIdx.y;
22 | int k = blockIdx.z;
23 | // cell size
24 | // int W = gridDim.x;
25 | int H = gridDim.y;
26 | int D = gridDim.z;
27 | int ind = i*H*D + j*D + k;
28 | int c = threadIdx.x;
29 | int C = blockDim.x;
30 |
31 | for (int p=0; p= i && px < i+grid_size && py >= j && py < j+grid_size && pz >= k && pz < k+grid_size){
37 | // max-pooling, update feat_cell if the feature is larger than the current feat_cell
38 | // can be async for max operation
39 | if ( feat_points[p*C + c] > feat_cell[ind*C + c] ){
40 | feat_cell[ind*C + c] = feat_points[p*C + c];
41 | indices[ind*C + c] = p;
42 | }
43 | }
44 | }
45 | }
46 | /**
47 | * back-propagate the loss from the max-pooled feature to point features
48 | * parallel over each cell and each feature dimension
49 | */
50 | template
51 | __global__ void grad_grid_pooling_kernel(
52 | const scalar_t *grad_output,
53 | const int *indices,
54 | scalar_t *grad_feat_points){
55 | // cell indices
56 | int i = blockIdx.x;
57 | int j = blockIdx.y;
58 | int k = blockIdx.z;
59 | // cell size
60 | // int W = gridDim.x;
61 | int H = gridDim.y;
62 | int D = gridDim.z;
63 | int ind = i*H*D + j*D + k;
64 | int c = threadIdx.x;
65 | int C = blockDim.x;
66 | long int p = indices[ind*C + c];
67 | if (p < 0) return;
68 |
69 | grad_feat_points[p*C + c] = grad_output[ind*C + c];
70 | }
71 | } //namespace
72 |
73 | /*
74 | * Forward function, project the point features to cells, perform max pooling in every cell
75 | * params:
76 | * point input, all points, Nx3
77 | * feat_points input, feature of all points, NxC
78 | * shape input, size of the grid [W, H, D], 3
79 | * feat_cell output, feature of all cells, (WxHxD)xC
80 | * indices output, indices of max pooling, saved for back propagation, (WxHxD)xC
81 | *
82 | */
83 |
84 | void grid_pooling_kernel_forward(
85 | at::Tensor point,
86 | at::Tensor feat_points,
87 | at::Tensor shape,
88 | at::Tensor feat_cell,
89 | at::Tensor indices){
90 | int W = shape.data()[0];
91 | int H = shape.data()[1];
92 | int D = shape.data()[2];
93 | int C = feat_cell.size(1);
94 |
95 | dim3 dimGrid(W, H, D);
96 | dim3 dimBlock(C, 1, 1);
97 | // lauch the kernel
98 | int n = point.size(0);
99 | grid_pooling_kernel<<< dimGrid, dimBlock>>>(
100 | point.data(),
101 | feat_points.data(),
102 | feat_cell.data(),
103 | indices.data(),
104 | n);
105 | }
106 |
107 | /*
108 | * Backward function, back-propagate the loss to the point features
109 | * params:
110 | * grad_output input, gradient on the output feature, WxHxC
111 | * shape input, size of the grid [W, H, D], 3
112 | * indices input, indices of max pooling, WxHxC
113 | * grad_feat_points output, gradient on the features, NxC
114 | *
115 | */
116 | void grid_pooling_kernel_backward(
117 | at::Tensor grad_output,
118 | at::Tensor shape,
119 | at::Tensor indices,
120 | at::Tensor grad_feat_points){
121 | int W = shape.data()[0];
122 | int H = shape.data()[1];
123 | int D = shape.data()[2];
124 | int C = grad_output.size(1);
125 | dim3 dimGrid(W, H, D);
126 | dim3 dimBlock(C, 1, 1);
127 | // lauch the kernel
128 | grad_grid_pooling_kernel<<< dimGrid, dimBlock>>>(
129 | grad_output.data(),
130 | indices.data(),
131 | grad_feat_points.data());
132 | }
133 |
134 |
135 |
--------------------------------------------------------------------------------
/im2mesh/dmc/ops/src/kernels.h:
--------------------------------------------------------------------------------
1 | #ifndef __DMC_CUDA_KERNELS__
2 | #define __DMC_CUDA_KERNELS__
3 |
4 | #include
5 | #include
6 |
7 |
8 | // Curvature constraint
9 | void curvature_constraint_kernel_forward(
10 | at::Tensor offset,
11 | at::Tensor topology,
12 | at::Tensor xTable,
13 | at::Tensor yTable,
14 | at::Tensor zTable,
15 | at::Tensor innerTable,
16 | at::Tensor loss_x,
17 | at::Tensor loss_y,
18 | at::Tensor loss_z,
19 | at::Tensor loss_inner);
20 |
21 |
22 | void curvature_constraint_kernel_backward(
23 | at::Tensor grad_output,
24 | at::Tensor offset,
25 | at::Tensor topology,
26 | at::Tensor xTable,
27 | at::Tensor yTable,
28 | at::Tensor zTable,
29 | at::Tensor innerTable,
30 | at::Tensor grad_offset);
31 |
32 | // Grid pooling
33 | void grid_pooling_kernel_forward(
34 | at::Tensor point,
35 | at::Tensor feat_points,
36 | at::Tensor shape,
37 | at::Tensor feat_cell,
38 | at::Tensor indices);
39 |
40 | void grid_pooling_kernel_backward(
41 | at::Tensor grad_output,
42 | at::Tensor shape,
43 | at::Tensor indices,
44 | at::Tensor grad_feat_points);
45 |
46 | // Occ2Topo
47 | void occupancy_to_topology_kernel_forward(
48 | at::Tensor occupancy,
49 | at::Tensor topology );
50 |
51 | void occupancy_to_topology_kernel_backward(
52 | at::Tensor grad_output,
53 | at::Tensor occupancy,
54 | at::Tensor topology,
55 | at::Tensor grad_occupancy);
56 |
57 | // OccConstraint
58 | void occupancy_connectivity_kernel_forward(
59 | at::Tensor occupancy,
60 | at::Tensor loss);
61 |
62 | void occupancy_connectivity_kernel_backward(
63 | at::Tensor grad_output,
64 | at::Tensor occupancy,
65 | at::Tensor grad_occupancy);
66 |
67 | // Points Triangle distance
68 | void point_topology_distance_kernel_forward(
69 | at::Tensor offset,
70 | at::Tensor points,
71 | at::Tensor distances,
72 | at::Tensor indices_all);
73 |
74 | void point_topology_distance_kernel_backward(
75 | at::Tensor grad_output,
76 | at::Tensor offset,
77 | at::Tensor points,
78 | at::Tensor indices_all,
79 | at::Tensor grad_offset);
80 |
81 | #endif
--------------------------------------------------------------------------------
/im2mesh/dmc/ops/tests/test_curvature.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | from torch.autograd import Variable
4 | import sys
5 |
6 | sys.path.append('../../../..')
7 | from im2mesh.dmc.ops.tests.loss_autograd import LossAutoGrad
8 | from im2mesh.dmc.ops.curvature_constraint import CurvatureConstraint
9 | import torch.nn.functional as F
10 | import numpy as np
11 | import time
12 |
13 | # check the cuda extension or c extension
14 |
15 | print ("Testing CUDA extension...")
16 | dtype = torch.cuda.FloatTensor
17 |
18 |
19 | # autograd loss
20 | num_cells = 4
21 | len_cell = 1.0
22 | W = H = D = num_cells
23 |
24 | loss_autograd = LossAutoGrad(num_cells, len_cell)
25 |
26 |
27 | # cffi loss
28 | class SmoothLoss(nn.Module):
29 | def __init__(self):
30 | super(SmoothLoss, self).__init__()
31 | self.smoothLoss = CurvatureConstraint()
32 |
33 | def forward(self, offset, topology):
34 | return self.smoothLoss(offset, topology)
35 |
36 |
37 | if __name__ == '__main__':
38 |
39 | # generate offset and topology with relatively low-dimension
40 | print ("=========== Input =============")
41 | T = 96
42 | W = num_cells
43 | H = num_cells
44 | D = num_cells
45 | offset = Variable((torch.rand(3, W+1, H+1, D+1)).type(dtype) * 0.1, requires_grad=True)
46 | topology = Variable(torch.rand(W*H*D, T).type(dtype), requires_grad=True)
47 | #print (offset)
48 | #print (topology)
49 |
50 | loss_cffi = SmoothLoss()
51 | l = loss_cffi(offset, F.softmax(topology, dim=1))
52 | l.backward()
53 | offset.grad.data.zero_()
54 |
55 | # evaluating the running time of the cffi extension
56 | print ("============= cffi ============")
57 | tf_c = time.time()
58 | l = loss_cffi(offset, F.softmax(topology, dim=1))
59 | print ("cffi loss:")
60 | print (l)
61 | tf_c = time.time()-tf_c
62 |
63 |
64 | tb_c = time.time()
65 | l.backward()
66 | print ("cffi gradient:")
67 | print( offset.grad)
68 | tb_c = time.time()-tb_c
69 | grad_np = np.copy(offset.grad.data.cpu().numpy())
70 |
71 |
72 | # evaluating the running time of the autograd version
73 | print ("============= auto ============")
74 | tf_py = time.time()
75 | l_auto = loss_autograd.loss_on_curvature_autograd(offset, topology)
76 | print ("auto loss:")
77 | print (l_auto)
78 | tf_py = time.time()-tf_py
79 |
80 | offset.grad.data.zero_()
81 | tb_py = time.time()
82 | l_auto.backward()
83 | print ("auto grad:")
84 | print (offset.grad)
85 | tb_py = time.time()-tb_py
86 | grad_auto_np = np.copy(offset.grad.data.cpu().numpy())
87 | assert np.sum(np.abs(grad_auto_np)) and np.sum(np.abs(grad_np)) != 0.0
88 | # print the loss and grad difference and the time comparison
89 | print ("========== summary ===========")
90 | print ("Forward difference between cffi and auto: ", (l-l_auto).data.cpu().numpy())
91 | print ("Backward difference between cffi and auto: ", np.sum(np.abs(grad_np-grad_auto_np)))
92 | print ("Backward difference between cffi and auto: ", np.mean(np.abs(grad_np-grad_auto_np)))
93 |
94 | print ("cffi forward time: %f, backward time: %f, full time: %f " % (tf_c, tb_c, tf_c+tb_c))
95 | print ("auto forward time: %f, backward time: %f, full time: %f " % (tf_py, tb_py, tf_py+tb_py))
96 | print ("ratio: ", (tf_py+tb_py)/(tf_c + tb_c))
--------------------------------------------------------------------------------
/im2mesh/dmc/ops/tests/test_distance.py:
--------------------------------------------------------------------------------
1 |
2 | import sys
3 | sys.path.append('../../../..')
4 |
5 | import torch
6 | import torch.nn as nn
7 | from torch.autograd import Variable
8 | import time
9 | import numpy as np
10 | import resource
11 |
12 | from im2mesh.dmc.ops.tests.loss_autograd import LossAutoGrad
13 | from im2mesh.dmc.ops.point_triangle_distance import PointTriangleDistance
14 |
15 |
16 | print("Testing CUDA extension...")
17 | dtype = torch.cuda.FloatTensor
18 | dtype_long = torch.cuda.LongTensor
19 | num_cells = 4
20 | # autograd loss
21 | loss_autograd = LossAutoGrad(num_cells, 1.0)
22 |
23 | multiGrids = PointTriangleDistance()
24 |
25 |
26 | if __name__ == '__main__':
27 |
28 | print("=========== Input =============")
29 | point = Variable(torch.rand(10, 3).view(-1,3).type(dtype) * 0.9) * num_cells
30 | offset = Variable(torch.zeros(3, num_cells+1, num_cells+1, num_cells+1).type(dtype)*0.5, requires_grad=True)
31 | print(point.shape)
32 | print(offset.shape)
33 |
34 | print("============= cuda extension ============")
35 | # forward
36 | tf_c = time.time()
37 | distance = multiGrids.forward(offset, point)
38 | tf_c = time.time() - tf_c
39 | distance_np = distance.data.cpu().numpy()
40 | print("cffi distance:")
41 | print(distance_np.shape)
42 |
43 | weight_rnd = Variable(torch.rand(distance.size()).type(dtype), requires_grad=False)
44 | distance_sum = torch.sum(torch.mul(distance, weight_rnd))
45 |
46 | # backward
47 | tb_c = time.time()
48 | grad = distance_sum.backward()
49 | tb_c = time.time() - tb_c
50 | offset_np = np.copy(offset.grad.data.cpu().numpy())
51 |
52 | print("cffi grad:")
53 | print(offset_np.shape)
54 |
55 | print("============= auto ============")
56 | # forward
57 | tf_py = time.time()
58 | distance_auto = loss_autograd.loss_point_to_mesh_distance_autograd(offset, point)
59 | tf_py = time.time()-tf_py
60 | distance_auto_np = distance_auto.data.cpu().numpy()
61 | print("auto distance:")
62 | print(distance_auto_np.shape)
63 | weight_rnd = Variable(weight_rnd.data)
64 | distance_sum_auto = torch.sum(torch.mul(distance_auto, weight_rnd))
65 |
66 | # backward
67 | offset.grad.data.zero_()
68 |
69 | tb_py = time.time()
70 | distance_sum_auto.backward()
71 | tb_py = time.time() - tb_py
72 | print("auto grad: ")
73 | offset_auto_np = np.copy(offset.grad.data.cpu().numpy())
74 | print(offset_auto_np.shape)
75 |
76 | print("========== summary ===========")
77 | print("Forward difference between cffi and auto: "+str(np.sum(np.abs(distance_np[:,:-1]-distance_auto_np[:,:-1]))))
78 | print("Backward difference between cffi and auto: "+str(np.sum(np.abs(offset_np-offset_auto_np))))
--------------------------------------------------------------------------------
/im2mesh/dmc/ops/tests/test_occupancy_connectivity.py:
--------------------------------------------------------------------------------
1 |
2 | import sys
3 | sys.path.append('../../../..')
4 |
5 | import torch
6 | import torch.nn as nn
7 | from torch.autograd import Variable
8 |
9 | import time
10 | import numpy as np
11 | from im2mesh.dmc.ops.occupancy_connectivity import OccupancyConnectivity
12 | #from loss import Loss
13 | #from loss_autograd import LossAutoGrad
14 | #from parse_args import parse_args
15 |
16 | # check the cuda extension or c extension
17 |
18 | def loss_on_smoothness_autograd( occupancy):
19 | """ Compute the smoothness loss using pytorch,
20 | implemented for gradient check of the c/c++ extensions
21 | """
22 |
23 | Wo=occupancy.size()[0]
24 | Ho=occupancy.size()[1]
25 | Do=occupancy.size()[2]
26 |
27 | loss = 0
28 | for x_ in range(Wo):
29 | for y_ in range(Ho):
30 | for z_ in range(Do):
31 | # horizontal direction
32 | if x_= threshold).to_mesh()
54 |
55 | return mesh
56 |
--------------------------------------------------------------------------------
/im2mesh/r2n2/models/__init__.py:
--------------------------------------------------------------------------------
1 | import torch.nn as nn
2 | from im2mesh.r2n2.models.decoder import Decoder
3 |
4 |
5 | # Decoder dictionary
6 | decoder_dict = {
7 | 'simple': Decoder,
8 | }
9 |
10 |
11 | class R2N2(nn.Module):
12 | ''' The 3D Recurrent Reconstruction Neural Network (3D-R2N2) model.
13 |
14 | For details regarding the model, please see
15 | https://arxiv.org/abs/1604.00449
16 |
17 | As single-view images are used as input, we do not use the recurrent
18 | module.
19 |
20 | Args:
21 | decoder (nn.Module): decoder network
22 | encoder (nn.Module): encoder network
23 | '''
24 |
25 | def __init__(self, decoder, encoder):
26 | super().__init__()
27 | self.decoder = decoder
28 | self.encoder = encoder
29 |
30 | def forward(self, x):
31 | c = self.encoder(x)
32 | occ_hat = self.decoder(c)
33 | return occ_hat
34 |
--------------------------------------------------------------------------------
/im2mesh/r2n2/models/decoder.py:
--------------------------------------------------------------------------------
1 | import torch.nn as nn
2 | import torch.nn.functional as F
3 |
4 |
5 | class Decoder(nn.Module):
6 | ''' Decoder network class for the R2N2 model.
7 |
8 | It consists of 4 transposed 3D-convolutional layers.
9 |
10 | Args:
11 | dim (int): input dimension
12 | c_dim (int): dimension of latent conditioned code c
13 | '''
14 |
15 | def __init__(self, dim=3, c_dim=128):
16 | super().__init__()
17 | self.actvn = F.relu
18 | self.fc_in = nn.Linear(c_dim, 256*4*4*4)
19 | self.convtrp_0 = nn.ConvTranspose3d(256, 128, 3, stride=2,
20 | padding=1, output_padding=1)
21 | self.convtrp_1 = nn.ConvTranspose3d(128, 64, 3, stride=2,
22 | padding=1, output_padding=1)
23 | self.convtrp_2 = nn.ConvTranspose3d(64, 32, 3, stride=2,
24 | padding=1, output_padding=1)
25 | self.conv_out = nn.Conv3d(32, 1, 1)
26 |
27 | def forward(self, c):
28 | batch_size = c.size(0)
29 |
30 | net = self.fc_in(c)
31 | net = net.view(batch_size, 256, 4, 4, 4)
32 | net = self.convtrp_0(self.actvn(net))
33 | net = self.convtrp_1(self.actvn(net))
34 | net = self.convtrp_2(self.actvn(net))
35 |
36 | occ_hat = self.conv_out(self.actvn(net))
37 |
38 | return occ_hat
39 |
--------------------------------------------------------------------------------
/im2mesh/training.py:
--------------------------------------------------------------------------------
1 | # from im2mesh import icp
2 | import numpy as np
3 | from collections import defaultdict
4 | from tqdm import tqdm
5 |
6 |
7 | class BaseTrainer(object):
8 | ''' Base trainer class.
9 | '''
10 |
11 | def evaluate(self, val_loader):
12 | ''' Performs an evaluation.
13 | Args:
14 | val_loader (dataloader): pytorch dataloader
15 | '''
16 | eval_list = defaultdict(list)
17 |
18 | for data in tqdm(val_loader):
19 | eval_step_dict = self.eval_step(data)
20 |
21 | for k, v in eval_step_dict.items():
22 | eval_list[k].append(v)
23 |
24 | eval_dict = {k: np.mean(v) for k, v in eval_list.items()}
25 | return eval_dict
26 |
27 | def train_step(self, *args, **kwargs):
28 | ''' Performs a training step.
29 | '''
30 | raise NotImplementedError
31 |
32 | def eval_step(self, *args, **kwargs):
33 | ''' Performs an evaluation step.
34 | '''
35 | raise NotImplementedError
36 |
37 | def visualize(self, *args, **kwargs):
38 | ''' Performs visualization.
39 | '''
40 | raise NotImplementedError
41 |
--------------------------------------------------------------------------------
/im2mesh/utils/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Jarrome/IMT_Mapping/974547684a4474be4c281a9380d4a15150f4bf48/im2mesh/utils/__init__.py
--------------------------------------------------------------------------------
/im2mesh/utils/icp.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from sklearn.neighbors import NearestNeighbors
3 |
4 |
5 | def best_fit_transform(A, B):
6 | '''
7 | Calculates the least-squares best-fit transform that maps corresponding
8 | points A to B in m spatial dimensions
9 | Input:
10 | A: Nxm numpy array of corresponding points
11 | B: Nxm numpy array of corresponding points
12 | Returns:
13 | T: (m+1)x(m+1) homogeneous transformation matrix that maps A on to B
14 | R: mxm rotation matrix
15 | t: mx1 translation vector
16 | '''
17 |
18 | assert A.shape == B.shape
19 |
20 | # get number of dimensions
21 | m = A.shape[1]
22 |
23 | # translate points to their centroids
24 | centroid_A = np.mean(A, axis=0)
25 | centroid_B = np.mean(B, axis=0)
26 | AA = A - centroid_A
27 | BB = B - centroid_B
28 |
29 | # rotation matrix
30 | H = np.dot(AA.T, BB)
31 | U, S, Vt = np.linalg.svd(H)
32 | R = np.dot(Vt.T, U.T)
33 |
34 | # special reflection case
35 | if np.linalg.det(R) < 0:
36 | Vt[m-1,:] *= -1
37 | R = np.dot(Vt.T, U.T)
38 |
39 | # translation
40 | t = centroid_B.T - np.dot(R,centroid_A.T)
41 |
42 | # homogeneous transformation
43 | T = np.identity(m+1)
44 | T[:m, :m] = R
45 | T[:m, m] = t
46 |
47 | return T, R, t
48 |
49 |
50 | def nearest_neighbor(src, dst):
51 | '''
52 | Find the nearest (Euclidean) neighbor in dst for each point in src
53 | Input:
54 | src: Nxm array of points
55 | dst: Nxm array of points
56 | Output:
57 | distances: Euclidean distances of the nearest neighbor
58 | indices: dst indices of the nearest neighbor
59 | '''
60 |
61 | assert src.shape == dst.shape
62 |
63 | neigh = NearestNeighbors(n_neighbors=1)
64 | neigh.fit(dst)
65 | distances, indices = neigh.kneighbors(src, return_distance=True)
66 | return distances.ravel(), indices.ravel()
67 |
68 |
69 | def icp(A, B, init_pose=None, max_iterations=20, tolerance=0.001):
70 | '''
71 | The Iterative Closest Point method: finds best-fit transform that maps
72 | points A on to points B
73 | Input:
74 | A: Nxm numpy array of source mD points
75 | B: Nxm numpy array of destination mD point
76 | init_pose: (m+1)x(m+1) homogeneous transformation
77 | max_iterations: exit algorithm after max_iterations
78 | tolerance: convergence criteria
79 | Output:
80 | T: final homogeneous transformation that maps A on to B
81 | distances: Euclidean distances (errors) of the nearest neighbor
82 | i: number of iterations to converge
83 | '''
84 |
85 | assert A.shape == B.shape
86 |
87 | # get number of dimensions
88 | m = A.shape[1]
89 |
90 | # make points homogeneous, copy them to maintain the originals
91 | src = np.ones((m+1,A.shape[0]))
92 | dst = np.ones((m+1,B.shape[0]))
93 | src[:m,:] = np.copy(A.T)
94 | dst[:m,:] = np.copy(B.T)
95 |
96 | # apply the initial pose estimation
97 | if init_pose is not None:
98 | src = np.dot(init_pose, src)
99 |
100 | prev_error = 0
101 |
102 | for i in range(max_iterations):
103 | # find the nearest neighbors between the current source and destination points
104 | distances, indices = nearest_neighbor(src[:m,:].T, dst[:m,:].T)
105 |
106 | # compute the transformation between the current source and nearest destination points
107 | T,_,_ = best_fit_transform(src[:m,:].T, dst[:m,indices].T)
108 |
109 | # update the current source
110 | src = np.dot(T, src)
111 |
112 | # check error
113 | mean_error = np.mean(distances)
114 | if np.abs(prev_error - mean_error) < tolerance:
115 | break
116 | prev_error = mean_error
117 |
118 | # calculate final transformation
119 | T,_,_ = best_fit_transform(A, src[:m,:].T)
120 |
121 | return T, distances, i
122 |
--------------------------------------------------------------------------------
/im2mesh/utils/io.py:
--------------------------------------------------------------------------------
1 | import os
2 | from plyfile import PlyElement, PlyData
3 | import numpy as np
4 |
5 |
6 | def export_pointcloud(vertices, out_file, as_text=True):
7 | assert(vertices.shape[1] == 3)
8 | vertices = vertices.astype(np.float32)
9 | vertices = np.ascontiguousarray(vertices)
10 | vector_dtype = [('x', 'f4'), ('y', 'f4'), ('z', 'f4')]
11 | vertices = vertices.view(dtype=vector_dtype).flatten()
12 | plyel = PlyElement.describe(vertices, 'vertex')
13 | plydata = PlyData([plyel], text=as_text)
14 | plydata.write(out_file)
15 |
16 |
17 | def load_pointcloud(in_file):
18 | plydata = PlyData.read(in_file)
19 | vertices = np.stack([
20 | plydata['vertex']['x'],
21 | plydata['vertex']['y'],
22 | plydata['vertex']['z']
23 | ], axis=1)
24 | return vertices
25 |
26 |
27 | def read_off(file):
28 | """
29 | Reads vertices and faces from an off file.
30 |
31 | :param file: path to file to read
32 | :type file: str
33 | :return: vertices and faces as lists of tuples
34 | :rtype: [(float)], [(int)]
35 | """
36 |
37 | assert os.path.exists(file), 'file %s not found' % file
38 |
39 | with open(file, 'r') as fp:
40 | lines = fp.readlines()
41 | lines = [line.strip() for line in lines]
42 |
43 | # Fix for ModelNet bug were 'OFF' and the number of vertices and faces
44 | # are all in the first line.
45 | if len(lines[0]) > 3:
46 | assert lines[0][:3] == 'OFF' or lines[0][:3] == 'off', \
47 | 'invalid OFF file %s' % file
48 |
49 | parts = lines[0][3:].split(' ')
50 | assert len(parts) == 3
51 |
52 | num_vertices = int(parts[0])
53 | assert num_vertices > 0
54 |
55 | num_faces = int(parts[1])
56 | assert num_faces > 0
57 |
58 | start_index = 1
59 | # This is the regular case!
60 | else:
61 | assert lines[0] == 'OFF' or lines[0] == 'off', \
62 | 'invalid OFF file %s' % file
63 |
64 | parts = lines[1].split(' ')
65 | assert len(parts) == 3
66 |
67 | num_vertices = int(parts[0])
68 | assert num_vertices > 0
69 |
70 | num_faces = int(parts[1])
71 | assert num_faces > 0
72 |
73 | start_index = 2
74 |
75 | vertices = []
76 | for i in range(num_vertices):
77 | vertex = lines[start_index + i].split(' ')
78 | vertex = [float(point.strip()) for point in vertex if point != '']
79 | assert len(vertex) == 3
80 |
81 | vertices.append(vertex)
82 |
83 | faces = []
84 | for i in range(num_faces):
85 | face = lines[start_index + num_vertices + i].split(' ')
86 | face = [index.strip() for index in face if index != '']
87 |
88 | # check to be sure
89 | for index in face:
90 | assert index != '', \
91 | 'found empty vertex index: %s (%s)' \
92 | % (lines[start_index + num_vertices + i], file)
93 |
94 | face = [int(index) for index in face]
95 |
96 | assert face[0] == len(face) - 1, \
97 | 'face should have %d vertices but as %d (%s)' \
98 | % (face[0], len(face) - 1, file)
99 | assert face[0] == 3, \
100 | 'only triangular meshes supported (%s)' % file
101 | for index in face:
102 | assert index >= 0 and index < num_vertices, \
103 | 'vertex %d (of %d vertices) does not exist (%s)' \
104 | % (index, num_vertices, file)
105 |
106 | assert len(face) > 1
107 |
108 | faces.append(face)
109 |
110 | return vertices, faces
111 |
112 | assert False, 'could not open %s' % file
113 |
--------------------------------------------------------------------------------
/im2mesh/utils/libkdtree/.gitignore:
--------------------------------------------------------------------------------
1 | build
2 |
--------------------------------------------------------------------------------
/im2mesh/utils/libkdtree/MANIFEST.in:
--------------------------------------------------------------------------------
1 | exclude pykdtree/render_template.py
2 | include LICENSE.txt
3 |
--------------------------------------------------------------------------------
/im2mesh/utils/libkdtree/__init__.py:
--------------------------------------------------------------------------------
1 | from .pykdtree.kdtree import KDTree
2 |
3 |
4 | __all__ = [
5 | KDTree
6 | ]
7 |
--------------------------------------------------------------------------------
/im2mesh/utils/libkdtree/pykdtree/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Jarrome/IMT_Mapping/974547684a4474be4c281a9380d4a15150f4bf48/im2mesh/utils/libkdtree/pykdtree/__init__.py
--------------------------------------------------------------------------------
/im2mesh/utils/libkdtree/pykdtree/render_template.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python
2 |
3 | from mako.template import Template
4 |
5 | mytemplate = Template(filename='_kdtree_core.c.mako')
6 | with open('_kdtree_core.c', 'w') as fp:
7 | fp.write(mytemplate.render())
8 |
--------------------------------------------------------------------------------
/im2mesh/utils/libkdtree/setup.cfg:
--------------------------------------------------------------------------------
1 | [bdist_rpm]
2 | requires=numpy
3 | release=1
4 |
5 |
6 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmcubes/.gitignore:
--------------------------------------------------------------------------------
1 | PyMCubes.egg-info
2 | build
3 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmcubes/LICENSE:
--------------------------------------------------------------------------------
1 | Copyright (c) 2012-2015, P. M. Neila
2 | All rights reserved.
3 |
4 | Redistribution and use in source and binary forms, with or without
5 | modification, are permitted provided that the following conditions are met:
6 |
7 | * Redistributions of source code must retain the above copyright notice, this
8 | list of conditions and the following disclaimer.
9 |
10 | * Redistributions in binary form must reproduce the above copyright notice,
11 | this list of conditions and the following disclaimer in the documentation
12 | and/or other materials provided with the distribution.
13 |
14 | * Neither the name of the copyright holder nor the names of its
15 | contributors may be used to endorse or promote products derived from
16 | this software without specific prior written permission.
17 |
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmcubes/README.rst:
--------------------------------------------------------------------------------
1 | ========
2 | PyMCubes
3 | ========
4 |
5 | PyMCubes is an implementation of the marching cubes algorithm to extract
6 | isosurfaces from volumetric data. The volumetric data can be given as a
7 | three-dimensional NumPy array or as a Python function ``f(x, y, z)``. The first
8 | option is much faster, but it requires more memory and becomes unfeasible for
9 | very large volumes.
10 |
11 | PyMCubes also provides a function to export the results of the marching cubes as
12 | COLLADA ``(.dae)`` files. This requires the
13 | `PyCollada `_ library.
14 |
15 | Installation
16 | ============
17 |
18 | Just as any standard Python package, clone or download the project
19 | and run::
20 |
21 | $ cd path/to/PyMCubes
22 | $ python setup.py build
23 | $ python setup.py install
24 |
25 | If you do not have write permission on the directory of Python packages,
26 | install with the ``--user`` option::
27 |
28 | $ python setup.py install --user
29 |
30 | Example
31 | =======
32 |
33 | The following example creates a data volume with spherical isosurfaces and
34 | extracts one of them (i.e., a sphere) with PyMCubes. The result is exported as
35 | ``sphere.dae``::
36 |
37 | >>> import numpy as np
38 | >>> import mcubes
39 |
40 | # Create a data volume (30 x 30 x 30)
41 | >>> X, Y, Z = np.mgrid[:30, :30, :30]
42 | >>> u = (X-15)**2 + (Y-15)**2 + (Z-15)**2 - 8**2
43 |
44 | # Extract the 0-isosurface
45 | >>> vertices, triangles = mcubes.marching_cubes(u, 0)
46 |
47 | # Export the result to sphere.dae
48 | >>> mcubes.export_mesh(vertices, triangles, "sphere.dae", "MySphere")
49 |
50 | The second example is very similar to the first one, but it uses a function
51 | to represent the volume instead of a NumPy array::
52 |
53 | >>> import numpy as np
54 | >>> import mcubes
55 |
56 | # Create the volume
57 | >>> f = lambda x, y, z: x**2 + y**2 + z**2
58 |
59 | # Extract the 16-isosurface
60 | >>> vertices, triangles = mcubes.marching_cubes_func((-10,-10,-10), (10,10,10),
61 | ... 100, 100, 100, f, 16)
62 |
63 | # Export the result to sphere2.dae
64 | >>> mcubes.export_mesh(vertices, triangles, "sphere2.dae", "MySphere")
65 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmcubes/__init__.py:
--------------------------------------------------------------------------------
1 | from im2mesh.utils.libmcubes.mcubes import (
2 | marching_cubes, marching_cubes_func
3 | )
4 | from im2mesh.utils.libmcubes.exporter import (
5 | export_mesh, export_obj, export_off
6 | )
7 |
8 |
9 | __all__ = [
10 | marching_cubes, marching_cubes_func,
11 | export_mesh, export_obj, export_off
12 | ]
13 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmcubes/exporter.py:
--------------------------------------------------------------------------------
1 |
2 | import numpy as np
3 |
4 |
5 | def export_obj(vertices, triangles, filename):
6 | """
7 | Exports a mesh in the (.obj) format.
8 | """
9 |
10 | with open(filename, 'w') as fh:
11 |
12 | for v in vertices:
13 | fh.write("v {} {} {}\n".format(*v))
14 |
15 | for f in triangles:
16 | fh.write("f {} {} {}\n".format(*(f + 1)))
17 |
18 |
19 | def export_off(vertices, triangles, filename):
20 | """
21 | Exports a mesh in the (.off) format.
22 | """
23 |
24 | with open(filename, 'w') as fh:
25 | fh.write('OFF\n')
26 | fh.write('{} {} 0\n'.format(len(vertices), len(triangles)))
27 |
28 | for v in vertices:
29 | fh.write("{} {} {}\n".format(*v))
30 |
31 | for f in triangles:
32 | fh.write("3 {} {} {}\n".format(*f))
33 |
34 |
35 | def export_mesh(vertices, triangles, filename, mesh_name="mcubes_mesh"):
36 | """
37 | Exports a mesh in the COLLADA (.dae) format.
38 |
39 | Needs PyCollada (https://github.com/pycollada/pycollada).
40 | """
41 |
42 | import collada
43 |
44 | mesh = collada.Collada()
45 |
46 | vert_src = collada.source.FloatSource("verts-array", vertices, ('X','Y','Z'))
47 | geom = collada.geometry.Geometry(mesh, "geometry0", mesh_name, [vert_src])
48 |
49 | input_list = collada.source.InputList()
50 | input_list.addInput(0, 'VERTEX', "#verts-array")
51 |
52 | triset = geom.createTriangleSet(np.copy(triangles), input_list, "")
53 | geom.primitives.append(triset)
54 | mesh.geometries.append(geom)
55 |
56 | geomnode = collada.scene.GeometryNode(geom, [])
57 | node = collada.scene.Node(mesh_name, children=[geomnode])
58 |
59 | myscene = collada.scene.Scene("mcubes_scene", [node])
60 | mesh.scenes.append(myscene)
61 | mesh.scene = myscene
62 |
63 | mesh.write(filename)
64 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmcubes/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) except +
18 | cdef object c_marching_cubes2 "marching_cubes2"(np.ndarray, double) except +
19 | cdef object c_marching_cubes3 "marching_cubes3"(np.ndarray, double) except +
20 | cdef object c_marching_cubes_func "marching_cubes_func"(tuple, tuple, int, int, int, object, double) except +
21 |
22 | def marching_cubes(np.ndarray volume, float isovalue):
23 |
24 | verts, faces = c_marching_cubes(volume, isovalue)
25 | verts.shape = (-1, 3)
26 | faces.shape = (-1, 3)
27 | return verts, faces
28 |
29 | def marching_cubes2(np.ndarray volume, float isovalue):
30 |
31 | verts, faces = c_marching_cubes2(volume, isovalue)
32 | verts.shape = (-1, 3)
33 | faces.shape = (-1, 3)
34 | return verts, faces
35 |
36 | def marching_cubes3(np.ndarray volume, float isovalue):
37 |
38 | verts, faces = c_marching_cubes3(volume, isovalue)
39 | verts.shape = (-1, 3)
40 | faces.shape = (-1, 3)
41 | return verts, faces
42 |
43 | def marching_cubes_func(tuple lower, tuple upper, int numx, int numy, int numz, object f, double isovalue):
44 |
45 | verts, faces = c_marching_cubes_func(lower, upper, numx, numy, numz, f, isovalue)
46 | verts.shape = (-1, 3)
47 | faces.shape = (-1, 3)
48 | return verts, faces
49 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmcubes/pyarray_symbol.h:
--------------------------------------------------------------------------------
1 |
2 | #define PY_ARRAY_UNIQUE_SYMBOL mcubes_PyArray_API
3 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmcubes/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);
11 | PyObject* marching_cubes2(PyArrayObject* arr, double isovalue);
12 | PyObject* marching_cubes3(PyArrayObject* arr, double isovalue);
13 | PyObject* marching_cubes_func(PyObject* lower, PyObject* upper,
14 | int numx, int numy, int numz, PyObject* f, double isovalue);
15 |
16 | #endif // _PYWRAPPER_H
17 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmesh/.gitignore:
--------------------------------------------------------------------------------
1 | triangle_hash.cpp
2 | build
3 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmesh/__init__.py:
--------------------------------------------------------------------------------
1 | from .inside_mesh import (
2 | check_mesh_contains, MeshIntersector, TriangleIntersector2d
3 | )
4 |
5 |
6 | __all__ = [
7 | check_mesh_contains, MeshIntersector, TriangleIntersector2d
8 | ]
9 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmesh/triangle_hash.pyx:
--------------------------------------------------------------------------------
1 |
2 | # distutils: language=c++
3 | import numpy as np
4 | cimport numpy as np
5 | cimport cython
6 | from libcpp.vector cimport vector
7 | from libc.math cimport floor, ceil
8 |
9 | cdef class TriangleHash:
10 | cdef vector[vector[int]] spatial_hash
11 | cdef int resolution
12 |
13 | def __cinit__(self, double[:, :, :] triangles, int resolution):
14 | self.spatial_hash.resize(resolution * resolution)
15 | self.resolution = resolution
16 | self._build_hash(triangles)
17 |
18 | @cython.boundscheck(False) # Deactivate bounds checking
19 | @cython.wraparound(False) # Deactivate negative indexing.
20 | cdef int _build_hash(self, double[:, :, :] triangles):
21 | assert(triangles.shape[1] == 3)
22 | assert(triangles.shape[2] == 2)
23 |
24 | cdef int n_tri = triangles.shape[0]
25 | cdef int bbox_min[2]
26 | cdef int bbox_max[2]
27 |
28 | cdef int i_tri, j, x, y
29 | cdef int spatial_idx
30 |
31 | for i_tri in range(n_tri):
32 | # Compute bounding box
33 | for j in range(2):
34 | bbox_min[j] = min(
35 | triangles[i_tri, 0, j], triangles[i_tri, 1, j], triangles[i_tri, 2, j]
36 | )
37 | bbox_max[j] = max(
38 | triangles[i_tri, 0, j], triangles[i_tri, 1, j], triangles[i_tri, 2, j]
39 | )
40 | bbox_min[j] = min(max(bbox_min[j], 0), self.resolution - 1)
41 | bbox_max[j] = min(max(bbox_max[j], 0), self.resolution - 1)
42 |
43 | # Find all voxels where bounding box intersects
44 | for x in range(bbox_min[0], bbox_max[0] + 1):
45 | for y in range(bbox_min[1], bbox_max[1] + 1):
46 | spatial_idx = self.resolution * x + y
47 | self.spatial_hash[spatial_idx].push_back(i_tri)
48 |
49 | @cython.boundscheck(False) # Deactivate bounds checking
50 | @cython.wraparound(False) # Deactivate negative indexing.
51 | cpdef query(self, double[:, :] points):
52 | assert(points.shape[1] == 2)
53 | cdef int n_points = points.shape[0]
54 |
55 | cdef vector[int] points_indices
56 | cdef vector[int] tri_indices
57 | # cdef int[:] points_indices_np
58 | # cdef int[:] tri_indices_np
59 |
60 | cdef int i_point, k, x, y
61 | cdef int spatial_idx
62 |
63 | for i_point in range(n_points):
64 | x = int(points[i_point, 0])
65 | y = int(points[i_point, 1])
66 | if not (0 <= x < self.resolution and 0 <= y < self.resolution):
67 | continue
68 |
69 | spatial_idx = self.resolution * x + y
70 | for i_tri in self.spatial_hash[spatial_idx]:
71 | points_indices.push_back(i_point)
72 | tri_indices.push_back(i_tri)
73 |
74 | points_indices_np = np.zeros(points_indices.size(), dtype=np.int32)
75 | tri_indices_np = np.zeros(tri_indices.size(), dtype=np.int32)
76 |
77 | cdef int[:] points_indices_view = points_indices_np
78 | cdef int[:] tri_indices_view = tri_indices_np
79 |
80 | for k in range(points_indices.size()):
81 | points_indices_view[k] = points_indices[k]
82 |
83 | for k in range(tri_indices.size()):
84 | tri_indices_view[k] = tri_indices[k]
85 |
86 | return points_indices_np, tri_indices_np
87 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmise/.gitignore:
--------------------------------------------------------------------------------
1 | mise.c
2 | mise.cpp
3 | mise.html
4 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmise/__init__.py:
--------------------------------------------------------------------------------
1 | from .mise import MISE
2 |
3 |
4 | __all__ = [
5 | MISE
6 | ]
7 |
--------------------------------------------------------------------------------
/im2mesh/utils/libmise/test.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from mise import MISE
3 | import time
4 |
5 | t0 = time.time()
6 | extractor = MISE(1, 2, 0.)
7 |
8 | p = extractor.query()
9 | i = 0
10 |
11 | while p.shape[0] != 0:
12 | print(i)
13 | print(p)
14 | v = 2 * (p.sum(axis=-1) > 2).astype(np.float64) - 1
15 | extractor.update(p, v)
16 | p = extractor.query()
17 | i += 1
18 | if (i >= 8):
19 | break
20 |
21 | print(extractor.to_dense())
22 | # p, v = extractor.get_points()
23 | # print(p)
24 | # print(v)
25 | print('Total time: %f' % (time.time() - t0))
26 |
--------------------------------------------------------------------------------
/im2mesh/utils/libsimplify/__init__.py:
--------------------------------------------------------------------------------
1 | from .simplify_mesh import (
2 | mesh_simplify
3 | )
4 | import trimesh
5 |
6 |
7 | def simplify_mesh(mesh, f_target=10000, agressiveness=7.):
8 | vertices = mesh.vertices
9 | faces = mesh.faces
10 |
11 | vertices, faces = mesh_simplify(vertices, faces, f_target, agressiveness)
12 |
13 | mesh_simplified = trimesh.Trimesh(vertices, faces, process=False)
14 |
15 | return mesh_simplified
16 |
--------------------------------------------------------------------------------
/im2mesh/utils/libsimplify/simplify_mesh.pyx:
--------------------------------------------------------------------------------
1 | # distutils: language = c++
2 | from libcpp.vector cimport vector
3 | import numpy as np
4 | cimport numpy as np
5 |
6 |
7 | cdef extern from "Simplify.h":
8 | cdef struct vec3f:
9 | double x, y, z
10 |
11 | cdef cppclass SymetricMatrix:
12 | SymetricMatrix() except +
13 |
14 |
15 | cdef extern from "Simplify.h" namespace "Simplify":
16 | cdef struct Triangle:
17 | int v[3]
18 | double err[4]
19 | int deleted, dirty, attr
20 | vec3f uvs[3]
21 | int material
22 |
23 | cdef struct Vertex:
24 | vec3f p
25 | int tstart, tcount
26 | SymetricMatrix q
27 | int border
28 |
29 | cdef vector[Triangle] triangles
30 | cdef vector[Vertex] vertices
31 | cdef void simplify_mesh(int, double)
32 |
33 |
34 | cpdef mesh_simplify(double[:, ::1] vertices_in, long[:, ::1] triangles_in,
35 | int f_target, double agressiveness=7.) except +:
36 | vertices.clear()
37 | triangles.clear()
38 |
39 | # Read in vertices and triangles
40 | cdef Vertex v
41 | for iv in range(vertices_in.shape[0]):
42 | v = Vertex()
43 | v.p.x = vertices_in[iv, 0]
44 | v.p.y = vertices_in[iv, 1]
45 | v.p.z = vertices_in[iv, 2]
46 | vertices.push_back(v)
47 |
48 | cdef Triangle t
49 | for it in range(triangles_in.shape[0]):
50 | t = Triangle()
51 | t.v[0] = triangles_in[it, 0]
52 | t.v[1] = triangles_in[it, 1]
53 | t.v[2] = triangles_in[it, 2]
54 | triangles.push_back(t)
55 |
56 | # Simplify
57 | # print('Simplify...')
58 | simplify_mesh(f_target, agressiveness)
59 |
60 | # Only use triangles that are not deleted
61 | cdef vector[Triangle] triangles_notdel
62 | triangles_notdel.reserve(triangles.size())
63 |
64 | for t in triangles:
65 | if not t.deleted:
66 | triangles_notdel.push_back(t)
67 |
68 | # Read out triangles
69 | vertices_out = np.empty((vertices.size(), 3), dtype=np.float64)
70 | triangles_out = np.empty((triangles_notdel.size(), 3), dtype=np.int64)
71 |
72 | cdef double[:, :] vertices_out_view = vertices_out
73 | cdef long[:, :] triangles_out_view = triangles_out
74 |
75 | for iv in range(vertices.size()):
76 | vertices_out_view[iv, 0] = vertices[iv].p.x
77 | vertices_out_view[iv, 1] = vertices[iv].p.y
78 | vertices_out_view[iv, 2] = vertices[iv].p.z
79 |
80 | for it in range(triangles_notdel.size()):
81 | triangles_out_view[it, 0] = triangles_notdel[it].v[0]
82 | triangles_out_view[it, 1] = triangles_notdel[it].v[1]
83 | triangles_out_view[it, 2] = triangles_notdel[it].v[2]
84 |
85 | # Clear vertices and triangles
86 | vertices.clear()
87 | triangles.clear()
88 |
89 | return vertices_out, triangles_out
--------------------------------------------------------------------------------
/im2mesh/utils/libsimplify/test.py:
--------------------------------------------------------------------------------
1 | from simplify_mesh import mesh_simplify
2 | import numpy as np
3 |
4 | v = np.random.rand(100, 3)
5 | f = np.random.choice(range(100), (50, 3))
6 |
7 | mesh_simplify(v, f, 50)
--------------------------------------------------------------------------------
/im2mesh/utils/libvoxelize/.gitignore:
--------------------------------------------------------------------------------
1 | voxelize.c
2 | voxelize.html
3 | build
4 |
--------------------------------------------------------------------------------
/im2mesh/utils/libvoxelize/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Jarrome/IMT_Mapping/974547684a4474be4c281a9380d4a15150f4bf48/im2mesh/utils/libvoxelize/__init__.py
--------------------------------------------------------------------------------
/im2mesh/utils/libvoxelize/voxelize.pyx:
--------------------------------------------------------------------------------
1 | cimport cython
2 | from libc.math cimport floor, ceil
3 | from cython.view cimport array as cvarray
4 |
5 | cdef extern from "tribox2.h":
6 | int triBoxOverlap(float boxcenter[3], float boxhalfsize[3],
7 | float tri0[3], float tri1[3], float tri2[3])
8 |
9 |
10 | @cython.boundscheck(False) # Deactivate bounds checking
11 | @cython.wraparound(False) # Deactivate negative indexing.
12 | cpdef int voxelize_mesh_(bint[:, :, :] occ, float[:, :, ::1] faces):
13 | assert(faces.shape[1] == 3)
14 | assert(faces.shape[2] == 3)
15 |
16 | n_faces = faces.shape[0]
17 | cdef int i
18 | for i in range(n_faces):
19 | voxelize_triangle_(occ, faces[i])
20 |
21 |
22 | @cython.boundscheck(False) # Deactivate bounds checking
23 | @cython.wraparound(False) # Deactivate negative indexing.
24 | cpdef int voxelize_triangle_(bint[:, :, :] occupancies, float[:, ::1] triverts):
25 | cdef int bbox_min[3]
26 | cdef int bbox_max[3]
27 | cdef int i, j, k
28 | cdef float boxhalfsize[3]
29 | cdef float boxcenter[3]
30 | cdef bint intersection
31 |
32 | boxhalfsize[:] = (0.5, 0.5, 0.5)
33 |
34 | for i in range(3):
35 | bbox_min[i] = (
36 | min(triverts[0, i], triverts[1, i], triverts[2, i])
37 | )
38 | bbox_min[i] = min(max(bbox_min[i], 0), occupancies.shape[i] - 1)
39 |
40 | for i in range(3):
41 | bbox_max[i] = (
42 | max(triverts[0, i], triverts[1, i], triverts[2, i])
43 | )
44 | bbox_max[i] = min(max(bbox_max[i], 0), occupancies.shape[i] - 1)
45 |
46 | for i in range(bbox_min[0], bbox_max[0] + 1):
47 | for j in range(bbox_min[1], bbox_max[1] + 1):
48 | for k in range(bbox_min[2], bbox_max[2] + 1):
49 | boxcenter[:] = (i + 0.5, j + 0.5, k + 0.5)
50 | intersection = triBoxOverlap(&boxcenter[0], &boxhalfsize[0],
51 | &triverts[0, 0], &triverts[1, 0], &triverts[2, 0])
52 | occupancies[i, j, k] |= intersection
53 |
54 |
55 | @cython.boundscheck(False) # Deactivate bounds checking
56 | @cython.wraparound(False) # Deactivate negative indexing.
57 | cdef int test_triangle_aabb(float[::1] boxcenter, float[::1] boxhalfsize, float[:, ::1] triverts):
58 | assert(boxcenter.shape[0] == 3)
59 | assert(boxhalfsize.shape[0] == 3)
60 | assert(triverts.shape[0] == triverts.shape[1] == 3)
61 |
62 | # print(triverts)
63 | # Call functions
64 | cdef int result = triBoxOverlap(&boxcenter[0], &boxhalfsize[0],
65 | &triverts[0, 0], &triverts[1, 0], &triverts[2, 0])
66 | return result
67 |
--------------------------------------------------------------------------------
/im2mesh/utils/visualize.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from matplotlib import pyplot as plt
3 | from mpl_toolkits.mplot3d import Axes3D
4 | from torchvision.utils import save_image
5 | import im2mesh.common as common
6 |
7 |
8 | def visualize_data(data, data_type, out_file):
9 | r''' Visualizes the data with regard to its type.
10 |
11 | Args:
12 | data (tensor): batch of data
13 | data_type (string): data type (img, voxels or pointcloud)
14 | out_file (string): output file
15 | '''
16 | if data_type == 'img':
17 | if data.dim() == 3:
18 | data = data.unsqueeze(0)
19 | save_image(data, out_file, nrow=4)
20 | elif data_type == 'voxels':
21 | visualize_voxels(data, out_file=out_file)
22 | elif data_type == 'pointcloud':
23 | visualize_pointcloud(data, out_file=out_file)
24 | elif data_type is None or data_type == 'idx':
25 | pass
26 | else:
27 | raise ValueError('Invalid data_type "%s"' % data_type)
28 |
29 |
30 | def visualize_voxels(voxels, out_file=None, show=False):
31 | r''' Visualizes voxel data.
32 |
33 | Args:
34 | voxels (tensor): voxel data
35 | out_file (string): output file
36 | show (bool): whether the plot should be shown
37 | '''
38 | # Use numpy
39 | voxels = np.asarray(voxels)
40 | # Create plot
41 | fig = plt.figure()
42 | ax = fig.gca(projection=Axes3D.name)
43 | voxels = voxels.transpose(2, 0, 1)
44 | ax.voxels(voxels, edgecolor='k')
45 | ax.set_xlabel('Z')
46 | ax.set_ylabel('X')
47 | ax.set_zlabel('Y')
48 | ax.view_init(elev=30, azim=45)
49 | if out_file is not None:
50 | plt.savefig(out_file)
51 | if show:
52 | plt.show()
53 | plt.close(fig)
54 |
55 |
56 | def visualize_pointcloud(points, normals=None,
57 | out_file=None, show=False):
58 | r''' Visualizes point cloud data.
59 |
60 | Args:
61 | points (tensor): point data
62 | normals (tensor): normal data (if existing)
63 | out_file (string): output file
64 | show (bool): whether the plot should be shown
65 | '''
66 | # Use numpy
67 | points = np.asarray(points)
68 | # Create plot
69 | fig = plt.figure()
70 | ax = fig.gca(projection=Axes3D.name)
71 | ax.scatter(points[:, 2], points[:, 0], points[:, 1])
72 | if normals is not None:
73 | ax.quiver(
74 | points[:, 2], points[:, 0], points[:, 1],
75 | normals[:, 2], normals[:, 0], normals[:, 1],
76 | length=0.1, color='k'
77 | )
78 | ax.set_xlabel('Z')
79 | ax.set_ylabel('X')
80 | ax.set_zlabel('Y')
81 | ax.set_xlim(-0.5, 0.5)
82 | ax.set_ylim(-0.5, 0.5)
83 | ax.set_zlim(-0.5, 0.5)
84 | ax.view_init(elev=30, azim=45)
85 | if out_file is not None:
86 | plt.savefig(out_file)
87 | if show:
88 | plt.show()
89 | plt.close(fig)
90 |
91 |
92 | def visualise_projection(
93 | self, points, world_mat, camera_mat, img, output_file='out.png'):
94 | r''' Visualizes the transformation and projection to image plane.
95 |
96 | The first points of the batch are transformed and projected to the
97 | respective image. After performing the relevant transformations, the
98 | visualization is saved in the provided output_file path.
99 |
100 | Arguments:
101 | points (tensor): batch of point cloud points
102 | world_mat (tensor): batch of matrices to rotate pc to camera-based
103 | coordinates
104 | camera_mat (tensor): batch of camera matrices to project to 2D image
105 | plane
106 | img (tensor): tensor of batch GT image files
107 | output_file (string): where the output should be saved
108 | '''
109 | points_transformed = common.transform_points(points, world_mat)
110 | points_img = common.project_to_camera(points_transformed, camera_mat)
111 | pimg2 = points_img[0].detach().cpu().numpy()
112 | image = img[0].cpu().numpy()
113 | plt.imshow(image.transpose(1, 2, 0))
114 | plt.plot(
115 | (pimg2[:, 0] + 1)*image.shape[1]/2,
116 | (pimg2[:, 1] + 1) * image.shape[2]/2, 'x')
117 | plt.savefig(output_file)
118 |
--------------------------------------------------------------------------------
/im2mesh/vnn_onet/__init__.py:
--------------------------------------------------------------------------------
1 | from im2mesh.vnn_onet import (
2 | config, generation, training, models
3 | )
4 |
5 | __all__ = [
6 | config, generation, training, models
7 | ]
--------------------------------------------------------------------------------
/im2mesh/vnn_onet/models/encoder_latent.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | import torch.nn.functional as F
4 |
5 |
6 | # Max Pooling operation
7 | def maxpool(x, dim=-1, keepdim=False):
8 | out, _ = x.max(dim=dim, keepdim=keepdim)
9 | return out
10 |
11 |
12 | class Encoder(nn.Module):
13 | ''' Latent encoder class.
14 | It encodes the input points and returns mean and standard deviation for the
15 | posterior Gaussian distribution.
16 | Args:
17 | z_dim (int): dimension if output code z
18 | c_dim (int): dimension of latent conditioned code c
19 | dim (int): input dimension
20 | leaky (bool): whether to use leaky ReLUs
21 | '''
22 | def __init__(self, z_dim=128, c_dim=128, dim=3, leaky=False):
23 | super().__init__()
24 | self.z_dim = z_dim
25 | self.c_dim = c_dim
26 |
27 | # Submodules
28 | self.fc_pos = nn.Linear(dim, 128)
29 |
30 | if c_dim != 0:
31 | self.fc_c = nn.Linear(c_dim, 128)
32 |
33 | self.fc_0 = nn.Linear(1, 128)
34 | self.fc_1 = nn.Linear(128, 128)
35 | self.fc_2 = nn.Linear(256, 128)
36 | self.fc_3 = nn.Linear(256, 128)
37 | self.fc_mean = nn.Linear(128, z_dim)
38 | self.fc_logstd = nn.Linear(128, z_dim)
39 |
40 | if not leaky:
41 | self.actvn = F.relu
42 | self.pool = maxpool
43 | else:
44 | self.actvn = lambda x: F.leaky_relu(x, 0.2)
45 | self.pool = torch.mean
46 |
47 | def forward(self, p, x, c=None, **kwargs):
48 | batch_size, T, D = p.size()
49 |
50 | # output size: B x T X F
51 | net = self.fc_0(x.unsqueeze(-1))
52 | net = net + self.fc_pos(p)
53 |
54 | if self.c_dim != 0:
55 | net = net + self.fc_c(c).unsqueeze(1)
56 |
57 | net = self.fc_1(self.actvn(net))
58 | pooled = self.pool(net, dim=1, keepdim=True).expand(net.size())
59 | net = torch.cat([net, pooled], dim=2)
60 |
61 | net = self.fc_2(self.actvn(net))
62 | pooled = self.pool(net, dim=1, keepdim=True).expand(net.size())
63 | net = torch.cat([net, pooled], dim=2)
64 |
65 | net = self.fc_3(self.actvn(net))
66 | # Reduce
67 | # to B x F
68 | net = self.pool(net, dim=1)
69 |
70 | mean = self.fc_mean(net)
71 | logstd = self.fc_logstd(net)
72 |
73 | return mean, logstd
--------------------------------------------------------------------------------
/network/criterion.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn.functional as F
3 | import torch.distributions
4 |
5 |
6 | # ----------------------------------
7 | # DATA CRITERIA:
8 | # ----------------------------------
9 |
10 | def l1_loss(args, info: dict, pd_sdf: torch.Tensor, gt_sdf: torch.Tensor, **kwargs):
11 | """
12 | L1 Loss: make pd_sdf and gt_sdf as close as possible.
13 | Theoretically, this loss is way easier than siren loss.
14 | But siren's advantage is that:
15 | 1) it does not perturbed samples.
16 | 2) it does not need perturbed gt_sdf.
17 | :return:
18 | """
19 | if args.enforce_minmax:
20 | gt_sdf = torch.clamp(gt_sdf, -args.clamping_distance, args.clamping_distance)
21 | pd_sdf = torch.clamp(pd_sdf, -args.clamping_distance, args.clamping_distance)
22 |
23 | sdf_loss = (gt_sdf - pd_sdf).abs().sum() / info["num_sdf_samples"]
24 | return {
25 | 'sdf': sdf_loss
26 | }
27 |
28 |
29 | def neg_log_likelihood(args, info: dict, pd_sdf: torch.Tensor, pd_sdf_std: torch.Tensor,
30 | gt_sdf: torch.Tensor, **kwargs):
31 | """
32 | Negative log likelihood of gt data under predicted gaussian distribution.
33 | """
34 | if args.enforce_minmax:
35 | gt_sdf = torch.clamp(gt_sdf, -args.clamping_distance, args.clamping_distance)
36 | pd_sdf = torch.clamp(pd_sdf, -args.clamping_distance, args.clamping_distance)
37 |
38 | pd_dist = torch.distributions.Normal(loc=pd_sdf.squeeze(), scale=pd_sdf_std.squeeze())
39 | # print(pd_dist.log_prob(gt_sdf.squeeze()))
40 | sdf_loss = -pd_dist.log_prob(gt_sdf.squeeze()).sum() / info["num_sdf_samples"]
41 | return {
42 | 'll': sdf_loss
43 | }
44 |
45 |
46 | def siren_loss(args, info: dict, pd_sdf: torch.Tensor, coords: torch.Tensor, gt_sdf: torch.Tensor, **kwargs):
47 | """
48 | SIREN Loss: Boundary sdf value = 0 +
49 | Non-boundary sdf value != 0 +
50 | boundary normal = gt-normal +
51 | gradient's norm is 1
52 | :param pd_sdf: (B, 1)
53 | :param coords: (B, 3): The computation graph from this to pd_sdf should be provided to compute the gradient!
54 | :param gt_sdf: (B, 3) nx, ny, nz
55 | :return: loss-dict differentiable w.r.t. pd_sdf.
56 | """
57 | # Note: Here torch.ones_like(pd_sdf) is okay because each sample in the batch is not relevant.
58 | # So that the derivative of pd_sdf in batch2 (S2) w.r.t. the coords in batch1 is always 0!
59 | # Hence d(S1+...+SB)/d(xyz) = d(S1)/d(xyz)
60 | # To compute a real full Jacobian, each element in coords should be evaluated separately!
61 | pd_sdf_grad = torch.autograd.grad(pd_sdf, [coords],
62 | grad_outputs=torch.ones_like(pd_sdf), create_graph=True)[0]
63 |
64 | gt_normals = gt_sdf
65 | gt_sdf = torch.sum(gt_sdf.abs(), dim=-1, keepdim=True) > 1e-6
66 |
67 | zero_loss_branch = torch.zeros_like(pd_sdf, requires_grad=False)
68 |
69 | sdf_in_loss = torch.where(gt_sdf, pd_sdf, zero_loss_branch)
70 | sdf_out_loss = torch.where(gt_sdf, zero_loss_branch, torch.exp(-1e2 * torch.abs(pd_sdf)))
71 | normal_loss = torch.where(gt_sdf, 1. - F.cosine_similarity(pd_sdf_grad, gt_normals, dim=-1).unsqueeze(1),
72 | zero_loss_branch)
73 | eikonal_loss = torch.abs(pd_sdf_grad.norm(dim=-1) - 1)
74 |
75 | return {
76 | 'sdf_in': torch.abs(sdf_in_loss).sum() / info["num_sdf_samples"] * args.siren_sdf_in,
77 | 'sdf_out': sdf_out_loss.sum() / info["num_sdf_samples"] * args.siren_sdf_out,
78 | 'normal': normal_loss.sum() / info["num_sdf_samples"] * args.siren_normal,
79 | 'eikonal': eikonal_loss.sum() / info["num_sdf_samples"] * args.siren_eikonal
80 | }
81 |
82 |
83 | # ----------------------------------
84 | # REG CRITERIA:
85 | # ----------------------------------
86 |
87 | def reg_loss(args, info: dict, latent_vecs: torch.Tensor, **kwargs):
88 | l2_size_loss = torch.sum(torch.norm(latent_vecs, dim=1))
89 | reg_loss = min(1, info["epoch"] / 100) * l2_size_loss / info["num_sdf_samples"]
90 | return {
91 | 'reg': reg_loss * args.code_reg_lambda
92 | }
93 |
--------------------------------------------------------------------------------
/network/di_decoder.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | # Copyright 2004-present Facebook. All Rights Reserved.
3 |
4 | import torch.nn as nn
5 | import torch
6 | import torch.nn.functional as F
7 |
8 |
9 | class Model(nn.Module):
10 | def __init__(self, latent_size, dims, dropout=None, dropout_prob=0.0, norm_layers=(), latent_in=(), weight_norm=False):
11 | """
12 | :param latent_size: Size of the latent vector
13 | :param dims: Intermediate network neurons
14 | :param dropout:
15 | :param dropout_prob:
16 | :param latent_in: From which layer to re-feed in the latent_size+3 input vector
17 | :param weight_norm: Whether to use weight normalization
18 | :param norm_layers: Layers to append normalization (Either WeightNorm or LayerNorm depend of weight_norm var)
19 | """
20 | super().__init__()
21 |
22 | dims = [latent_size + 3] + dims + [1]
23 |
24 | self.num_layers = len(dims)
25 | self.norm_layers = norm_layers
26 | self.latent_in = latent_in
27 | self.weight_norm = weight_norm
28 |
29 | for layer in range(self.num_layers - 1):
30 | # a linear layer with input `dims[layer]` and output `dims[layer + 1]`.
31 | # If the next layer is going to take latent vec, we reduce output of this layer.
32 | if layer + 1 in latent_in:
33 | out_dim = dims[layer + 1] - dims[0]
34 | else:
35 | out_dim = dims[layer + 1]
36 |
37 | if weight_norm and layer in self.norm_layers:
38 | setattr(self, "lin" + str(layer),
39 | nn.utils.weight_norm(nn.Linear(dims[layer], out_dim)),
40 | )
41 | else:
42 | setattr(self, "lin" + str(layer), nn.Linear(dims[layer], out_dim))
43 |
44 | if (not weight_norm) and self.norm_layers is not None and layer in self.norm_layers:
45 | setattr(self, "bn" + str(layer), nn.LayerNorm(out_dim))
46 |
47 | self.uncertainty_layer = nn.Linear(dims[-2], 1)
48 |
49 | self.relu = nn.ReLU()
50 | self.dropout_prob = dropout_prob
51 | self.dropout = dropout
52 | self.th = nn.Tanh()
53 |
54 | # input: N x (L+3)
55 | def forward(self, input):
56 | x = input
57 | std = None
58 |
59 | for layer in range(0, self.num_layers - 1):
60 | lin = getattr(self, "lin" + str(layer))
61 | if layer in self.latent_in:
62 | x = torch.cat([x, input], 1)
63 |
64 | # For the last layer, also branch out to output uncertainty.
65 | if layer == self.num_layers - 2:
66 | std = self.uncertainty_layer(x)
67 | # std = 0.1 + 0.9 * F.softplus(std)
68 | std = 0.05 + 0.5 * F.softplus(std)
69 |
70 | x = lin(x)
71 |
72 | # For all layers other than the last layer.
73 | if layer < self.num_layers - 2:
74 | if (
75 | self.norm_layers is not None
76 | and layer in self.norm_layers
77 | and not self.weight_norm
78 | ):
79 | bn = getattr(self, "bn" + str(layer))
80 | x = bn(x)
81 | x = self.relu(x)
82 | if self.dropout is not None and layer in self.dropout:
83 | x = F.dropout(x, p=self.dropout_prob, training=self.training)
84 | x = self.th(x)
85 |
86 | return x, std
87 |
--------------------------------------------------------------------------------
/network/di_encoder.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | from utils.pt_util import SharedMLP
4 |
5 |
6 | class Model(nn.Module):
7 | def __init__(self, bn, latent_size, per_point_feat, mode='cnp'):
8 | super().__init__()
9 | assert mode in ['train', 'cnp']
10 | self.mode = mode
11 | self.aggr_mode = 'mean'
12 | self.mlp = SharedMLP(per_point_feat + [latent_size], bn=bn, last_act=False)
13 |
14 | def forward(self, x):
15 | if self.mode == 'train':
16 | # B x N x 3 -> B x latent_size
17 | x = x.transpose(-1, -2)
18 | x = self.mlp(x) # (B, L, N)
19 | if self.aggr_mode == 'max':
20 | r, _ = torch.max(x, dim=-1)
21 | elif self.aggr_mode == 'mean':
22 | r = torch.mean(x, dim=-1)
23 | else:
24 | raise NotImplementedError
25 | return r
26 | elif self.mode == 'cnp':
27 | # B x 3 -> B x latent_size
28 | x = x.unsqueeze(-1) # (B, 3, 1)
29 | x = self.mlp(x) # (B, L, 1)
30 | '''
31 | B,N,d = x.shape
32 | x = x.reshape((B*N,d)).unsqueeze(-1)
33 | x = self.mlp(x) # (BN, L, 1)
34 | _,L,_ = x.shape
35 | x = x.reshape((B,N,L,1))
36 | '''
37 |
38 |
39 | return x.squeeze(-1)
40 | else:
41 | raise NotImplementedError
42 |
--------------------------------------------------------------------------------
/network/di_vnn_encoder.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | from utils.pt_util import SharedMLP
4 | from network.vnn import MY_VNN_SimplePointnet,VNN_ResnetPointnet
5 |
6 |
7 | class Model(nn.Module):
8 | def __init__(self, bn, latent_size, per_point_feat, mode='cnp'):
9 | super().__init__()
10 | assert mode in ['train', 'cnp']
11 | self.mode = mode
12 | self.vnn = MY_VNN_SimplePointnet(c_dim=latent_size, hidden_dim=per_point_feat[2],meta_output='equivariant_latent')
13 | def forward(self, x):
14 | if self.mode == 'train':
15 | # B x N x 3 -> B x latent_size
16 | #x = x.transpose(-1, -2)
17 | r = self.vnn(x[:,:,:])
18 | return r.reshape((r.shape[0],-1))
19 | elif self.mode == 'cnp':
20 | x = x.unsqueeze(1) #input is B,1,3
21 | r = self.vnn(x[:,:,:]) # (B, L)
22 | return r
23 | else:
24 | raise NotImplementedError
25 |
--------------------------------------------------------------------------------
/network_trainer.py:
--------------------------------------------------------------------------------
1 | ./trainer/main.py
--------------------------------------------------------------------------------
/pose_fmt.py:
--------------------------------------------------------------------------------
1 | import os
2 | import numpy as np
3 |
4 | from itertools import chain
5 | from collections import defaultdict
6 | from pyquaternion import Quaternion
7 | import glob
8 | import time
9 |
10 | import ifr.ifr_main as ifr
11 | from ifr.ifr_main import vis_param
12 |
13 | import pdb
14 |
15 |
16 | # namespace
17 | args = vis_param.args
18 | sequence = vis_param.sequence
19 | # scene name
20 | scene = args.scene
21 | # where the pose stream is
22 | elasticfusion_pose_folder = args.pose_folder
23 | # where we store the incremental result for demonstration
24 | os.makedirs(args.outdir,exist_ok=True)
25 |
26 |
27 | contains = glob.glob(elasticfusion_pose_folder+'poses_*.txt')
28 | idxs = [int(ct.split('.')[-2].split('_')[-1]) for ct in contains]
29 | contains_dict = {}
30 | for i,idx in enumerate(idxs):
31 | contains_dict[idx] = contains[i]
32 |
33 |
34 | def read_elasticfusion_file(pose_id, kf_idx):
35 | with open(contains_dict[pose_id]) as f:
36 | lines = f.readlines()
37 | poses = []
38 | for line_id, line in enumerate(lines):
39 | if line_id in kf_idx:
40 | vs = [float(v) for v in line.strip().split(' ')]
41 | v_t = vs[1:4]
42 | #v_q = vs[4:] # xyzw
43 | v_q = Quaternion(vs[-1],*vs[4:-1])
44 | pose = v_q.transformation_matrix
45 | pose[:3,3] = np.array(v_t)
46 | poses.append(pose)
47 | return poses
48 |
49 |
50 |
51 |
52 |
53 | if __name__ == '__main__':
54 | import os
55 | import sys
56 | from dataset_ptam import TUMRGBDDataset, ICLNUIMDataset, ReplicaRGBDDataset
57 |
58 | dataset = args.dataset_type
59 |
60 | if 'tum' in dataset.lower():
61 | dataset = TUMRGBDDataset(sequence.path)
62 | elif 'replica' in dataset.lower():
63 | dataset = ReplicaRGBDDataset(sequence.path)
64 | else:
65 | assert "Not supported data type"
66 |
67 | '''
68 | load gt traj to check correctness
69 | '''
70 | GT = args.use_gt
71 | if GT:
72 | gt_traj = np.genfromtxt(str(sequence.path)+'/livingRoom'+scene+'.gt.freiburg')
73 | gt_poses = []
74 |
75 |
76 | durations = []
77 | data_i = 0
78 | kf_idx = []
79 | def run_algo(vis):
80 | global data_i
81 | i = data_i#data_next()
82 | data_i += 1
83 |
84 |
85 | if i % 20 == 0:#
86 | is_keyframe = True
87 | kf_idx.append(i)
88 | else:
89 | is_keyframe = False
90 | if dataset.timestamps is None:
91 | timestamp = i / 20.
92 | else:
93 | timestamp = dataset.timestamps[i]
94 |
95 | time_start = time.time()
96 |
97 | # 0. check if current keyframe
98 | if is_keyframe:
99 | gt_pose = gt_traj[i,:] if GT else None
100 | # 1. prepare current frame to get torch frame_data
101 | frame_data = (dataset.rgb[i],dataset.depth[i])
102 |
103 | # 2. get all the poses of keyframe
104 | new_poses = []
105 | if not GT:
106 | poses = read_elasticfusion_file(i, kf_idx)
107 | new_poses= poses
108 | else:
109 | gt_poses.append(gt_pose)
110 | new_poses = gt_poses
111 |
112 | # 3.2 if some pose changed, update map
113 | ifr.refresh(frame_data, new_poses, frame_id = i, vis=vis, ptam_p = not GT, scene_name = 'lrkt'+scene)
114 | else:
115 | return
116 |
117 |
118 | duration = time.time() - time_start
119 | durations.append(duration)
120 | print('duration', duration)
121 | print()
122 | print()
123 |
124 | if ifr.engine:
125 | ifr.engine.register_animation_callback(callback_func = run_algo)
126 | vis_ph = ifr.vis_util.wireframe_bbox([-4., -4., -4.], [4., 4., 4.])
127 | ifr.engine.add_geometry(vis_ph)
128 | ifr.engine.remove_geometry(vis_ph, reset_bounding_box=False)
129 | ifr.engine.run()
130 | ifr.engine.destroy_window()
131 | else:
132 | try:
133 | while True:
134 | run_algo(None)
135 | except Exception as e:
136 | print(e)
137 |
138 |
139 |
140 | print('num frames', len(durations))
141 | print('num keyframes', len(kf_idx))
142 | print('average time', np.mean(durations))
143 |
144 |
--------------------------------------------------------------------------------
/sampler_cuda/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | project(Sampler LANGUAGES CXX CUDA)
2 | cmake_minimum_required(VERSION 3.8)
3 |
4 | find_package(CLI11 CONFIG REQUIRED)
5 | find_package(Eigen3 3.3 REQUIRED)
6 | find_package(Pangolin REQUIRED)
7 | find_package(flann REQUIRED)
8 |
9 | add_executable(PreprocessMeshCUDA PreprocessMesh.cu ShaderProgram.cpp Utils.cu)
10 | target_link_libraries(PreprocessMeshCUDA PRIVATE Eigen3::Eigen CLI11::CLI11 pangolin flann_cuda -lcurand)
11 | target_compile_features(PreprocessMeshCUDA PRIVATE cxx_std_14)
12 | set_target_properties(PreprocessMeshCUDA PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin")
13 | target_compile_options(PreprocessMeshCUDA PRIVATE
14 | $<$: --use_fast_math>
15 | $<$: -fPIC -O3 -march=native >
16 | )
17 | set_target_properties(PreprocessMeshCUDA PROPERTIES CUDA_SEPARABLE_COMPILATION ON)
18 |
--------------------------------------------------------------------------------
/sampler_cuda/ShaderProgram.cpp:
--------------------------------------------------------------------------------
1 | // Copyright 2004-present Facebook. All Rights Reserved.
2 |
3 | #include
4 |
5 | constexpr const char* shaderText = R"Shader(
6 | @start vertex
7 | #version 330 core
8 |
9 | layout(location = 0) in vec3 vertex;
10 |
11 | out vec4 position_world;
12 | out vec4 position_camera;
13 | out vec3 viewDirection_camera;
14 |
15 | uniform mat4 MVP;
16 | uniform mat4 V;
17 |
18 | void main(){
19 |
20 | // Projected image coordinate
21 | gl_Position = MVP * vec4(vertex,1);
22 |
23 | // world coordinate location of the vertex
24 | position_world = vec4(vertex,1);
25 | position_camera = V * vec4(vertex, 1);
26 |
27 | viewDirection_camera = normalize(vec3(0,0,0) - position_camera.xyz);
28 |
29 | }
30 |
31 | @start geometry
32 | #version 330
33 |
34 | layout ( triangles ) in;
35 | layout ( triangle_strip, max_vertices = 3 ) out;
36 |
37 | in vec4 position_world[];
38 | in vec3 viewDirection_camera[];
39 |
40 | out vec3 normal_camera;
41 | out vec3 normal_world;
42 | out vec4 xyz_world;
43 | out vec3 viewDirection_cam;
44 | out vec4 xyz_camera;
45 |
46 | uniform mat4 V;
47 |
48 | void main()
49 | {
50 | vec3 A = position_world[1].xyz - position_world[0].xyz;
51 | vec3 B = position_world[2].xyz - position_world[0].xyz;
52 | vec3 normal = normalize(cross(A,B));
53 | vec3 normal_cam = (V * vec4(normal,0)).xyz;
54 |
55 | gl_Position = gl_in[0].gl_Position;
56 | normal_camera = normal_cam;
57 | normal_world = normal;
58 | xyz_world = position_world[0];
59 | xyz_camera = V * xyz_world;
60 | viewDirection_cam = viewDirection_camera[0];
61 | gl_PrimitiveID = gl_PrimitiveIDIn;
62 | EmitVertex();
63 |
64 | gl_Position = gl_in[1].gl_Position;
65 | normal_camera = normal_cam;
66 | normal_world = normal;
67 | xyz_world = position_world[1];
68 | xyz_camera = V * xyz_world;
69 | viewDirection_cam = viewDirection_camera[1];
70 | gl_PrimitiveID = gl_PrimitiveIDIn;
71 |
72 | EmitVertex();
73 |
74 | gl_Position = gl_in[2].gl_Position;
75 | normal_camera = normal_cam;
76 | normal_world = normal;
77 | xyz_world = position_world[2];
78 | xyz_camera = V * xyz_world;
79 | viewDirection_cam = viewDirection_camera[2];
80 | gl_PrimitiveID = gl_PrimitiveIDIn;
81 |
82 | EmitVertex();
83 | EndPrimitive();
84 | }
85 |
86 | @start fragment
87 | #version 330 core
88 |
89 | in vec3 viewDirection_cam;
90 | in vec3 normal_world;
91 | in vec3 normal_camera;
92 | in vec4 xyz_world;
93 | in vec4 xyz_camera;
94 | in int gl_PrimitiveID ;
95 |
96 | layout(location = 0) out vec4 out_xyz;
97 | layout(location = 1) out vec4 out_normal;
98 |
99 | void main(){
100 | vec3 view_vector = vec3(0,0,1);
101 |
102 | // Check if we need to flip the normal.
103 | vec3 normal_world_cor;
104 | float d = dot(normalize(normal_camera), normalize(view_vector));
105 |
106 | if (abs(d) < 0.001) {
107 | out_xyz = vec4(0,0,0,0);
108 | out_normal = vec4(0,0,0,0);
109 | return;
110 | }
111 | else{
112 | if (d < 0) {
113 | normal_world_cor = -normal_world;
114 | } else {
115 | normal_world_cor= normal_world;
116 | }
117 |
118 | out_xyz = xyz_world;
119 | out_xyz.w = gl_PrimitiveID + 1.0f;
120 |
121 | out_normal = vec4(normalize(normal_world_cor),1);
122 | out_normal.w = gl_PrimitiveID + 1.0f;
123 | }
124 | }
125 | )Shader";
126 |
127 | pangolin::GlSlProgram GetShaderProgram() {
128 | pangolin::GlSlProgram program;
129 |
130 | program.AddShader(pangolin::GlSlAnnotatedShader, shaderText);
131 | program.Link();
132 |
133 | return program;
134 | }
135 |
--------------------------------------------------------------------------------
/sampler_cuda/Utils.h:
--------------------------------------------------------------------------------
1 | // Copyright 2004-present Facebook. All Rights Reserved.
2 | // The GPU version is drafted by heiwang1997@github.com
3 |
4 | #include
5 |
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 | std::vector EquiDistPointsOnSphere(const uint numSamples, const float radius);
14 |
15 | unsigned int ValidPointsNormalCUDA(cudaArray_t normals, cudaArray_t verts, unsigned int img_width, unsigned int img_height,
16 | thrust::device_vector& tri_pos, thrust::device_vector& tri_total,
17 | float4* output_normals, float4* output_xyz);
18 |
19 | __device__ int lower_bound(const float* __restrict__ A, float val, int n);
20 |
21 | __global__ void TriangleAreaKernel(unsigned char* __restrict__ vertices, size_t vertices_pitch,
22 | unsigned char* __restrict__ triangles, size_t triangles_pitch, size_t num_tris, float* __restrict__ areas);
23 |
24 | __global__ void RNGSetupKernel(curandState *state, size_t num_kernel);
25 |
26 | void ComputeNormalizationParameters(
27 | pangolin::Geometry& geom,
28 | const float buffer, float2& ub_x, float2& ub_y, float2& ub_z);
29 |
30 | void LinearizeObject(pangolin::Geometry& geom);
31 |
32 | #ifndef cudaSafeCall
33 | #define cudaSafeCall(err) __cudaSafeCall(err, __FILE__, __LINE__)
34 |
35 | inline void __cudaSafeCall( cudaError err, const char *file, const int line )
36 | {
37 | if( cudaSuccess != err) {
38 | printf("%s(%i) : cudaSafeCall() Runtime API error : %s.\n",
39 | file, line, cudaGetErrorString(err) );
40 | exit(-1);
41 | }
42 | }
43 |
44 | #endif
45 |
46 | #ifndef cudaKernelCheck
47 |
48 | // For normal operation
49 | #define cudaKernelCheck
50 |
51 | // For debugging purposes
52 | //#define cudaKernelCheck { cudaDeviceSynchronize(); __cudaSafeCall(cudaPeekAtLastError(), __FILE__, __LINE__); }
53 |
54 | #endif
--------------------------------------------------------------------------------
/system/ext/__init__.py:
--------------------------------------------------------------------------------
1 | from pathlib import Path
2 | from torch.utils.cpp_extension import load
3 |
4 |
5 | def p(rel_path):
6 | abs_path = Path(__file__).parent / rel_path
7 | return str(abs_path)
8 |
9 |
10 | __COMPILE_VERBOSE = True
11 |
12 | # Load in Marching cubes.
13 | _marching_cubes_module = load(name='marching_cubes',
14 | sources=[p('marching_cubes/mc.cpp'),
15 | p('marching_cubes/mc_interp_kernel.cu')],
16 | verbose=__COMPILE_VERBOSE)
17 | marching_cubes_interp = _marching_cubes_module.marching_cubes_sparse_interp
18 |
19 | # Load in Image processing modules.
20 | _imgproc_module = load(name='imgproc',
21 | sources=[p('imgproc/imgproc.cu'), p('imgproc/imgproc.cpp'), p('imgproc/photometric.cu')],
22 | verbose=__COMPILE_VERBOSE)
23 | unproject_depth = _imgproc_module.unproject_depth
24 | compute_normal_weight = _imgproc_module.compute_normal_weight
25 | compute_normal_weight_robust = _imgproc_module.compute_normal_weight_robust
26 | filter_depth = _imgproc_module.filter_depth
27 | rgb_odometry = _imgproc_module.rgb_odometry
28 | gradient_xy = _imgproc_module.gradient_xy
29 |
30 | # Load in Indexing modules. (which deal with complicated indexing scheme)
31 | _indexing_module = load(name='indexing',
32 | sources=[p('indexing/indexing.cpp'), p('indexing/indexing.cu')],
33 | verbose=__COMPILE_VERBOSE)
34 | pack_batch = _indexing_module.pack_batch
35 | groupby_sum = _indexing_module.groupby_sum
36 |
37 | # We need point cloud processing module.
38 | _pcproc_module = load(name='pcproc',
39 | sources=[p('pcproc/pcproc.cpp'), p('pcproc/pcproc.cu'), p('pcproc/cuda_kdtree.cu')],
40 | verbose=__COMPILE_VERBOSE)
41 | remove_radius_outlier = _pcproc_module.remove_radius_outlier
42 | estimate_normals = _pcproc_module.estimate_normals
43 |
--------------------------------------------------------------------------------
/system/ext/imgproc/common.cuh:
--------------------------------------------------------------------------------
1 | #include
2 |
3 | #include
4 | #include
5 | #include
6 |
7 | #define CHECK_CUDA(x) TORCH_CHECK(x.is_cuda(), #x " must be a CUDA tensor")
8 | #define CHECK_CONTIGUOUS(x) TORCH_CHECK(x.is_contiguous(), #x " must be contiguous")
9 | #define CHECK_INPUT(x) CHECK_CUDA(x); CHECK_CONTIGUOUS(x)
10 |
11 | using DepthAccessor = torch::PackedTensorAccessor32;
12 | using PCMapAccessor = torch::PackedTensorAccessor32;
13 | using IntensityAccessor = torch::PackedTensorAccessor32;
14 | using GradientAccessor = torch::PackedTensorAccessor32;
15 | //using MaskAccessor = torch::PackedTensorAccessor32;
16 |
17 | struct matrix3
18 | {
19 | float3 r1{0.0, 0.0, 0.0};
20 | float3 r2{0.0, 0.0, 0.0};
21 | float3 r3{0.0, 0.0, 0.0};
22 |
23 | explicit matrix3(const std::vector& data) {
24 | r1.x = data[0]; r1.y = data[1]; r1.z = data[2];
25 | r2.x = data[3]; r2.y = data[4]; r2.z = data[5];
26 | r3.x = data[6]; r3.y = data[7]; r3.z = data[8];
27 | }
28 |
29 | __host__ __device__ float3 operator*(const float3& rv) const {
30 | return make_float3(
31 | r1.x * rv.x + r1.y * rv.y + r1.z * rv.z,
32 | r2.x * rv.x + r2.y * rv.y + r2.z * rv.z,
33 | r3.x * rv.x + r3.y * rv.y + r3.z * rv.z);
34 | }
35 | };
36 |
37 | static uint div_up(const uint a, const uint b) {
38 | return (a + b - 1) / b;
39 | }
40 |
41 | __device__ __forceinline__ static float3 get_vec3(const PCMapAccessor map, const uint i, const uint j) {
42 | return make_float3(map[i][j][0], map[i][j][1], map[i][j][2]);
43 | }
44 |
45 | inline __host__ __device__ float3 operator+(const float3& a, const float3& b) {
46 | return make_float3(a.x + b.x, a.y + b.y, a.z + b.z);
47 | }
48 |
49 | inline __host__ __device__ float3 operator-(const float3& a, const float3& b) {
50 | return make_float3(a.x - b.x, a.y - b.y, a.z - b.z);
51 | }
52 |
53 | inline __host__ __device__ void operator/=(float3 &a, float b) {
54 | a.x /= b; a.y /= b; a.z /= b;
55 | }
56 |
57 | inline __host__ __device__ void operator+=(float3 &a, const float3& b) {
58 | a.x += b.x; a.y += b.y; a.z += b.z;
59 | }
60 |
61 | inline __host__ __device__ void operator-=(float3 &a, const float3& b) {
62 | a.x -= b.x; a.y -= b.y; a.z -= b.z;
63 | }
64 |
65 | inline __host__ __device__ float3 cross(const float3& a, const float3& b) {
66 | return make_float3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
67 | }
68 |
69 | inline __host__ __device__ float dot(const float3& a, const float3& b) {
70 | return a.x * b.x + a.y * b.y + a.z * b.z;
71 | }
72 |
73 | inline __host__ __device__ float length(const float3& v) {
74 | return sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
75 | }
76 |
77 | inline __host__ __device__ float squared_length(const float3& v) {
78 | return v.x * v.x + v.y * v.y + v.z * v.z;
79 | }
80 |
--------------------------------------------------------------------------------
/system/ext/imgproc/imgproc.cpp:
--------------------------------------------------------------------------------
1 | #include
2 |
3 | torch::Tensor unproject_depth(torch::Tensor depth, float fx, float fy, float cx, float cy);
4 | //torch::Tensor unproject_depth(torch::Tensor depth, float fx, float fy, float cx, float cy);
5 |
6 | // We might do this several times, this interface enables re-use memory.
7 | void filter_depth(torch::Tensor depth_in, torch::Tensor depth_out);
8 | torch::Tensor compute_normal_weight(torch::Tensor pc_map);
9 | torch::Tensor compute_normal_weight_robust(torch::Tensor pc_map);
10 |
11 | // Compute rgb-image based odometry. Will return per-correspondence residual (M, ) and jacobian (M, 6) w.r.t. lie algebra.
12 | // ... based on current given estimate ( T(xi) * prev_xyz = cur_xyz ).
13 | // prev_intensity (H, W), float32, raning from 0 to 1.
14 | std::vector rgb_odometry(
15 | torch::Tensor prev_intensity, torch::Tensor prev_depth,
16 | torch::Tensor cur_intensity, torch::Tensor cur_depth, torch::Tensor cur_dIdxy,
17 | const std::vector& intr,
18 | const std::vector& krkinv_data,
19 | const std::vector& kt_data,
20 | float min_grad_scale, float max_depth_delta, bool compute_J);
21 | torch::Tensor gradient_xy(torch::Tensor cur_intensity);
22 |
23 | PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
24 | m.def("unproject_depth", &unproject_depth, "Unproject Depth (CUDA)");
25 | m.def("filter_depth", &filter_depth, "Filter Depth (CUDA)");
26 | m.def("compute_normal_weight", &compute_normal_weight, "Compute normal and weight (CUDA)");
27 | m.def("compute_normal_weight_robust", &compute_normal_weight_robust, "Compute normal and weight (CUDA)");
28 | m.def("rgb_odometry", &rgb_odometry, "Compute the function value and gradient for RGB Odometry (CUDA)");
29 | m.def("gradient_xy", &gradient_xy, "Compute Gradient of an image for jacobian computation. (CUDA)");
30 | }
31 |
--------------------------------------------------------------------------------
/system/ext/indexing/indexing.cpp:
--------------------------------------------------------------------------------
1 | #include
2 |
3 | torch::Tensor pack_batch(torch::Tensor indices, uint n_batch, uint n_point);
4 | std::vector groupby_sum(torch::Tensor values, torch::Tensor indices, uint C);
5 |
6 | PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
7 | m.def("pack_batch", &pack_batch, "Pack Batch (CUDA)");
8 | m.def("groupby_sum", &groupby_sum, "GroupBy Sum (CUDA)");
9 | }
10 |
--------------------------------------------------------------------------------
/system/ext/indexing/indexing.cu:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 |
4 | #define CHECK_CUDA(x) TORCH_CHECK(x.is_cuda(), #x " must be a CUDA tensor")
5 | #define CHECK_CONTIGUOUS(x) TORCH_CHECK(x.is_contiguous(), #x " must be contiguous")
6 | #define CHECK_INPUT(x) CHECK_CUDA(x); CHECK_CONTIGUOUS(x)
7 |
8 | using CountAccessor = torch::PackedTensorAccessor32;
9 | using IndexAccessor = torch::PackedTensorAccessor32;
10 | using PackedIndAccessor = torch::PackedTensorAccessor32;
11 | using ValueAccessor = torch::PackedTensorAccessor32;
12 |
13 | static uint div_up(const uint a, const uint b) {
14 | return (a + b - 1) / b;
15 | }
16 |
17 | __global__ static void pack_batch_kernel(const IndexAccessor indices, PackedIndAccessor packed_inds, const uint n_all,
18 | const uint n_batch, const uint n_point, int* __restrict__ filled_count) {
19 | const uint i_data = blockIdx.x * blockDim.x + threadIdx.x;
20 | if (i_data >= n_all) {
21 | return;
22 | }
23 |
24 | long i_group = indices[i_data];
25 | if (i_group >= n_batch) {
26 | return;
27 | }
28 |
29 | // Get one valid id.
30 | int cur_count = atomicAdd(filled_count + i_group, 1);
31 | if (cur_count >= n_point) {
32 | return;
33 | }
34 | packed_inds[i_group][cur_count] = i_data;
35 | }
36 |
37 | __device__ static float atomicMax(float* __restrict__ address, float val) {
38 | int* address_as_i = (int*) address;
39 | int old = *address_as_i, assumed;
40 | do {
41 | assumed = old;
42 | old = ::atomicCAS(address_as_i, assumed,
43 | __float_as_int(::fmaxf(val, __int_as_float(assumed))));
44 | } while (assumed != old);
45 | return __int_as_float(old);
46 | }
47 |
48 | __global__ static void groupby_max_kernel(const ValueAccessor values, const IndexAccessor indices, ValueAccessor reduced_values) {
49 | const uint i = blockIdx.x;
50 | const uint l = threadIdx.x;
51 |
52 | float value = values[i][l];
53 | long index = indices[i];
54 |
55 | float* rptr = reduced_values[index].data() + l;
56 | atomicMax(rptr, value);
57 | }
58 |
59 | __global__ static void groupby_sum_kernel(const ValueAccessor values, const IndexAccessor indices, ValueAccessor sum_values, CountAccessor sum_counts) {
60 | const uint i = blockIdx.x;
61 | const uint l = threadIdx.x;
62 |
63 | float value = values[i][l];
64 | long index = indices[i];
65 |
66 | float* rptr = sum_values[index].data() + l;
67 | int* iptr = &(sum_counts[index]);
68 |
69 | atomicAdd(rptr, value);
70 | atomicAdd(iptr, 1);
71 | }
72 |
73 | torch::Tensor pack_batch(torch::Tensor indices, uint n_batch, uint n_point) {
74 | CHECK_INPUT(indices);
75 | torch::Tensor packed_inds = torch::empty({n_batch, n_point}, torch::dtype(torch::kLong).device(torch::kCUDA));
76 | thrust::device_vector filled_count(n_batch, 0);
77 | const uint n_all = indices.size(0);
78 |
79 | dim3 dimBlock = dim3(128);
80 | dim3 dimGrid = dim3(div_up(n_all, dimBlock.x));
81 | pack_batch_kernel<<>>(
82 | indices.packed_accessor32(),
83 | packed_inds.packed_accessor32(),
84 | n_all, n_batch, n_point, filled_count.data().get());
85 | return packed_inds;
86 | }
87 |
88 |
89 | std::vector groupby_sum(torch::Tensor values, torch::Tensor indices, uint C) {
90 | CHECK_INPUT(values);
91 | CHECK_INPUT(indices);
92 |
93 | const uint N = values.size(0);
94 | const uint L = values.size(1);
95 |
96 | torch::Tensor sum_values = torch::zeros({C, L}, torch::dtype(torch::kFloat32).device(torch::kCUDA));
97 | torch::Tensor sum_count = torch::zeros({C}, torch::dtype(torch::kInt32).device(torch::kCUDA));
98 |
99 | dim3 dimBlock = dim3(L);
100 | dim3 dimGrid = dim3(N);
101 | groupby_sum_kernel<<>>(
102 | values.packed_accessor32(),
103 | indices.packed_accessor32(),
104 | sum_values.packed_accessor32(),
105 | sum_count.packed_accessor32()
106 | );
107 |
108 | return {sum_values, sum_count};
109 | }
110 |
--------------------------------------------------------------------------------
/system/ext/marching_cubes/mc.cpp:
--------------------------------------------------------------------------------
1 | #include
2 |
3 | std::vector marching_cubes_sparse_interp_cuda(
4 | torch::Tensor indexer, // (nx, ny, nz) -> data_id
5 | torch::Tensor valid_blocks, // (K, )
6 | torch::Tensor vec_batch_mapping,
7 | torch::Tensor cube_sdf, // (M, rx, ry, rz)
8 | torch::Tensor cube_std, // (M, rx, ry, rz)
9 | int max_n_triangles, // Maximum number of triangle buffer.
10 | const std::vector& n_xyz, // [nx, ny, nz]
11 | float max_std // Prune all vertices
12 | );
13 |
14 | PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
15 | m.def("marching_cubes_sparse_interp", &marching_cubes_sparse_interp_cuda, "Marching Cubes with Interpolation (CUDA)");
16 | }
17 |
--------------------------------------------------------------------------------
/system/ext/pcproc/cuda_kdtree.cuh:
--------------------------------------------------------------------------------
1 | #ifndef FLANN_CUDA_KD_TREE_BUILDER_H_
2 | #define FLANN_CUDA_KD_TREE_BUILDER_H_
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include "cutil_math.h"
11 | #include
12 | #include | |