├── .gitignore ├── LICENSE ├── README.md ├── configs ├── __init__.py ├── kitti.yaml ├── mvp_rg.yaml └── threedmatch.yaml ├── cpp_wrappers ├── compile_wrappers.sh ├── cpp_neighbors │ ├── build.bat │ ├── neighbors │ │ ├── neighbors.cpp │ │ └── neighbors.h │ ├── setup.py │ └── wrapper.cpp ├── cpp_subsampling │ ├── build.bat │ ├── grid_subsampling │ │ ├── grid_subsampling.cpp │ │ └── grid_subsampling.h │ ├── setup.py │ └── wrapper.cpp └── cpp_utils │ ├── cloud │ ├── cloud.cpp │ └── cloud.h │ └── nanoflann │ └── nanoflann.hpp ├── data ├── Kitti.py ├── Kitti │ ├── test_kitti.txt │ ├── train_kitti.txt │ └── val_kitti.txt ├── MVP_RG.py ├── ThreeDMatch.py ├── ThreeDMatch │ ├── 3DLoMatch.pkl │ ├── 3DMatch.pkl │ ├── gt │ │ ├── 3DLoMatch │ │ │ ├── 7-scenes-redkitchen │ │ │ │ ├── gt.info │ │ │ │ ├── gt.log │ │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-home_at-home_at_scan1_2013_jan_1 │ │ │ │ ├── gt.info │ │ │ │ ├── gt.log │ │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-home_md-home_md_scan9_2012_sep_30 │ │ │ │ ├── gt.info │ │ │ │ ├── gt.log │ │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-hotel_uc-scan3 │ │ │ │ ├── gt.info │ │ │ │ ├── gt.log │ │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-hotel_umd-maryland_hotel1 │ │ │ │ ├── gt.info │ │ │ │ ├── gt.log │ │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-hotel_umd-maryland_hotel3 │ │ │ │ ├── gt.info │ │ │ │ ├── gt.log │ │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-mit_76_studyroom-76-1studyroom2 │ │ │ │ ├── gt.info │ │ │ │ ├── gt.log │ │ │ │ └── gt_overlap.log │ │ │ └── sun3d-mit_lab_hj-lab_hj_tea_nov_2_2012_scan1_erika │ │ │ │ ├── gt.info │ │ │ │ ├── gt.log │ │ │ │ └── gt_overlap.log │ │ └── 3DMatch │ │ │ ├── 7-scenes-redkitchen │ │ │ ├── gt.info │ │ │ ├── gt.log │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-home_at-home_at_scan1_2013_jan_1 │ │ │ ├── gt.info │ │ │ ├── gt.log │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-home_md-home_md_scan9_2012_sep_30 │ │ │ ├── gt.info │ │ │ ├── gt.log │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-hotel_uc-scan3 │ │ │ ├── gt.info │ │ │ ├── gt.log │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-hotel_umd-maryland_hotel1 │ │ │ ├── gt.info │ │ │ ├── gt.log │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-hotel_umd-maryland_hotel3 │ │ │ ├── gt.info │ │ │ ├── gt.log │ │ │ └── gt_overlap.log │ │ │ ├── sun3d-mit_76_studyroom-76-1studyroom2 │ │ │ ├── gt.info │ │ │ ├── gt.log │ │ │ └── gt_overlap.log │ │ │ └── sun3d-mit_lab_hj-lab_hj_tea_nov_2_2012_scan1_erika │ │ │ ├── gt.info │ │ │ ├── gt.log │ │ │ └── gt_overlap.log │ ├── train_3dmatch.txt │ ├── train_info.pkl │ ├── val_3dmatch.txt │ └── val_info.pkl ├── __init__.py └── dataloader.py ├── demo.py ├── demo_data ├── 3dmatch.png ├── cloud_bin_21.pth ├── cloud_bin_34.pth ├── my_data1.png ├── my_data2.png ├── src1.ply ├── src2.ply ├── tgt1.ply └── tgt2.ply ├── eval_3dmatch.py ├── eval_kitti.py ├── eval_mvp_rg.py ├── losses ├── __init__.py └── loss.py ├── metrics ├── __init__.py ├── kitti.py ├── mvp_rg.py └── threedmatch.py ├── misc └── plot_fmr.py ├── models ├── KPConv │ ├── __init__.py │ ├── blocks.py │ ├── kernel_points.py │ └── kernels │ │ └── k_015_center.ply ├── NgeNet.py ├── __init__.py ├── architecures.py ├── information_interactive.py └── vote.py ├── reg_results ├── 3DLoMatch-pred │ ├── 7-scenes-redkitchen │ │ └── est.log │ ├── sun3d-home_at-home_at_scan1_2013_jan_1 │ │ └── est.log │ ├── sun3d-home_md-home_md_scan9_2012_sep_30 │ │ └── est.log │ ├── sun3d-hotel_uc-scan3 │ │ └── est.log │ ├── sun3d-hotel_umd-maryland_hotel1 │ │ └── est.log │ ├── sun3d-hotel_umd-maryland_hotel3 │ │ └── est.log │ ├── sun3d-mit_76_studyroom-76-1studyroom2 │ │ └── est.log │ └── sun3d-mit_lab_hj-lab_hj_tea_nov_2_2012_scan1_erika │ │ └── est.log └── 3DMatch-pred │ ├── 7-scenes-redkitchen │ └── est.log │ ├── sun3d-home_at-home_at_scan1_2013_jan_1 │ └── est.log │ ├── sun3d-home_md-home_md_scan9_2012_sep_30 │ └── est.log │ ├── sun3d-hotel_uc-scan3 │ └── est.log │ ├── sun3d-hotel_umd-maryland_hotel1 │ └── est.log │ ├── sun3d-hotel_umd-maryland_hotel3 │ └── est.log │ ├── sun3d-mit_76_studyroom-76-1studyroom2 │ └── est.log │ └── sun3d-mit_lab_hj-lab_hj_tea_nov_2_2012_scan1_erika │ └── est.log ├── requirements.txt ├── train.py └── utils ├── __init__.py ├── o3d.py ├── process.py └── yaml.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | *.py[cod] 3 | *$py.class 4 | .idea/ 5 | test*.py 6 | checkpoints/ 7 | model/.DS_Store 8 | .DS_Store 9 | tmp.py 10 | work_dirs* 11 | cpp_wrappers/cpp_neighbors/build/ 12 | *.so 13 | train_logs/ 14 | results/ 15 | Datasets/ 16 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Lifa Zhu 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # [Leveraging Inlier Correspondences Proportion for Point Cloud Registration](https://arxiv.org/pdf/2201.12094.pdf) 2 | 3 | [![PWC](https://img.shields.io/endpoint.svg?url=https://paperswithcode.com/badge/neighborhood-aware-geometric-encoding-network/point-cloud-registration-on-3dmatch-at-least-2)](https://paperswithcode.com/sota/point-cloud-registration-on-3dmatch-at-least-2?p=neighborhood-aware-geometric-encoding-network) 4 | [![PWC](https://img.shields.io/endpoint.svg?url=https://paperswithcode.com/badge/neighborhood-aware-geometric-encoding-network/point-cloud-registration-on-3dlomatch-10-30)](https://paperswithcode.com/sota/point-cloud-registration-on-3dlomatch-10-30?p=neighborhood-aware-geometric-encoding-network) 5 | [![PWC](https://img.shields.io/endpoint.svg?url=https://paperswithcode.com/badge/neighborhood-aware-geometric-encoding-network/point-cloud-registration-on-3dmatch-at-least-1)](https://paperswithcode.com/sota/point-cloud-registration-on-3dmatch-at-least-1?p=neighborhood-aware-geometric-encoding-network) 6 | 7 | 8 | Paper "Neighborhood-aware Geometric Encoding Network for Point Cloud Registration" was renamed to "Leveraging Inlier Correspondences Proportion for Point Cloud Registration" (NgeNet -> GCNet). 9 | 10 | ## Results (saved in reg_results/3DMatch*-pred) 11 | 12 | - Recall on 3DMatch and 3DLoMatch (correspondences RMSE below 0.2) 13 | 14 | | Dataset | npairs | Scene Recall (%) | Pair Recall (%) | 15 | | :---: | :---: | :---: | :---: | 16 | | 3DMatch | 1279 | 92.9 | 93.9 | 17 | | 3DLoMatch | 1726 | 71.9 | 74.5 | 18 | 19 | - Recall on 3DMatch and 3DLoMatch (under 0.3m && 15 degrees) 20 | 21 | | Dataset | npairs | Pair Recall (%) | 22 | | :---: | :---: | :---: | 23 | | 3DMatch | 1623 | 95.0 | 24 | | 3DLoMatch | 1781 | 75.1 | 25 | 26 | - Results on Odometry KITTI 27 | 28 | | Dataset | RTE(cm) | RRE(°) | Recall (%) | 29 | | :---: | :---: | :---: | :---: | 30 | | Odometry KITTI | 6.1 | 0.26 | 99.8 | 31 | 32 | - Results on MVP-RG 33 | 34 | | Dataset | RRE(°) | RTE | RMSE | 35 | | :---: | :---: | :---: | :---: | 36 | | MVP-RG | 7.99 | 0.048 | 0.093 | 37 | 38 | ## Environments 39 | 40 | - All experiments were run on a RTX 3090 GPU with an Intel 8255C CPU at 2.50GHz CPU. Dependencies can be found in `requirements.txt`. 41 | 42 | - Compile python bindings 43 | 44 | ``` 45 | # Compile 46 | 47 | cd NgeNet/cpp_wrappers 48 | sh compile_wrappers.sh 49 | ``` 50 | 51 | ## [Pretrained weights (Optional)] 52 | 53 | Download pretrained weights for 3DMatch, 3DLoMatch, Odometry KITTI and MVP-RG from [GoogleDrive](https://drive.google.com/drive/folders/1JDn6zQfLdZfAVVboXRrrrCVRo48pRjyW?usp=sharing) or [BaiduDisk](https://pan.baidu.com/s/18G_Deim1UlSkY8wWoOiwnw) (pwd: `vr9g`). 54 | 55 | ## [3DMatch and 3DLoMatch] 56 | 57 | ### 1.1 dataset 58 | 59 | We adopt the 3DMatch and 3DLoMatch provided from [PREDATOR](https://github.com/overlappredator/OverlapPredator), and download it [here](https://share.phys.ethz.ch/~gsg/Predator/data.zip) [**936.1MB**]. 60 | Unzip it, then we should get the following directories structure: 61 | 62 | ``` 63 | | -- indoor 64 | | -- train (#82, cats: #54) 65 | | -- 7-scenes-chess 66 | | -- 7-scenes-fire 67 | | -- ... 68 | | -- sun3d-mit_w20_athena-sc_athena_oct_29_2012_scan1_erika_4 69 | | -- test (#8, cats: #8) 70 | | -- 7-scenes-redkitchen 71 | | -- sun3d-home_md-home_md_scan9_2012_sep_30 72 | | -- ... 73 | | -- sun3d-mit_lab_hj-lab_hj_tea_nov_2_2012_scan1_erika 74 | ``` 75 | 76 | ### 1.2 train 77 | 78 | ``` 79 | ## Reconfigure configs/threedmatch.yaml by updating the following values based on your dataset. 80 | 81 | # exp_dir: your_saved_path for checkpoints and summary. 82 | # root: your_data_path for the 3dMatch dataset. 83 | 84 | cd NgeNet 85 | python train.py configs/threedmatch.yaml 86 | 87 | # note: The code `torch.cuda.empty_cache()` in `train.py` has some impact on the training speed. 88 | # You can remove it or change its postion according to your GPU memory. 89 | ``` 90 | 91 | ### 1.3 evaluate and visualize 92 | 93 | ``` 94 | cd NgeNet 95 | 96 | python eval_3dmatch.py --benchmark 3DMatch --data_root your_path/indoor --checkpoint your_path/3dmatch.pth --saved_path work_dirs/3dmatch [--vis] [--no_cuda] 97 | 98 | python eval_3dmatch.py --benchmark 3DLoMatch --data_root your_path/indoor --checkpoint your_path/3dmatch.pth --saved_path work_dirs/3dlomatch [--vis] [--no_cuda] 99 | ``` 100 | 101 | ## [Odometry KITTI] 102 | 103 | ### 2.1 dataset 104 | 105 | Download odometry kitti [here](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) with `[velodyne laser data, 80 GB]` and `[ground truth poses (4 MB)]`, then unzip and organize in the following format. 106 | 107 | ``` 108 | | -- kitti 109 | | -- dataset 110 | | -- poses (#11 txt) 111 | | -- sequences (#11 / #22) 112 | | -- icp (generated automatically when training and testing) 113 | | -- 0_0_11.npy 114 | | -- ... 115 | | -- 9_992_1004.npy 116 | ``` 117 | 118 | ### 2.2 train 119 | 120 | ``` 121 | ## Reconfigure configs/kitti.yaml by updating the following values based on your dataset. 122 | 123 | # exp_dir: your_saved_path for checkpoints and summary. 124 | # root: your_data_path for the Odometry KITTI. 125 | 126 | cd NgeNet 127 | python train.py configs/kitti.yaml 128 | ``` 129 | 130 | ### 2.3 evaluate and visualize 131 | 132 | ``` 133 | cd NgeNet 134 | python eval_kitti.py --data_root your_path/kitti --checkpoint your_path/kitti.pth [--vis] [--no_cuda] 135 | ``` 136 | 137 | ## [MVP-RG] 138 | 139 | ### 3.1 dataset 140 | 141 | Download MVP-RG dataset [here](https://mvp-dataset.github.io/MVP/Registration.html), then organize in the following format. 142 | 143 | ``` 144 | | -- mvp_rg 145 | | -- MVP_Train_RG.h5 146 | | -- MVP_Test_RG.h5 147 | ``` 148 | 149 | ### 3.2 train 150 | 151 | ``` 152 | ## Reconfigure configs/mvp_rg.yaml by updating the following values based on your dataset. 153 | 154 | # exp_dir: your_saved_path for checkpoints and summary. 155 | # root: your_data_path for the MVP-RG. 156 | 157 | python train.py configs/mvp_rg.yaml 158 | 159 | # note: The code `torch.cuda.empty_cache()` in `train.py` has some impact on the training speed. 160 | # You can remove it or change its postion according to your GPU memory. 161 | ``` 162 | 163 | ### 3.3 evaluate and visualize 164 | 165 | ``` 166 | python eval_mvp_rg.py --data_root your_path/mvp_rg --checkpoint your_path/mvp_rg.pth [--vis] [--no_cuda] 167 | ``` 168 | 169 | ## [Demo] 170 | 171 | ### 4.1 3DMatch 172 | 173 | ``` 174 | python demo.py --src_path demo_data/cloud_bin_21.pth --tgt_path demo_data/cloud_bin_34.pth --checkpoint your_path/3dmatch.pth --voxel_size 0.025 --npts 5000 175 | ``` 176 | ![](demo_data/3dmatch.png) 177 | 178 | ### 4.2 Personal data (with the same voxel size as 3DMatch) 179 | 180 | ``` 181 | python demo.py --src_path demo_data/src1.ply --tgt_path demo_data/tgt1.ply --checkpoint your_path/3dmatch.pth --voxel_size 0.025 --npts 20000 182 | ``` 183 | ![](demo_data/my_data1.png) 184 | 185 | ### 4.3 Personal data (with different voxel size from 3DMatch) 186 | 187 | ``` 188 | python demo.py --src_path demo_data/src2.ply --tgt_path demo_data/tgt2.ply --checkpoint your_path/3dmatch.pth --voxel_size 3 --npts 20000 189 | ``` 190 | ![](demo_data/my_data2.png) 191 | 192 | Set an appropriate `voxel_size` for your test data. If you want to test on point cloud pair with **large amount of points**, please **set a large `voxel_size` according to your data**. 193 | 194 | ## Citation 195 | 196 | ``` 197 | @article{zhu2022leveraging, 198 | title={Leveraging Inlier Correspondences Proportion for Point Cloud Registration}, 199 | author={Zhu, Lifa and Guan, Haining and Lin, Changwei and Han, Renmin}, 200 | journal={arXiv preprint arXiv:2201.12094}, 201 | year={2022} 202 | } 203 | ``` 204 | 205 | ## Acknowledgements 206 | 207 | Thanks for the open source code [OverlapPredator](https://github.com/overlappredator/OverlapPredator), [KPConv-PyTorch](https://github.com/HuguesTHOMAS/KPConv-PyTorch), [KPConv.pytorch](https://github.com/XuyangBai/KPConv.pytorch), [FCGF](https://github.com/chrischoy/FCGF), [D3Feat.pytorch](https://github.com/XuyangBai/D3Feat.pytorch), [MVP_Benchmark](https://github.com/paul007pl/MVP_Benchmark) and [ROPNet](https://github.com/zhulf0804/ROPNet). 208 | -------------------------------------------------------------------------------- /configs/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/configs/__init__.py -------------------------------------------------------------------------------- /configs/kitti.yaml: -------------------------------------------------------------------------------- 1 | log: 2 | exp_dir: train_logs/kitti 3 | log_freq: 8 4 | 5 | model: 6 | num_layers: 4 7 | in_points_dim: 3 8 | first_feats_dim: 256 9 | final_feats_dim: 32 10 | first_subsampling_dl: 0.3 11 | in_feats_dim: 1 12 | conv_radius: 4.25 13 | deform_radius: 5.0 14 | num_kernel_points: 15 15 | KP_extent: 2.0 16 | KP_influence: linear 17 | aggregation_mode: sum 18 | fixed_kernel_points: center 19 | use_batch_norm: True 20 | batch_norm_momentum: 0.02 21 | 22 | overlap_attention_module: 23 | gnn_feats_dim: 256 24 | dgcnn_k: 10 25 | num_head: 4 26 | ppf_k: 64 27 | radius_mul: 32 28 | bottleneck: True 29 | nets: ['gge','cross_attn','gge'] 30 | 31 | loss: 32 | pos_margin: 0.1 33 | neg_margin: 1.4 34 | log_scale: 16 35 | pos_radius: 0.21 36 | safe_radius: 0.75 37 | overlap_radius: 0.45 38 | matchability_radius: 0.3 39 | w_circle_loss: 1.0 40 | w_overlap_loss: 1.0 41 | w_saliency_loss: 0.0 42 | max_points: 512 43 | 44 | optimiser: 45 | optimizer: SGD 46 | max_epoch: 150 47 | lr: 0.05 48 | weight_decay: 0.000001 49 | momentum: 0.98 50 | scheduler: ExpLR 51 | # scheduler: CosA 52 | T_0: 10 53 | T_mult: 4 54 | eta_min: 0.00001 55 | scheduler_gamma: 0.99 56 | scheduler_freq: 1 57 | iter_size: 1 58 | 59 | dataset: 60 | dataset: kitti 61 | benchmark: odometryKITTI 62 | root: Datasets/kitti 63 | batch_size: 1 64 | num_workers: 6 65 | # augment_noise: 0.01 66 | # augment_shift_range: 2.0 67 | # augment_scale_max: 1.2 68 | # augment_scale_min: 0.8 69 | -------------------------------------------------------------------------------- /configs/mvp_rg.yaml: -------------------------------------------------------------------------------- 1 | log: 2 | exp_dir: train_logs/mvp_rg 3 | log_freq: 8 4 | 5 | model: 6 | num_layers: 3 7 | in_points_dim: 3 8 | first_feats_dim: 512 9 | final_feats_dim: 96 10 | first_subsampling_dl: 0.04 11 | in_feats_dim: 1 12 | conv_radius: 2.75 13 | deform_radius: 5.0 14 | num_kernel_points: 15 15 | KP_extent: 2.0 16 | KP_influence: linear 17 | aggregation_mode: sum 18 | fixed_kernel_points: center 19 | use_batch_norm: True 20 | batch_norm_momentum: 0.02 21 | 22 | overlap_attention_module: 23 | gnn_feats_dim: 256 24 | dgcnn_k: 10 25 | num_head: 4 26 | ppf_k: 64 27 | radius_mul: 16 28 | bottleneck: False 29 | nets: ['gge','cross_attn','gge'] 30 | 31 | loss: 32 | pos_margin: 0.1 33 | neg_margin: 1.4 34 | log_scale: 64 35 | pos_radius: 0.018 36 | safe_radius: 0.06 37 | overlap_radius: 0.04 38 | matchability_radius: 0.04 39 | w_circle_loss: 1.0 40 | w_overlap_loss: 1.0 41 | w_saliency_loss: 0.0 42 | max_points: 768 43 | 44 | optimiser: 45 | optimizer: SGD 46 | max_epoch: 200 47 | lr: 0.01 48 | weight_decay: 0.000001 49 | momentum: 0.98 50 | scheduler: ExpLR 51 | scheduler_gamma: 0.95 52 | scheduler_freq: 1 53 | iter_size: 4 54 | 55 | dataset: 56 | dataset: mvp_rg 57 | benchmark: mvp_rg 58 | root: Datasets/mvp_rg 59 | batch_size: 1 60 | num_workers: 6 61 | augment_noise: 0.005 62 | rot_mag: 360.0 63 | trans_mag: 0.5 64 | dataset_type: mvp_rg 65 | -------------------------------------------------------------------------------- /configs/threedmatch.yaml: -------------------------------------------------------------------------------- 1 | log: 2 | log_freq: 8 3 | exp_dir: train_logs/3dmatch 4 | 5 | model: 6 | num_layers: 4 7 | in_points_dim: 3 8 | first_feats_dim: 128 9 | final_feats_dim: 32 10 | first_subsampling_dl: 0.025 11 | in_feats_dim: 1 12 | conv_radius: 2.5 13 | deform_radius: 5.0 14 | num_kernel_points: 15 15 | KP_extent: 2.0 16 | KP_influence: linear 17 | aggregation_mode: sum 18 | fixed_kernel_points: center 19 | use_batch_norm: True 20 | batch_norm_momentum: 0.02 21 | 22 | overlap_attention_module: 23 | gnn_feats_dim: 256 24 | dgcnn_k: 10 25 | num_head: 4 26 | ppf_k: 64 27 | radius_mul: 32 28 | bottleneck: False 29 | nets: ['gge','cross_attn','gge'] 30 | 31 | loss: 32 | pos_margin: 0.1 33 | neg_margin: 1.4 34 | log_scale: 16 35 | pos_radius: 0.0375 36 | safe_radius: 0.1 37 | overlap_radius: 0.0375 38 | matchability_radius: 0.05 39 | w_circle_loss: 1.0 40 | w_overlap_loss: 1.0 41 | w_saliency_loss: 0.0 42 | max_points: 256 43 | 44 | optimiser: 45 | optimizer: SGD 46 | max_epoch: 40 47 | lr: 0.005 48 | weight_decay: 0.000001 49 | momentum: 0.98 50 | scheduler: ExpLR 51 | T_0: 10 52 | T_mult: 4 53 | eta_min: 0.00001 54 | scheduler_gamma: 0.95 55 | scheduler_freq: 1 56 | iter_size: 1 57 | 58 | dataset: 59 | dataset: threedmatch 60 | benchmark: 3DMatch 61 | root: Datasets/indoor 62 | batch_size: 1 63 | num_workers: 6 64 | augment_noise: 0.005 65 | train_info: configs/indoor/train_info.pkl 66 | val_info: configs/indoor/val_info.pkl 67 | -------------------------------------------------------------------------------- /cpp_wrappers/compile_wrappers.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Compile cpp subsampling 4 | cd cpp_subsampling 5 | python setup.py build_ext --inplace 6 | cd .. 7 | 8 | # Compile cpp neighbors 9 | cd cpp_neighbors 10 | python setup.py build_ext --inplace 11 | cd .. 12 | -------------------------------------------------------------------------------- /cpp_wrappers/cpp_neighbors/build.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | py setup.py build_ext --inplace 3 | 4 | 5 | pause -------------------------------------------------------------------------------- /cpp_wrappers/cpp_neighbors/neighbors/neighbors.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "neighbors.h" 3 | 4 | 5 | void brute_neighbors(vector& queries, vector& supports, vector& neighbors_indices, float radius, int verbose) 6 | { 7 | 8 | // Initialize variables 9 | // ****************** 10 | 11 | // square radius 12 | float r2 = radius * radius; 13 | 14 | // indices 15 | int i0 = 0; 16 | 17 | // Counting vector 18 | int max_count = 0; 19 | vector> tmp(queries.size()); 20 | 21 | // Search neigbors indices 22 | // *********************** 23 | 24 | for (auto& p0 : queries) 25 | { 26 | int i = 0; 27 | for (auto& p : supports) 28 | { 29 | if ((p0 - p).sq_norm() < r2) 30 | { 31 | tmp[i0].push_back(i); 32 | if (tmp[i0].size() > max_count) 33 | max_count = tmp[i0].size(); 34 | } 35 | i++; 36 | } 37 | i0++; 38 | } 39 | 40 | // Reserve the memory 41 | neighbors_indices.resize(queries.size() * max_count); 42 | i0 = 0; 43 | for (auto& inds : tmp) 44 | { 45 | for (int j = 0; j < max_count; j++) 46 | { 47 | if (j < inds.size()) 48 | neighbors_indices[i0 * max_count + j] = inds[j]; 49 | else 50 | neighbors_indices[i0 * max_count + j] = -1; 51 | } 52 | i0++; 53 | } 54 | 55 | return; 56 | } 57 | 58 | void ordered_neighbors(vector& queries, 59 | vector& supports, 60 | vector& neighbors_indices, 61 | float radius) 62 | { 63 | 64 | // Initialize variables 65 | // ****************** 66 | 67 | // square radius 68 | float r2 = radius * radius; 69 | 70 | // indices 71 | int i0 = 0; 72 | 73 | // Counting vector 74 | int max_count = 0; 75 | float d2; 76 | vector> tmp(queries.size()); 77 | vector> dists(queries.size()); 78 | 79 | // Search neigbors indices 80 | // *********************** 81 | 82 | for (auto& p0 : queries) 83 | { 84 | int i = 0; 85 | for (auto& p : supports) 86 | { 87 | d2 = (p0 - p).sq_norm(); 88 | if (d2 < r2) 89 | { 90 | // Find order of the new point 91 | auto it = std::upper_bound(dists[i0].begin(), dists[i0].end(), d2); 92 | int index = std::distance(dists[i0].begin(), it); 93 | 94 | // Insert element 95 | dists[i0].insert(it, d2); 96 | tmp[i0].insert(tmp[i0].begin() + index, i); 97 | 98 | // Update max count 99 | if (tmp[i0].size() > max_count) 100 | max_count = tmp[i0].size(); 101 | } 102 | i++; 103 | } 104 | i0++; 105 | } 106 | 107 | // Reserve the memory 108 | neighbors_indices.resize(queries.size() * max_count); 109 | i0 = 0; 110 | for (auto& inds : tmp) 111 | { 112 | for (int j = 0; j < max_count; j++) 113 | { 114 | if (j < inds.size()) 115 | neighbors_indices[i0 * max_count + j] = inds[j]; 116 | else 117 | neighbors_indices[i0 * max_count + j] = -1; 118 | } 119 | i0++; 120 | } 121 | 122 | return; 123 | } 124 | 125 | void batch_ordered_neighbors(vector& queries, 126 | vector& supports, 127 | vector& q_batches, 128 | vector& s_batches, 129 | vector& neighbors_indices, 130 | float radius) 131 | { 132 | 133 | // Initialize variables 134 | // ****************** 135 | 136 | // square radius 137 | float r2 = radius * radius; 138 | 139 | // indices 140 | int i0 = 0; 141 | 142 | // Counting vector 143 | int max_count = 0; 144 | float d2; 145 | vector> tmp(queries.size()); 146 | vector> dists(queries.size()); 147 | 148 | // batch index 149 | int b = 0; 150 | int sum_qb = 0; 151 | int sum_sb = 0; 152 | 153 | 154 | // Search neigbors indices 155 | // *********************** 156 | 157 | for (auto& p0 : queries) 158 | { 159 | // Check if we changed batch 160 | if (i0 == sum_qb + q_batches[b]) 161 | { 162 | sum_qb += q_batches[b]; 163 | sum_sb += s_batches[b]; 164 | b++; 165 | } 166 | 167 | // Loop only over the supports of current batch 168 | vector::iterator p_it; 169 | int i = 0; 170 | for(p_it = supports.begin() + sum_sb; p_it < supports.begin() + sum_sb + s_batches[b]; p_it++ ) 171 | { 172 | d2 = (p0 - *p_it).sq_norm(); 173 | if (d2 < r2) 174 | { 175 | // Find order of the new point 176 | auto it = std::upper_bound(dists[i0].begin(), dists[i0].end(), d2); 177 | int index = std::distance(dists[i0].begin(), it); 178 | 179 | // Insert element 180 | dists[i0].insert(it, d2); 181 | tmp[i0].insert(tmp[i0].begin() + index, sum_sb + i); 182 | 183 | // Update max count 184 | if (tmp[i0].size() > max_count) 185 | max_count = tmp[i0].size(); 186 | } 187 | i++; 188 | } 189 | i0++; 190 | } 191 | 192 | // Reserve the memory 193 | neighbors_indices.resize(queries.size() * max_count); 194 | i0 = 0; 195 | for (auto& inds : tmp) 196 | { 197 | for (int j = 0; j < max_count; j++) 198 | { 199 | if (j < inds.size()) 200 | neighbors_indices[i0 * max_count + j] = inds[j]; 201 | else 202 | neighbors_indices[i0 * max_count + j] = supports.size(); 203 | } 204 | i0++; 205 | } 206 | 207 | return; 208 | } 209 | 210 | 211 | void batch_nanoflann_neighbors(vector& queries, 212 | vector& supports, 213 | vector& q_batches, 214 | vector& s_batches, 215 | vector& neighbors_indices, 216 | float radius) 217 | { 218 | 219 | // Initialize variables 220 | // ****************** 221 | 222 | // indices 223 | int i0 = 0; 224 | 225 | // Square radius 226 | float r2 = radius * radius; 227 | 228 | // Counting vector 229 | int max_count = 0; 230 | float d2; 231 | vector>> all_inds_dists(queries.size()); 232 | 233 | // batch index 234 | int b = 0; 235 | int sum_qb = 0; 236 | int sum_sb = 0; 237 | 238 | // Nanoflann related variables 239 | // *************************** 240 | 241 | // CLoud variable 242 | PointCloud current_cloud; 243 | 244 | // Tree parameters 245 | nanoflann::KDTreeSingleIndexAdaptorParams tree_params(10 /* max leaf */); 246 | 247 | // KDTree type definition 248 | typedef nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Simple_Adaptor , 249 | PointCloud, 250 | 3 > my_kd_tree_t; 251 | 252 | // Pointer to trees 253 | my_kd_tree_t* index; 254 | 255 | // Build KDTree for the first batch element 256 | current_cloud.pts = vector(supports.begin() + sum_sb, supports.begin() + sum_sb + s_batches[b]); 257 | index = new my_kd_tree_t(3, current_cloud, tree_params); 258 | index->buildIndex(); 259 | 260 | 261 | // Search neigbors indices 262 | // *********************** 263 | 264 | // Search params 265 | nanoflann::SearchParams search_params; 266 | search_params.sorted = true; 267 | 268 | for (auto& p0 : queries) 269 | { 270 | 271 | // Check if we changed batch 272 | if (i0 == sum_qb + q_batches[b]) 273 | { 274 | sum_qb += q_batches[b]; 275 | sum_sb += s_batches[b]; 276 | b++; 277 | 278 | // Change the points 279 | current_cloud.pts.clear(); 280 | current_cloud.pts = vector(supports.begin() + sum_sb, supports.begin() + sum_sb + s_batches[b]); 281 | 282 | // Build KDTree of the current element of the batch 283 | delete index; 284 | index = new my_kd_tree_t(3, current_cloud, tree_params); 285 | index->buildIndex(); 286 | } 287 | 288 | // Initial guess of neighbors size 289 | all_inds_dists[i0].reserve(max_count); 290 | 291 | // Find neighbors 292 | float query_pt[3] = { p0.x, p0.y, p0.z}; 293 | size_t nMatches = index->radiusSearch(query_pt, r2, all_inds_dists[i0], search_params); 294 | 295 | // Update max count 296 | if (nMatches > max_count) 297 | max_count = nMatches; 298 | 299 | // Increment query idx 300 | i0++; 301 | } 302 | 303 | // Reserve the memory 304 | neighbors_indices.resize(queries.size() * max_count); 305 | i0 = 0; 306 | sum_sb = 0; 307 | sum_qb = 0; 308 | b = 0; 309 | for (auto& inds_dists : all_inds_dists) 310 | { 311 | // Check if we changed batch 312 | if (i0 == sum_qb + q_batches[b]) 313 | { 314 | sum_qb += q_batches[b]; 315 | sum_sb += s_batches[b]; 316 | b++; 317 | } 318 | 319 | for (int j = 0; j < max_count; j++) 320 | { 321 | if (j < inds_dists.size()) 322 | neighbors_indices[i0 * max_count + j] = inds_dists[j].first + sum_sb; 323 | else 324 | neighbors_indices[i0 * max_count + j] = supports.size(); 325 | } 326 | i0++; 327 | } 328 | 329 | delete index; 330 | 331 | return; 332 | } 333 | 334 | -------------------------------------------------------------------------------- /cpp_wrappers/cpp_neighbors/neighbors/neighbors.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "../../cpp_utils/cloud/cloud.h" 4 | #include "../../cpp_utils/nanoflann/nanoflann.hpp" 5 | 6 | #include 7 | #include 8 | 9 | using namespace std; 10 | 11 | 12 | void ordered_neighbors(vector& queries, 13 | vector& supports, 14 | vector& neighbors_indices, 15 | float radius); 16 | 17 | void batch_ordered_neighbors(vector& queries, 18 | vector& supports, 19 | vector& q_batches, 20 | vector& s_batches, 21 | vector& neighbors_indices, 22 | float radius); 23 | 24 | void batch_nanoflann_neighbors(vector& queries, 25 | vector& supports, 26 | vector& q_batches, 27 | vector& s_batches, 28 | vector& neighbors_indices, 29 | float radius); 30 | -------------------------------------------------------------------------------- /cpp_wrappers/cpp_neighbors/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup, Extension 2 | import numpy.distutils.misc_util 3 | 4 | # Adding OpenCV to project 5 | # ************************ 6 | 7 | # Adding sources of the project 8 | # ***************************** 9 | 10 | SOURCES = ["../cpp_utils/cloud/cloud.cpp", 11 | "neighbors/neighbors.cpp", 12 | "wrapper.cpp"] 13 | 14 | module = Extension(name="radius_neighbors", 15 | sources=SOURCES, 16 | extra_compile_args=['-std=c++11', 17 | '-D_GLIBCXX_USE_CXX11_ABI=0']) 18 | 19 | 20 | setup(ext_modules=[module], include_dirs=numpy.distutils.misc_util.get_numpy_include_dirs()) 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /cpp_wrappers/cpp_neighbors/wrapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "neighbors/neighbors.h" 4 | #include 5 | 6 | 7 | 8 | // docstrings for our module 9 | // ************************* 10 | 11 | static char module_docstring[] = "This module provides two methods to compute radius neighbors from pointclouds or batch of pointclouds"; 12 | 13 | static char batch_query_docstring[] = "Method to get radius neighbors in a batch of stacked pointclouds"; 14 | 15 | 16 | // Declare the functions 17 | // ********************* 18 | 19 | static PyObject *batch_neighbors(PyObject *self, PyObject *args, PyObject *keywds); 20 | 21 | 22 | // Specify the members of the module 23 | // ********************************* 24 | 25 | static PyMethodDef module_methods[] = 26 | { 27 | { "batch_query", (PyCFunction)batch_neighbors, METH_VARARGS | METH_KEYWORDS, batch_query_docstring }, 28 | {NULL, NULL, 0, NULL} 29 | }; 30 | 31 | 32 | // Initialize the module 33 | // ********************* 34 | 35 | static struct PyModuleDef moduledef = 36 | { 37 | PyModuleDef_HEAD_INIT, 38 | "radius_neighbors", // m_name 39 | module_docstring, // m_doc 40 | -1, // m_size 41 | module_methods, // m_methods 42 | NULL, // m_reload 43 | NULL, // m_traverse 44 | NULL, // m_clear 45 | NULL, // m_free 46 | }; 47 | 48 | PyMODINIT_FUNC PyInit_radius_neighbors(void) 49 | { 50 | import_array(); 51 | return PyModule_Create(&moduledef); 52 | } 53 | 54 | 55 | // Definition of the batch_subsample method 56 | // ********************************** 57 | 58 | static PyObject* batch_neighbors(PyObject* self, PyObject* args, PyObject* keywds) 59 | { 60 | 61 | // Manage inputs 62 | // ************* 63 | 64 | // Args containers 65 | PyObject* queries_obj = NULL; 66 | PyObject* supports_obj = NULL; 67 | PyObject* q_batches_obj = NULL; 68 | PyObject* s_batches_obj = NULL; 69 | 70 | // Keywords containers 71 | static char* kwlist[] = { "queries", "supports", "q_batches", "s_batches", "radius", NULL }; 72 | float radius = 0.1; 73 | 74 | // Parse the input 75 | if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOOO|$f", kwlist, &queries_obj, &supports_obj, &q_batches_obj, &s_batches_obj, &radius)) 76 | { 77 | PyErr_SetString(PyExc_RuntimeError, "Error parsing arguments"); 78 | return NULL; 79 | } 80 | 81 | 82 | // Interpret the input objects as numpy arrays. 83 | PyObject* queries_array = PyArray_FROM_OTF(queries_obj, NPY_FLOAT, NPY_IN_ARRAY); 84 | PyObject* supports_array = PyArray_FROM_OTF(supports_obj, NPY_FLOAT, NPY_IN_ARRAY); 85 | PyObject* q_batches_array = PyArray_FROM_OTF(q_batches_obj, NPY_INT, NPY_IN_ARRAY); 86 | PyObject* s_batches_array = PyArray_FROM_OTF(s_batches_obj, NPY_INT, NPY_IN_ARRAY); 87 | 88 | // Verify data was load correctly. 89 | if (queries_array == NULL) 90 | { 91 | Py_XDECREF(queries_array); 92 | Py_XDECREF(supports_array); 93 | Py_XDECREF(q_batches_array); 94 | Py_XDECREF(s_batches_array); 95 | PyErr_SetString(PyExc_RuntimeError, "Error converting query points to numpy arrays of type float32"); 96 | return NULL; 97 | } 98 | if (supports_array == NULL) 99 | { 100 | Py_XDECREF(queries_array); 101 | Py_XDECREF(supports_array); 102 | Py_XDECREF(q_batches_array); 103 | Py_XDECREF(s_batches_array); 104 | PyErr_SetString(PyExc_RuntimeError, "Error converting support points to numpy arrays of type float32"); 105 | return NULL; 106 | } 107 | if (q_batches_array == NULL) 108 | { 109 | Py_XDECREF(queries_array); 110 | Py_XDECREF(supports_array); 111 | Py_XDECREF(q_batches_array); 112 | Py_XDECREF(s_batches_array); 113 | PyErr_SetString(PyExc_RuntimeError, "Error converting query batches to numpy arrays of type int32"); 114 | return NULL; 115 | } 116 | if (s_batches_array == NULL) 117 | { 118 | Py_XDECREF(queries_array); 119 | Py_XDECREF(supports_array); 120 | Py_XDECREF(q_batches_array); 121 | Py_XDECREF(s_batches_array); 122 | PyErr_SetString(PyExc_RuntimeError, "Error converting support batches to numpy arrays of type int32"); 123 | return NULL; 124 | } 125 | 126 | // Check that the input array respect the dims 127 | if ((int)PyArray_NDIM(queries_array) != 2 || (int)PyArray_DIM(queries_array, 1) != 3) 128 | { 129 | Py_XDECREF(queries_array); 130 | Py_XDECREF(supports_array); 131 | Py_XDECREF(q_batches_array); 132 | Py_XDECREF(s_batches_array); 133 | PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : query.shape is not (N, 3)"); 134 | return NULL; 135 | } 136 | if ((int)PyArray_NDIM(supports_array) != 2 || (int)PyArray_DIM(supports_array, 1) != 3) 137 | { 138 | Py_XDECREF(queries_array); 139 | Py_XDECREF(supports_array); 140 | Py_XDECREF(q_batches_array); 141 | Py_XDECREF(s_batches_array); 142 | PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : support.shape is not (N, 3)"); 143 | return NULL; 144 | } 145 | if ((int)PyArray_NDIM(q_batches_array) > 1) 146 | { 147 | Py_XDECREF(queries_array); 148 | Py_XDECREF(supports_array); 149 | Py_XDECREF(q_batches_array); 150 | Py_XDECREF(s_batches_array); 151 | PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : queries_batches.shape is not (B,) "); 152 | return NULL; 153 | } 154 | if ((int)PyArray_NDIM(s_batches_array) > 1) 155 | { 156 | Py_XDECREF(queries_array); 157 | Py_XDECREF(supports_array); 158 | Py_XDECREF(q_batches_array); 159 | Py_XDECREF(s_batches_array); 160 | PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : supports_batches.shape is not (B,) "); 161 | return NULL; 162 | } 163 | if ((int)PyArray_DIM(q_batches_array, 0) != (int)PyArray_DIM(s_batches_array, 0)) 164 | { 165 | Py_XDECREF(queries_array); 166 | Py_XDECREF(supports_array); 167 | Py_XDECREF(q_batches_array); 168 | Py_XDECREF(s_batches_array); 169 | PyErr_SetString(PyExc_RuntimeError, "Wrong number of batch elements: different for queries and supports "); 170 | return NULL; 171 | } 172 | 173 | // Number of points 174 | int Nq = (int)PyArray_DIM(queries_array, 0); 175 | int Ns= (int)PyArray_DIM(supports_array, 0); 176 | 177 | // Number of batches 178 | int Nb = (int)PyArray_DIM(q_batches_array, 0); 179 | 180 | // Call the C++ function 181 | // ********************* 182 | 183 | // Convert PyArray to Cloud C++ class 184 | vector queries; 185 | vector supports; 186 | vector q_batches; 187 | vector s_batches; 188 | queries = vector((PointXYZ*)PyArray_DATA(queries_array), (PointXYZ*)PyArray_DATA(queries_array) + Nq); 189 | supports = vector((PointXYZ*)PyArray_DATA(supports_array), (PointXYZ*)PyArray_DATA(supports_array) + Ns); 190 | q_batches = vector((int*)PyArray_DATA(q_batches_array), (int*)PyArray_DATA(q_batches_array) + Nb); 191 | s_batches = vector((int*)PyArray_DATA(s_batches_array), (int*)PyArray_DATA(s_batches_array) + Nb); 192 | 193 | // Create result containers 194 | vector neighbors_indices; 195 | 196 | // Compute results 197 | //batch_ordered_neighbors(queries, supports, q_batches, s_batches, neighbors_indices, radius); 198 | batch_nanoflann_neighbors(queries, supports, q_batches, s_batches, neighbors_indices, radius); 199 | 200 | // Check result 201 | if (neighbors_indices.size() < 1) 202 | { 203 | PyErr_SetString(PyExc_RuntimeError, "Error"); 204 | return NULL; 205 | } 206 | 207 | // Manage outputs 208 | // ************** 209 | 210 | // Maximal number of neighbors 211 | int max_neighbors = neighbors_indices.size() / Nq; 212 | 213 | // Dimension of output containers 214 | npy_intp* neighbors_dims = new npy_intp[2]; 215 | neighbors_dims[0] = Nq; 216 | neighbors_dims[1] = max_neighbors; 217 | 218 | // Create output array 219 | PyObject* res_obj = PyArray_SimpleNew(2, neighbors_dims, NPY_INT); 220 | PyObject* ret = NULL; 221 | 222 | // Fill output array with values 223 | size_t size_in_bytes = Nq * max_neighbors * sizeof(int); 224 | memcpy(PyArray_DATA(res_obj), neighbors_indices.data(), size_in_bytes); 225 | 226 | // Merge results 227 | ret = Py_BuildValue("N", res_obj); 228 | 229 | // Clean up 230 | // ******** 231 | 232 | Py_XDECREF(queries_array); 233 | Py_XDECREF(supports_array); 234 | Py_XDECREF(q_batches_array); 235 | Py_XDECREF(s_batches_array); 236 | 237 | return ret; 238 | } 239 | -------------------------------------------------------------------------------- /cpp_wrappers/cpp_subsampling/build.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | py setup.py build_ext --inplace 3 | 4 | 5 | pause -------------------------------------------------------------------------------- /cpp_wrappers/cpp_subsampling/grid_subsampling/grid_subsampling.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "grid_subsampling.h" 3 | 4 | 5 | void grid_subsampling(vector& original_points, 6 | vector& subsampled_points, 7 | vector& original_features, 8 | vector& subsampled_features, 9 | vector& original_classes, 10 | vector& subsampled_classes, 11 | float sampleDl, 12 | int verbose) { 13 | 14 | // Initialize variables 15 | // ****************** 16 | 17 | // Number of points in the cloud 18 | size_t N = original_points.size(); 19 | 20 | // Dimension of the features 21 | size_t fdim = original_features.size() / N; 22 | size_t ldim = original_classes.size() / N; 23 | 24 | // Limits of the cloud 25 | PointXYZ minCorner = min_point(original_points); 26 | PointXYZ maxCorner = max_point(original_points); 27 | PointXYZ originCorner = floor(minCorner * (1/sampleDl)) * sampleDl; 28 | 29 | // Dimensions of the grid 30 | size_t sampleNX = (size_t)floor((maxCorner.x - originCorner.x) / sampleDl) + 1; 31 | size_t sampleNY = (size_t)floor((maxCorner.y - originCorner.y) / sampleDl) + 1; 32 | //size_t sampleNZ = (size_t)floor((maxCorner.z - originCorner.z) / sampleDl) + 1; 33 | 34 | // Check if features and classes need to be processed 35 | bool use_feature = original_features.size() > 0; 36 | bool use_classes = original_classes.size() > 0; 37 | 38 | 39 | // Create the sampled map 40 | // ********************** 41 | 42 | // Verbose parameters 43 | int i = 0; 44 | int nDisp = N / 100; 45 | 46 | // Initialize variables 47 | size_t iX, iY, iZ, mapIdx; 48 | unordered_map data; 49 | 50 | for (auto& p : original_points) 51 | { 52 | // Position of point in sample map 53 | iX = (size_t)floor((p.x - originCorner.x) / sampleDl); 54 | iY = (size_t)floor((p.y - originCorner.y) / sampleDl); 55 | iZ = (size_t)floor((p.z - originCorner.z) / sampleDl); 56 | mapIdx = iX + sampleNX*iY + sampleNX*sampleNY*iZ; 57 | 58 | // If not already created, create key 59 | if (data.count(mapIdx) < 1) 60 | data.emplace(mapIdx, SampledData(fdim, ldim)); 61 | 62 | // Fill the sample map 63 | if (use_feature && use_classes) 64 | data[mapIdx].update_all(p, original_features.begin() + i * fdim, original_classes.begin() + i * ldim); 65 | else if (use_feature) 66 | data[mapIdx].update_features(p, original_features.begin() + i * fdim); 67 | else if (use_classes) 68 | data[mapIdx].update_classes(p, original_classes.begin() + i * ldim); 69 | else 70 | data[mapIdx].update_points(p); 71 | 72 | // Display 73 | i++; 74 | if (verbose > 1 && i%nDisp == 0) 75 | std::cout << "\rSampled Map : " << std::setw(3) << i / nDisp << "%"; 76 | 77 | } 78 | 79 | // Divide for barycentre and transfer to a vector 80 | subsampled_points.reserve(data.size()); 81 | if (use_feature) 82 | subsampled_features.reserve(data.size() * fdim); 83 | if (use_classes) 84 | subsampled_classes.reserve(data.size() * ldim); 85 | for (auto& v : data) 86 | { 87 | subsampled_points.push_back(v.second.point * (1.0 / v.second.count)); 88 | if (use_feature) 89 | { 90 | float count = (float)v.second.count; 91 | transform(v.second.features.begin(), 92 | v.second.features.end(), 93 | v.second.features.begin(), 94 | [count](float f) { return f / count;}); 95 | subsampled_features.insert(subsampled_features.end(),v.second.features.begin(),v.second.features.end()); 96 | } 97 | if (use_classes) 98 | { 99 | for (int i = 0; i < ldim; i++) 100 | subsampled_classes.push_back(max_element(v.second.labels[i].begin(), v.second.labels[i].end(), 101 | [](const pair&a, const pair&b){return a.second < b.second;})->first); 102 | } 103 | } 104 | 105 | return; 106 | } 107 | 108 | 109 | void batch_grid_subsampling(vector& original_points, 110 | vector& subsampled_points, 111 | vector& original_features, 112 | vector& subsampled_features, 113 | vector& original_classes, 114 | vector& subsampled_classes, 115 | vector& original_batches, 116 | vector& subsampled_batches, 117 | float sampleDl, 118 | int max_p) 119 | { 120 | // Initialize variables 121 | // ****************** 122 | 123 | int b = 0; 124 | int sum_b = 0; 125 | 126 | // Number of points in the cloud 127 | size_t N = original_points.size(); 128 | 129 | // Dimension of the features 130 | size_t fdim = original_features.size() / N; 131 | size_t ldim = original_classes.size() / N; 132 | 133 | // Handle max_p = 0 134 | if (max_p < 1) 135 | max_p = N; 136 | 137 | // Loop over batches 138 | // ***************** 139 | 140 | for (b = 0; b < original_batches.size(); b++) 141 | { 142 | 143 | // Extract batch points features and labels 144 | vector b_o_points = vector(original_points.begin () + sum_b, 145 | original_points.begin () + sum_b + original_batches[b]); 146 | 147 | vector b_o_features; 148 | if (original_features.size() > 0) 149 | { 150 | b_o_features = vector(original_features.begin () + sum_b * fdim, 151 | original_features.begin () + (sum_b + original_batches[b]) * fdim); 152 | } 153 | 154 | vector b_o_classes; 155 | if (original_classes.size() > 0) 156 | { 157 | b_o_classes = vector(original_classes.begin () + sum_b * ldim, 158 | original_classes.begin () + sum_b + original_batches[b] * ldim); 159 | } 160 | 161 | 162 | // Create result containers 163 | vector b_s_points; 164 | vector b_s_features; 165 | vector b_s_classes; 166 | 167 | // Compute subsampling on current batch 168 | grid_subsampling(b_o_points, 169 | b_s_points, 170 | b_o_features, 171 | b_s_features, 172 | b_o_classes, 173 | b_s_classes, 174 | sampleDl, 175 | 0); 176 | 177 | // Stack batches points features and labels 178 | // **************************************** 179 | 180 | // If too many points remove some 181 | if (b_s_points.size() <= max_p) 182 | { 183 | subsampled_points.insert(subsampled_points.end(), b_s_points.begin(), b_s_points.end()); 184 | 185 | if (original_features.size() > 0) 186 | subsampled_features.insert(subsampled_features.end(), b_s_features.begin(), b_s_features.end()); 187 | 188 | if (original_classes.size() > 0) 189 | subsampled_classes.insert(subsampled_classes.end(), b_s_classes.begin(), b_s_classes.end()); 190 | 191 | subsampled_batches.push_back(b_s_points.size()); 192 | } 193 | else 194 | { 195 | subsampled_points.insert(subsampled_points.end(), b_s_points.begin(), b_s_points.begin() + max_p); 196 | 197 | if (original_features.size() > 0) 198 | subsampled_features.insert(subsampled_features.end(), b_s_features.begin(), b_s_features.begin() + max_p * fdim); 199 | 200 | if (original_classes.size() > 0) 201 | subsampled_classes.insert(subsampled_classes.end(), b_s_classes.begin(), b_s_classes.begin() + max_p * ldim); 202 | 203 | subsampled_batches.push_back(max_p); 204 | } 205 | 206 | // Stack new batch lengths 207 | sum_b += original_batches[b]; 208 | } 209 | 210 | return; 211 | } 212 | -------------------------------------------------------------------------------- /cpp_wrappers/cpp_subsampling/grid_subsampling/grid_subsampling.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "../../cpp_utils/cloud/cloud.h" 4 | 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | class SampledData 11 | { 12 | public: 13 | 14 | // Elements 15 | // ******** 16 | 17 | int count; 18 | PointXYZ point; 19 | vector features; 20 | vector> labels; 21 | 22 | 23 | // Methods 24 | // ******* 25 | 26 | // Constructor 27 | SampledData() 28 | { 29 | count = 0; 30 | point = PointXYZ(); 31 | } 32 | 33 | SampledData(const size_t fdim, const size_t ldim) 34 | { 35 | count = 0; 36 | point = PointXYZ(); 37 | features = vector(fdim); 38 | labels = vector>(ldim); 39 | } 40 | 41 | // Method Update 42 | void update_all(const PointXYZ p, vector::iterator f_begin, vector::iterator l_begin) 43 | { 44 | count += 1; 45 | point += p; 46 | transform (features.begin(), features.end(), f_begin, features.begin(), plus()); 47 | int i = 0; 48 | for(vector::iterator it = l_begin; it != l_begin + labels.size(); ++it) 49 | { 50 | labels[i][*it] += 1; 51 | i++; 52 | } 53 | return; 54 | } 55 | void update_features(const PointXYZ p, vector::iterator f_begin) 56 | { 57 | count += 1; 58 | point += p; 59 | transform (features.begin(), features.end(), f_begin, features.begin(), plus()); 60 | return; 61 | } 62 | void update_classes(const PointXYZ p, vector::iterator l_begin) 63 | { 64 | count += 1; 65 | point += p; 66 | int i = 0; 67 | for(vector::iterator it = l_begin; it != l_begin + labels.size(); ++it) 68 | { 69 | labels[i][*it] += 1; 70 | i++; 71 | } 72 | return; 73 | } 74 | void update_points(const PointXYZ p) 75 | { 76 | count += 1; 77 | point += p; 78 | return; 79 | } 80 | }; 81 | 82 | void grid_subsampling(vector& original_points, 83 | vector& subsampled_points, 84 | vector& original_features, 85 | vector& subsampled_features, 86 | vector& original_classes, 87 | vector& subsampled_classes, 88 | float sampleDl, 89 | int verbose); 90 | 91 | void batch_grid_subsampling(vector& original_points, 92 | vector& subsampled_points, 93 | vector& original_features, 94 | vector& subsampled_features, 95 | vector& original_classes, 96 | vector& subsampled_classes, 97 | vector& original_batches, 98 | vector& subsampled_batches, 99 | float sampleDl, 100 | int max_p); 101 | 102 | -------------------------------------------------------------------------------- /cpp_wrappers/cpp_subsampling/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup, Extension 2 | import numpy.distutils.misc_util 3 | 4 | # Adding OpenCV to project 5 | # ************************ 6 | 7 | # Adding sources of the project 8 | # ***************************** 9 | 10 | SOURCES = ["../cpp_utils/cloud/cloud.cpp", 11 | "grid_subsampling/grid_subsampling.cpp", 12 | "wrapper.cpp"] 13 | 14 | module = Extension(name="grid_subsampling", 15 | sources=SOURCES, 16 | extra_compile_args=['-std=c++11', 17 | '-D_GLIBCXX_USE_CXX11_ABI=0']) 18 | 19 | 20 | setup(ext_modules=[module], include_dirs=numpy.distutils.misc_util.get_numpy_include_dirs()) 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /cpp_wrappers/cpp_utils/cloud/cloud.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // 0==========================0 4 | // | Local feature test | 5 | // 0==========================0 6 | // 7 | // version 1.0 : 8 | // > 9 | // 10 | //--------------------------------------------------- 11 | // 12 | // Cloud source : 13 | // Define usefull Functions/Methods 14 | // 15 | //---------------------------------------------------- 16 | // 17 | // Hugues THOMAS - 10/02/2017 18 | // 19 | 20 | 21 | #include "cloud.h" 22 | 23 | 24 | // Getters 25 | // ******* 26 | 27 | PointXYZ max_point(std::vector points) 28 | { 29 | // Initialize limits 30 | PointXYZ maxP(points[0]); 31 | 32 | // Loop over all points 33 | for (auto p : points) 34 | { 35 | if (p.x > maxP.x) 36 | maxP.x = p.x; 37 | 38 | if (p.y > maxP.y) 39 | maxP.y = p.y; 40 | 41 | if (p.z > maxP.z) 42 | maxP.z = p.z; 43 | } 44 | 45 | return maxP; 46 | } 47 | 48 | PointXYZ min_point(std::vector points) 49 | { 50 | // Initialize limits 51 | PointXYZ minP(points[0]); 52 | 53 | // Loop over all points 54 | for (auto p : points) 55 | { 56 | if (p.x < minP.x) 57 | minP.x = p.x; 58 | 59 | if (p.y < minP.y) 60 | minP.y = p.y; 61 | 62 | if (p.z < minP.z) 63 | minP.z = p.z; 64 | } 65 | 66 | return minP; 67 | } -------------------------------------------------------------------------------- /cpp_wrappers/cpp_utils/cloud/cloud.h: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // 0==========================0 4 | // | Local feature test | 5 | // 0==========================0 6 | // 7 | // version 1.0 : 8 | // > 9 | // 10 | //--------------------------------------------------- 11 | // 12 | // Cloud header 13 | // 14 | //---------------------------------------------------- 15 | // 16 | // Hugues THOMAS - 10/02/2017 17 | // 18 | 19 | 20 | # pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | 34 | 35 | 36 | // Point class 37 | // *********** 38 | 39 | 40 | class PointXYZ 41 | { 42 | public: 43 | 44 | // Elements 45 | // ******** 46 | 47 | float x, y, z; 48 | 49 | 50 | // Methods 51 | // ******* 52 | 53 | // Constructor 54 | PointXYZ() { x = 0; y = 0; z = 0; } 55 | PointXYZ(float x0, float y0, float z0) { x = x0; y = y0; z = z0; } 56 | 57 | // array type accessor 58 | float operator [] (int i) const 59 | { 60 | if (i == 0) return x; 61 | else if (i == 1) return y; 62 | else return z; 63 | } 64 | 65 | // opperations 66 | float dot(const PointXYZ P) const 67 | { 68 | return x * P.x + y * P.y + z * P.z; 69 | } 70 | 71 | float sq_norm() 72 | { 73 | return x*x + y*y + z*z; 74 | } 75 | 76 | PointXYZ cross(const PointXYZ P) const 77 | { 78 | return PointXYZ(y*P.z - z*P.y, z*P.x - x*P.z, x*P.y - y*P.x); 79 | } 80 | 81 | PointXYZ& operator+=(const PointXYZ& P) 82 | { 83 | x += P.x; 84 | y += P.y; 85 | z += P.z; 86 | return *this; 87 | } 88 | 89 | PointXYZ& operator-=(const PointXYZ& P) 90 | { 91 | x -= P.x; 92 | y -= P.y; 93 | z -= P.z; 94 | return *this; 95 | } 96 | 97 | PointXYZ& operator*=(const float& a) 98 | { 99 | x *= a; 100 | y *= a; 101 | z *= a; 102 | return *this; 103 | } 104 | }; 105 | 106 | 107 | // Point Opperations 108 | // ***************** 109 | 110 | inline PointXYZ operator + (const PointXYZ A, const PointXYZ B) 111 | { 112 | return PointXYZ(A.x + B.x, A.y + B.y, A.z + B.z); 113 | } 114 | 115 | inline PointXYZ operator - (const PointXYZ A, const PointXYZ B) 116 | { 117 | return PointXYZ(A.x - B.x, A.y - B.y, A.z - B.z); 118 | } 119 | 120 | inline PointXYZ operator * (const PointXYZ P, const float a) 121 | { 122 | return PointXYZ(P.x * a, P.y * a, P.z * a); 123 | } 124 | 125 | inline PointXYZ operator * (const float a, const PointXYZ P) 126 | { 127 | return PointXYZ(P.x * a, P.y * a, P.z * a); 128 | } 129 | 130 | inline std::ostream& operator << (std::ostream& os, const PointXYZ P) 131 | { 132 | return os << "[" << P.x << ", " << P.y << ", " << P.z << "]"; 133 | } 134 | 135 | inline bool operator == (const PointXYZ A, const PointXYZ B) 136 | { 137 | return A.x == B.x && A.y == B.y && A.z == B.z; 138 | } 139 | 140 | inline PointXYZ floor(const PointXYZ P) 141 | { 142 | return PointXYZ(std::floor(P.x), std::floor(P.y), std::floor(P.z)); 143 | } 144 | 145 | 146 | PointXYZ max_point(std::vector points); 147 | PointXYZ min_point(std::vector points); 148 | 149 | 150 | struct PointCloud 151 | { 152 | 153 | std::vector pts; 154 | 155 | // Must return the number of data points 156 | inline size_t kdtree_get_point_count() const { return pts.size(); } 157 | 158 | // Returns the dim'th component of the idx'th point in the class: 159 | // Since this is inlined and the "dim" argument is typically an immediate value, the 160 | // "if/else's" are actually solved at compile time. 161 | inline float kdtree_get_pt(const size_t idx, const size_t dim) const 162 | { 163 | if (dim == 0) return pts[idx].x; 164 | else if (dim == 1) return pts[idx].y; 165 | else return pts[idx].z; 166 | } 167 | 168 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 169 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 170 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 171 | template 172 | bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } 173 | 174 | }; 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | -------------------------------------------------------------------------------- /data/Kitti.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import glob 3 | import numpy as np 4 | import os 5 | import pickle 6 | import random 7 | import torch 8 | import open3d as o3d 9 | from scipy.spatial.transform import Rotation 10 | from torch.utils.data import Dataset 11 | CUR = os.path.dirname(os.path.abspath(__file__)) 12 | import sys 13 | sys.path.append(os.path.dirname(CUR)) 14 | from utils import npy2pcd, pcd2npy, vis_plys, get_correspondences, format_lines, voxel_ds, normal 15 | import cpp_wrappers.cpp_subsampling.grid_subsampling as cpp_subsampling 16 | 17 | 18 | class Kitti(Dataset): 19 | def __init__(self, root, split, aug, voxel_size, overlap_radius, max_coors, 20 | noise_scale=0.01, 21 | augment_scale_min=0.8, 22 | augment_scale_max=1.2, 23 | augment_shift_range=2.0): 24 | super().__init__() 25 | self.root = root 26 | self.split = split 27 | self.aug = aug 28 | self.overlap_radius = overlap_radius 29 | self.voxel_size = voxel_size 30 | self.max_coors = max_coors 31 | self.noise_scale = noise_scale 32 | self.augment_scale_min = augment_scale_min 33 | self.augment_scale_max = augment_scale_max 34 | self.augment_shift_range = augment_shift_range 35 | 36 | self.dataset_root = os.path.join(root, 'dataset') 37 | self.icp_root = os.path.join(root, 'icp2') 38 | os.makedirs(self.icp_root, exist_ok=True) 39 | 40 | self.pose_cache, self.icp_cache = {}, {} 41 | 42 | assert split in ['train', 'val', 'test'] 43 | seq_file = os.path.join(CUR, 'Kitti', f'{split}_kitti.txt') 44 | with open(seq_file, 'r') as f: 45 | lines = f.readlines() 46 | seq_ids = [f'{int(line.strip()):02}' for line in lines] 47 | self.pairs_info = self.prepare_pairs(seq_ids) 48 | 49 | if split=='test': 50 | self.pairs_info.remove(['08', 15, 58]) 51 | print(f'Num_{split}: {len(self.pairs_info)}') 52 | 53 | def prepare_pairs(self, seq_ids): 54 | pairs_info = [] 55 | for seq_id in seq_ids: 56 | pose_file = os.path.join(self.dataset_root, 'poses', f'{seq_id}.txt') 57 | poses = np.genfromtxt(pose_file) # (n, 12) 58 | poses = np.array([np.vstack([pose.reshape(3, 4), [0, 0, 0, 1]]) for pose in poses]) # (n, 4, 4) 59 | self.pose_cache[seq_id] = poses 60 | Ts = poses[:, :3, 3] # (n, 3) 61 | dists = np.sqrt(np.sum((Ts[:, None, :] - Ts[None, :, :]) ** 2, axis=-1)) # (n, n) 62 | 63 | bin_path = os.path.join(self.dataset_root, 'sequences', seq_id, 'velodyne') 64 | bin_files = sorted(glob.glob(os.path.join(bin_path, '*.bin'))) 65 | bin_ids = [int(os.path.split(bin_file)[-1][:-4]) for bin_file in bin_files] 66 | 67 | # select pairs which are about 10m away from each other 68 | dists_bool = dists > 10 69 | cur_bin_id, mmax_id = bin_ids[0], np.max(bin_ids) 70 | while cur_bin_id < mmax_id: 71 | valid_ids = np.where(dists_bool[cur_bin_id][cur_bin_id:cur_bin_id + 100])[0] 72 | # print(dists_bool[cur_bin_id][cur_bin_id:cur_bin_id + 100]) 73 | # print(cur_bin_id, valid_ids) 74 | if len(valid_ids) == 0: 75 | cur_bin_id += 1 76 | continue 77 | next_bin_id = cur_bin_id + valid_ids[0] - 1 78 | pairs_info.append([seq_id, cur_bin_id, next_bin_id]) 79 | cur_bin_id = next_bin_id + 1 80 | return pairs_info 81 | 82 | @property 83 | def velo2cam(self): 84 | try: 85 | velo2cam = self._velo2cam 86 | except AttributeError: 87 | R = np.array([ 88 | 7.533745e-03, -9.999714e-01, -6.166020e-04, 1.480249e-02, 7.280733e-04, 89 | -9.998902e-01, 9.998621e-01, 7.523790e-03, 1.480755e-02 90 | ]).reshape(3, 3) 91 | t = np.array([-4.069766e-03, -7.631618e-02, -2.717806e-01]).reshape(3, 1) 92 | velo2cam = np.hstack([R, t]) 93 | self._velo2cam = np.vstack((velo2cam, [0, 0, 0, 1])).T 94 | return self._velo2cam 95 | 96 | def __len__(self): 97 | return len(self.pairs_info) 98 | 99 | def __getitem__(self, item): 100 | seq_id, src_id, tgt_id = self.pairs_info[item] 101 | src_points_path = os.path.join(os.path.join(self.dataset_root, 'sequences', seq_id, 'velodyne', f'{src_id:06}.bin')) 102 | tgt_points_path = os.path.join(os.path.join(self.dataset_root, 'sequences', seq_id, 'velodyne', f'{tgt_id:06}.bin')) 103 | src_points = np.fromfile(src_points_path, dtype=np.float32).reshape(-1, 4)[:, :3] 104 | tgt_points = np.fromfile(tgt_points_path, dtype=np.float32).reshape(-1, 4)[:, :3] 105 | src_position, tgt_position = self.pose_cache[seq_id][[src_id, tgt_id]] 106 | 107 | # Following D3Feat and PREDATOR, we use ICP to refine the ground_truth pose, and we don't voxelize the point clouds here. 108 | key = f'{seq_id}_{src_id}_{tgt_id}' 109 | filename = os.path.join(self.icp_root, f'{key}.npy') 110 | if key not in self.icp_cache: 111 | if not os.path.exists(filename): 112 | # print('missing ICP files, recompute it') 113 | M = (self.velo2cam @ src_position.T @ np.linalg.inv(tgt_position.T) 114 | @ np.linalg.inv(self.velo2cam)).T 115 | reg = o3d.registration.registration_icp(npy2pcd(src_points), npy2pcd(tgt_points), 0.2, M, 116 | o3d.registration.TransformationEstimationPointToPoint(), 117 | o3d.registration.ICPConvergenceCriteria(max_iteration=200)) 118 | T = reg.transformation 119 | np.save(filename, T) 120 | else: 121 | T = np.load(filename) 122 | self.icp_cache[key] = T 123 | else: 124 | T = self.icp_cache[key] 125 | 126 | src_pcd, tgt_pcd = normal(npy2pcd(src_points), radius=3, max_nn=64, loc=(0, 0, 100)), normal(npy2pcd(tgt_points), radius=3, max_nn=64, loc=(0, 0, 100)) 127 | src_normals = np.array(src_pcd.normals).astype(np.float32) 128 | tgt_normals = np.array(tgt_pcd.normals).astype(np.float32) 129 | len_src, len_tgt = len(src_points), len(tgt_points) 130 | new_points, new_len, new_normals = cpp_subsampling.subsample_batch(np.vstack([src_points, tgt_points]), 131 | [len_src, len_tgt], 132 | features=np.vstack([src_normals, tgt_normals]), 133 | sampleDl=self.voxel_size, 134 | max_p=0, 135 | verbose=0) 136 | src_points, tgt_points = new_points[:new_len[0]], new_points[new_len[0]:] 137 | src_normals, tgt_normals = new_normals[:new_len[0]], new_normals[new_len[0]:] 138 | 139 | # src_points = pcd2npy(voxel_ds(npy2pcd(src_points), self.voxel_size)) 140 | # tgt_points = pcd2npy(voxel_ds(npy2pcd(tgt_points), self.voxel_size)) 141 | 142 | coors = get_correspondences(npy2pcd(src_points), 143 | npy2pcd(tgt_points), 144 | T, 145 | self.overlap_radius) 146 | 147 | if coors.shape[0] < self.max_coors and self.split == 'train': 148 | return self.__getitem__(np.random.choice(len(self.pairs_info), 1)[0]) 149 | 150 | src_feats = np.ones_like(src_points[:, :1], dtype=np.float32) 151 | tgt_feats = np.ones_like(tgt_points[:, :1], dtype=np.float32) 152 | 153 | src_points_raw = copy.deepcopy(src_points) 154 | tgt_points_raw = copy.deepcopy(tgt_points) 155 | 156 | if self.aug: 157 | # noise 158 | src_points += (np.random.rand(src_points.shape[0], 3) - 0.5) * self.noise_scale 159 | tgt_points += (np.random.rand(tgt_points.shape[0], 3) - 0.5) * self.noise_scale 160 | 161 | # rotation 162 | euler_ab = np.random.rand(3) * 2 * np.pi 163 | rot_ab = Rotation.from_euler('zyx', euler_ab).as_matrix() 164 | if np.random.rand() > 0.5: 165 | src_points = src_points @ rot_ab.T 166 | src_normals = src_normals @ rot_ab.T 167 | else: 168 | tgt_points = tgt_points @ rot_ab.T 169 | tgt_normals = tgt_normals @ rot_ab.T 170 | 171 | # scale 172 | scale = self.augment_scale_min + (self.augment_scale_max - self.augment_scale_min) * random.random() 173 | src_points = src_points * scale 174 | tgt_points = tgt_points * scale 175 | 176 | # shift 177 | shift_src = np.random.uniform(-self.augment_shift_range, self.augment_shift_range, 3) 178 | shift_tgt = np.random.uniform(-self.augment_shift_range, self.augment_shift_range, 3) 179 | src_points = src_points + shift_src 180 | tgt_points = tgt_points + shift_tgt 181 | 182 | pair = dict( 183 | src_points=src_points, 184 | tgt_points=tgt_points, 185 | src_feats=src_feats, 186 | tgt_feats=tgt_feats, 187 | src_normals=src_normals, 188 | tgt_normals=tgt_normals, 189 | transf=T, 190 | coors=coors, 191 | src_points_raw=src_points_raw, 192 | tgt_points_raw=tgt_points_raw) 193 | return pair 194 | -------------------------------------------------------------------------------- /data/Kitti/test_kitti.txt: -------------------------------------------------------------------------------- 1 | 8 2 | 9 3 | 10 -------------------------------------------------------------------------------- /data/Kitti/train_kitti.txt: -------------------------------------------------------------------------------- 1 | 0 2 | 1 3 | 2 4 | 3 5 | 4 6 | 5 -------------------------------------------------------------------------------- /data/Kitti/val_kitti.txt: -------------------------------------------------------------------------------- 1 | 6 2 | 7 -------------------------------------------------------------------------------- /data/MVP_RG.py: -------------------------------------------------------------------------------- 1 | import os, h5py 2 | import numpy as np 3 | import open3d as o3d 4 | from torch.utils.data import Dataset 5 | from utils import npy2pcd, get_correspondences, normal 6 | 7 | 8 | def jitter_pcd(pcd, sigma=0.01, clip=0.05): 9 | pcd += np.clip(sigma * np.random.randn(*pcd.shape), -1 * clip, clip) 10 | return pcd 11 | 12 | 13 | def random_pose(max_angle, max_trans): 14 | R = random_rotation(max_angle) 15 | t = random_translation(max_trans) 16 | return np.concatenate([np.concatenate([R, t], 1), [[0, 0, 0, 1]]], 0) 17 | 18 | 19 | def random_rotation(max_angle): 20 | axis = np.random.randn(3) 21 | axis /= np.linalg.norm(axis) 22 | angle = np.random.rand() * max_angle 23 | A = np.array([[0, -axis[2], axis[1]], 24 | [axis[2], 0, -axis[0]], 25 | [-axis[1], axis[0], 0]]) 26 | R = np.eye(3) + np.sin(angle) * A + (1 - np.cos(angle)) * np.dot(A, A) 27 | return R 28 | 29 | 30 | def random_translation(max_dist): 31 | t = np.random.randn(3) 32 | t /= np.linalg.norm(t) 33 | t *= np.random.rand() * max_dist 34 | return np.expand_dims(t, 1) 35 | 36 | 37 | class MVP_RG(Dataset): 38 | def __init__(self, root, split, rot_mag, trans_mag, overlap_radius): 39 | """ 40 | Args: 41 | root (str): Folder containing processed dataset 42 | subset (str): Dataset subset, either 'train' or 'test' 43 | categories (list): Categories to use 44 | transform (callable, optional): Optional transform to be applied 45 | on a sample. 46 | """ 47 | self._root = root 48 | self._subset = split 49 | self.max_angle, self.max_trans = rot_mag, trans_mag 50 | self.overlap_radius = overlap_radius # 0.04 51 | 52 | if split == "train": 53 | h5_filelist = [os.path.join(root, 'MVP_Train_RG.h5')] 54 | elif split == "val": 55 | h5_filelist = [os.path.join(root, 'MVP_Test_RG.h5')] 56 | elif split == "test": 57 | h5_filelist = [os.path.join(root, 'MVP_ExtraTest_RG.h5')] 58 | 59 | self._src_tol, self._tgt_tol, self._transforms_tol, self._labels = \ 60 | self._read_h5_files(h5_filelist) 61 | # self._data, self._labels = self._data[:32], self._labels[:32, ...] 62 | 63 | def __getitem__(self, item): 64 | sample = {'points_src': self._src_tol[item, :, :], 65 | 'points_ref': self._tgt_tol[item, :, :], 66 | 'transform_gt': self._transforms_tol[item, :, :], 67 | 'label': self._labels[item], 68 | 'idx': np.array(item, dtype=np.int32)} 69 | 70 | if 'train' in self._subset: 71 | transform = random_pose(self.max_angle, self.max_trans / 2) 72 | pose1 = random_pose(np.pi, self.max_trans) 73 | pose2 = transform @ pose1 74 | sample['transform_gt'] = transform 75 | sample['points_src'][:, :3] = sample['points_src'][:, :3] @ pose1[:3, :3].T + pose1[:3, 3] 76 | sample['points_ref'][:, :3] = sample['points_ref'][:, :3] @ pose2[:3, :3].T + pose2[:3, 3] 77 | 78 | # transform to our format 79 | src_points = sample['points_src'][:, :3].astype(np.float32) 80 | tgt_points = sample['points_ref'][:, :3].astype(np.float32) 81 | rot = sample['transform_gt'][:3, :3].astype(np.float32) 82 | trans = sample['transform_gt'][:3, 3][:, None].astype(np.float32) 83 | transf = np.eye(4, dtype=np.float32) 84 | transf[:3, :3] = rot 85 | transf[:3, 3:] = trans 86 | matching_inds = get_correspondences(npy2pcd(src_points), 87 | npy2pcd(tgt_points), 88 | transf, 89 | self.overlap_radius) 90 | 91 | src_pcd = normal(npy2pcd(src_points), radius=0.2, max_nn=30) #, loc=(0, 0, 1)) # new loc 92 | tgt_pcd = normal(npy2pcd(tgt_points), radius=0.2, max_nn=30) #, loc=(0, 0, 1)) # new loc 93 | src_normals = np.array(src_pcd.normals).astype(np.float32) 94 | tgt_normals = np.array(tgt_pcd.normals).astype(np.float32) 95 | 96 | src_feats = np.ones_like(src_points[:, :1]).astype(np.float32) 97 | tgt_feats = np.ones_like(tgt_points[:, :1]).astype(np.float32) 98 | 99 | pair = dict( 100 | src_points=src_points, 101 | tgt_points=tgt_points, 102 | src_feats=src_feats, 103 | tgt_feats=tgt_feats, 104 | src_normals=src_normals, 105 | tgt_normals=tgt_normals, 106 | transf=transf, 107 | coors=matching_inds, 108 | src_points_raw=src_points, 109 | tgt_points_raw=tgt_points) 110 | return pair 111 | 112 | def __len__(self): 113 | return self._labels.shape[0] 114 | 115 | @property 116 | def classes(self): 117 | return self._classes 118 | 119 | def _read_h5_files(self, fnames): 120 | 121 | src_tol, tgt_tol, transforms_tol = [], [], [] 122 | all_labels = [] 123 | 124 | for fname in fnames: 125 | f = h5py.File(fname, mode='r') 126 | labels = f['cat_labels'][:].astype('int32') 127 | if self._subset == "test": 128 | src = np.array(f['rotated_src'][:].astype('float32')) 129 | tgt = np.array(f['rotated_tgt'][:].astype('float32')) 130 | transforms = np.array([np.eye(4, dtype=np.float32) for _ in range(src.shape[0])]).astype(np.float32) 131 | else: 132 | if "train" in self._subset: 133 | src = np.array(f['src'][:].astype('float32')) 134 | tgt = np.array(f['tgt'][:].astype('float32')) 135 | transforms = np.array([np.eye(4, dtype=np.float32) for _ in range(src.shape[0])]).astype(np.float32) 136 | elif self._subset == "val": 137 | src = np.array(f['rotated_src'][:].astype('float32')) 138 | tgt = np.array(f['rotated_tgt'][:].astype('float32')) 139 | transforms = np.array( 140 | f['transforms'][:].astype('float32')) 141 | 142 | src_tol.append(src) 143 | tgt_tol.append(tgt) 144 | transforms_tol.append(transforms) 145 | all_labels.append(labels) 146 | src_tol = np.concatenate(src_tol, axis=0).astype(np.float32) 147 | tgt_tol = np.concatenate(tgt_tol, axis=0).astype(np.float32) 148 | transforms_tol = np.concatenate(transforms_tol, axis=0).astype(np.float32) 149 | all_labels = np.concatenate(all_labels, axis=0).astype(np.float32) 150 | return src_tol, tgt_tol, transforms_tol, all_labels 151 | -------------------------------------------------------------------------------- /data/ThreeDMatch.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import pickle 4 | import torch 5 | from scipy.spatial.transform import Rotation 6 | from torch.utils.data import Dataset 7 | CUR = os.path.dirname(os.path.abspath(__file__)) 8 | from utils import npy2pcd, pcd2npy, vis_plys, get_correspondences, format_lines, normal 9 | 10 | 11 | class ThreeDMatch(Dataset): 12 | def __init__(self, root, split, aug, overlap_radius, noise_scale=0.005): 13 | super().__init__() 14 | self.root = root 15 | self.split = split 16 | self.aug = aug 17 | self.noise_scale = noise_scale 18 | self.overlap_radius = overlap_radius 19 | self.max_points = 30000 20 | 21 | pkl_path = os.path.join(CUR, 'ThreeDMatch', f'{split}_info.pkl') 22 | if not os.path.exists(pkl_path): 23 | pkl_path = os.path.join(CUR, 'ThreeDMatch', f'{split}.pkl') 24 | with open(pkl_path, 'rb') as f: 25 | self.infos = pickle.load(f) 26 | for k, v in self.infos.items(): 27 | print(k, len(v), type(v)) 28 | 29 | def __len__(self): 30 | return len(self.infos['rot']) 31 | 32 | def __getitem__(self, item): 33 | src_path, tgt_path = self.infos['src'][item], self.infos['tgt'][item] # str, str 34 | rot, trans = self.infos['rot'][item], self.infos['trans'][item] # (3, 3), (3, 1) 35 | overlap = self.infos['overlap'][item] # float 36 | 37 | src_points = torch.load(os.path.join(self.root, src_path)) # npy, (n, 3) 38 | tgt_points = torch.load(os.path.join(self.root, tgt_path)) # npy, (m, 3) 39 | 40 | # for gpu memory 41 | if (src_points.shape[0] > self.max_points): 42 | idx = np.random.permutation(src_points.shape[0])[:self.max_points] 43 | src_points = src_points[idx] 44 | if (tgt_points.shape[0] > self.max_points): 45 | idx = np.random.permutation(tgt_points.shape[0])[:self.max_points] 46 | tgt_points = tgt_points[idx] 47 | 48 | if self.aug: 49 | euler_ab = np.random.rand(3) * 2 * np.pi 50 | rot_ab = Rotation.from_euler('zyx', euler_ab).as_matrix() 51 | if np.random.rand() > 0.5: 52 | src_points = src_points @ rot_ab.T 53 | rot = rot @ rot_ab.T 54 | else: 55 | tgt_points = tgt_points @ rot_ab.T 56 | rot = rot_ab @ rot 57 | trans = rot_ab @ trans 58 | 59 | src_points += (np.random.rand(src_points.shape[0], 3) - 0.5) * self.noise_scale 60 | tgt_points += (np.random.rand(tgt_points.shape[0], 3) - 0.5) * self.noise_scale 61 | 62 | T = np.eye(4).astype(np.float32) 63 | T[:3, :3] = rot 64 | T[:3, 3:] = trans 65 | 66 | coors = get_correspondences(npy2pcd(src_points), 67 | npy2pcd(tgt_points), 68 | T, 69 | self.overlap_radius) 70 | 71 | src_feats = np.ones_like(src_points[:, :1], dtype=np.float32) 72 | tgt_feats = np.ones_like(tgt_points[:, :1], dtype=np.float32) 73 | 74 | src_pcd, tgt_pcd = normal(npy2pcd(src_points)), normal(npy2pcd(tgt_points)) 75 | src_normals = np.array(src_pcd.normals).astype(np.float32) 76 | tgt_normals = np.array(tgt_pcd.normals).astype(np.float32) 77 | 78 | pair = dict( 79 | src_points=src_points, 80 | tgt_points=tgt_points, 81 | src_feats=src_feats, 82 | tgt_feats=tgt_feats, 83 | src_normals=src_normals, 84 | tgt_normals=tgt_normals, 85 | transf=T, 86 | coors=coors, 87 | src_points_raw=src_points, 88 | tgt_points_raw=tgt_points) 89 | return pair 90 | -------------------------------------------------------------------------------- /data/ThreeDMatch/3DLoMatch.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/data/ThreeDMatch/3DLoMatch.pkl -------------------------------------------------------------------------------- /data/ThreeDMatch/3DMatch.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/data/ThreeDMatch/3DMatch.pkl -------------------------------------------------------------------------------- /data/ThreeDMatch/gt/3DLoMatch/sun3d-hotel_umd-maryland_hotel3/gt_overlap.log: -------------------------------------------------------------------------------- 1 | 0,1,0.5325 2 | 0,2,0.0683 3 | 0,3,0.0000 4 | 0,4,0.0000 5 | 0,5,0.0000 6 | 0,6,0.0000 7 | 0,7,0.0000 8 | 0,8,0.0000 9 | 0,9,0.0000 10 | 0,10,0.0269 11 | 0,11,0.1804 12 | 0,12,0.3909 13 | 0,13,0.3089 14 | 0,14,0.0004 15 | 0,15,0.0000 16 | 0,16,0.0000 17 | 0,17,0.0000 18 | 0,18,0.0000 19 | 0,19,0.0000 20 | 0,20,0.0000 21 | 0,21,0.0000 22 | 0,22,0.0263 23 | 0,23,0.0105 24 | 0,24,0.0098 25 | 0,25,0.0002 26 | 0,26,0.0000 27 | 0,27,0.1104 28 | 0,28,0.0002 29 | 0,29,0.0000 30 | 0,30,0.0000 31 | 0,31,0.0000 32 | 0,32,0.0000 33 | 0,33,0.0000 34 | 0,34,0.0000 35 | 0,35,0.0000 36 | 0,36,0.0000 37 | 1,2,0.3132 38 | 1,3,0.0537 39 | 1,4,0.0000 40 | 1,5,0.0000 41 | 1,6,0.0000 42 | 1,7,0.0000 43 | 1,8,0.0000 44 | 1,9,0.0000 45 | 1,10,0.0000 46 | 1,11,0.1110 47 | 1,12,0.2444 48 | 1,13,0.1772 49 | 1,14,0.0000 50 | 1,15,0.0000 51 | 1,16,0.0000 52 | 1,17,0.0000 53 | 1,18,0.0000 54 | 1,19,0.0000 55 | 1,20,0.0000 56 | 1,21,0.0000 57 | 1,22,0.0000 58 | 1,23,0.0000 59 | 1,24,0.0000 60 | 1,25,0.0000 61 | 1,26,0.0000 62 | 1,27,0.0436 63 | 1,28,0.0000 64 | 1,29,0.0000 65 | 1,30,0.0000 66 | 1,31,0.0000 67 | 1,32,0.0000 68 | 1,33,0.0000 69 | 1,34,0.0000 70 | 1,35,0.0000 71 | 1,36,0.0000 72 | 2,3,0.4890 73 | 2,4,0.0686 74 | 2,5,0.0600 75 | 2,6,0.0000 76 | 2,7,0.0000 77 | 2,8,0.0000 78 | 2,9,0.0000 79 | 2,10,0.0000 80 | 2,11,0.0000 81 | 2,12,0.0000 82 | 2,13,0.0000 83 | 2,14,0.0000 84 | 2,15,0.0000 85 | 2,16,0.0000 86 | 2,17,0.0000 87 | 2,18,0.0000 88 | 2,19,0.0000 89 | 2,20,0.0000 90 | 2,21,0.0000 91 | 2,22,0.0000 92 | 2,23,0.0000 93 | 2,24,0.0000 94 | 2,25,0.0000 95 | 2,26,0.0000 96 | 2,27,0.0000 97 | 2,28,0.0000 98 | 2,29,0.0000 99 | 2,30,0.0000 100 | 2,31,0.0000 101 | 2,32,0.0000 102 | 2,33,0.0000 103 | 2,34,0.0000 104 | 2,35,0.0000 105 | 2,36,0.0000 106 | 3,4,0.5921 107 | 3,5,0.2586 108 | 3,6,0.0192 109 | 3,7,0.0000 110 | 3,8,0.0000 111 | 3,9,0.0000 112 | 3,10,0.0000 113 | 3,11,0.0000 114 | 3,12,0.0000 115 | 3,13,0.0000 116 | 3,14,0.0000 117 | 3,15,0.0000 118 | 3,16,0.0000 119 | 3,17,0.0000 120 | 3,18,0.0000 121 | 3,19,0.0000 122 | 3,20,0.0000 123 | 3,21,0.0000 124 | 3,22,0.0000 125 | 3,23,0.0000 126 | 3,24,0.0000 127 | 3,25,0.0000 128 | 3,26,0.0000 129 | 3,27,0.0000 130 | 3,28,0.0000 131 | 3,29,0.0000 132 | 3,30,0.0000 133 | 3,31,0.0000 134 | 3,32,0.0000 135 | 3,33,0.0000 136 | 3,34,0.0000 137 | 3,35,0.0000 138 | 3,36,0.0000 139 | 4,5,0.3325 140 | 4,6,0.0784 141 | 4,7,0.0000 142 | 4,8,0.0000 143 | 4,9,0.0000 144 | 4,10,0.0000 145 | 4,11,0.0000 146 | 4,12,0.0000 147 | 4,13,0.0000 148 | 4,14,0.0000 149 | 4,15,0.0000 150 | 4,16,0.0000 151 | 4,17,0.0000 152 | 4,18,0.0000 153 | 4,19,0.0000 154 | 4,20,0.0000 155 | 4,21,0.0000 156 | 4,22,0.0000 157 | 4,23,0.0000 158 | 4,24,0.0000 159 | 4,25,0.0000 160 | 4,26,0.0000 161 | 4,27,0.0000 162 | 4,28,0.0000 163 | 4,29,0.0000 164 | 4,30,0.0000 165 | 4,31,0.0000 166 | 4,32,0.0000 167 | 4,33,0.0000 168 | 4,34,0.0000 169 | 4,35,0.0000 170 | 4,36,0.0000 171 | 5,6,0.4993 172 | 5,7,0.1245 173 | 5,8,0.0000 174 | 5,9,0.0000 175 | 5,10,0.0000 176 | 5,11,0.0000 177 | 5,12,0.0000 178 | 5,13,0.0000 179 | 5,14,0.0000 180 | 5,15,0.0000 181 | 5,16,0.0000 182 | 5,17,0.0000 183 | 5,18,0.0000 184 | 5,19,0.0000 185 | 5,20,0.0000 186 | 5,21,0.0000 187 | 5,22,0.0000 188 | 5,23,0.0000 189 | 5,24,0.0000 190 | 5,25,0.0000 191 | 5,26,0.0000 192 | 5,27,0.0000 193 | 5,28,0.0000 194 | 5,29,0.0000 195 | 5,30,0.0000 196 | 5,31,0.0000 197 | 5,32,0.0000 198 | 5,33,0.0000 199 | 5,34,0.0000 200 | 5,35,0.0000 201 | 5,36,0.0000 202 | 6,7,0.4663 203 | 6,8,0.0432 204 | 6,9,0.0000 205 | 6,10,0.0000 206 | 6,11,0.0000 207 | 6,12,0.0000 208 | 6,13,0.0000 209 | 6,14,0.0000 210 | 6,15,0.0000 211 | 6,16,0.0000 212 | 6,17,0.0000 213 | 6,18,0.0000 214 | 6,19,0.0000 215 | 6,20,0.0000 216 | 6,21,0.0000 217 | 6,22,0.0000 218 | 6,23,0.0000 219 | 6,24,0.0000 220 | 6,25,0.0000 221 | 6,26,0.0000 222 | 6,27,0.0000 223 | 6,28,0.0000 224 | 6,29,0.0000 225 | 6,30,0.0000 226 | 6,31,0.0000 227 | 6,32,0.0000 228 | 6,33,0.0000 229 | 6,34,0.0000 230 | 6,35,0.0000 231 | 6,36,0.0000 232 | 7,8,0.3918 233 | 7,9,0.0272 234 | 7,10,0.0796 235 | 7,11,0.0000 236 | 7,12,0.0000 237 | 7,13,0.0000 238 | 7,14,0.0605 239 | 7,15,0.0651 240 | 7,16,0.0000 241 | 7,17,0.0000 242 | 7,18,0.0000 243 | 7,19,0.0000 244 | 7,20,0.0000 245 | 7,21,0.0000 246 | 7,22,0.0000 247 | 7,23,0.0000 248 | 7,24,0.0000 249 | 7,25,0.0000 250 | 7,26,0.0000 251 | 7,27,0.0000 252 | 7,28,0.0000 253 | 7,29,0.0000 254 | 7,30,0.0000 255 | 7,31,0.0000 256 | 7,32,0.0000 257 | 7,33,0.0000 258 | 7,34,0.0000 259 | 7,35,0.0000 260 | 7,36,0.0000 261 | 8,9,0.4565 262 | 8,10,0.3677 263 | 8,11,0.0000 264 | 8,12,0.0000 265 | 8,13,0.0000 266 | 8,14,0.1760 267 | 8,15,0.4993 268 | 8,16,0.2940 269 | 8,17,0.0577 270 | 8,18,0.0006 271 | 8,19,0.0000 272 | 8,20,0.0000 273 | 8,21,0.0000 274 | 8,22,0.0000 275 | 8,23,0.0000 276 | 8,24,0.0000 277 | 8,25,0.0000 278 | 8,26,0.0000 279 | 8,27,0.0000 280 | 8,28,0.0000 281 | 8,29,0.0000 282 | 8,30,0.0000 283 | 8,31,0.0000 284 | 8,32,0.0000 285 | 8,33,0.0000 286 | 8,34,0.0000 287 | 8,35,0.0000 288 | 8,36,0.0000 289 | 9,10,0.2301 290 | 9,11,0.0546 291 | 9,12,0.0000 292 | 9,13,0.0274 293 | 9,14,0.0741 294 | 9,15,0.2451 295 | 9,16,0.1903 296 | 9,17,0.0907 297 | 9,18,0.0404 298 | 9,19,0.0087 299 | 9,20,0.0000 300 | 9,21,0.0000 301 | 9,22,0.0000 302 | 9,23,0.0000 303 | 9,24,0.0000 304 | 9,25,0.0000 305 | 9,26,0.0000 306 | 9,27,0.0000 307 | 9,28,0.0000 308 | 9,29,0.0000 309 | 9,30,0.0000 310 | 9,31,0.0000 311 | 9,32,0.0000 312 | 9,33,0.0000 313 | 9,34,0.0000 314 | 9,35,0.0000 315 | 9,36,0.0000 316 | 10,11,0.2857 317 | 10,12,0.0900 318 | 10,13,0.1489 319 | 10,14,0.5227 320 | 10,15,0.5015 321 | 10,16,0.4256 322 | 10,17,0.2176 323 | 10,18,0.0341 324 | 10,19,0.0106 325 | 10,20,0.0000 326 | 10,21,0.0000 327 | 10,22,0.0000 328 | 10,23,0.0005 329 | 10,24,0.0018 330 | 10,25,0.0010 331 | 10,26,0.0000 332 | 10,27,0.0011 333 | 10,28,0.0007 334 | 10,29,0.0000 335 | 10,30,0.0000 336 | 10,31,0.0000 337 | 10,32,0.0000 338 | 10,33,0.0000 339 | 10,34,0.0000 340 | 10,35,0.0000 341 | 10,36,0.0000 342 | 11,12,0.6637 343 | 11,13,0.7383 344 | 11,14,0.2992 345 | 11,15,0.0000 346 | 11,16,0.0219 347 | 11,17,0.0121 348 | 11,18,0.0000 349 | 11,19,0.0000 350 | 11,20,0.0000 351 | 11,21,0.0006 352 | 11,22,0.0436 353 | 11,23,0.0238 354 | 11,24,0.0194 355 | 11,25,0.0047 356 | 11,26,0.0028 357 | 11,27,0.1267 358 | 11,28,0.0028 359 | 11,29,0.0000 360 | 11,30,0.0000 361 | 11,31,0.0000 362 | 11,32,0.0000 363 | 11,33,0.0000 364 | 11,34,0.0000 365 | 11,35,0.0000 366 | 11,36,0.0000 367 | 12,13,0.5171 368 | 12,14,0.0847 369 | 12,15,0.0000 370 | 12,16,0.0000 371 | 12,17,0.0000 372 | 12,18,0.0000 373 | 12,19,0.0000 374 | 12,20,0.0000 375 | 12,21,0.0008 376 | 12,22,0.0525 377 | 12,23,0.0422 378 | 12,24,0.0159 379 | 12,25,0.0030 380 | 12,26,0.0073 381 | 12,27,0.1169 382 | 12,28,0.0028 383 | 12,29,0.0093 384 | 12,30,0.0000 385 | 12,31,0.0000 386 | 12,32,0.0000 387 | 12,33,0.0000 388 | 12,34,0.0000 389 | 12,35,0.0000 390 | 12,36,0.0000 391 | 13,14,0.1723 392 | 13,15,0.0003 393 | 13,16,0.0000 394 | 13,17,0.0000 395 | 13,18,0.0000 396 | 13,19,0.0000 397 | 13,20,0.0000 398 | 13,21,0.0017 399 | 13,22,0.0478 400 | 13,23,0.0371 401 | 13,24,0.0210 402 | 13,25,0.0049 403 | 13,26,0.0000 404 | 13,27,0.1237 405 | 13,28,0.0030 406 | 13,29,0.0050 407 | 13,30,0.0000 408 | 13,31,0.0000 409 | 13,32,0.0000 410 | 13,33,0.0000 411 | 13,34,0.0000 412 | 13,35,0.0000 413 | 13,36,0.0000 414 | 14,15,0.3409 415 | 14,16,0.2692 416 | 14,17,0.2338 417 | 14,18,0.0061 418 | 14,19,0.0075 419 | 14,20,0.0079 420 | 14,21,0.0065 421 | 14,22,0.0011 422 | 14,23,0.0043 423 | 14,24,0.0063 424 | 14,25,0.0023 425 | 14,26,0.0000 426 | 14,27,0.0045 427 | 14,28,0.0027 428 | 14,29,0.0000 429 | 14,30,0.0000 430 | 14,31,0.0000 431 | 14,32,0.0000 432 | 14,33,0.0000 433 | 14,34,0.0000 434 | 14,35,0.0000 435 | 14,36,0.0002 436 | 15,16,0.6412 437 | 15,17,0.3784 438 | 15,18,0.0479 439 | 15,19,0.0572 440 | 15,20,0.0059 441 | 15,21,0.0000 442 | 15,22,0.0000 443 | 15,23,0.0000 444 | 15,24,0.0000 445 | 15,25,0.0000 446 | 15,26,0.0000 447 | 15,27,0.0000 448 | 15,28,0.0000 449 | 15,29,0.0000 450 | 15,30,0.0000 451 | 15,31,0.0000 452 | 15,32,0.0000 453 | 15,33,0.0000 454 | 15,34,0.0000 455 | 15,35,0.0000 456 | 15,36,0.0000 457 | 16,17,0.6605 458 | 16,18,0.1551 459 | 16,19,0.1564 460 | 16,20,0.0070 461 | 16,21,0.0037 462 | 16,22,0.0000 463 | 16,23,0.0000 464 | 16,24,0.0000 465 | 16,25,0.0000 466 | 16,26,0.0000 467 | 16,27,0.0000 468 | 16,28,0.0000 469 | 16,29,0.0000 470 | 16,30,0.0000 471 | 16,31,0.0000 472 | 16,32,0.0000 473 | 16,33,0.0000 474 | 16,34,0.0000 475 | 16,35,0.0003 476 | 16,36,0.0065 477 | 17,18,0.4449 478 | 17,19,0.5912 479 | 17,20,0.3402 480 | 17,21,0.0151 481 | 17,22,0.0000 482 | 17,23,0.0000 483 | 17,24,0.0000 484 | 17,25,0.0000 485 | 17,26,0.0000 486 | 17,27,0.0000 487 | 17,28,0.0000 488 | 17,29,0.0000 489 | 17,30,0.0000 490 | 17,31,0.0000 491 | 17,32,0.0000 492 | 17,33,0.0144 493 | 17,34,0.0610 494 | 17,35,0.0111 495 | 17,36,0.0755 496 | 18,19,0.5922 497 | 18,20,0.2836 498 | 18,21,0.0132 499 | 18,22,0.0000 500 | 18,23,0.0000 501 | 18,24,0.0000 502 | 18,25,0.0000 503 | 18,26,0.0000 504 | 18,27,0.0000 505 | 18,28,0.0000 506 | 18,29,0.0000 507 | 18,30,0.0000 508 | 18,31,0.0000 509 | 18,32,0.0000 510 | 18,33,0.0000 511 | 18,34,0.0055 512 | 18,35,0.0089 513 | 18,36,0.0382 514 | 19,20,0.4955 515 | 19,21,0.0496 516 | 19,22,0.0000 517 | 19,23,0.0000 518 | 19,24,0.0000 519 | 19,25,0.0000 520 | 19,26,0.0000 521 | 19,27,0.0000 522 | 19,28,0.0000 523 | 19,29,0.0000 524 | 19,30,0.0000 525 | 19,31,0.0000 526 | 19,32,0.0000 527 | 19,33,0.0274 528 | 19,34,0.0596 529 | 19,35,0.0140 530 | 19,36,0.0950 531 | 20,21,0.3810 532 | 20,22,0.0238 533 | 20,23,0.0040 534 | 20,24,0.0045 535 | 20,25,0.0010 536 | 20,26,0.0000 537 | 20,27,0.0045 538 | 20,28,0.0062 539 | 20,29,0.0000 540 | 20,30,0.0053 541 | 20,31,0.0576 542 | 20,32,0.0000 543 | 20,33,0.1121 544 | 20,34,0.1527 545 | 20,35,0.0143 546 | 20,36,0.1041 547 | 21,22,0.5275 548 | 21,23,0.3011 549 | 21,24,0.1952 550 | 21,25,0.0320 551 | 21,26,0.0016 552 | 21,27,0.0899 553 | 21,28,0.3441 554 | 21,29,0.2048 555 | 21,30,0.2257 556 | 21,31,0.2364 557 | 21,32,0.0316 558 | 21,33,0.0744 559 | 21,34,0.0670 560 | 21,35,0.0050 561 | 21,36,0.0172 562 | 22,23,0.7330 563 | 22,24,0.2303 564 | 22,25,0.0266 565 | 22,26,0.0267 566 | 22,27,0.1673 567 | 22,28,0.3191 568 | 22,29,0.4485 569 | 22,30,0.2956 570 | 22,31,0.1013 571 | 22,32,0.0161 572 | 22,33,0.0044 573 | 22,34,0.0085 574 | 22,35,0.0046 575 | 22,36,0.0075 576 | 23,24,0.2493 577 | 23,25,0.0259 578 | 23,26,0.0426 579 | 23,27,0.2199 580 | 23,28,0.3316 581 | 23,29,0.5661 582 | 23,30,0.3621 583 | 23,31,0.0670 584 | 23,32,0.0031 585 | 23,33,0.0000 586 | 23,34,0.0000 587 | 23,35,0.0000 588 | 23,36,0.0000 589 | 24,25,0.2683 590 | 24,26,0.0284 591 | 24,27,0.4720 592 | 24,28,0.6725 593 | 24,29,0.3175 594 | 24,30,0.1250 595 | 24,31,0.0131 596 | 24,32,0.0000 597 | 24,33,0.0000 598 | 24,34,0.0000 599 | 24,35,0.0000 600 | 24,36,0.0000 601 | 25,26,0.2499 602 | 25,27,0.4611 603 | 25,28,0.2101 604 | 25,29,0.0000 605 | 25,30,0.0000 606 | 25,31,0.0000 607 | 25,32,0.0000 608 | 25,33,0.0000 609 | 25,34,0.0000 610 | 25,35,0.0000 611 | 25,36,0.0000 612 | 26,27,0.7114 613 | 26,28,0.0033 614 | 26,29,0.0000 615 | 26,30,0.0000 616 | 26,31,0.0000 617 | 26,32,0.0000 618 | 26,33,0.0000 619 | 26,34,0.0000 620 | 26,35,0.0000 621 | 26,36,0.0000 622 | 27,28,0.1769 623 | 27,29,0.0335 624 | 27,30,0.0000 625 | 27,31,0.0000 626 | 27,32,0.0000 627 | 27,33,0.0000 628 | 27,34,0.0000 629 | 27,35,0.0000 630 | 27,36,0.0000 631 | 28,29,0.5445 632 | 28,30,0.3390 633 | 28,31,0.1712 634 | 28,32,0.0000 635 | 28,33,0.0000 636 | 28,34,0.0000 637 | 28,35,0.0000 638 | 28,36,0.0000 639 | 29,30,0.6915 640 | 29,31,0.1352 641 | 29,32,0.0507 642 | 29,33,0.0016 643 | 29,34,0.0000 644 | 29,35,0.0000 645 | 29,36,0.0000 646 | 30,31,0.4260 647 | 30,32,0.2778 648 | 30,33,0.1563 649 | 30,34,0.0450 650 | 30,35,0.0000 651 | 30,36,0.0080 652 | 31,32,0.4631 653 | 31,33,0.5335 654 | 31,34,0.3379 655 | 31,35,0.1129 656 | 31,36,0.2214 657 | 32,33,0.6403 658 | 32,34,0.3848 659 | 32,35,0.3697 660 | 32,36,0.3271 661 | 33,34,0.7574 662 | 33,35,0.3338 663 | 33,36,0.4225 664 | 34,35,0.4932 665 | 34,36,0.5212 666 | 35,36,0.5585 667 | -------------------------------------------------------------------------------- /data/ThreeDMatch/gt/3DMatch/sun3d-hotel_umd-maryland_hotel3/gt_overlap.log: -------------------------------------------------------------------------------- 1 | 0,1,0.5325 2 | 0,2,0.0683 3 | 0,3,0.0000 4 | 0,4,0.0000 5 | 0,5,0.0000 6 | 0,6,0.0000 7 | 0,7,0.0000 8 | 0,8,0.0000 9 | 0,9,0.0000 10 | 0,10,0.0269 11 | 0,11,0.1804 12 | 0,12,0.3909 13 | 0,13,0.3089 14 | 0,14,0.0004 15 | 0,15,0.0000 16 | 0,16,0.0000 17 | 0,17,0.0000 18 | 0,18,0.0000 19 | 0,19,0.0000 20 | 0,20,0.0000 21 | 0,21,0.0000 22 | 0,22,0.0263 23 | 0,23,0.0105 24 | 0,24,0.0098 25 | 0,25,0.0002 26 | 0,26,0.0000 27 | 0,27,0.1104 28 | 0,28,0.0002 29 | 0,29,0.0000 30 | 0,30,0.0000 31 | 0,31,0.0000 32 | 0,32,0.0000 33 | 0,33,0.0000 34 | 0,34,0.0000 35 | 0,35,0.0000 36 | 0,36,0.0000 37 | 1,2,0.3132 38 | 1,3,0.0537 39 | 1,4,0.0000 40 | 1,5,0.0000 41 | 1,6,0.0000 42 | 1,7,0.0000 43 | 1,8,0.0000 44 | 1,9,0.0000 45 | 1,10,0.0000 46 | 1,11,0.1110 47 | 1,12,0.2444 48 | 1,13,0.1772 49 | 1,14,0.0000 50 | 1,15,0.0000 51 | 1,16,0.0000 52 | 1,17,0.0000 53 | 1,18,0.0000 54 | 1,19,0.0000 55 | 1,20,0.0000 56 | 1,21,0.0000 57 | 1,22,0.0000 58 | 1,23,0.0000 59 | 1,24,0.0000 60 | 1,25,0.0000 61 | 1,26,0.0000 62 | 1,27,0.0436 63 | 1,28,0.0000 64 | 1,29,0.0000 65 | 1,30,0.0000 66 | 1,31,0.0000 67 | 1,32,0.0000 68 | 1,33,0.0000 69 | 1,34,0.0000 70 | 1,35,0.0000 71 | 1,36,0.0000 72 | 2,3,0.4890 73 | 2,4,0.0686 74 | 2,5,0.0600 75 | 2,6,0.0000 76 | 2,7,0.0000 77 | 2,8,0.0000 78 | 2,9,0.0000 79 | 2,10,0.0000 80 | 2,11,0.0000 81 | 2,12,0.0000 82 | 2,13,0.0000 83 | 2,14,0.0000 84 | 2,15,0.0000 85 | 2,16,0.0000 86 | 2,17,0.0000 87 | 2,18,0.0000 88 | 2,19,0.0000 89 | 2,20,0.0000 90 | 2,21,0.0000 91 | 2,22,0.0000 92 | 2,23,0.0000 93 | 2,24,0.0000 94 | 2,25,0.0000 95 | 2,26,0.0000 96 | 2,27,0.0000 97 | 2,28,0.0000 98 | 2,29,0.0000 99 | 2,30,0.0000 100 | 2,31,0.0000 101 | 2,32,0.0000 102 | 2,33,0.0000 103 | 2,34,0.0000 104 | 2,35,0.0000 105 | 2,36,0.0000 106 | 3,4,0.5921 107 | 3,5,0.2586 108 | 3,6,0.0192 109 | 3,7,0.0000 110 | 3,8,0.0000 111 | 3,9,0.0000 112 | 3,10,0.0000 113 | 3,11,0.0000 114 | 3,12,0.0000 115 | 3,13,0.0000 116 | 3,14,0.0000 117 | 3,15,0.0000 118 | 3,16,0.0000 119 | 3,17,0.0000 120 | 3,18,0.0000 121 | 3,19,0.0000 122 | 3,20,0.0000 123 | 3,21,0.0000 124 | 3,22,0.0000 125 | 3,23,0.0000 126 | 3,24,0.0000 127 | 3,25,0.0000 128 | 3,26,0.0000 129 | 3,27,0.0000 130 | 3,28,0.0000 131 | 3,29,0.0000 132 | 3,30,0.0000 133 | 3,31,0.0000 134 | 3,32,0.0000 135 | 3,33,0.0000 136 | 3,34,0.0000 137 | 3,35,0.0000 138 | 3,36,0.0000 139 | 4,5,0.3325 140 | 4,6,0.0784 141 | 4,7,0.0000 142 | 4,8,0.0000 143 | 4,9,0.0000 144 | 4,10,0.0000 145 | 4,11,0.0000 146 | 4,12,0.0000 147 | 4,13,0.0000 148 | 4,14,0.0000 149 | 4,15,0.0000 150 | 4,16,0.0000 151 | 4,17,0.0000 152 | 4,18,0.0000 153 | 4,19,0.0000 154 | 4,20,0.0000 155 | 4,21,0.0000 156 | 4,22,0.0000 157 | 4,23,0.0000 158 | 4,24,0.0000 159 | 4,25,0.0000 160 | 4,26,0.0000 161 | 4,27,0.0000 162 | 4,28,0.0000 163 | 4,29,0.0000 164 | 4,30,0.0000 165 | 4,31,0.0000 166 | 4,32,0.0000 167 | 4,33,0.0000 168 | 4,34,0.0000 169 | 4,35,0.0000 170 | 4,36,0.0000 171 | 5,6,0.4993 172 | 5,7,0.1245 173 | 5,8,0.0000 174 | 5,9,0.0000 175 | 5,10,0.0000 176 | 5,11,0.0000 177 | 5,12,0.0000 178 | 5,13,0.0000 179 | 5,14,0.0000 180 | 5,15,0.0000 181 | 5,16,0.0000 182 | 5,17,0.0000 183 | 5,18,0.0000 184 | 5,19,0.0000 185 | 5,20,0.0000 186 | 5,21,0.0000 187 | 5,22,0.0000 188 | 5,23,0.0000 189 | 5,24,0.0000 190 | 5,25,0.0000 191 | 5,26,0.0000 192 | 5,27,0.0000 193 | 5,28,0.0000 194 | 5,29,0.0000 195 | 5,30,0.0000 196 | 5,31,0.0000 197 | 5,32,0.0000 198 | 5,33,0.0000 199 | 5,34,0.0000 200 | 5,35,0.0000 201 | 5,36,0.0000 202 | 6,7,0.4663 203 | 6,8,0.0432 204 | 6,9,0.0000 205 | 6,10,0.0000 206 | 6,11,0.0000 207 | 6,12,0.0000 208 | 6,13,0.0000 209 | 6,14,0.0000 210 | 6,15,0.0000 211 | 6,16,0.0000 212 | 6,17,0.0000 213 | 6,18,0.0000 214 | 6,19,0.0000 215 | 6,20,0.0000 216 | 6,21,0.0000 217 | 6,22,0.0000 218 | 6,23,0.0000 219 | 6,24,0.0000 220 | 6,25,0.0000 221 | 6,26,0.0000 222 | 6,27,0.0000 223 | 6,28,0.0000 224 | 6,29,0.0000 225 | 6,30,0.0000 226 | 6,31,0.0000 227 | 6,32,0.0000 228 | 6,33,0.0000 229 | 6,34,0.0000 230 | 6,35,0.0000 231 | 6,36,0.0000 232 | 7,8,0.3918 233 | 7,9,0.0272 234 | 7,10,0.0796 235 | 7,11,0.0000 236 | 7,12,0.0000 237 | 7,13,0.0000 238 | 7,14,0.0605 239 | 7,15,0.0651 240 | 7,16,0.0000 241 | 7,17,0.0000 242 | 7,18,0.0000 243 | 7,19,0.0000 244 | 7,20,0.0000 245 | 7,21,0.0000 246 | 7,22,0.0000 247 | 7,23,0.0000 248 | 7,24,0.0000 249 | 7,25,0.0000 250 | 7,26,0.0000 251 | 7,27,0.0000 252 | 7,28,0.0000 253 | 7,29,0.0000 254 | 7,30,0.0000 255 | 7,31,0.0000 256 | 7,32,0.0000 257 | 7,33,0.0000 258 | 7,34,0.0000 259 | 7,35,0.0000 260 | 7,36,0.0000 261 | 8,9,0.4565 262 | 8,10,0.3677 263 | 8,11,0.0000 264 | 8,12,0.0000 265 | 8,13,0.0000 266 | 8,14,0.1760 267 | 8,15,0.4993 268 | 8,16,0.2940 269 | 8,17,0.0577 270 | 8,18,0.0006 271 | 8,19,0.0000 272 | 8,20,0.0000 273 | 8,21,0.0000 274 | 8,22,0.0000 275 | 8,23,0.0000 276 | 8,24,0.0000 277 | 8,25,0.0000 278 | 8,26,0.0000 279 | 8,27,0.0000 280 | 8,28,0.0000 281 | 8,29,0.0000 282 | 8,30,0.0000 283 | 8,31,0.0000 284 | 8,32,0.0000 285 | 8,33,0.0000 286 | 8,34,0.0000 287 | 8,35,0.0000 288 | 8,36,0.0000 289 | 9,10,0.2301 290 | 9,11,0.0546 291 | 9,12,0.0000 292 | 9,13,0.0274 293 | 9,14,0.0741 294 | 9,15,0.2451 295 | 9,16,0.1903 296 | 9,17,0.0907 297 | 9,18,0.0404 298 | 9,19,0.0087 299 | 9,20,0.0000 300 | 9,21,0.0000 301 | 9,22,0.0000 302 | 9,23,0.0000 303 | 9,24,0.0000 304 | 9,25,0.0000 305 | 9,26,0.0000 306 | 9,27,0.0000 307 | 9,28,0.0000 308 | 9,29,0.0000 309 | 9,30,0.0000 310 | 9,31,0.0000 311 | 9,32,0.0000 312 | 9,33,0.0000 313 | 9,34,0.0000 314 | 9,35,0.0000 315 | 9,36,0.0000 316 | 10,11,0.2857 317 | 10,12,0.0900 318 | 10,13,0.1489 319 | 10,14,0.5227 320 | 10,15,0.5015 321 | 10,16,0.4256 322 | 10,17,0.2176 323 | 10,18,0.0341 324 | 10,19,0.0106 325 | 10,20,0.0000 326 | 10,21,0.0000 327 | 10,22,0.0000 328 | 10,23,0.0005 329 | 10,24,0.0018 330 | 10,25,0.0010 331 | 10,26,0.0000 332 | 10,27,0.0011 333 | 10,28,0.0007 334 | 10,29,0.0000 335 | 10,30,0.0000 336 | 10,31,0.0000 337 | 10,32,0.0000 338 | 10,33,0.0000 339 | 10,34,0.0000 340 | 10,35,0.0000 341 | 10,36,0.0000 342 | 11,12,0.6637 343 | 11,13,0.7383 344 | 11,14,0.2992 345 | 11,15,0.0000 346 | 11,16,0.0219 347 | 11,17,0.0121 348 | 11,18,0.0000 349 | 11,19,0.0000 350 | 11,20,0.0000 351 | 11,21,0.0006 352 | 11,22,0.0436 353 | 11,23,0.0238 354 | 11,24,0.0194 355 | 11,25,0.0047 356 | 11,26,0.0028 357 | 11,27,0.1267 358 | 11,28,0.0028 359 | 11,29,0.0000 360 | 11,30,0.0000 361 | 11,31,0.0000 362 | 11,32,0.0000 363 | 11,33,0.0000 364 | 11,34,0.0000 365 | 11,35,0.0000 366 | 11,36,0.0000 367 | 12,13,0.5171 368 | 12,14,0.0847 369 | 12,15,0.0000 370 | 12,16,0.0000 371 | 12,17,0.0000 372 | 12,18,0.0000 373 | 12,19,0.0000 374 | 12,20,0.0000 375 | 12,21,0.0008 376 | 12,22,0.0525 377 | 12,23,0.0422 378 | 12,24,0.0159 379 | 12,25,0.0030 380 | 12,26,0.0073 381 | 12,27,0.1169 382 | 12,28,0.0028 383 | 12,29,0.0093 384 | 12,30,0.0000 385 | 12,31,0.0000 386 | 12,32,0.0000 387 | 12,33,0.0000 388 | 12,34,0.0000 389 | 12,35,0.0000 390 | 12,36,0.0000 391 | 13,14,0.1723 392 | 13,15,0.0003 393 | 13,16,0.0000 394 | 13,17,0.0000 395 | 13,18,0.0000 396 | 13,19,0.0000 397 | 13,20,0.0000 398 | 13,21,0.0017 399 | 13,22,0.0478 400 | 13,23,0.0371 401 | 13,24,0.0210 402 | 13,25,0.0049 403 | 13,26,0.0000 404 | 13,27,0.1237 405 | 13,28,0.0030 406 | 13,29,0.0050 407 | 13,30,0.0000 408 | 13,31,0.0000 409 | 13,32,0.0000 410 | 13,33,0.0000 411 | 13,34,0.0000 412 | 13,35,0.0000 413 | 13,36,0.0000 414 | 14,15,0.3409 415 | 14,16,0.2692 416 | 14,17,0.2338 417 | 14,18,0.0061 418 | 14,19,0.0075 419 | 14,20,0.0079 420 | 14,21,0.0065 421 | 14,22,0.0011 422 | 14,23,0.0043 423 | 14,24,0.0063 424 | 14,25,0.0023 425 | 14,26,0.0000 426 | 14,27,0.0045 427 | 14,28,0.0027 428 | 14,29,0.0000 429 | 14,30,0.0000 430 | 14,31,0.0000 431 | 14,32,0.0000 432 | 14,33,0.0000 433 | 14,34,0.0000 434 | 14,35,0.0000 435 | 14,36,0.0002 436 | 15,16,0.6412 437 | 15,17,0.3784 438 | 15,18,0.0479 439 | 15,19,0.0572 440 | 15,20,0.0059 441 | 15,21,0.0000 442 | 15,22,0.0000 443 | 15,23,0.0000 444 | 15,24,0.0000 445 | 15,25,0.0000 446 | 15,26,0.0000 447 | 15,27,0.0000 448 | 15,28,0.0000 449 | 15,29,0.0000 450 | 15,30,0.0000 451 | 15,31,0.0000 452 | 15,32,0.0000 453 | 15,33,0.0000 454 | 15,34,0.0000 455 | 15,35,0.0000 456 | 15,36,0.0000 457 | 16,17,0.6605 458 | 16,18,0.1551 459 | 16,19,0.1564 460 | 16,20,0.0070 461 | 16,21,0.0037 462 | 16,22,0.0000 463 | 16,23,0.0000 464 | 16,24,0.0000 465 | 16,25,0.0000 466 | 16,26,0.0000 467 | 16,27,0.0000 468 | 16,28,0.0000 469 | 16,29,0.0000 470 | 16,30,0.0000 471 | 16,31,0.0000 472 | 16,32,0.0000 473 | 16,33,0.0000 474 | 16,34,0.0000 475 | 16,35,0.0003 476 | 16,36,0.0065 477 | 17,18,0.4449 478 | 17,19,0.5912 479 | 17,20,0.3402 480 | 17,21,0.0151 481 | 17,22,0.0000 482 | 17,23,0.0000 483 | 17,24,0.0000 484 | 17,25,0.0000 485 | 17,26,0.0000 486 | 17,27,0.0000 487 | 17,28,0.0000 488 | 17,29,0.0000 489 | 17,30,0.0000 490 | 17,31,0.0000 491 | 17,32,0.0000 492 | 17,33,0.0144 493 | 17,34,0.0610 494 | 17,35,0.0111 495 | 17,36,0.0755 496 | 18,19,0.5922 497 | 18,20,0.2836 498 | 18,21,0.0132 499 | 18,22,0.0000 500 | 18,23,0.0000 501 | 18,24,0.0000 502 | 18,25,0.0000 503 | 18,26,0.0000 504 | 18,27,0.0000 505 | 18,28,0.0000 506 | 18,29,0.0000 507 | 18,30,0.0000 508 | 18,31,0.0000 509 | 18,32,0.0000 510 | 18,33,0.0000 511 | 18,34,0.0055 512 | 18,35,0.0089 513 | 18,36,0.0382 514 | 19,20,0.4955 515 | 19,21,0.0496 516 | 19,22,0.0000 517 | 19,23,0.0000 518 | 19,24,0.0000 519 | 19,25,0.0000 520 | 19,26,0.0000 521 | 19,27,0.0000 522 | 19,28,0.0000 523 | 19,29,0.0000 524 | 19,30,0.0000 525 | 19,31,0.0000 526 | 19,32,0.0000 527 | 19,33,0.0274 528 | 19,34,0.0596 529 | 19,35,0.0140 530 | 19,36,0.0950 531 | 20,21,0.3810 532 | 20,22,0.0238 533 | 20,23,0.0040 534 | 20,24,0.0045 535 | 20,25,0.0010 536 | 20,26,0.0000 537 | 20,27,0.0045 538 | 20,28,0.0062 539 | 20,29,0.0000 540 | 20,30,0.0053 541 | 20,31,0.0576 542 | 20,32,0.0000 543 | 20,33,0.1121 544 | 20,34,0.1527 545 | 20,35,0.0143 546 | 20,36,0.1041 547 | 21,22,0.5275 548 | 21,23,0.3011 549 | 21,24,0.1952 550 | 21,25,0.0320 551 | 21,26,0.0016 552 | 21,27,0.0899 553 | 21,28,0.3441 554 | 21,29,0.2048 555 | 21,30,0.2257 556 | 21,31,0.2364 557 | 21,32,0.0316 558 | 21,33,0.0744 559 | 21,34,0.0670 560 | 21,35,0.0050 561 | 21,36,0.0172 562 | 22,23,0.7330 563 | 22,24,0.2303 564 | 22,25,0.0266 565 | 22,26,0.0267 566 | 22,27,0.1673 567 | 22,28,0.3191 568 | 22,29,0.4485 569 | 22,30,0.2956 570 | 22,31,0.1013 571 | 22,32,0.0161 572 | 22,33,0.0044 573 | 22,34,0.0085 574 | 22,35,0.0046 575 | 22,36,0.0075 576 | 23,24,0.2493 577 | 23,25,0.0259 578 | 23,26,0.0426 579 | 23,27,0.2199 580 | 23,28,0.3316 581 | 23,29,0.5661 582 | 23,30,0.3621 583 | 23,31,0.0670 584 | 23,32,0.0031 585 | 23,33,0.0000 586 | 23,34,0.0000 587 | 23,35,0.0000 588 | 23,36,0.0000 589 | 24,25,0.2683 590 | 24,26,0.0284 591 | 24,27,0.4720 592 | 24,28,0.6725 593 | 24,29,0.3175 594 | 24,30,0.1250 595 | 24,31,0.0131 596 | 24,32,0.0000 597 | 24,33,0.0000 598 | 24,34,0.0000 599 | 24,35,0.0000 600 | 24,36,0.0000 601 | 25,26,0.2499 602 | 25,27,0.4611 603 | 25,28,0.2101 604 | 25,29,0.0000 605 | 25,30,0.0000 606 | 25,31,0.0000 607 | 25,32,0.0000 608 | 25,33,0.0000 609 | 25,34,0.0000 610 | 25,35,0.0000 611 | 25,36,0.0000 612 | 26,27,0.7114 613 | 26,28,0.0033 614 | 26,29,0.0000 615 | 26,30,0.0000 616 | 26,31,0.0000 617 | 26,32,0.0000 618 | 26,33,0.0000 619 | 26,34,0.0000 620 | 26,35,0.0000 621 | 26,36,0.0000 622 | 27,28,0.1769 623 | 27,29,0.0335 624 | 27,30,0.0000 625 | 27,31,0.0000 626 | 27,32,0.0000 627 | 27,33,0.0000 628 | 27,34,0.0000 629 | 27,35,0.0000 630 | 27,36,0.0000 631 | 28,29,0.5445 632 | 28,30,0.3390 633 | 28,31,0.1712 634 | 28,32,0.0000 635 | 28,33,0.0000 636 | 28,34,0.0000 637 | 28,35,0.0000 638 | 28,36,0.0000 639 | 29,30,0.6915 640 | 29,31,0.1352 641 | 29,32,0.0507 642 | 29,33,0.0016 643 | 29,34,0.0000 644 | 29,35,0.0000 645 | 29,36,0.0000 646 | 30,31,0.4260 647 | 30,32,0.2778 648 | 30,33,0.1563 649 | 30,34,0.0450 650 | 30,35,0.0000 651 | 30,36,0.0080 652 | 31,32,0.4631 653 | 31,33,0.5335 654 | 31,34,0.3379 655 | 31,35,0.1129 656 | 31,36,0.2214 657 | 32,33,0.6403 658 | 32,34,0.3848 659 | 32,35,0.3697 660 | 32,36,0.3271 661 | 33,34,0.7574 662 | 33,35,0.3338 663 | 33,36,0.4225 664 | 34,35,0.4932 665 | 34,36,0.5212 666 | 35,36,0.5585 667 | -------------------------------------------------------------------------------- /data/ThreeDMatch/gt/3DMatch/sun3d-mit_lab_hj-lab_hj_tea_nov_2_2012_scan1_erika/gt_overlap.log: -------------------------------------------------------------------------------- 1 | 0,1,0.6983 2 | 0,2,0.1538 3 | 0,3,0.0141 4 | 0,4,0.0006 5 | 0,5,0.0877 6 | 0,6,0.2780 7 | 0,7,0.2465 8 | 0,8,0.3627 9 | 0,9,0.4328 10 | 0,10,0.4738 11 | 0,11,0.1940 12 | 0,12,0.0399 13 | 0,13,0.0220 14 | 0,14,0.0052 15 | 0,15,0.0048 16 | 0,16,0.0051 17 | 0,17,0.0000 18 | 0,18,0.0000 19 | 0,19,0.0000 20 | 0,20,0.0000 21 | 0,21,0.0310 22 | 0,22,0.0380 23 | 0,23,0.0526 24 | 0,24,0.1042 25 | 0,25,0.0453 26 | 0,26,0.1419 27 | 0,27,0.3931 28 | 0,28,0.3217 29 | 0,29,0.0000 30 | 0,30,0.0000 31 | 0,31,0.0000 32 | 0,32,0.0000 33 | 0,33,0.0000 34 | 0,34,0.0000 35 | 0,35,0.0000 36 | 0,36,0.0000 37 | 0,37,0.0000 38 | 1,2,0.4768 39 | 1,3,0.0820 40 | 1,4,0.0484 41 | 1,5,0.2321 42 | 1,6,0.4124 43 | 1,7,0.2067 44 | 1,8,0.3769 45 | 1,9,0.4220 46 | 1,10,0.3575 47 | 1,11,0.0899 48 | 1,12,0.0046 49 | 1,13,0.0031 50 | 1,14,0.0000 51 | 1,15,0.0000 52 | 1,16,0.0000 53 | 1,17,0.0000 54 | 1,18,0.0000 55 | 1,19,0.0000 56 | 1,20,0.0000 57 | 1,21,0.0024 58 | 1,22,0.0040 59 | 1,23,0.0078 60 | 1,24,0.0374 61 | 1,25,0.0125 62 | 1,26,0.1152 63 | 1,27,0.3743 64 | 1,28,0.3775 65 | 1,29,0.0282 66 | 1,30,0.0001 67 | 1,31,0.0000 68 | 1,32,0.0000 69 | 1,33,0.0000 70 | 1,34,0.0000 71 | 1,35,0.0000 72 | 1,36,0.0000 73 | 1,37,0.0000 74 | 2,3,0.5100 75 | 2,4,0.3651 76 | 2,5,0.3231 77 | 2,6,0.2628 78 | 2,7,0.2614 79 | 2,8,0.3450 80 | 2,9,0.1202 81 | 2,10,0.0596 82 | 2,11,0.0054 83 | 2,12,0.0000 84 | 2,13,0.0000 85 | 2,14,0.0000 86 | 2,15,0.0000 87 | 2,16,0.0000 88 | 2,17,0.0000 89 | 2,18,0.0000 90 | 2,19,0.0000 91 | 2,20,0.0000 92 | 2,21,0.0000 93 | 2,22,0.0000 94 | 2,23,0.0000 95 | 2,24,0.0004 96 | 2,25,0.0000 97 | 2,26,0.0177 98 | 2,27,0.0735 99 | 2,28,0.1629 100 | 2,29,0.0674 101 | 2,30,0.1724 102 | 2,31,0.0000 103 | 2,32,0.0000 104 | 2,33,0.0000 105 | 2,34,0.0000 106 | 2,35,0.0000 107 | 2,36,0.0000 108 | 2,37,0.0000 109 | 3,4,0.6330 110 | 3,5,0.2891 111 | 3,6,0.2800 112 | 3,7,0.2325 113 | 3,8,0.1925 114 | 3,9,0.0529 115 | 3,10,0.0140 116 | 3,11,0.0000 117 | 3,12,0.0000 118 | 3,13,0.0000 119 | 3,14,0.0000 120 | 3,15,0.0000 121 | 3,16,0.0000 122 | 3,17,0.0000 123 | 3,18,0.0000 124 | 3,19,0.0000 125 | 3,20,0.0000 126 | 3,21,0.0000 127 | 3,22,0.0000 128 | 3,23,0.0000 129 | 3,24,0.0000 130 | 3,25,0.0000 131 | 3,26,0.0010 132 | 3,27,0.0085 133 | 3,28,0.0998 134 | 3,29,0.0682 135 | 3,30,0.1489 136 | 3,31,0.0000 137 | 3,32,0.0000 138 | 3,33,0.0000 139 | 3,34,0.0000 140 | 3,35,0.0000 141 | 3,36,0.0000 142 | 3,37,0.0000 143 | 4,5,0.6392 144 | 4,6,0.4032 145 | 4,7,0.2834 146 | 4,8,0.2068 147 | 4,9,0.0331 148 | 4,10,0.0013 149 | 4,11,0.0000 150 | 4,12,0.0000 151 | 4,13,0.0000 152 | 4,14,0.0000 153 | 4,15,0.0000 154 | 4,16,0.0000 155 | 4,17,0.0000 156 | 4,18,0.0000 157 | 4,19,0.0000 158 | 4,20,0.0000 159 | 4,21,0.0000 160 | 4,22,0.0000 161 | 4,23,0.0000 162 | 4,24,0.0000 163 | 4,25,0.0000 164 | 4,26,0.0000 165 | 4,27,0.0059 166 | 4,28,0.0347 167 | 4,29,0.0355 168 | 4,30,0.0499 169 | 4,31,0.0000 170 | 4,32,0.0000 171 | 4,33,0.0000 172 | 4,34,0.0000 173 | 4,35,0.0000 174 | 4,36,0.0000 175 | 4,37,0.0000 176 | 5,6,0.7890 177 | 5,7,0.5244 178 | 5,8,0.4097 179 | 5,9,0.1090 180 | 5,10,0.0152 181 | 5,11,0.0000 182 | 5,12,0.0000 183 | 5,13,0.0000 184 | 5,14,0.0000 185 | 5,15,0.0000 186 | 5,16,0.0000 187 | 5,17,0.0000 188 | 5,18,0.0000 189 | 5,19,0.0000 190 | 5,20,0.0000 191 | 5,21,0.0000 192 | 5,22,0.0000 193 | 5,23,0.0000 194 | 5,24,0.0000 195 | 5,25,0.0000 196 | 5,26,0.0042 197 | 5,27,0.0189 198 | 5,28,0.0493 199 | 5,29,0.0144 200 | 5,30,0.0000 201 | 5,31,0.0000 202 | 5,32,0.0000 203 | 5,33,0.0000 204 | 5,34,0.0000 205 | 5,35,0.0000 206 | 5,36,0.0000 207 | 5,37,0.0000 208 | 6,7,0.6664 209 | 6,8,0.5129 210 | 6,9,0.2499 211 | 6,10,0.1559 212 | 6,11,0.0097 213 | 6,12,0.0000 214 | 6,13,0.0000 215 | 6,14,0.0000 216 | 6,15,0.0000 217 | 6,16,0.0000 218 | 6,17,0.0000 219 | 6,18,0.0000 220 | 6,19,0.0000 221 | 6,20,0.0000 222 | 6,21,0.0000 223 | 6,22,0.0000 224 | 6,23,0.0000 225 | 6,24,0.0000 226 | 6,25,0.0000 227 | 6,26,0.0065 228 | 6,27,0.0414 229 | 6,28,0.0385 230 | 6,29,0.0104 231 | 6,30,0.0000 232 | 6,31,0.0000 233 | 6,32,0.0000 234 | 6,33,0.0000 235 | 6,34,0.0000 236 | 6,35,0.0000 237 | 6,36,0.0000 238 | 6,37,0.0000 239 | 7,8,0.6659 240 | 7,9,0.0968 241 | 7,10,0.0807 242 | 7,11,0.0869 243 | 7,12,0.0000 244 | 7,13,0.0000 245 | 7,14,0.0000 246 | 7,15,0.0000 247 | 7,16,0.0000 248 | 7,17,0.0000 249 | 7,18,0.0000 250 | 7,19,0.0000 251 | 7,20,0.0000 252 | 7,21,0.0000 253 | 7,22,0.0000 254 | 7,23,0.0000 255 | 7,24,0.0000 256 | 7,25,0.0000 257 | 7,26,0.0072 258 | 7,27,0.0267 259 | 7,28,0.0386 260 | 7,29,0.0095 261 | 7,30,0.0000 262 | 7,31,0.0000 263 | 7,32,0.0000 264 | 7,33,0.0000 265 | 7,34,0.0000 266 | 7,35,0.0000 267 | 7,36,0.0000 268 | 7,37,0.0000 269 | 8,9,0.2425 270 | 8,10,0.1401 271 | 8,11,0.0713 272 | 8,12,0.0019 273 | 8,13,0.0000 274 | 8,14,0.0000 275 | 8,15,0.0000 276 | 8,16,0.0000 277 | 8,17,0.0000 278 | 8,18,0.0000 279 | 8,19,0.0000 280 | 8,20,0.0000 281 | 8,21,0.0000 282 | 8,22,0.0000 283 | 8,23,0.0001 284 | 8,24,0.0002 285 | 8,25,0.0002 286 | 8,26,0.0073 287 | 8,27,0.0336 288 | 8,28,0.0468 289 | 8,29,0.0047 290 | 8,30,0.0000 291 | 8,31,0.0000 292 | 8,32,0.0000 293 | 8,33,0.0000 294 | 8,34,0.0000 295 | 8,35,0.0000 296 | 8,36,0.0000 297 | 8,37,0.0000 298 | 9,10,0.6296 299 | 9,11,0.2832 300 | 9,12,0.0666 301 | 9,13,0.0009 302 | 9,14,0.0000 303 | 9,15,0.0000 304 | 9,16,0.0000 305 | 9,17,0.0000 306 | 9,18,0.0000 307 | 9,19,0.0000 308 | 9,20,0.0000 309 | 9,21,0.0086 310 | 9,22,0.0424 311 | 9,23,0.0457 312 | 9,24,0.0366 313 | 9,25,0.0265 314 | 9,26,0.0507 315 | 9,27,0.1673 316 | 9,28,0.1784 317 | 9,29,0.0022 318 | 9,30,0.0000 319 | 9,31,0.0000 320 | 9,32,0.0000 321 | 9,33,0.0000 322 | 9,34,0.0000 323 | 9,35,0.0000 324 | 9,36,0.0000 325 | 9,37,0.0000 326 | 10,11,0.3034 327 | 10,12,0.0421 328 | 10,13,0.0019 329 | 10,14,0.0000 330 | 10,15,0.0000 331 | 10,16,0.0000 332 | 10,17,0.0000 333 | 10,18,0.0000 334 | 10,19,0.0000 335 | 10,20,0.0000 336 | 10,21,0.0157 337 | 10,22,0.0722 338 | 10,23,0.0828 339 | 10,24,0.0801 340 | 10,25,0.0607 341 | 10,26,0.0783 342 | 10,27,0.2330 343 | 10,28,0.1576 344 | 10,29,0.0000 345 | 10,30,0.0000 346 | 10,31,0.0000 347 | 10,32,0.0000 348 | 10,33,0.0000 349 | 10,34,0.0000 350 | 10,35,0.0000 351 | 10,36,0.0000 352 | 10,37,0.0000 353 | 11,12,0.3842 354 | 11,13,0.0509 355 | 11,14,0.0018 356 | 11,15,0.0016 357 | 11,16,0.0019 358 | 11,17,0.0000 359 | 11,18,0.0000 360 | 11,19,0.0000 361 | 11,20,0.0000 362 | 11,21,0.0215 363 | 11,22,0.0286 364 | 11,23,0.0292 365 | 11,24,0.0260 366 | 11,25,0.0240 367 | 11,26,0.0232 368 | 11,27,0.0368 369 | 11,28,0.0086 370 | 11,29,0.0000 371 | 11,30,0.0000 372 | 11,31,0.0000 373 | 11,32,0.0000 374 | 11,33,0.0000 375 | 11,34,0.0000 376 | 11,35,0.0000 377 | 11,36,0.0000 378 | 11,37,0.0000 379 | 12,13,0.3239 380 | 12,14,0.0287 381 | 12,15,0.0381 382 | 12,16,0.0433 383 | 12,17,0.0000 384 | 12,18,0.0000 385 | 12,19,0.0000 386 | 12,20,0.0000 387 | 12,21,0.0321 388 | 12,22,0.0359 389 | 12,23,0.0366 390 | 12,24,0.0366 391 | 12,25,0.0299 392 | 12,26,0.0223 393 | 12,27,0.0142 394 | 12,28,0.0000 395 | 12,29,0.0000 396 | 12,30,0.0000 397 | 12,31,0.0000 398 | 12,32,0.0000 399 | 12,33,0.0000 400 | 12,34,0.0000 401 | 12,35,0.0000 402 | 12,36,0.0000 403 | 12,37,0.0000 404 | 13,14,0.3967 405 | 13,15,0.3393 406 | 13,16,0.2876 407 | 13,17,0.1754 408 | 13,18,0.1115 409 | 13,19,0.0028 410 | 13,20,0.0000 411 | 13,21,0.0105 412 | 13,22,0.0115 413 | 13,23,0.0115 414 | 13,24,0.0114 415 | 13,25,0.0079 416 | 13,26,0.0049 417 | 13,27,0.0022 418 | 13,28,0.0000 419 | 13,29,0.0000 420 | 13,30,0.0000 421 | 13,31,0.0000 422 | 13,32,0.0000 423 | 13,33,0.0000 424 | 13,34,0.0000 425 | 13,35,0.0000 426 | 13,36,0.0000 427 | 13,37,0.0000 428 | 14,15,0.8881 429 | 14,16,0.7457 430 | 14,17,0.4629 431 | 14,18,0.2293 432 | 14,19,0.0353 433 | 14,20,0.0000 434 | 14,21,0.0044 435 | 14,22,0.0044 436 | 14,23,0.0043 437 | 14,24,0.0040 438 | 14,25,0.0030 439 | 14,26,0.0007 440 | 14,27,0.0000 441 | 14,28,0.0000 442 | 14,29,0.0000 443 | 14,30,0.0000 444 | 14,31,0.0000 445 | 14,32,0.0000 446 | 14,33,0.0000 447 | 14,34,0.0000 448 | 14,35,0.0000 449 | 14,36,0.0000 450 | 14,37,0.0000 451 | 15,16,0.8778 452 | 15,17,0.5385 453 | 15,18,0.2538 454 | 15,19,0.0490 455 | 15,20,0.0071 456 | 15,21,0.0036 457 | 15,22,0.0036 458 | 15,23,0.0036 459 | 15,24,0.0033 460 | 15,25,0.0025 461 | 15,26,0.0002 462 | 15,27,0.0000 463 | 15,28,0.0000 464 | 15,29,0.0000 465 | 15,30,0.0000 466 | 15,31,0.0000 467 | 15,32,0.0000 468 | 15,33,0.0000 469 | 15,34,0.0000 470 | 15,35,0.0000 471 | 15,36,0.0000 472 | 15,37,0.0000 473 | 16,17,0.6637 474 | 16,18,0.3079 475 | 16,19,0.0749 476 | 16,20,0.0431 477 | 16,21,0.0103 478 | 16,22,0.0044 479 | 16,23,0.0044 480 | 16,24,0.0042 481 | 16,25,0.0032 482 | 16,26,0.0003 483 | 16,27,0.0000 484 | 16,28,0.0000 485 | 16,29,0.0000 486 | 16,30,0.0000 487 | 16,31,0.0000 488 | 16,32,0.0000 489 | 16,33,0.0000 490 | 16,34,0.0000 491 | 16,35,0.0000 492 | 16,36,0.0000 493 | 16,37,0.0000 494 | 17,18,0.5182 495 | 17,19,0.3154 496 | 17,20,0.2819 497 | 17,21,0.0338 498 | 17,22,0.0000 499 | 17,23,0.0000 500 | 17,24,0.0000 501 | 17,25,0.0000 502 | 17,26,0.0000 503 | 17,27,0.0000 504 | 17,28,0.0000 505 | 17,29,0.0000 506 | 17,30,0.0000 507 | 17,31,0.0000 508 | 17,32,0.0000 509 | 17,33,0.0000 510 | 17,34,0.0000 511 | 17,35,0.0000 512 | 17,36,0.0000 513 | 17,37,0.0000 514 | 18,19,0.6182 515 | 18,20,0.3568 516 | 18,21,0.0808 517 | 18,22,0.0000 518 | 18,23,0.0000 519 | 18,24,0.0000 520 | 18,25,0.0000 521 | 18,26,0.0000 522 | 18,27,0.0000 523 | 18,28,0.0000 524 | 18,29,0.0000 525 | 18,30,0.0000 526 | 18,31,0.0000 527 | 18,32,0.0000 528 | 18,33,0.0000 529 | 18,34,0.0000 530 | 18,35,0.0000 531 | 18,36,0.0000 532 | 18,37,0.0000 533 | 19,20,0.6984 534 | 19,21,0.2884 535 | 19,22,0.0655 536 | 19,23,0.0052 537 | 19,24,0.0000 538 | 19,25,0.0000 539 | 19,26,0.0000 540 | 19,27,0.0000 541 | 19,28,0.0000 542 | 19,29,0.0000 543 | 19,30,0.0000 544 | 19,31,0.0000 545 | 19,32,0.0000 546 | 19,33,0.0000 547 | 19,34,0.0000 548 | 19,35,0.0000 549 | 19,36,0.0000 550 | 19,37,0.0000 551 | 20,21,0.4386 552 | 20,22,0.1434 553 | 20,23,0.0487 554 | 20,24,0.0160 555 | 20,25,0.0084 556 | 20,26,0.0003 557 | 20,27,0.0000 558 | 20,28,0.0000 559 | 20,29,0.0000 560 | 20,30,0.0000 561 | 20,31,0.0000 562 | 20,32,0.0000 563 | 20,33,0.0000 564 | 20,34,0.0000 565 | 20,35,0.0000 566 | 20,36,0.0000 567 | 20,37,0.0000 568 | 21,22,0.4197 569 | 21,23,0.2469 570 | 21,24,0.1464 571 | 21,25,0.1165 572 | 21,26,0.0489 573 | 21,27,0.0157 574 | 21,28,0.0004 575 | 21,29,0.0000 576 | 21,30,0.0000 577 | 21,31,0.0000 578 | 21,32,0.0000 579 | 21,33,0.0000 580 | 21,34,0.0004 581 | 21,35,0.0041 582 | 21,36,0.0000 583 | 21,37,0.0000 584 | 22,23,0.6861 585 | 22,24,0.3625 586 | 22,25,0.2918 587 | 22,26,0.1578 588 | 22,27,0.0992 589 | 22,28,0.0066 590 | 22,29,0.0000 591 | 22,30,0.0000 592 | 22,31,0.0000 593 | 22,32,0.0000 594 | 22,33,0.0000 595 | 22,34,0.0000 596 | 22,35,0.0000 597 | 22,36,0.0000 598 | 22,37,0.0000 599 | 23,24,0.5630 600 | 23,25,0.4609 601 | 23,26,0.2676 602 | 23,27,0.1814 603 | 23,28,0.0407 604 | 23,29,0.0000 605 | 23,30,0.0000 606 | 23,31,0.0000 607 | 23,32,0.0000 608 | 23,33,0.0000 609 | 23,34,0.0000 610 | 23,35,0.0000 611 | 23,36,0.0000 612 | 23,37,0.0000 613 | 24,25,0.6586 614 | 24,26,0.2640 615 | 24,27,0.2054 616 | 24,28,0.0963 617 | 24,29,0.0000 618 | 24,30,0.0000 619 | 24,31,0.0000 620 | 24,32,0.0516 621 | 24,33,0.1919 622 | 24,34,0.1384 623 | 24,35,0.1420 624 | 24,36,0.1246 625 | 24,37,0.1587 626 | 25,26,0.3551 627 | 25,27,0.1786 628 | 25,28,0.0466 629 | 25,29,0.0033 630 | 25,30,0.1161 631 | 25,31,0.1050 632 | 25,32,0.1914 633 | 25,33,0.2382 634 | 25,34,0.2224 635 | 25,35,0.1893 636 | 25,36,0.2075 637 | 25,37,0.2094 638 | 26,27,0.7045 639 | 26,28,0.3344 640 | 26,29,0.0000 641 | 26,30,0.0000 642 | 26,31,0.0000 643 | 26,32,0.0000 644 | 26,33,0.0000 645 | 26,34,0.0000 646 | 26,35,0.0000 647 | 26,36,0.0000 648 | 26,37,0.0000 649 | 27,28,0.5784 650 | 27,29,0.0180 651 | 27,30,0.0014 652 | 27,31,0.0000 653 | 27,32,0.0000 654 | 27,33,0.0000 655 | 27,34,0.0000 656 | 27,35,0.0000 657 | 27,36,0.0000 658 | 27,37,0.0000 659 | 28,29,0.1223 660 | 28,30,0.1538 661 | 28,31,0.0012 662 | 28,32,0.0000 663 | 28,33,0.0000 664 | 28,34,0.0000 665 | 28,35,0.0000 666 | 28,36,0.0000 667 | 28,37,0.0000 668 | 29,30,0.5859 669 | 29,31,0.2812 670 | 29,32,0.0460 671 | 29,33,0.0339 672 | 29,34,0.0343 673 | 29,35,0.0393 674 | 29,36,0.0506 675 | 29,37,0.0315 676 | 30,31,0.3801 677 | 30,32,0.1849 678 | 30,33,0.1707 679 | 30,34,0.1292 680 | 30,35,0.1212 681 | 30,36,0.1598 682 | 30,37,0.1377 683 | 31,32,0.4794 684 | 31,33,0.3929 685 | 31,34,0.3085 686 | 31,35,0.2958 687 | 31,36,0.3909 688 | 31,37,0.3200 689 | 32,33,0.9021 690 | 32,34,0.5685 691 | 32,35,0.5605 692 | 32,36,0.9270 693 | 32,37,0.7007 694 | 33,34,0.7599 695 | 33,35,0.6514 696 | 33,36,0.8516 697 | 33,37,0.8799 698 | 34,35,0.8392 699 | 34,36,0.8554 700 | 34,37,0.8323 701 | 35,36,0.8529 702 | 35,37,0.6675 703 | 36,37,0.6923 704 | -------------------------------------------------------------------------------- /data/ThreeDMatch/train_3dmatch.txt: -------------------------------------------------------------------------------- 1 | 7-scenes-chess 2 | 7-scenes-fire 3 | 7-scenes-office 4 | 7-scenes-pumpkin 5 | 7-scenes-stairs 6 | analysis-by-synthesis-apt1-kitchen 7 | analysis-by-synthesis-apt1-living 8 | analysis-by-synthesis-apt2-bed 9 | analysis-by-synthesis-apt2-kitchen 10 | analysis-by-synthesis-apt2-living 11 | analysis-by-synthesis-apt2-luke 12 | analysis-by-synthesis-office2-5a 13 | analysis-by-synthesis-office2-5b 14 | bundlefusion-apt0_1 15 | bundlefusion-apt0_2 16 | bundlefusion-apt0_3 17 | bundlefusion-apt0_4 18 | bundlefusion-apt1_1 19 | bundlefusion-apt1_2 20 | bundlefusion-apt1_3 21 | bundlefusion-apt1_4 22 | bundlefusion-apt2_1 23 | bundlefusion-apt2_2 24 | bundlefusion-copyroom_1 25 | bundlefusion-copyroom_2 26 | bundlefusion-office1_1 27 | bundlefusion-office1_2 28 | bundlefusion-office2 29 | bundlefusion-office3 30 | rgbd-scenes-v2-scene_01 31 | rgbd-scenes-v2-scene_02 32 | rgbd-scenes-v2-scene_03 33 | rgbd-scenes-v2-scene_04 34 | rgbd-scenes-v2-scene_05 35 | rgbd-scenes-v2-scene_06 36 | rgbd-scenes-v2-scene_07 37 | rgbd-scenes-v2-scene_08 38 | rgbd-scenes-v2-scene_09 39 | rgbd-scenes-v2-scene_11 40 | rgbd-scenes-v2-scene_12 41 | rgbd-scenes-v2-scene_13 42 | rgbd-scenes-v2-scene_14 43 | sun3d-brown_bm_1-brown_bm_1_1 44 | sun3d-brown_bm_1-brown_bm_1_2 45 | sun3d-brown_bm_1-brown_bm_1_3 46 | sun3d-brown_cogsci_1-brown_cogsci_1 47 | sun3d-brown_cs_2-brown_cs2_1 48 | sun3d-brown_cs_2-brown_cs2_2 49 | sun3d-brown_cs_3-brown_cs3 50 | sun3d-harvard_c3-hv_c3_1 51 | sun3d-harvard_c5-hv_c5_1 52 | sun3d-harvard_c6-hv_c6_1 53 | sun3d-harvard_c8-hv_c8_3 54 | sun3d-hotel_nips2012-nips_4_1 55 | sun3d-hotel_nips2012-nips_4_2 56 | sun3d-hotel_sf-scan1_1 57 | sun3d-hotel_sf-scan1_2 58 | sun3d-hotel_sf-scan1_3 59 | sun3d-hotel_sf-scan1_4 60 | sun3d-mit_32_d507-d507_2_1 61 | sun3d-mit_32_d507-d507_2_2 62 | sun3d-mit_46_ted_lab1-ted_lab_2_1 63 | sun3d-mit_46_ted_lab1-ted_lab_2_2 64 | sun3d-mit_46_ted_lab1-ted_lab_2_3 65 | sun3d-mit_46_ted_lab1-ted_lab_2_4 66 | sun3d-mit_76_417-76-417b_1 67 | sun3d-mit_76_417-76-417b_2_1 68 | sun3d-mit_76_417-76-417b_3 69 | sun3d-mit_76_417-76-417b_4 70 | sun3d-mit_76_417-76-417b_5 71 | sun3d-mit_dorm_next_sj-dorm_next_sj_oct_30_2012_scan1_erika 72 | sun3d-mit_w20_athena-sc_athena_oct_29_2012_scan1_erika_1 73 | sun3d-mit_w20_athena-sc_athena_oct_29_2012_scan1_erika_2 74 | sun3d-mit_w20_athena-sc_athena_oct_29_2012_scan1_erika_3 75 | sun3d-mit_w20_athena-sc_athena_oct_29_2012_scan1_erika_4 76 | -------------------------------------------------------------------------------- /data/ThreeDMatch/train_info.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/data/ThreeDMatch/train_info.pkl -------------------------------------------------------------------------------- /data/ThreeDMatch/val_3dmatch.txt: -------------------------------------------------------------------------------- 1 | sun3d-brown_bm_4-brown_bm_4 2 | sun3d-harvard_c11-hv_c11_2 3 | 7-scenes-heads 4 | rgbd-scenes-v2-scene_10 5 | bundlefusion-office0_1 6 | bundlefusion-office0_2 7 | bundlefusion-office0_3 8 | analysis-by-synthesis-apt2-kitchen 9 | -------------------------------------------------------------------------------- /data/ThreeDMatch/val_info.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/data/ThreeDMatch/val_info.pkl -------------------------------------------------------------------------------- /data/__init__.py: -------------------------------------------------------------------------------- 1 | from .ThreeDMatch import ThreeDMatch 2 | from .Kitti import Kitti 3 | from .MVP_RG import MVP_RG 4 | from .dataloader import get_dataset, get_dataloader, collate_fn 5 | -------------------------------------------------------------------------------- /demo.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import copy 3 | import numpy as np 4 | import os 5 | import torch 6 | from easydict import EasyDict as edict 7 | import sys 8 | sys.path.append(os.path.dirname(os.path.abspath(__file__))) 9 | 10 | from data import collate_fn 11 | from models import architectures, NgeNet, vote 12 | from utils import decode_config, npy2pcd, pcd2npy, execute_global_registration, \ 13 | npy2feat, setup_seed, get_blue, get_yellow, voxel_ds, normal, \ 14 | read_cloud, vis_plys 15 | 16 | CUR = os.path.dirname(os.path.abspath(__file__)) 17 | 18 | 19 | class NgeNet_pipeline(): 20 | def __init__(self, ckpt_path, voxel_size, vote_flag, cuda=True): 21 | self.voxel_size_3dmatch = 0.025 22 | self.voxel_size = voxel_size 23 | self.scale = self.voxel_size / self.voxel_size_3dmatch 24 | self.cuda = cuda 25 | self.vote_flag = vote_flag 26 | config = self.prepare_config() 27 | self.neighborhood_limits = [38, 36, 35, 38] 28 | model = NgeNet(config) 29 | if self.cuda: 30 | model = model.cuda() 31 | model.load_state_dict(torch.load(ckpt_path)) 32 | else: 33 | model.load_state_dict( 34 | torch.load(ckpt_path, map_location=torch.device('cpu'))) 35 | self.model = model 36 | self.config = config 37 | self.model.eval() 38 | 39 | def prepare_config(self): 40 | config = decode_config(os.path.join(CUR, 'configs', 'threedmatch.yaml')) 41 | config = edict(config) 42 | # config.first_subsampling_dl = self.voxel_size 43 | config.architecture = architectures[config.dataset] 44 | return config 45 | 46 | def prepare_inputs(self, source, target): 47 | src_pcd_input = pcd2npy(voxel_ds(copy.deepcopy(source), self.voxel_size)) 48 | tgt_pcd_input = pcd2npy(voxel_ds(copy.deepcopy(target), self.voxel_size)) 49 | 50 | src_pcd_input /= self.scale 51 | tgt_pcd_input /= self.scale 52 | 53 | src_feats = np.ones_like(src_pcd_input[:, :1]) 54 | tgt_feats = np.ones_like(tgt_pcd_input[:, :1]) 55 | 56 | src_pcd = normal(npy2pcd(src_pcd_input), radius=4*self.voxel_size_3dmatch, max_nn=30, loc=(0, 0, 0)) 57 | tgt_pcd = normal(npy2pcd(tgt_pcd_input), radius=4*self.voxel_size_3dmatch, max_nn=30, loc=(0, 0, 0)) 58 | src_normals = np.array(src_pcd.normals).astype(np.float32) 59 | tgt_normals = np.array(tgt_pcd.normals).astype(np.float32) 60 | 61 | T = np.eye(4) 62 | coors = np.array([[0, 0], [1, 1]]) 63 | src_pcd = pcd2npy(source) 64 | tgt_pcd = pcd2npy(target) 65 | 66 | pair = dict( 67 | src_points=src_pcd_input, 68 | tgt_points=tgt_pcd_input, 69 | src_feats=src_feats, 70 | tgt_feats=tgt_feats, 71 | src_normals=src_normals, 72 | tgt_normals=tgt_normals, 73 | transf=T, 74 | coors=coors, 75 | src_points_raw=src_pcd, 76 | tgt_points_raw=tgt_pcd) 77 | 78 | dict_inputs = collate_fn([pair], self.config, self.neighborhood_limits) 79 | if self.cuda: 80 | for k, v in dict_inputs.items(): 81 | if isinstance(v, list): 82 | for i in range(len(v)): 83 | dict_inputs[k][i] = dict_inputs[k][i].cuda() 84 | else: 85 | dict_inputs[k] = dict_inputs[k].cuda() 86 | 87 | return dict_inputs 88 | 89 | def pipeline(self, source, target, npts=20000): 90 | inputs = self.prepare_inputs(source, target) 91 | 92 | batched_feats_h, batched_feats_m, batched_feats_l = self.model(inputs) 93 | stack_points = inputs['points'] 94 | stack_lengths = inputs['stacked_lengths'] 95 | coords_src = stack_points[0][:stack_lengths[0][0]] 96 | coords_tgt = stack_points[0][stack_lengths[0][0]:] 97 | feats_src_h = batched_feats_h[:stack_lengths[0][0]] 98 | feats_tgt_h = batched_feats_h[stack_lengths[0][0]:] 99 | feats_src_m = batched_feats_m[:stack_lengths[0][0]] 100 | feats_tgt_m = batched_feats_m[stack_lengths[0][0]:] 101 | feats_src_l = batched_feats_l[:stack_lengths[0][0]] 102 | feats_tgt_l = batched_feats_l[stack_lengths[0][0]:] 103 | 104 | source_npy = coords_src.detach().cpu().numpy() * self.scale 105 | target_npy = coords_tgt.detach().cpu().numpy() * self.scale 106 | 107 | source_feats_h = feats_src_h[:, :-2].detach().cpu().numpy() 108 | target_feats_h = feats_tgt_h[:, :-2].detach().cpu().numpy() 109 | source_feats_m = feats_src_m.detach().cpu().numpy() 110 | target_feats_m = feats_tgt_m.detach().cpu().numpy() 111 | source_feats_l = feats_src_l.detach().cpu().numpy() 112 | target_feats_l = feats_tgt_l.detach().cpu().numpy() 113 | 114 | source_overlap_scores = feats_src_h[:, -2].detach().cpu().numpy() 115 | target_overlap_scores = feats_tgt_h[:, -2].detach().cpu().numpy() 116 | source_scores = source_overlap_scores 117 | target_scores = target_overlap_scores 118 | 119 | npoints = npts 120 | if npoints > 0: 121 | if source_npy.shape[0] > npoints: 122 | p = source_scores / np.sum(source_scores) 123 | idx = np.random.choice(len(source_npy), size=npoints, replace=False, p=p) 124 | source_npy = source_npy[idx] 125 | source_feats_h = source_feats_h[idx] 126 | source_feats_m = source_feats_m[idx] 127 | source_feats_l = source_feats_l[idx] 128 | 129 | if target_npy.shape[0] > npoints: 130 | p = target_scores / np.sum(target_scores) 131 | idx = np.random.choice(len(target_npy), size=npoints, replace=False, p=p) 132 | target_npy = target_npy[idx] 133 | target_feats_h = target_feats_h[idx] 134 | target_feats_m = target_feats_m[idx] 135 | target_feats_l = target_feats_l[idx] 136 | 137 | if self.vote_flag: 138 | after_vote = vote(source_npy=source_npy, 139 | target_npy=target_npy, 140 | source_feats=[source_feats_h, source_feats_m, source_feats_l], 141 | target_feats=[target_feats_h, target_feats_m, target_feats_l], 142 | voxel_size=self.voxel_size * 2, 143 | use_cuda=self.cuda) 144 | source_npy, target_npy, source_feats_npy, target_feats_npy = after_vote 145 | else: 146 | source_feats_npy, target_feats_npy = source_feats_h, target_feats_h 147 | source, target = npy2pcd(source_npy), npy2pcd(target_npy) 148 | source_feats, target_feats = npy2feat(source_feats_npy), npy2feat(target_feats_npy) 149 | pred_T, estimate = execute_global_registration(source=source, 150 | target=target, 151 | source_feats=source_feats, 152 | target_feats=target_feats, 153 | voxel_size=self.voxel_size*2) 154 | 155 | torch.cuda.empty_cache() 156 | return pred_T 157 | 158 | 159 | if __name__ == '__main__': 160 | parser = argparse.ArgumentParser(description='Configuration Parameters') 161 | parser.add_argument('--src_path', required=True, help='source point cloud path') 162 | parser.add_argument('--tgt_path', required=True, help='target point cloud path') 163 | parser.add_argument('--checkpoint', required=True, help='checkpoint path') 164 | parser.add_argument('--voxel_size', type=float, required=True, help='voxel size') 165 | parser.add_argument('--npts', type=int, default=20000, 166 | help='the number of sampled points for registration') 167 | parser.add_argument('--no_vote', action='store_true', 168 | help='whether to use multi-level consistent voting') 169 | parser.add_argument('--no_vis', action='store_true', 170 | help='whether to visualize the point clouds') 171 | parser.add_argument('--no_cuda', action='store_true', 172 | help='whether to use cuda') 173 | args = parser.parse_args() 174 | 175 | # input data 176 | source, target = read_cloud(args.src_path), read_cloud(args.tgt_path) 177 | 178 | # loading model 179 | cuda = not args.no_cuda 180 | vote_flag = not args.no_vote 181 | model = NgeNet_pipeline( 182 | ckpt_path=args.checkpoint, 183 | voxel_size=args.voxel_size, 184 | vote_flag=vote_flag, 185 | cuda=cuda) 186 | 187 | # registration 188 | T = model.pipeline(source, target, npts=args.npts) 189 | print('Estimated transformation matrix: ', T) 190 | 191 | # vis 192 | if not args.no_vis: 193 | # voxelization for fluent visualization 194 | source = voxel_ds(source, args.voxel_size) 195 | target = voxel_ds(target, args.voxel_size) 196 | estimate = copy.deepcopy(source).transform(T) 197 | source.paint_uniform_color(get_yellow()) 198 | source.estimate_normals() 199 | target.paint_uniform_color(get_blue()) 200 | target.estimate_normals() 201 | vis_plys([source, target], need_color=False) 202 | 203 | estimate.paint_uniform_color(get_yellow()) 204 | estimate.estimate_normals() 205 | vis_plys([estimate, target], need_color=False) 206 | -------------------------------------------------------------------------------- /demo_data/3dmatch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/demo_data/3dmatch.png -------------------------------------------------------------------------------- /demo_data/cloud_bin_21.pth: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/demo_data/cloud_bin_21.pth -------------------------------------------------------------------------------- /demo_data/cloud_bin_34.pth: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/demo_data/cloud_bin_34.pth -------------------------------------------------------------------------------- /demo_data/my_data1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/demo_data/my_data1.png -------------------------------------------------------------------------------- /demo_data/my_data2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/demo_data/my_data2.png -------------------------------------------------------------------------------- /eval_kitti.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import copy 3 | import glob 4 | import numpy as np 5 | import os 6 | import pdb 7 | import torch 8 | import open3d as o3d 9 | from easydict import EasyDict as edict 10 | from tqdm import tqdm 11 | 12 | from data import Kitti, get_dataloader 13 | from models import architectures, NgeNet, vote 14 | from utils import decode_config, npy2pcd, pcd2npy, execute_global_registration, \ 15 | npy2feat, vis_plys, setup_seed, fmat, to_tensor, get_blue, get_yellow 16 | from metrics import Error_R, Error_t 17 | 18 | CUR = os.path.dirname(os.path.abspath(__file__)) 19 | 20 | 21 | def main(args): 22 | setup_seed(22) 23 | config = decode_config(os.path.join(CUR, 'configs', 'kitti.yaml')) 24 | config = edict(config) 25 | config.architecture = architectures[config.dataset] 26 | config.num_workers = 4 27 | test_dataset = Kitti(root=args.data_root, 28 | split='test', 29 | aug=False, 30 | voxel_size=config.first_subsampling_dl, 31 | overlap_radius=config.overlap_radius, 32 | max_coors=config.max_points) 33 | 34 | test_dataloader, neighborhood_limits = get_dataloader(config=config, 35 | dataset=test_dataset, 36 | batch_size=config.batch_size, 37 | num_workers=config.num_workers, 38 | shuffle=False, 39 | neighborhood_limits=None) 40 | 41 | print(neighborhood_limits) 42 | model = NgeNet(config) 43 | use_cuda = not args.no_cuda 44 | if use_cuda: 45 | model = model.cuda() 46 | model.load_state_dict(torch.load(args.checkpoint)) 47 | else: 48 | model.load_state_dict( 49 | torch.load(args.checkpoint, map_location=torch.device('cpu'))) 50 | model.eval() 51 | 52 | Ts, gt_Ts = [], [] 53 | with torch.no_grad(): 54 | for pair_ind, inputs in enumerate(tqdm(test_dataloader)): 55 | if use_cuda: 56 | for k, v in inputs.items(): 57 | if isinstance(v, list): 58 | for i in range(len(v)): 59 | inputs[k][i] = inputs[k][i].cuda() 60 | else: 61 | inputs[k] = inputs[k].cuda() 62 | 63 | batched_feats_h, batched_feats_m, batched_feats_l = model(inputs) 64 | stack_points = inputs['points'] 65 | stack_lengths = inputs['stacked_lengths'] 66 | coords_src = stack_points[0][:stack_lengths[0][0]] 67 | coords_tgt = stack_points[0][stack_lengths[0][0]:] 68 | feats_src_h = batched_feats_h[:stack_lengths[0][0]] 69 | feats_tgt_h = batched_feats_h[stack_lengths[0][0]:] 70 | feats_src_m = batched_feats_m[:stack_lengths[0][0]] 71 | feats_tgt_m = batched_feats_m[stack_lengths[0][0]:] 72 | feats_src_l = batched_feats_l[:stack_lengths[0][0]] 73 | feats_tgt_l = batched_feats_l[stack_lengths[0][0]:] 74 | 75 | coors = inputs['coors'][0] # list, [coors1, coors2, ..], preparation for batchsize > 1 76 | transf = inputs['transf'][0] # (1, 4, 4), preparation for batchsize > 1 77 | 78 | coors = coors.detach().cpu().numpy() 79 | T = transf.detach().cpu().numpy() 80 | 81 | source_npy = coords_src.detach().cpu().numpy() 82 | target_npy = coords_tgt.detach().cpu().numpy() 83 | 84 | source_npy_raw = copy.deepcopy(source_npy) 85 | target_npy_raw = copy.deepcopy(target_npy) 86 | source_feats_h = feats_src_h[:, :-2].detach().cpu().numpy() 87 | target_feats_h = feats_tgt_h[:, :-2].detach().cpu().numpy() 88 | source_feats_m = feats_src_m.detach().cpu().numpy() 89 | target_feats_m = feats_tgt_m.detach().cpu().numpy() 90 | source_feats_l = feats_src_l.detach().cpu().numpy() 91 | target_feats_l = feats_tgt_l.detach().cpu().numpy() 92 | 93 | source_overlap_scores = feats_src_h[:, -2].detach().cpu().numpy() 94 | target_overlap_scores = feats_tgt_h[:, -2].detach().cpu().numpy() 95 | source_saliency_scores = feats_src_h[:, -1].detach().cpu().numpy() 96 | target_saliency_scores = feats_tgt_h[:, -1].detach().cpu().numpy() 97 | 98 | source_scores = source_overlap_scores * source_saliency_scores 99 | target_scores = target_overlap_scores * target_saliency_scores 100 | 101 | npoints = args.npts 102 | if source_npy.shape[0] > npoints: 103 | p = source_scores / np.sum(source_scores) 104 | idx = np.random.choice(len(source_npy), size=npoints, replace=False, p=p) 105 | source_npy = source_npy[idx] 106 | source_feats_h = source_feats_h[idx] 107 | source_feats_m = source_feats_m[idx] 108 | source_feats_l = source_feats_l[idx] 109 | 110 | if target_npy.shape[0] > npoints: 111 | p = target_scores / np.sum(target_scores) 112 | idx = np.random.choice(len(target_npy), size=npoints, replace=False, p=p) 113 | target_npy = target_npy[idx] 114 | target_feats_h = target_feats_h[idx] 115 | target_feats_m = target_feats_m[idx] 116 | target_feats_l = target_feats_l[idx] 117 | 118 | after_vote = vote(source_npy=source_npy, 119 | target_npy=target_npy, 120 | source_feats=[source_feats_h, source_feats_m, source_feats_l], 121 | target_feats=[target_feats_h, target_feats_m, target_feats_l], 122 | voxel_size=config.first_subsampling_dl, 123 | use_cuda=use_cuda) 124 | source_npy, target_npy, source_feats_npy, target_feats_npy = after_vote 125 | 126 | M = torch.cdist(to_tensor(source_feats_npy, use_cuda), to_tensor(target_feats_npy, use_cuda)) 127 | row_max_inds = torch.min(M, dim=-1)[1].cpu().numpy() 128 | col_max_inds = torch.min(M, dim=0)[1].cpu().numpy() 129 | 130 | source, target = npy2pcd(source_npy), npy2pcd(target_npy) 131 | 132 | source_feats, target_feats = npy2feat(source_feats_npy), npy2feat(target_feats_npy) 133 | pred_T, estimate = execute_global_registration(source=source, 134 | target=target, 135 | source_feats=source_feats, 136 | target_feats=target_feats, 137 | voxel_size=config.first_subsampling_dl) 138 | Ts.append(pred_T) 139 | gt_Ts.append(T) 140 | 141 | if args.vis: 142 | source_ply = npy2pcd(source_npy_raw) 143 | source_ply.paint_uniform_color(get_yellow()) 144 | estimate_ply = copy.deepcopy(source_ply).transform(pred_T) 145 | target_ply = npy2pcd(target_npy_raw) 146 | target_ply.paint_uniform_color(get_blue()) 147 | vis_plys([target_ply, estimate_ply], need_color=False) 148 | 149 | Ts, gt_Ts = np.array(Ts), np.array(gt_Ts) 150 | 151 | rot_error = Error_R(Ts[:, :3, :3], gt_Ts[:, :3, :3]) 152 | trans_error = Error_t(Ts[:, :3, 3], gt_Ts[:, :3, 3]) 153 | 154 | rot_threshold = 5 155 | trans_threshold = 2 156 | rot_flag = rot_error < rot_threshold 157 | trans_flag = trans_error < trans_threshold 158 | 159 | recall = (rot_flag & trans_flag).sum() / len(rot_flag) 160 | RRE = np.mean(rot_error[rot_flag]) 161 | RTE = np.mean(trans_error[trans_flag]) 162 | 163 | print('Recall: ', fmat(recall)) 164 | print('RRE: ', fmat(RRE)) 165 | print('RTE: ', fmat(RTE)) 166 | 167 | 168 | if __name__ == '__main__': 169 | parser = argparse.ArgumentParser(description='Configuration Parameters') 170 | parser.add_argument('--npts', type=int, default=5000, 171 | help='the number of sampled points for registration') 172 | parser.add_argument('--data_root', required=True, help='data root') 173 | parser.add_argument('--checkpoint', required=True, help='checkpoint path') 174 | parser.add_argument('--vis', action='store_true', 175 | help='whether to visualize the point clouds') 176 | parser.add_argument('--no_cuda', action='store_true', 177 | help='whether to use cuda') 178 | args = parser.parse_args() 179 | main(args) 180 | -------------------------------------------------------------------------------- /eval_mvp_rg.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import copy 3 | import glob 4 | import numpy as np 5 | import os 6 | import pdb 7 | import torch 8 | import open3d as o3d 9 | from easydict import EasyDict as edict 10 | from tqdm import tqdm 11 | 12 | from data import MVP_RG, get_dataloader 13 | from models import architectures, NgeNet, vote 14 | from utils import decode_config, npy2pcd, pcd2npy, execute_global_registration, \ 15 | npy2feat, vis_plys, setup_seed, fmat, to_tensor, get_blue, get_yellow 16 | from metrics import Error_R, Error_t, RMSE 17 | 18 | CUR = os.path.dirname(os.path.abspath(__file__)) 19 | 20 | 21 | def main(args): 22 | setup_seed(22) 23 | config = decode_config(os.path.join(CUR, 'configs', 'mvp_rg.yaml')) 24 | config = edict(config) 25 | config.architecture = architectures[config.dataset] 26 | config.num_workers = 4 27 | test_dataset = MVP_RG(root=args.data_root, 28 | split='val', 29 | rot_mag=config.rot_mag, 30 | trans_mag=config.trans_mag, 31 | overlap_radius=config.overlap_radius) 32 | 33 | test_dataloader, neighborhood_limits = get_dataloader(config=config, 34 | dataset=test_dataset, 35 | batch_size=config.batch_size, 36 | num_workers=config.num_workers, 37 | shuffle=False, 38 | neighborhood_limits=None) 39 | 40 | print(neighborhood_limits) 41 | model = NgeNet(config) 42 | use_cuda = not args.no_cuda 43 | if use_cuda: 44 | model = model.cuda() 45 | model.load_state_dict(torch.load(args.checkpoint)) 46 | else: 47 | model.load_state_dict( 48 | torch.load(args.checkpoint, map_location=torch.device('cpu'))) 49 | model.eval() 50 | 51 | Ts, gt_Ts, srcs = [], [], [] 52 | with torch.no_grad(): 53 | for pair_ind, inputs in enumerate(tqdm(test_dataloader)): 54 | if use_cuda: 55 | for k, v in inputs.items(): 56 | if isinstance(v, list): 57 | for i in range(len(v)): 58 | inputs[k][i] = inputs[k][i].cuda() 59 | else: 60 | inputs[k] = inputs[k].cuda() 61 | 62 | batched_feats_h, batched_feats_m, batched_feats_l = model(inputs) 63 | stack_points = inputs['points'] 64 | stack_lengths = inputs['stacked_lengths'] 65 | coords_src = stack_points[0][:stack_lengths[0][0]] 66 | coords_tgt = stack_points[0][stack_lengths[0][0]:] 67 | feats_src_h = batched_feats_h[:stack_lengths[0][0]] 68 | feats_tgt_h = batched_feats_h[stack_lengths[0][0]:] 69 | feats_src_m = batched_feats_m[:stack_lengths[0][0]] 70 | feats_tgt_m = batched_feats_m[stack_lengths[0][0]:] 71 | feats_src_l = batched_feats_l[:stack_lengths[0][0]] 72 | feats_tgt_l = batched_feats_l[stack_lengths[0][0]:] 73 | 74 | coors = inputs['coors'][0] # list, [coors1, coors2, ..], preparation for batchsize > 1 75 | transf = inputs['transf'][0] # (1, 4, 4), preparation for batchsize > 1 76 | 77 | coors = coors.detach().cpu().numpy() 78 | T = transf.detach().cpu().numpy() 79 | 80 | source_npy = coords_src.detach().cpu().numpy() 81 | target_npy = coords_tgt.detach().cpu().numpy() 82 | srcs.append(source_npy) 83 | 84 | source_npy_raw = copy.deepcopy(source_npy) 85 | target_npy_raw = copy.deepcopy(target_npy) 86 | source_feats_h = feats_src_h[:, :-2].detach().cpu().numpy() 87 | target_feats_h = feats_tgt_h[:, :-2].detach().cpu().numpy() 88 | source_feats_m = feats_src_m.detach().cpu().numpy() 89 | target_feats_m = feats_tgt_m.detach().cpu().numpy() 90 | source_feats_l = feats_src_l.detach().cpu().numpy() 91 | target_feats_l = feats_tgt_l.detach().cpu().numpy() 92 | 93 | source_overlap_scores = feats_src_h[:, -2].detach().cpu().numpy() 94 | target_overlap_scores = feats_tgt_h[:, -2].detach().cpu().numpy() 95 | source_saliency_scores = feats_src_h[:, -1].detach().cpu().numpy() 96 | target_saliency_scores = feats_tgt_h[:, -1].detach().cpu().numpy() 97 | 98 | source_scores = source_overlap_scores * source_saliency_scores 99 | target_scores = target_overlap_scores * target_saliency_scores 100 | 101 | npoints = args.npts 102 | if source_npy.shape[0] > npoints: 103 | p = source_scores / np.sum(source_scores) 104 | idx = np.random.choice(len(source_npy), size=npoints, replace=False, p=p) 105 | source_npy = source_npy[idx] 106 | source_feats_h = source_feats_h[idx] 107 | source_feats_m = source_feats_m[idx] 108 | source_feats_l = source_feats_l[idx] 109 | 110 | # if target_npy.shape[0] > npoints: 111 | # p = target_scores / np.sum(target_scores) 112 | # idx = np.random.choice(len(target_npy), size=npoints, replace=False, p=p) 113 | # target_npy = target_npy[idx] 114 | # target_feats_h = target_feats_h[idx] 115 | # target_feats_m = target_feats_m[idx] 116 | # target_feats_l = target_feats_l[idx] 117 | 118 | after_vote = vote(source_npy=source_npy, 119 | target_npy=target_npy, 120 | source_feats=[source_feats_h, source_feats_m, source_feats_l], 121 | target_feats=[target_feats_h, target_feats_m, target_feats_l], 122 | voxel_size=config.first_subsampling_dl, 123 | use_cuda=use_cuda) 124 | source_npy, target_npy, source_feats_npy, target_feats_npy = after_vote 125 | 126 | M = torch.cdist(to_tensor(source_feats_npy, use_cuda), to_tensor(target_feats_npy, use_cuda)) 127 | row_max_inds = torch.min(M, dim=-1)[1].cpu().numpy() 128 | col_max_inds = torch.min(M, dim=0)[1].cpu().numpy() 129 | 130 | source, target = npy2pcd(source_npy), npy2pcd(target_npy) 131 | 132 | source_feats, target_feats = npy2feat(source_feats_npy), npy2feat(target_feats_npy) 133 | pred_T, estimate = execute_global_registration(source=source, 134 | target=target, 135 | source_feats=source_feats, 136 | target_feats=target_feats, 137 | voxel_size=0.02) 138 | Ts.append(pred_T) 139 | gt_Ts.append(T) 140 | 141 | if args.vis: 142 | source_ply = npy2pcd(source_npy_raw) 143 | source_ply.paint_uniform_color(get_yellow()) 144 | estimate_ply = copy.deepcopy(source_ply).transform(pred_T) 145 | target_ply = npy2pcd(target_npy_raw) 146 | target_ply.paint_uniform_color(get_blue()) 147 | vis_plys([target_ply, estimate_ply], need_color=False) 148 | 149 | Ts, gt_Ts, srcs = np.array(Ts), np.array(gt_Ts), np.array(srcs) 150 | 151 | rot_error = Error_R(Ts[:, :3, :3], gt_Ts[:, :3, :3]) 152 | trans_error = Error_t(Ts[:, :3, 3], gt_Ts[:, :3, 3]) 153 | rmse = RMSE(srcs, Ts[:, :3, :3], gt_Ts[:, :3, :3], Ts[:, :3, 3], gt_Ts[:, :3, 3]) 154 | re = np.mean(rot_error) 155 | te = np.mean(trans_error) 156 | rmse = np.mean(rmse) 157 | 158 | print('Rotation error: ', fmat(re)) 159 | print('translation error: ', fmat(te)) 160 | print('RMSE: ', fmat(rmse)) 161 | 162 | 163 | if __name__ == '__main__': 164 | parser = argparse.ArgumentParser(description='Configuration Parameters') 165 | parser.add_argument('--npts', type=int, default=768, 166 | help='the number of sampled points for registration') 167 | parser.add_argument('--data_root', required=True, help='data root') 168 | parser.add_argument('--checkpoint', required=True, help='checkpoint path') 169 | parser.add_argument('--vis', action='store_true', 170 | help='whether for visualization') 171 | parser.add_argument('--no_cuda', action='store_true', 172 | help='whether to use cuda') 173 | args = parser.parse_args() 174 | main(args) 175 | -------------------------------------------------------------------------------- /losses/__init__.py: -------------------------------------------------------------------------------- 1 | from .loss import Loss 2 | -------------------------------------------------------------------------------- /losses/loss.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | import torch.nn as nn 4 | from utils import square_dists 5 | 6 | 7 | class Loss(nn.Module): 8 | def __init__(self, config): 9 | super().__init__() 10 | self.final_feats_dim = config.final_feats_dim 11 | self.n_samples = config.max_points 12 | self.pos_margin = config.pos_margin 13 | self.neg_margin = config.neg_margin 14 | self.pos_radius = config.pos_radius 15 | self.safe_radius = config.safe_radius 16 | self.matchability_radius = config.matchability_radius 17 | 18 | self.log_scale = config.log_scale 19 | self.w_circle_loss = config.w_circle_loss 20 | self.w_overlap_loss = config.w_overlap_loss 21 | # self.w_saliency_loss = config.w_saliency_loss 22 | 23 | self.soft_plus = nn.Softplus() 24 | self.bce_loss = nn.BCELoss(reduction='none') 25 | 26 | def circle_loss(self, coords_dist, feats_dist): 27 | ''' 28 | 29 | :param coors_dist: (n, m) 30 | :param feats_dist: (n, m) 31 | :return: 32 | ''' 33 | # set weights. 34 | # for positive pairs, if feats_dist < self.pos_margin, set weight=0, 35 | # else set weight=feats_dist - self.pos_margin 36 | pos_mask = coords_dist < self.pos_radius 37 | neg_mask = coords_dist > self.safe_radius 38 | pos_weight = pos_mask * torch.clamp(feats_dist - self.pos_margin, min=0) 39 | neg_weight = neg_mask * torch.clamp(self.neg_margin - feats_dist, min=0) 40 | pos_weight.detach_() 41 | neg_weight.detach_() 42 | 43 | pos_loss_row = torch.logsumexp(self.log_scale * pos_weight * (feats_dist - self.pos_margin), dim=1) 44 | neg_loss_row = torch.logsumexp(self.log_scale * neg_weight * (self.neg_margin - feats_dist), dim=1) 45 | pos_loss_col = torch.logsumexp(self.log_scale * pos_weight * (feats_dist - self.pos_margin), dim=0) 46 | neg_loss_col = torch.logsumexp(self.log_scale * neg_weight * (self.neg_margin - feats_dist), dim=0) 47 | 48 | loss_row = self.soft_plus(pos_loss_row + neg_loss_row) / self.log_scale 49 | loss_col = self.soft_plus(pos_loss_col + neg_loss_col) / self.log_scale 50 | rol_sel = (pos_mask.sum(1) > 0) * (neg_mask.sum(1) > 0) 51 | col_sel = (pos_mask.sum(0)) > 0 * (neg_mask.sum(0) > 0) 52 | return (loss_row[rol_sel].mean() + loss_col[col_sel].mean()) / 2 53 | 54 | def overlap_loss(self, ol_scores, ol_gt): 55 | ''' 56 | 57 | :param ol_scores: (N, ) or (M, ) 58 | :param ol_gt: (N, ) or (M, ) 59 | :return: 60 | ''' 61 | 62 | ol_loss = self.bce_loss(ol_scores, ol_gt) 63 | ratio = ol_gt.sum(0) / ol_gt.size(0) 64 | weights = torch.zeros_like(ol_gt).to(ol_gt) 65 | weights[ol_gt > 0.5] = 1 - ratio 66 | weights[ol_gt < 0.5] = ratio 67 | weighted_ol_loss = torch.mean(ol_loss * weights) 68 | return weighted_ol_loss 69 | 70 | def saliency_loss(self, saliency, saliency_gt): 71 | ''' 72 | 73 | :param saliency: (n, ) or (m, ) 74 | :param saliency_gt: (n, ) or (m, ) 75 | :return: 76 | ''' 77 | sa_loss = self.bce_loss(saliency, saliency_gt) 78 | ratio = saliency_gt.sum(0) / saliency_gt.size(0) 79 | weights = torch.zeros_like(saliency_gt).to(saliency_gt) 80 | weights[saliency_gt > 0.5] = 1 - ratio 81 | weights[saliency_gt < 0.5] = ratio 82 | weighted_sa_loss = torch.mean(sa_loss * weights) 83 | return weighted_sa_loss 84 | 85 | def get_recall(self, coords_dist, feats_dist): 86 | ''' 87 | used for updating saliency loss weight and selecting the best recall checkpoint. 88 | :param coors_dist: (n, m) 89 | :param feats_dist: (n, m) 90 | ''' 91 | pos_mask = coords_dist < self.pos_radius 92 | n_gt = torch.sum(torch.sum(pos_mask, dim=1) > 0) + 1e-8 93 | inds = torch.min(feats_dist, dim=1)[1] 94 | sel_dist = torch.gather(coords_dist, dim=-1, index=inds[:, None])[pos_mask.sum(1) > 0] 95 | n_pred = torch.sum(sel_dist < self.pos_radius) 96 | recall = n_pred / n_gt 97 | return recall 98 | 99 | def forward(self, coords_src, coords_tgt, feats_src, feats_tgt, feats_src_m, feats_tgt_m, feats_src_l, feats_tgt_l, coors, transf, w_saliency): 100 | ''' 101 | 102 | :param coords_src: (N, 3) 103 | :param coords_tgt: (M, 3) 104 | :param feats_src: (N, C) 105 | :param feats_tgt: (M, C) 106 | :param coors: (L, 2) 107 | :param transf: (4, 4) 108 | :return: 109 | ''' 110 | loss_dict = {} 111 | # 0. output parsing 112 | ol_scores_src, ol_scores_tgt = feats_src[:, -2], feats_tgt[:, -2] 113 | saliency_src, saliency_tgt = feats_src[:, -1], feats_tgt[:, -1] 114 | feats_src, feats_tgt = feats_src[:, :-2], feats_tgt[:, :-2] 115 | 116 | R, t = transf[:3, :3], transf[:3, 3:] 117 | coords_src = (R @ coords_src.transpose(0, 1) + t).transpose(0, 1) 118 | 119 | # there are lots of repetitive indices, such as (5, 10), (5, 11), (5, 13) ... 120 | # we need to implement deduplication operations. important for memory! 121 | # tolist() is a good function. 122 | inds_src, inds_tgt = list(set(coors[:, 0].int().tolist())), \ 123 | list(set(coors[:, 1].int().tolist())) 124 | 125 | # 1. overlap loss 126 | ol_gt_src = torch.zeros(coords_src.size(0), dtype=torch.float32).to(coords_src) 127 | ol_gt_tgt = torch.zeros(coords_tgt.size(0), dtype=torch.float32).to(coords_tgt) 128 | ol_gt_src[inds_src] = 1 129 | ol_gt_tgt[inds_tgt] = 1 130 | overlap_loss_v = 0.5 * self.overlap_loss(ol_scores_src, ol_gt_src) + \ 131 | 0.5 * self.overlap_loss(ol_scores_tgt, ol_gt_tgt) 132 | loss_dict['overlap_loss'] = overlap_loss_v 133 | 134 | # 2. saliency loss (based overlapping points) 135 | coords_src_sel, coords_tgt_sel = coords_src[inds_src], coords_tgt[inds_tgt] 136 | feats_src_sel, feats_tgt_sel = feats_src[inds_src], feats_tgt[inds_tgt] 137 | feats_dist = torch.matmul(feats_src_sel, feats_tgt_sel.transpose(0, 1)) # (n, m) 138 | inds1 = torch.max(feats_dist, dim=1)[1] 139 | dists1 = torch.norm(coords_src_sel - coords_tgt_sel[inds1], dim=1) # (n, ) 140 | inds2 = torch.max(feats_dist, dim=0)[1] 141 | dists2 = torch.norm(coords_tgt_sel - coords_src_sel[inds2], dim=1) # (m, ) 142 | saliency_src_sel = saliency_src[inds_src] 143 | saliency_tgt_sel = saliency_tgt[inds_tgt] 144 | saliency_loss_v = 0.5 * self.saliency_loss(saliency_src_sel, (dists1 < self.matchability_radius).float()) + \ 145 | 0.5 * self.saliency_loss(saliency_tgt_sel, (dists2 < self.matchability_radius).float()) 146 | loss_dict['saliency_loss'] = saliency_loss_v 147 | 148 | # 3. descriptor loss 149 | # select n_samples (which matches each other) points for feature description loss 150 | # based on overlapping points, we filter some points. 151 | # because the values of overlap_radius and pos_radius may be different. 152 | inds = torch.norm(coords_src[coors[:, 0]] - coords_tgt[coors[:, 1]], dim=1) < self.pos_radius - 0.001 153 | coors = coors[inds] 154 | 155 | if coors.size(0) > self.n_samples: 156 | inds = np.random.choice(coors.size(0), self.n_samples, replace=False) 157 | coors = coors[inds] 158 | 159 | # there may be some repeated points in source point cloud. 160 | # we need to keep valid pairs. 161 | inds_src, inds_tgt = coors[:, 0], coors[:, 1] 162 | coords_src, coords_tgt = coords_src[inds_src], coords_tgt[inds_tgt] 163 | feats_src, feats_tgt = feats_src[inds_src], feats_tgt[inds_tgt] 164 | feats_src_m, feats_tgt_m = feats_src_m[inds_src], feats_tgt_m[inds_tgt] 165 | feats_src_l, feats_tgt_l = feats_src_l[inds_src], feats_tgt_l[inds_tgt] 166 | coords_dist = torch.sqrt(square_dists(coords_src[None, :, :], coords_tgt[None, :, :]).squeeze(0)) 167 | feats_dist = torch.sqrt(square_dists(feats_src[None, :, :], feats_tgt[None, :, :]).squeeze(0)) 168 | feats_dist_m = torch.sqrt(square_dists(feats_src_m[None, :, :], feats_tgt_m[None, :, :]).squeeze(0)) 169 | feats_dist_l = torch.sqrt(square_dists(feats_src_l[None, :, :], feats_tgt_l[None, :, :]).squeeze(0)) 170 | 171 | 172 | circle_loss_v = self.circle_loss(coords_dist=coords_dist, 173 | feats_dist=feats_dist) 174 | circle_loss_vm = self.circle_loss(coords_dist=coords_dist, 175 | feats_dist=feats_dist_m) 176 | circle_loss_vl = self.circle_loss(coords_dist=coords_dist, 177 | feats_dist=feats_dist_l) 178 | recall = self.get_recall(coords_dist=coords_dist, feats_dist=feats_dist) 179 | recall_m = self.get_recall(coords_dist=coords_dist, feats_dist=feats_dist_m) 180 | recall_l = self.get_recall(coords_dist=coords_dist, feats_dist=feats_dist_l) 181 | loss_dict['circle_loss'] = circle_loss_v 182 | loss_dict['circle_loss_m'] = circle_loss_vm 183 | loss_dict['circle_loss_l'] = circle_loss_vl 184 | loss_dict['recall'] = recall 185 | loss_dict['recall_m'] = recall_m 186 | loss_dict['recall_l'] = recall_l 187 | 188 | loss = self.w_circle_loss * circle_loss_v + \ 189 | self.w_circle_loss * circle_loss_vm + \ 190 | self.w_circle_loss * circle_loss_vl + \ 191 | self.w_overlap_loss * overlap_loss_v + \ 192 | w_saliency * saliency_loss_v 193 | loss_dict['total_loss'] = loss 194 | 195 | return loss_dict 196 | -------------------------------------------------------------------------------- /metrics/__init__.py: -------------------------------------------------------------------------------- 1 | from .threedmatch import inlier_ratio_core, registration_recall_core, Metric 2 | from .kitti import Error_R, Error_t 3 | from .mvp_rg import RMSE 4 | -------------------------------------------------------------------------------- /metrics/kitti.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | 5 | def Error_R(r1, r2): 6 | ''' 7 | Calculate isotropic rotation degree error between r1 and r2. 8 | :param r1: shape=(B, 3, 3), pred 9 | :param r2: shape=(B, 3, 3), gt 10 | :return: 11 | ''' 12 | r2_inv = r2.transpose(0, 2, 1) 13 | r1r2 = np.matmul(r2_inv, r1) 14 | tr = r1r2[:, 0, 0] + r1r2[:, 1, 1] + r1r2[:, 2, 2] 15 | rads = np.arccos(np.clip((tr - 1) / 2, -1, 1)) 16 | degrees = rads / math.pi * 180 17 | return degrees 18 | 19 | 20 | def Error_t(t1, t2): 21 | ''' 22 | calculate translation mse error. 23 | :param t1: shape=(B, 3) 24 | :param t2: shape=(B, 3) 25 | :return: 26 | ''' 27 | assert t1.shape == t2.shape 28 | error_t = np.sqrt(np.sum((t1 - t2) ** 2, axis=1)) 29 | return error_t 30 | -------------------------------------------------------------------------------- /metrics/mvp_rg.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | 5 | def transform(src, r, t): 6 | r = r.transpose(0, 2, 1) 7 | res = src @ r + t[:, None, :] 8 | return res 9 | 10 | 11 | def RMSE(src, r1, r2, t1, t2): 12 | ''' 13 | calculate rmse. 14 | :param src: shape=(B, n, 3), point clouds 15 | :param r1: shape=(B, 3, 3), pred 16 | :param r2: shape=(B, 3, 3), gt 17 | :param t1: shape=(B, 3), pred 18 | :param t2: shape=(B, 3), gt 19 | :return: 20 | ''' 21 | tgt1 = transform(src, r1, t1) 22 | tgt2 = transform(src, r2, t2) 23 | rmse = np.sum(np.sqrt(np.sum((tgt1 - tgt2) ** 2, axis=-1)), axis=-1) / src.shape[1] 24 | return rmse 25 | -------------------------------------------------------------------------------- /metrics/threedmatch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | import nibabel.quaternions as nq 4 | 5 | from metrics.kitti import Error_R, Error_t 6 | 7 | 8 | def inlier_ratio_core(points_src, points_tgt, row_max_inds, col_max_inds, transf, inlier_threshold=0.1): 9 | 10 | R, t = transf[:3, :3], transf[:3, 3] 11 | 12 | points_src = points_src @ R.T + t 13 | inlier_mask = np.sum((points_src - points_tgt[row_max_inds]) ** 2, axis=1) < inlier_threshold ** 2 14 | inlier_ratio = np.sum(inlier_mask) / len(inlier_mask) 15 | 16 | # mutual inlier ratio 17 | mutual_corrs = [] 18 | for i in range(len(points_src)): 19 | if col_max_inds[row_max_inds[i]] == i: 20 | mutual_corrs.append([i, row_max_inds[i]]) 21 | mutual_corrs = np.array(mutual_corrs, dtype=np.int) 22 | mutual_mask = np.sum((points_src[mutual_corrs[:, 0]] - points_tgt[mutual_corrs[:, 1]]) ** 2, axis=1) < inlier_threshold ** 2 23 | mutual_inlier_ratio = np.sum(mutual_mask) / len(mutual_corrs) 24 | 25 | return inlier_ratio, mutual_inlier_ratio 26 | 27 | 28 | def registration_recall_core(points_src, points_tgt, gt_corrs, pred_T): 29 | ''' 30 | 31 | :param points_src: (n, 3) 32 | :param points_tgt: (m, 3) 33 | :param gt_corrs: (n1, 2) 34 | :param pred_T: (4, 4) 35 | :return: float 36 | ''' 37 | points_src = points_src[gt_corrs[:, 0]] 38 | points_tgt = points_tgt[gt_corrs[:, 1]] 39 | R, t = pred_T[:3, :3], pred_T[:3, 3] 40 | points_src = points_src @ R.T + t 41 | mse = np.mean(np.sum((points_src - points_tgt) ** 2, axis=1)) 42 | rmse = np.sqrt(mse) 43 | 44 | return rmse 45 | 46 | 47 | class Metric(object): 48 | 49 | def __init__(self): 50 | self.err2 = 0.2 ** 2 51 | self.re_thre = 15 52 | self.te_thre = 30 53 | 54 | def benchmark(self, est_folder, gt_folder='configs/benchmarks/3DMatch'): 55 | scenes = sorted(os.listdir(gt_folder)) 56 | 57 | n_valids, n_totals = [], [] 58 | predator_style_recall_per_scene, dsc_style_recall_per_scene = [], [] 59 | error_rs, error_ts = [], [] 60 | dsc_error_rs, dsc_error_ts = [], [] 61 | error_rs_all, error_ts_all = [], [] 62 | for scene_i, scene in enumerate(scenes): 63 | est_pairs, est_traj = self.read_trajectory(os.path.join(est_folder, scene, 'est.log')) 64 | gt_pairs, gt_traj = self.read_trajectory(os.path.join(gt_folder, scene, 'gt.log')) 65 | n_fragments, gt_traj_cov = self.read_trajectory_info(os.path.join(gt_folder, scene, "gt.info")) 66 | 67 | n_valid = 0 68 | for ele in gt_pairs: 69 | diff = abs(int(ele[0]) - int(ele[1])) 70 | n_valid += diff > 1 71 | n_valids.append(n_valid) 72 | n_totals.append(len(est_traj)) 73 | 74 | predator_recall, dsc_recall, valid_num, error_r, error_t, dsc_error_r, dsc_error_t, error_r_all, error_t_all = \ 75 | self.evaluate_both_recall(est_pairs, gt_pairs, est_traj, gt_traj, n_fragments, gt_traj_cov) 76 | assert valid_num == n_valids[scene_i] 77 | predator_style_recall_per_scene.append(predator_recall / valid_num) 78 | dsc_style_recall_per_scene.append(dsc_recall / len(est_traj)) 79 | error_rs.append(error_r) 80 | error_ts.append(error_t) 81 | dsc_error_rs.append(dsc_error_r) 82 | dsc_error_ts.append(dsc_error_t) 83 | error_rs_all.append(error_r_all) 84 | error_ts_all.append(error_t_all) 85 | 86 | return np.array(predator_style_recall_per_scene), \ 87 | error_rs, \ 88 | error_ts, \ 89 | np.array(dsc_style_recall_per_scene), \ 90 | dsc_error_rs, \ 91 | dsc_error_ts, \ 92 | error_rs_all, \ 93 | error_ts_all, \ 94 | np.array(n_valids), \ 95 | np.array(n_totals) 96 | 97 | def evaluate_both_recall(self, est_pairs, gt_pairs, est_traj, gt_traj, n_fragments, gt_traj_cov): 98 | assert (gt_pairs == est_pairs).all() 99 | 100 | predator_recall, dsc_recall = 0, 0 101 | valid = 0 102 | flags, flags_dsc = [], [] 103 | for i in range(len(est_pairs)): 104 | ind_i, ind_j, _ = est_pairs[i] 105 | this_gt, this_pred, this_info = gt_traj[i], est_traj[i], gt_traj_cov[i] 106 | if self.dsc_style_recall(this_pred, this_gt): 107 | dsc_recall += 1 108 | flags_dsc.append(0) 109 | else: 110 | flags_dsc.append(1) 111 | if int(ind_j) - int(ind_i) > 1: 112 | valid += 1 113 | if self.predator_style_recall(this_pred, this_gt, this_info): 114 | predator_recall += 1 115 | flags.append(0) 116 | else: 117 | flags.append(1) 118 | else: 119 | flags.append(2) 120 | 121 | error_rs = Error_R(est_traj[:, :3, :3], gt_traj[:, :3, :3])[np.array(flags) == 0] 122 | error_ts = Error_t(est_traj[:, :3, 3], gt_traj[:, :3, 3])[np.array(flags) == 0] 123 | dsc_error_rs = Error_R(est_traj[:, :3, :3], gt_traj[:, :3, :3])[np.array(flags_dsc) == 0] 124 | dsc_error_ts = Error_t(est_traj[:, :3, 3], gt_traj[:, :3, 3])[np.array(flags_dsc) == 0] 125 | error_rs_all = Error_R(est_traj[:, :3, :3], gt_traj[:, :3, :3]) 126 | error_ts_all = Error_t(est_traj[:, :3, 3], gt_traj[:, :3, 3]) 127 | 128 | return predator_recall, dsc_recall, valid, error_rs, error_ts, \ 129 | dsc_error_rs, dsc_error_ts, error_rs_all, error_ts_all 130 | 131 | def dsc_style_recall(self, pred, gt): 132 | pred_R, pred_t = self.decompose_trans(pred) 133 | gt_R, gt_t = self.decompose_trans(gt) 134 | re = np.arccos(np.clip((np.trace(pred_R.T @ gt_R) - 1) / 2.0, -1, 1)) 135 | te = np.sqrt(np.sum((pred_t - gt_t) ** 2)) 136 | re = re * 180 / np.pi 137 | te = te * 100 138 | return bool(re < self.re_thre and te < self.te_thre) 139 | 140 | def predator_style_recall(self, pred, gt, info): 141 | 142 | p = self.computeTransformationErr(np.linalg.inv(gt) @ pred, info) 143 | return p <= self.err2 144 | 145 | def computeTransformationErr(self, trans, info): 146 | 147 | t = trans[:3, 3] 148 | r = trans[:3, :3] 149 | q = nq.mat2quat(r) 150 | er = np.concatenate([t, q[1:]], axis=0) 151 | p = er.reshape(1, 6) @ info @ er.reshape(6, 1) / info[0, 0] 152 | return p.item() 153 | 154 | def decompose_trans(self, trans): 155 | 156 | if len(trans.shape) == 3: 157 | return trans[:, :3, :3], trans[:, :3, 3:4] 158 | else: 159 | return trans[:3, :3], trans[:3, 3:4] 160 | 161 | def read_trajectory(self, filename, dim=4): 162 | with open(filename) as f: 163 | lines = f.readlines() 164 | 165 | # Extract the point cloud pairs 166 | keys = lines[0::(dim + 1)] 167 | temp_keys = [] 168 | for i in range(len(keys)): 169 | temp_keys.append(keys[i].split('\t')[0:3]) 170 | 171 | final_keys = [] 172 | for i in range(len(temp_keys)): 173 | final_keys.append([temp_keys[i][0].strip(), temp_keys[i][1].strip(), temp_keys[i][2].strip()]) 174 | 175 | traj = [] 176 | for i in range(len(lines)): 177 | if i % 5 != 0: 178 | traj.append(lines[i].split('\t')[0:dim]) 179 | 180 | traj = np.asarray(traj, dtype=np.float).reshape(-1, dim, dim) 181 | final_keys = np.asarray(final_keys) 182 | 183 | return final_keys, traj 184 | 185 | def read_trajectory_info(self, filename, dim=6): 186 | 187 | with open(filename) as fid: 188 | contents = fid.readlines() 189 | n_pairs = len(contents) // 7 190 | assert (len(contents) == 7 * n_pairs) 191 | info_list = [] 192 | n_frame = 0 193 | 194 | for i in range(n_pairs): 195 | frame_idx0, frame_idx1, n_frame = [int(item) for item in contents[i * 7].strip().split()] 196 | info_matrix = np.concatenate( 197 | [np.fromstring(item, sep='\t').reshape(1, -1) for item in contents[i * 7 + 1:i * 7 + 7]], axis=0) 198 | info_list.append(info_matrix) 199 | 200 | cov_matrix = np.asarray(info_list, dtype=np.float).reshape(-1, dim, dim) 201 | return n_frame, cov_matrix 202 | -------------------------------------------------------------------------------- /misc/plot_fmr.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | 5 | linestyle_str = [ 6 | ('solid', 'solid'), # Same as (0, ()) or '-' 7 | ('dotted', 'dotted'), # Same as (0, (1, 1)) or '.' 8 | ('dashed', 'dashed'), # Same as '--' 9 | ('dashdot', 'dashdot')] # Same as '-.' 10 | 11 | linestyle_tuple = [ 12 | ('loosely dotted', (0, (1, 10))), 13 | ('dotted', (0, (1, 1))), 14 | ('densely dotted', (0, (1, 1))), 15 | 16 | ('loosely dashed', (0, (5, 10))), 17 | ('dashed', (0, (5, 5))), 18 | ('densely dashed', (0, (5, 1))), 19 | 20 | ('loosely dashdotted', (0, (3, 10, 1, 10))), 21 | ('dashdotted', (0, (3, 5, 1, 5))), 22 | ('densely dashdotted', (0, (3, 1, 1, 1))), 23 | 24 | ('dashdotdotted', (0, (3, 5, 1, 5, 1, 5))), 25 | ('loosely dashdotdotted', (0, (3, 10, 1, 10, 1, 10))), 26 | ('densely dashdotdotted', (0, (3, 1, 1, 1, 1, 1))), 27 | ('extra1', (0, (4, 1, 2, 1)))] 28 | 29 | x = np.linspace(0.01, 0.2, 20) 30 | threedmatch = np.array( 31 | [0.34988788, 1.735477, 6.3130245, 13.2243185, 20.68582, 28.49041, 34.01513, 40.062992, 45.687683, 52.00012, 32 | 55.463696, 59.081406, 61.986275, 65.45967, 68.43626, 70.95725, 73.55156, 76.07139, 77.85846, 80.27285]) 33 | fpfh = np.array( 34 | [0.32518435, 3.0640497, 7.408897, 12.879427, 17.654156, 21.894594, 24.814766, 28.676664, 32.31617, 35.307827, 35 | 38.895893, 41.363243, 44.120644, 47.001804, 49.11531, 52.33542, 55.02593, 56.853123, 59.70479, 61.781036]) 36 | spinimages = np.array( 37 | [0.2650882, 1.4736623, 3.4342945, 5.703008, 8.022694, 9.878324, 12.34205, 15.342176, 18.735947, 21.89705, 24.729364, 38 | 28.054333, 31.125975, 33.943203, 36.965153, 40.642887, 43.33119, 46.57485, 49.260128, 52.152588]) 39 | shot = np.array( 40 | [0.47008017, 2.6745644, 5.1028447, 7.4799767, 9.238545, 11.927085, 16.089037, 18.879076, 22.094217, 24.974531, 41 | 27.351418, 29.76674, 32.573856, 34.916573, 37.086315, 40.306644, 43.138355, 45.73636, 48.786892, 50.972153]) 42 | predator = np.array( 43 | [1.598646, 43.764183, 72.060312, 84.277961, 90.004648, 92.64463, 94.014723, 94.990602, 95.96624, 96.601441, 44 | 96.806433, 97.099785, 97.35597, 97.35597, 97.380674, 97.380674, 97.611186, 97.90688, 98.178721, 98.238818]) 45 | fcgf = np.array( 46 | [4.135307, 50.302743, 76.513382, 87.786066, 92.033676, 93.6785, 94.900559, 96.311261, 96.912816, 97.353521, 47 | 97.738293, 98.016699, 98.573519, 98.633615, 98.676423, 98.731733, 98.854555, 98.854555, 98.854555, 98.99945]) 48 | d3feat = np.array( 49 | [1.326839, 31.021349, 61.608349, 75.575873, 82.991841, 88.585381, 91.007284, 92.794678, 94.556689, 95.631483, 50 | 96.580489, 96.768193, 96.8176, 96.920505, 97.085318, 97.128126, 97.695815, 97.816008, 97.985607, 97.985607]) 51 | dsn = np.array( 52 | [6.925493, 42.819659, 65.908609, 78.549499, 85.100889, 88.986995, 91.870439, 93.402519, 94.398583, 95.019629, 53 | 95.537113, 95.72003, 95.900319, 96.135917, 96.358351, 96.631008, 96.831329, 96.911342, 97.031535, 97.249845]) 54 | ppffold = np.array( 55 | [0.6783977, 7.9283724, 22.183943, 34.720436, 44.628433, 51.92579, 57.17631, 61.85746, 66.01428, 69.106926, 71.37115, 56 | 73.30209, 74.196884, 75.28364, 76.539635, 77.94661, 79.70165, 80.93724, 81.7935, 82.764626]) 57 | cofinet = np.array( 58 | [ 0. , 6.1493262, 61.6405896, 84.110013 , 91.1524802, 94.0575963, 95.6404122, 96.636305 , 97.5596297, 98.0637688, 59 | 98.5893126, 98.6494088, 98.7951213, 98.8980257, 99.018218 , 99.2184236, 99.2184236, 99.2184236, 99.2184236, 99.2184236]) 60 | ngenet = np.array([ 2.2959765, 52.5827081, 81.422583 , 91.0088155, 94.1351968, 96.3979454, 97.4483312, 97.4531176, 98.103794 , 98.2191998, 61 | 98.6839846, 98.6123493, 98.8222816, 98.8897934, 99.1328072, 99.2029295, 99.2054049, 99.2523366, 99.4286554, 99.4352543]) 62 | 63 | fig = plt.figure(figsize=(12, 6), dpi=180) 64 | ax1 = plt.subplot(121) 65 | plt.xlabel("Inlier Distance Threshold (m)") 66 | plt.ylabel("Feature Match Recall") 67 | plt.plot(x, threedmatch, color='r', linewidth=3, linestyle='dotted') 68 | plt.plot(x, fpfh, color='sienna', linewidth=3, linestyle='dashed') 69 | plt.plot(x, spinimages, color='darkorange', linewidth=3, linestyle=linestyle_tuple[11][-1]) 70 | plt.plot(x, shot, color='darkgoldenrod', linewidth=3, linestyle=linestyle_tuple[8][-1]) 71 | plt.plot(x, predator, color='olive', linewidth=3, linestyle=linestyle_tuple[1][-1]) 72 | plt.plot(x, fcgf, color='lawngreen', linewidth=3, linestyle=linestyle_tuple[9][-1]) 73 | plt.plot(x, d3feat, color='lightcoral', linewidth=3, linestyle=linestyle_tuple[12][-1]) 74 | plt.plot(x, dsn, color='lightseagreen', linewidth=3, linestyle=linestyle_tuple[4][-1]) 75 | plt.plot(x, ppffold, color='royalblue', linewidth=3, linestyle=linestyle_tuple[5][-1]) 76 | plt.plot(x, cofinet, color='tomato', linewidth=3, linestyle='dashdot') 77 | plt.plot(x, ngenet, color='magenta', linewidth=3, linestyle='solid') 78 | plt.vlines(0.10, 0, 100, colors="gray", linestyles="dashed") # vertical gray line 79 | plt.xlim((0.01, 0.2)) 80 | plt.ylim((0, 100)) 81 | 82 | ax2 = plt.subplot(122) 83 | plt.xlabel("Inlier Ratio Threshold") 84 | threedmatch_ = np.array( 85 | [93.57423, 84.989296, 72.26616, 60.18662, 50.84514, 40.988052, 35.264496, 29.492676, 24.875406, 20.927317, 86 | 18.053923, 14.7144, 12.210866, 10.735968, 9.550746, 8.107913, 7.4702764, 6.172435, 5.2574573, 4.3098807]) 87 | fpfh_ = np.array( 88 | [84.33316, 68.71096, 54.144688, 43.4266, 35.885315, 29.24774, 24.669365, 21.693983, 18.398415, 15.19658, 13.052581, 89 | 10.593815, 9.197111, 7.121167, 6.0142927, 5.459413, 4.7025414, 3.7748396, 3.2582376, 2.9224794]) 90 | spinimages_ = np.array( 91 | [80.370415, 57.09011, 41.317715, 30.377916, 22.666761, 16.668486, 12.008004, 9.49888, 6.9450083, 5.2102103, 92 | 4.6000204, 3.9652426, 3.6801224, 2.8298047, 2.4588833, 1.6771933, 1.4722013, 1.3379945, 1.1484778, 0.9681894]) 93 | shot_ = np.array( 94 | [74.7338, 55.754692, 41.421146, 31.38755, 23.82493, 18.377901, 14.077372, 11.229745, 9.498036, 7.2400875, 6.259093, 95 | 5.4715004, 4.264607, 3.6964087, 3.070859, 2.418819, 2.0778346, 1.5370839, 1.5123804, 1.4522841]) 96 | predator_ = np.array( 97 | [98.859889, 97.909077, 97.500866, 97.125458, 96.601441, 95.734758, 94.609147, 94.146195, 93.172484, 92.713764, 98 | 92.183001, 91.71345, 91.044471, 90.592342, 90.185631, 89.322503, 88.736845, 88.36471, 87.145778, 86.287232]) 99 | fcgf_ = np.array( 100 | [99.526474, 98.940171, 98.226632, 97.884006, 97.353521, 96.627542, 96.318829, 96.029037, 95.339938, 94.679256, 101 | 93.99646, 93.517994, 92.618115, 92.43327, 92.128233, 91.267739, 90.514984, 90.144248, 89.87309, 89.340131]) 102 | d3feat_ = np.array( 103 | [98.452534, 97.660423, 97.020136, 96.768193, 95.631483, 94.327522, 92.314021, 90.721366, 89.756974, 88.743718, 104 | 88.126642, 87.331697, 85.993739, 84.35534, 83.309026, 82.154434, 80.440724, 79.07386, 77.657352, 75.881642]) 105 | dsn_ = np.array( 106 | [99.053496, 97.806905, 97.138431, 96.063319, 95.019629, 93.43627, 92.676226, 91.676057, 89.926433, 88.093154, 86.428362, 107 | 84.396438, 83.500374, 82.442937, 80.94519, 78.946443, 77.480061, 75.891795, 74.209464, 72.884469]) 108 | ppffold_ = np.array( 109 | [90.44259, 83.66921, 77.68714, 72.016045, 68.04171, 62.860317, 58.093655, 54.45246, 50.859642, 47.77691, 45.581078, 110 | 42.854107, 39.8563, 36.73241, 33.889057, 32.212887, 30.558687, 28.446907, 27.44521, 25.17946]) 111 | cofinet_ = np.array( 112 | [99.5437618, 99.1583275, 98.674929 , 98.6321208, 98.0637688, 97.5596297, 97.1585488, 96.7813547, 95.9402301, 95.4706589, 113 | 94.8710045, 94.4715556, 93.8568359, 93.4439539, 92.976464, 92.5283043, 91.9945671, 91.2894446, 90.9440743, 90.36093]) 114 | ngenet_ = np.array( 115 | [99.6064872, 99.2136372, 99.1454626, 98.6791982, 98.4062411, 98.1716052, 98.1335834, 97.4019246, 96.8714405, 96.3678561, 116 | 96.2485274, 95.7483501, 95.5010978, 95.2512231, 95.0998699, 94.3443617, 94.1912335, 93.9154229, 93.2886161, 93.0445291]) 117 | 118 | plt.plot(x, threedmatch_, color='r', linewidth=3, label='3DMatch', linestyle='dotted') 119 | plt.plot(x, fpfh_, color='sienna', linewidth=3, label='FPFH', linestyle='dashed') 120 | plt.plot(x, spinimages_, color='darkorange', linewidth=3, label='SpinImages', linestyle=linestyle_tuple[11][-1]) 121 | plt.plot(x, shot_, color='darkgoldenrod', linewidth=3, label='SHOT', linestyle=linestyle_tuple[8][-1]) 122 | plt.plot(x, predator_, color='olive', linewidth=3, label='PREDATOR', linestyle=linestyle_tuple[1][-1]) 123 | plt.plot(x, fcgf_, color='lawngreen', linewidth=3, label='FCGF', linestyle=linestyle_tuple[2][-1]) 124 | plt.plot(x, d3feat_, color='lightcoral', linewidth=3, label='D3Feat', linestyle=linestyle_tuple[12][-1]) 125 | plt.plot(x, dsn_, color='lightseagreen', linewidth=3, label='3DSN', linestyle=linestyle_tuple[4][-1]) 126 | plt.plot(x, ppffold_, color='royalblue', linewidth=3, label='PPF-FoldNet', linestyle=linestyle_tuple[5][-1]) 127 | plt.plot(x, cofinet_, color='tomato', linewidth=3, label='CoFiNet', linestyle='dashdot') 128 | plt.plot(x, ngenet_, color='magenta', linewidth=3, label='NgeNet', linestyle='solid') 129 | plt.vlines(0.05, 0, 100, colors="gray", linestyles="dashed") 130 | plt.xlim((0.01, 0.2)) 131 | plt.ylim((0, 100)) 132 | 133 | plt.legend(bbox_to_anchor=(1.05, 0.28, 0.32, 0.5), loc=4, 134 | ncol=1, mode="expand", borderaxespad=0.) 135 | 136 | fig.savefig('fig.pdf', dpi=1200, format='pdf', bbox_inches='tight') 137 | 138 | plt.show() 139 | -------------------------------------------------------------------------------- /models/KPConv/__init__.py: -------------------------------------------------------------------------------- 1 | from .kernel_points import load_kernels 2 | from .blocks import block_decider 3 | -------------------------------------------------------------------------------- /models/KPConv/kernel_points.py: -------------------------------------------------------------------------------- 1 | # 2 | # 3 | # 0=================================0 4 | # | Kernel Point Convolutions | 5 | # 0=================================0 6 | # 7 | # Simplified from https://github.com/HuguesTHOMAS/KPConv-PyTorch/blob/master/kernels/kernel_points.py 8 | 9 | import numpy as np 10 | import os 11 | import open3d as o3d 12 | CUR = os.path.dirname(os.path.abspath(__file__)) 13 | 14 | 15 | def create_3D_rotations(axis, angle): 16 | """ 17 | Create rotation matrices from a list of axes and angles. Code from wikipedia on quaternions 18 | :param axis: float32[N, 3] 19 | :param angle: float32[N,] 20 | :return: float32[N, 3, 3] 21 | """ 22 | 23 | t1 = np.cos(angle) 24 | t2 = 1 - t1 25 | t3 = axis[:, 0] * axis[:, 0] 26 | t6 = t2 * axis[:, 0] 27 | t7 = t6 * axis[:, 1] 28 | t8 = np.sin(angle) 29 | t9 = t8 * axis[:, 2] 30 | t11 = t6 * axis[:, 2] 31 | t12 = t8 * axis[:, 1] 32 | t15 = axis[:, 1] * axis[:, 1] 33 | t19 = t2 * axis[:, 1] * axis[:, 2] 34 | t20 = t8 * axis[:, 0] 35 | t24 = axis[:, 2] * axis[:, 2] 36 | R = np.stack([t1 + t2 * t3, 37 | t7 - t9, 38 | t11 + t12, 39 | t7 + t9, 40 | t1 + t2 * t15, 41 | t19 - t20, 42 | t11 - t12, 43 | t19 + t20, 44 | t1 + t2 * t24], axis=1) 45 | 46 | return np.reshape(R, (-1, 3, 3)) 47 | 48 | 49 | def kernel_point_optimization_debug(radius, num_points, num_kernels=1, dimension=3, 50 | fixed='center', ratio=0.66): 51 | """ 52 | Creation of kernel point via optimization of potentials. 53 | :param radius: Radius of the kernels 54 | :param num_points: points composing kernels 55 | :param num_kernels: number of wanted kernels 56 | :param dimension: dimension of the space 57 | :param fixed: fix position of certain kernel points ('none', 'center' or 'verticals') 58 | :param ratio: ratio of the radius where you want the kernels points to be placed 59 | :return: points [num_kernels, num_points, dimension] 60 | """ 61 | 62 | ####################### 63 | # Parameters definition 64 | ####################### 65 | 66 | # Radius used for optimization (points are rescaled afterwards) 67 | radius0 = 1 68 | diameter0 = 2 69 | 70 | # Factor multiplicating gradients for moving points (~learning rate) 71 | moving_factor = 1e-2 72 | continuous_moving_decay = 0.9995 73 | 74 | # Gradient threshold to stop optimization 75 | thresh = 1e-5 76 | 77 | # Gradient clipping value 78 | clip = 0.05 * radius0 79 | 80 | ####################### 81 | # Kernel initialization 82 | ####################### 83 | 84 | # Random kernel points 85 | kernel_points = np.random.rand(num_kernels * num_points - 1, dimension) * diameter0 - radius0 86 | while (kernel_points.shape[0] < num_kernels * num_points): 87 | new_points = np.random.rand(num_kernels * num_points - 1, dimension) * diameter0 - radius0 88 | kernel_points = np.vstack((kernel_points, new_points)) 89 | d2 = np.sum(np.power(kernel_points, 2), axis=1) 90 | kernel_points = kernel_points[d2 < 0.5 * radius0 * radius0, :] 91 | kernel_points = kernel_points[:num_kernels * num_points, :].reshape((num_kernels, num_points, -1)) 92 | 93 | # Optionnal fixing 94 | if fixed == 'center': 95 | kernel_points[:, 0, :] *= 0 96 | if fixed == 'verticals': 97 | kernel_points[:, :3, :] *= 0 98 | kernel_points[:, 1, -1] += 2 * radius0 / 3 99 | kernel_points[:, 2, -1] -= 2 * radius0 / 3 100 | 101 | ##################### 102 | # Kernel optimization 103 | ##################### 104 | 105 | saved_gradient_norms = np.zeros((10000, num_kernels)) 106 | old_gradient_norms = np.zeros((num_kernels, num_points)) 107 | step = -1 108 | while step < 10000: 109 | 110 | # Increment 111 | step += 1 112 | 113 | # Compute gradients 114 | # ***************** 115 | 116 | # Derivative of the sum of potentials of all points 117 | A = np.expand_dims(kernel_points, axis=2) 118 | B = np.expand_dims(kernel_points, axis=1) 119 | interd2 = np.sum(np.power(A - B, 2), axis=-1) 120 | inter_grads = (A - B) / (np.power(np.expand_dims(interd2, -1), 3/2) + 1e-6) 121 | inter_grads = np.sum(inter_grads, axis=1) 122 | 123 | # Derivative of the radius potential 124 | circle_grads = 10*kernel_points 125 | 126 | # All gradients 127 | gradients = inter_grads + circle_grads 128 | 129 | if fixed == 'verticals': 130 | gradients[:, 1:3, :-1] = 0 131 | 132 | # Stop condition 133 | # ************** 134 | 135 | # Compute norm of gradients 136 | gradients_norms = np.sqrt(np.sum(np.power(gradients, 2), axis=-1)) 137 | saved_gradient_norms[step, :] = np.max(gradients_norms, axis=1) 138 | 139 | # Stop if all moving points are gradients fixed (low gradients diff) 140 | 141 | if fixed == 'center' and np.max(np.abs(old_gradient_norms[:, 1:] - gradients_norms[:, 1:])) < thresh: 142 | break 143 | elif fixed == 'verticals' and np.max(np.abs(old_gradient_norms[:, 3:] - gradients_norms[:, 3:])) < thresh: 144 | break 145 | elif np.max(np.abs(old_gradient_norms - gradients_norms)) < thresh: 146 | break 147 | old_gradient_norms = gradients_norms 148 | 149 | # Move points 150 | # *********** 151 | 152 | # Clip gradient to get moving dists 153 | moving_dists = np.minimum(moving_factor * gradients_norms, clip) 154 | 155 | # Fix central point 156 | if fixed == 'center': 157 | moving_dists[:, 0] = 0 158 | if fixed == 'verticals': 159 | moving_dists[:, 0] = 0 160 | 161 | # Move points 162 | kernel_points -= np.expand_dims(moving_dists, -1) * gradients / np.expand_dims(gradients_norms + 1e-6, -1) 163 | 164 | # moving factor decay 165 | moving_factor *= continuous_moving_decay 166 | 167 | # Remove unused lines in the saved gradients 168 | if step < 10000: 169 | saved_gradient_norms = saved_gradient_norms[:step+1, :] 170 | 171 | # Rescale radius to fit the wanted ratio of radius 172 | r = np.sqrt(np.sum(np.power(kernel_points, 2), axis=-1)) 173 | kernel_points *= ratio / np.mean(r[:, 1:]) 174 | 175 | # Rescale kernels with real radius 176 | return kernel_points * radius, saved_gradient_norms 177 | 178 | 179 | def load_kernels(radius, num_kpoints, fixed='center'): 180 | ''' 181 | 182 | :param radius: 183 | :param num_kpoints: 184 | :param fixed: 185 | :return: kernel points, shape=(num_kpoints, 3) 186 | ''' 187 | 188 | # Kernel directory 189 | kernel_dir = os.path.join(CUR, 'kernels') 190 | os.makedirs(kernel_dir, exist_ok=True) 191 | 192 | # Kernel_file 193 | kernel_file = os.path.join(kernel_dir, 'k_{:03d}_{:s}.ply'.format(num_kpoints, fixed)) 194 | 195 | # Check if already done 196 | if not os.path.exists(kernel_file): 197 | kernel_points, grad_norms = kernel_point_optimization_debug(1.0, 198 | num_kpoints, 199 | num_kernels=100, 200 | fixed=fixed) 201 | 202 | # Find best candidate 203 | best_k = np.argmin(grad_norms[-1, :]) 204 | 205 | # Save points 206 | kernel_points = kernel_points[best_k, :, :] 207 | kernel_points_ply = o3d.geometry.PointCloud() 208 | kernel_points_ply.points = o3d.utility.Vector3dVector(kernel_points) 209 | o3d.io.write_point_cloud(kernel_file, kernel_points_ply) 210 | 211 | else: 212 | kernel_points = o3d.io.read_point_cloud(kernel_file) 213 | kernel_points = np.array(kernel_points.points) 214 | 215 | # Random roations for the kernel 216 | # N.B. 4D random rotations not supported yet 217 | theta = np.random.rand() * 2 * np.pi 218 | 219 | if fixed != 'vertical': 220 | c, s = np.cos(theta), np.sin(theta) 221 | R = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], dtype=np.float32) 222 | 223 | else: 224 | phi = (np.random.rand() - 0.5) * np.pi 225 | 226 | # Create the first vector in carthesian coordinates 227 | u = np.array([np.cos(theta) * np.cos(phi), np.sin(theta) * np.cos(phi), np.sin(phi)]) 228 | 229 | # Choose a random rotation angle 230 | alpha = np.random.rand() * 2 * np.pi 231 | 232 | # Create the rotation matrix with this vector and angle 233 | R = create_3D_rotations(np.reshape(u, (1, -1)), np.reshape(alpha, (1, -1)))[0] 234 | 235 | R = R.astype(np.float32) 236 | 237 | # Add a small noise 238 | kernel_points = kernel_points + np.random.normal(scale=0.01, size=kernel_points.shape) 239 | 240 | # Scale kernels 241 | kernel_points = radius * kernel_points 242 | 243 | # Rotate kernels 244 | kernel_points = np.matmul(kernel_points, R) 245 | 246 | return kernel_points.astype(np.float32) 247 | 248 | 249 | if __name__ == '__main__': 250 | kernel_points = load_kernels(2, 15, fixed='center') 251 | ply = o3d.geometry.PointCloud() 252 | ply.points = o3d.utility.Vector3dVector(kernel_points) 253 | o3d.visualization.draw_geometries([ply]) 254 | -------------------------------------------------------------------------------- /models/KPConv/kernels/k_015_center.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhulf0804/GCNet/c010dcf315760dff9d58ea8f9bf4158d83efc992/models/KPConv/kernels/k_015_center.ply -------------------------------------------------------------------------------- /models/NgeNet.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import torch 4 | import torch.nn as nn 5 | 6 | from models.KPConv import block_decider 7 | from models import InformationInteractive 8 | 9 | 10 | class NgeNet(nn.Module): 11 | def __init__(self, config): 12 | super().__init__() 13 | r = config.first_subsampling_dl * config.conv_radius 14 | in_dim, out_dim = config.in_feats_dim, config.first_feats_dim 15 | K = config.num_kernel_points 16 | 17 | # Encoder blocks 18 | self.encoder_blocks = nn.ModuleList() 19 | self.encoder_skips = [] # record the index of layers to be needed in decoder layer. 20 | self.encoder_skip_dims = [] # record the dims before pooling or strided-conv. 21 | block_i, layer_ind = 0, 0 22 | for block in config.architecture: 23 | if 'upsample' in block: 24 | break 25 | if np.any([skip_block in block for skip_block in ['strided', 'pool']]): 26 | self.encoder_skips.append(block_i) 27 | self.encoder_skip_dims.append(in_dim) 28 | self.encoder_blocks.append(block_decider(block_name=block, 29 | radius=r, 30 | in_dim=in_dim, 31 | out_dim=out_dim, 32 | use_bn=config.use_batch_norm, 33 | bn_momentum=config.batch_norm_momentum, 34 | layer_ind=layer_ind, 35 | config=config)) 36 | 37 | in_dim = out_dim // 2 if 'simple' in block else out_dim 38 | if np.any([skip_block in block for skip_block in ['strided', 'pool']]): 39 | r *= 2 40 | out_dim *= 2 41 | layer_ind += 1 42 | block_i += 1 43 | 44 | # bottleneck layer 45 | self.bottleneck = nn.Conv1d(out_dim, config.gnn_feats_dim, 1) 46 | 47 | # Information Interactive block 48 | self.info_interative = InformationInteractive(layer_names=config.nets, 49 | feat_dims=config.gnn_feats_dim, 50 | gcn_k=config.dgcnn_k, 51 | ppf_k=config.ppf_k, 52 | radius=config.first_subsampling_dl*config.radius_mul, 53 | bottleneck=config.bottleneck, 54 | nhead=config.num_head) 55 | self.pro_gnn = nn.Conv1d(config.gnn_feats_dim, config.gnn_feats_dim, 1) 56 | self.attn_score = nn.Conv1d(config.gnn_feats_dim, 1, 1) 57 | self.epsilon = nn.Parameter(torch.tensor(-5.0)) # how to set ? 58 | 59 | # Decoder blocks 60 | out_dim = config.gnn_feats_dim + 2 61 | self.decoder_blocks = nn.ModuleList() 62 | self.decoder_skips = [] 63 | layer = len(self.encoder_skip_dims) - 1 64 | 65 | self.decoder_blocks_m = nn.ModuleList() 66 | self.decoder_blocks_l = nn.ModuleList() 67 | cnt_upsample, mid_flag, low_flag = 0, True, True 68 | for block in config.architecture[block_i:]: 69 | if 'upsample' in block: 70 | layer_ind -= 1 71 | self.decoder_skips.append(block_i + 1) 72 | 73 | self.decoder_blocks.append(block_decider(block_name=block, 74 | radius=r, 75 | in_dim=in_dim, # how to set for the first loop 76 | out_dim=out_dim, 77 | use_bn=config.use_batch_norm, 78 | bn_momentum=config.batch_norm_momentum, 79 | layer_ind=layer_ind, 80 | config=config)) 81 | 82 | if cnt_upsample >= 1: 83 | if cnt_upsample == 1 and mid_flag: 84 | in_dim_clean = self.encoder_skip_dims[layer+1] 85 | mid_flag = False 86 | else: 87 | in_dim_clean = in_dim 88 | 89 | out_dim_clean = -1 if block == 'last_unary' else out_dim 90 | 91 | self.decoder_blocks_m.append(block_decider(block_name=block, 92 | radius=r, 93 | in_dim=in_dim_clean, # how to set for the first loop 94 | out_dim=out_dim_clean, 95 | use_bn=config.use_batch_norm, 96 | bn_momentum=config.batch_norm_momentum, 97 | layer_ind=layer_ind, 98 | config=config)) 99 | 100 | if cnt_upsample >= 2: 101 | if cnt_upsample == 2 and low_flag: 102 | in_dim_clean = self.encoder_skip_dims[layer+1] 103 | low_flag = False 104 | else: 105 | in_dim_clean = in_dim 106 | out_dim_clean = -1 if block == 'last_unary' else out_dim 107 | self.decoder_blocks_l.append(block_decider(block_name=block, 108 | radius=r, 109 | in_dim=in_dim_clean, # how to set for the first loop 110 | out_dim=out_dim_clean, 111 | use_bn=config.use_batch_norm, 112 | bn_momentum=config.batch_norm_momentum, 113 | layer_ind=layer_ind, 114 | config=config)) 115 | 116 | in_dim = out_dim 117 | 118 | if 'upsample' in block: 119 | r *= 0.5 120 | in_dim += self.encoder_skip_dims[layer] 121 | layer -= 1 122 | out_dim = out_dim // 2 123 | cnt_upsample += 1 124 | 125 | block_i += 1 126 | 127 | # self.decoder_blocks_m = ['unary', 'nearest_upsample', 'unary', 'nearest_upsample', 'last_unary'] 128 | # self.decoder_blocks_l = ['unary', 'nearest_upsample', 'last_unary'] 129 | 130 | self.sigmoid = nn.Sigmoid() 131 | 132 | def forward(self, inputs): 133 | stack_points = inputs['points'] 134 | stacked_normals = inputs['normals'] 135 | # stack_neighbors = inputs['neighbors'] 136 | # stack_pools = inputs['pools'] 137 | # stack_upsamples = inputs['upsamples'] 138 | stack_lengths = inputs['stacked_lengths'] 139 | # batched_coors = inputs['coors'] 140 | # batched_transf = inputs['transf'] 141 | 142 | # 1. encoder 143 | batched_feats = inputs['feats'] 144 | block_i = 0 145 | skip_feats = [] 146 | for block in self.encoder_blocks: 147 | if block_i in self.encoder_skips: 148 | skip_feats.append(batched_feats) 149 | batched_feats = block(batched_feats, inputs) 150 | block_i += 1 151 | 152 | # 2.1 bottleneck layer 153 | batched_feats = self.bottleneck(batched_feats.transpose(0, 1).unsqueeze(0)) # (1, gnn_feats_dim, n) 154 | 155 | # 2.2 information interaction 156 | len_src, len_tgt = stack_lengths[-1][0], stack_lengths[-1][1] 157 | coords_src, coords_tgt = stack_points[-1][:len_src], stack_points[-1][len_src:] 158 | coords_src, coords_tgt = coords_src.transpose(0, 1).unsqueeze(0), \ 159 | coords_tgt.transpose(0, 1).unsqueeze(0) 160 | feats_src, feats_tgt = batched_feats[:, :, :len_src], \ 161 | batched_feats[:, :, len_src:] 162 | normals_src = stacked_normals[-1][:len_src].transpose(0, 1).unsqueeze(0) 163 | normals_tgt = stacked_normals[-1][len_src:].transpose(0, 1).unsqueeze(0) 164 | feats_src, feats_tgt = self.info_interative(coords_src, feats_src, coords_tgt, feats_tgt, normals_src, normals_tgt) 165 | batched_feats = torch.cat([feats_src, feats_tgt], dim=-1) 166 | batched_feats = self.pro_gnn(batched_feats) # why this one ? 167 | 168 | # 2.3 overlap score 169 | attn_scores = self.attn_score(batched_feats).squeeze(0).transpose(0, 1) # (n, 1) 170 | temperature = torch.exp(self.epsilon) + 0.03 171 | batched_feats_norm = batched_feats / (torch.norm(batched_feats, dim=1, keepdim=True) + 1e-8) 172 | batched_feats_norm = batched_feats_norm.squeeze(0).transpose(0, 1) # (n, c) 173 | feats_norm_src, feats_norm_tgt = batched_feats_norm[:len_src], \ 174 | batched_feats_norm[len_src:] 175 | inner_product = torch.matmul(feats_norm_src, feats_norm_tgt.transpose(0, 1)) # (n1, n2), n1 + n2 176 | attn_scores_src, attn_scores_tgt = attn_scores[:len_src], attn_scores[len_src:] # (n1, 1), (n2, 1) 177 | ol_scores_src = torch.matmul(torch.softmax(inner_product / temperature, dim=1), attn_scores_tgt) # (n1, 1) 178 | ol_scores_tgt = torch.matmul(torch.softmax(inner_product.transpose(0, 1) / temperature, dim=1), attn_scores_src) # (n2, 1) 179 | ol_scores = torch.cat([ol_scores_src, ol_scores_tgt], dim=0) # (n, 1) 180 | 181 | # 2.4 feats 182 | batched_feats_raw = batched_feats.squeeze(0).transpose(0, 1) # (n, c) 183 | batched_feats = torch.cat([batched_feats_raw, attn_scores, ol_scores], dim=1) 184 | 185 | # 3. decoder 186 | cnt_decoder = 0 187 | for ind, block in enumerate(self.decoder_blocks): 188 | if block_i in self.decoder_skips: 189 | cnt_decoder += 1 190 | cur_skip_feats = skip_feats.pop() 191 | batched_feats = torch.cat([batched_feats, cur_skip_feats], dim=-1) 192 | if cnt_decoder >= 1: 193 | if cnt_decoder == 1: 194 | batched_feats_m = cur_skip_feats 195 | else: 196 | batched_feats_m = torch.cat([batched_feats_m, cur_skip_feats], dim=-1) 197 | if cnt_decoder >= 2: 198 | if cnt_decoder == 2: 199 | batched_feats_l = cur_skip_feats 200 | else: 201 | batched_feats_l = torch.cat([batched_feats_l, cur_skip_feats], dim=-1) 202 | 203 | if cnt_decoder >= 1: 204 | block_m = self.decoder_blocks_m[ind - 1] 205 | batched_feats_m = block_m(batched_feats_m, inputs) 206 | 207 | if cnt_decoder >= 2: 208 | block_l = self.decoder_blocks_l[ind - (self.decoder_skips[1] - self.decoder_skips[0] + 1)] 209 | batched_feats_l = block_l(batched_feats_l, inputs) 210 | 211 | batched_feats = block(batched_feats, inputs) 212 | block_i += 1 213 | 214 | overlap_scores = self.sigmoid(batched_feats[:, -2:-1]) 215 | saliency_scores = self.sigmoid(batched_feats[:, -1:]) 216 | batched_feats = batched_feats[:, :-2] / torch.norm(batched_feats[:, :-2], dim=1, keepdim=True) 217 | batched_feats_m = batched_feats_m / torch.norm(batched_feats_m, dim=1, keepdim=True) 218 | batched_feats_l = batched_feats_l / torch.norm(batched_feats_l, dim=1, keepdim=True) 219 | batched_feats = torch.cat([batched_feats, overlap_scores, saliency_scores], dim=-1) 220 | 221 | return batched_feats, batched_feats_m, batched_feats_l -------------------------------------------------------------------------------- /models/__init__.py: -------------------------------------------------------------------------------- 1 | from .architecures import architectures 2 | from .information_interactive import InformationInteractive 3 | from .vote import vote 4 | from .NgeNet import NgeNet 5 | -------------------------------------------------------------------------------- /models/architecures.py: -------------------------------------------------------------------------------- 1 | architectures = dict() 2 | 3 | architectures['threedmatch'] = [ 4 | 'simple', 5 | 'resnetb', 6 | 'resnetb_strided', 7 | 'resnetb', 8 | 'resnetb', 9 | 'resnetb_strided', 10 | 'resnetb', 11 | 'resnetb', 12 | 'resnetb_strided', 13 | 'resnetb', 14 | 'resnetb', 15 | 'nearest_upsample', 16 | 'unary', 17 | 'nearest_upsample', 18 | 'unary', 19 | 'nearest_upsample', 20 | 'last_unary' 21 | ] 22 | 23 | architectures['kitti'] = [ 24 | 'simple', 25 | 'resnetb', 26 | 'resnetb_strided', 27 | 'resnetb', 28 | 'resnetb', 29 | 'resnetb_strided', 30 | 'resnetb', 31 | 'resnetb', 32 | 'resnetb_strided', 33 | 'resnetb', 34 | 'resnetb', 35 | 'nearest_upsample', 36 | 'unary', 37 | 'nearest_upsample', 38 | 'unary', 39 | 'nearest_upsample', 40 | 'last_unary' 41 | ] 42 | 43 | architectures['mvp_rg'] = [ 44 | 'simple', 45 | 'resnetb', 46 | 'resnetb', 47 | 'resnetb_strided', 48 | 'resnetb', 49 | 'resnetb', 50 | 'resnetb_strided', 51 | 'resnetb', 52 | 'resnetb', 53 | 'nearest_upsample', 54 | 'unary', 55 | 'unary', 56 | 'nearest_upsample', 57 | 'unary', 58 | 'last_unary' 59 | ] 60 | -------------------------------------------------------------------------------- /models/information_interactive.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import torch 3 | import torch.nn as nn 4 | from utils import square_dists, gather_points, sample_and_group, angle 5 | 6 | 7 | def get_graph_features(feats, coords, k=10): 8 | ''' 9 | 10 | :param feats: (B, N, C) 11 | :param coords: (B, N, 3) 12 | :param k: float 13 | :return: (B, N, k, 2C) 14 | ''' 15 | 16 | sq_dists = square_dists(coords, coords) 17 | n = coords.size(1) 18 | inds = torch.topk(sq_dists, min(n, k+1), dim=-1, largest=False, sorted=True)[1] 19 | inds = inds[:, :, 1:] # (B, N, k) 20 | 21 | neigh_feats = gather_points(feats, inds) # (B, N, k, c) 22 | feats = torch.unsqueeze(feats, 2).repeat(1, 1, min(n-1, k), 1) # (B, N, k, c) 23 | return torch.cat([feats, neigh_feats - feats], dim=-1) 24 | 25 | 26 | class LocalFeatureFused(nn.Module): 27 | def __init__(self, in_dim, out_dims): 28 | super(LocalFeatureFused, self).__init__() 29 | self.blocks = nn.Sequential() 30 | for i, out_dim in enumerate(out_dims): 31 | self.blocks.add_module(f'conv2d_{i}', 32 | nn.Conv2d(in_dim, out_dim, 1, bias=False)) 33 | self.blocks.add_module(f'in_{i}', 34 | nn.InstanceNorm2d(out_dims)) 35 | self.blocks.add_module(f'relu_{i}', nn.ReLU(inplace=True)) 36 | in_dim = out_dim 37 | 38 | def forward(self, x): 39 | ''' 40 | :param x: (B, C1, K, M) 41 | :return: (B, C2, M) 42 | ''' 43 | x = self.blocks(x) 44 | x = torch.max(x, dim=2)[0] 45 | return x 46 | 47 | 48 | class PPF(nn.Module): 49 | def __init__(self, feats_dim, k, radius): 50 | super().__init__() 51 | self.k = k 52 | self.radius = radius 53 | self.local_feature_fused = LocalFeatureFused(in_dim=10, 54 | out_dims=feats_dim) 55 | 56 | def forward(self, coords, feats): 57 | ''' 58 | 59 | :param coors: (B, 3, N) 60 | :param feats: (B, 3, N) 61 | :param k: int 62 | :return: (B, C, N) 63 | ''' 64 | 65 | feats = feats.permute(0, 2, 1).contiguous() 66 | coords = coords.permute(0, 2, 1).contiguous() 67 | new_xyz, new_points, grouped_inds, grouped_xyz = \ 68 | sample_and_group(xyz=coords, 69 | points=feats, 70 | M=-1, 71 | radius=self.radius, 72 | K=self.k) 73 | nr_d = angle(feats[:, :, None, :], grouped_xyz) 74 | ni_d = angle(new_points[..., 3:], grouped_xyz) 75 | nr_ni = angle(feats[:, :, None, :], new_points[..., 3:]) 76 | d_norm = torch.norm(grouped_xyz, dim=-1) 77 | ppf_feat = torch.stack([nr_d, ni_d, nr_ni, d_norm], dim=-1) # (B, N, K, 4) 78 | new_points = torch.cat([new_points[..., :3], ppf_feat], dim=-1) 79 | 80 | coords = torch.unsqueeze(coords, dim=2).repeat(1, 1, min(self.k, new_points.size(2)), 1) 81 | new_points = torch.cat([coords, new_points], dim=-1) 82 | feature_local = new_points.permute(0, 3, 2, 1).contiguous() # (B, C1 + 3, K, M) 83 | feature_local = self.local_feature_fused(feature_local) 84 | return feature_local 85 | 86 | 87 | class GCN(nn.Module): 88 | def __init__(self, feats_dim, k): 89 | super().__init__() 90 | self.conv1 = nn.Sequential( 91 | nn.Conv2d(feats_dim * 2, feats_dim, 1, bias=False), 92 | nn.InstanceNorm2d(feats_dim), 93 | nn.LeakyReLU(0.2) 94 | ) 95 | self.conv2 = nn.Sequential( 96 | nn.Conv2d(feats_dim * 2, feats_dim * 2, 1, bias=False), 97 | nn.InstanceNorm2d(feats_dim * 2), 98 | nn.LeakyReLU(0.2) 99 | ) 100 | self.conv3 = nn.Sequential( 101 | nn.Conv1d(feats_dim * 4, feats_dim, 1, bias=False), 102 | nn.InstanceNorm1d(feats_dim), 103 | nn.LeakyReLU(0.2) 104 | ) 105 | self.k = k 106 | 107 | def forward(self, coords, feats): 108 | ''' 109 | 110 | :param coors: (B, 3, N) 111 | :param feats: (B, C, N) 112 | :param k: int 113 | :return: (B, C, N) 114 | ''' 115 | feats1 = get_graph_features(feats=feats.permute(0, 2, 1).contiguous(), 116 | coords=coords.permute(0, 2, 1).contiguous(), 117 | k=self.k) 118 | feats1 = self.conv1(feats1.permute(0, 3, 1, 2).contiguous()) 119 | feats1 = torch.max(feats1, dim=-1)[0] 120 | 121 | feats2 = get_graph_features(feats=feats1.permute(0, 2, 1).contiguous(), 122 | coords=coords.permute(0, 2, 1).contiguous(), 123 | k=self.k) 124 | feats2 = self.conv2(feats2.permute(0, 3, 1, 2).contiguous()) 125 | feats2 = torch.max(feats2, dim=-1)[0] 126 | 127 | feats3 = torch.cat([feats, feats1, feats2], dim=1) 128 | feats3 = self.conv3(feats3) 129 | 130 | return feats3 131 | 132 | 133 | class GGE(nn.Module): 134 | def __init__(self, feats_dim, gcn_k, ppf_k, radius, bottleneck): 135 | super().__init__() 136 | self.gcn = GCN(feats_dim, gcn_k) 137 | if bottleneck: 138 | self.ppf = PPF([feats_dim // 2, feats_dim, feats_dim // 2], ppf_k, radius) 139 | self.fused = nn.Sequential( 140 | nn.Conv1d(feats_dim + feats_dim // 2, feats_dim + feats_dim // 2, 1), 141 | nn.InstanceNorm1d(feats_dim + feats_dim // 2), 142 | nn.LeakyReLU(0.2), 143 | nn.Conv1d(feats_dim + feats_dim // 2, feats_dim, 1), 144 | nn.InstanceNorm1d(feats_dim), 145 | nn.LeakyReLU(0.2) 146 | ) 147 | else: 148 | self.ppf = PPF([feats_dim, feats_dim*2, feats_dim], ppf_k, radius) 149 | self.fused = nn.Sequential( 150 | nn.Conv1d(feats_dim * 2, feats_dim * 2, 1), 151 | nn.InstanceNorm1d(feats_dim * 2), 152 | nn.LeakyReLU(0.2), 153 | nn.Conv1d(feats_dim * 2, feats_dim, 1), 154 | nn.InstanceNorm1d(feats_dim), 155 | nn.LeakyReLU(0.2) 156 | ) 157 | 158 | def forward(self, coords, feats, normals): 159 | feats_ppf = self.ppf(coords, normals) 160 | feats_gcn = self.gcn(coords, feats) 161 | feats_fused = self.fused(torch.cat([feats_ppf, feats_gcn], dim=1)) 162 | return feats_fused 163 | 164 | 165 | def multi_head_attention(query, key, value): 166 | ''' 167 | 168 | :param query: (B, dim, nhead, N) 169 | :param key: (B, dim, nhead, M) 170 | :param value: (B, dim, nhead, M) 171 | :return: (B, dim, nhead, N) 172 | ''' 173 | dim = query.size(1) 174 | scores = torch.einsum('bdhn, bdhm->bhnm', query, key) / dim**0.5 175 | attention = torch.nn.functional.softmax(scores, dim=-1) 176 | feats = torch.einsum('bhnm, bdhm->bdhn', attention, value) 177 | return feats 178 | 179 | 180 | class Cross_Attention(nn.Module): 181 | def __init__(self, feat_dims, nhead): 182 | super().__init__() 183 | assert feat_dims % nhead == 0 184 | self.feats_dim = feat_dims 185 | self.nhead = nhead 186 | # self.q_conv = nn.Conv1d(feat_dims, feat_dims, 1, bias=True) 187 | # self.k_conv = nn.Conv1d(feat_dims, feat_dims, 1, bias=True) 188 | # self.v_conv = nn.Conv1d(feat_dims, feat_dims, 1, bias=True) 189 | self.conv = nn.Conv1d(feat_dims, feat_dims, 1) 190 | self.q_conv, self.k_conv, self.v_conv = [copy.deepcopy(self.conv) for _ in range(3)] # a good way than better ? 191 | self.mlp = nn.Sequential( 192 | nn.Conv1d(feat_dims * 2, feat_dims * 2, 1), 193 | nn.InstanceNorm1d(feat_dims * 2), 194 | nn.ReLU(inplace=True), 195 | nn.Conv1d(feat_dims * 2, feat_dims, 1), 196 | ) 197 | 198 | def forward(self, feats1, feats2): 199 | ''' 200 | 201 | :param feats1: (B, C, N) 202 | :param feats2: (B, C, M) 203 | :return: (B, C, N) 204 | ''' 205 | b = feats1.size(0) 206 | dims = self.feats_dim // self.nhead 207 | query = self.q_conv(feats1).reshape(b, dims, self.nhead, -1) 208 | key = self.k_conv(feats2).reshape(b, dims, self.nhead, -1) 209 | value = self.v_conv(feats2).reshape(b, dims, self.nhead, -1) 210 | feats = multi_head_attention(query, key, value) 211 | feats = feats.reshape(b, self.feats_dim, -1) 212 | feats = self.conv(feats) 213 | cross_feats = self.mlp(torch.cat([feats1, feats], dim=1)) 214 | return cross_feats 215 | 216 | 217 | class InformationInteractive(nn.Module): 218 | def __init__(self, layer_names, feat_dims, gcn_k, ppf_k, radius, bottleneck, nhead): 219 | super().__init__() 220 | self.layer_names = layer_names 221 | self.blocks = nn.ModuleList() 222 | for layer_name in layer_names: 223 | if layer_name == 'gcn': 224 | self.blocks.append(GCN(feat_dims, gcn_k)) 225 | elif layer_name == 'gge': 226 | self.blocks.append(GGE(feat_dims, gcn_k, ppf_k, radius, bottleneck)) 227 | elif layer_name == 'cross_attn': 228 | self.blocks.append(Cross_Attention(feat_dims, nhead)) 229 | else: 230 | raise NotImplementedError 231 | 232 | def forward(self, coords1, feats1, coords2, feats2, normals1, normals2): 233 | ''' 234 | 235 | :param coords1: (B, 3, N) 236 | :param feats1: (B, C, N) 237 | :param coords2: (B, 3, M) 238 | :param feats2: (B, C, M) 239 | :return: feats1=(B, C, N), feats2=(B, C, M) 240 | ''' 241 | for layer_name, block in zip(self.layer_names, self.blocks): 242 | if layer_name == 'gcn': 243 | feats1 = block(coords1, feats1) 244 | feats2 = block(coords2, feats2) 245 | elif layer_name == 'gge': 246 | feats1 = block(coords1, feats1, normals1) 247 | feats2 = block(coords2, feats2, normals2) 248 | elif layer_name == 'cross_attn': 249 | feats1 = feats1 + block(feats1, feats2) 250 | feats2 = feats2 + block(feats2, feats1) 251 | else: 252 | raise NotImplementedError 253 | 254 | return feats1, feats2 255 | -------------------------------------------------------------------------------- /models/vote.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from utils import to_tensor 4 | 5 | 6 | def get_coor_points(source_feats_npy, target_feats_npy, target_npy, use_cuda): 7 | dists = torch.cdist(to_tensor(source_feats_npy, use_cuda), to_tensor(target_feats_npy, use_cuda)) # (n, m) 8 | inds = torch.min(dists, dim=-1)[1].cpu().numpy() 9 | return target_npy[inds], inds 10 | 11 | 12 | def vote(source_npy, target_npy, source_feats, target_feats, voxel_size, use_cuda=True): 13 | source_feats_h, source_feats_m, source_feats_l = source_feats 14 | target_feats_h, target_feats_m, target_feats_l = target_feats 15 | 16 | coor_y1, coor_inds1 = get_coor_points(source_feats_h, target_feats_h, target_npy, use_cuda) 17 | coor_y2, coor_inds2 = get_coor_points(source_feats_m, target_feats_m, target_npy, use_cuda) 18 | coor_y3, coor_inds3 = get_coor_points(source_feats_l, target_feats_l, target_npy, use_cuda) 19 | 20 | d12 = np.sqrt(np.sum((coor_y1 - coor_y2) ** 2, axis=-1)) 21 | d13 = np.sqrt(np.sum((coor_y1 - coor_y3) ** 2, axis=-1)) 22 | d23 = np.sqrt(np.sum((coor_y2 - coor_y3) ** 2, axis=-1)) 23 | 24 | thresh = voxel_size * 2 25 | source_sel_h = np.any([d12 < thresh, d13 < thresh], axis=0) 26 | source_sel_m = d23 < thresh 27 | source_sel = np.any([source_sel_h, source_sel_m], axis=0) 28 | 29 | source_sel_replace = np.all([~source_sel_h, source_sel_m], axis=0) 30 | source_feats_h[source_sel_replace] = source_feats_m[source_sel_replace] 31 | target_feats_h[coor_inds2[source_sel_replace]] = target_feats_m[coor_inds2[source_sel_replace]] 32 | 33 | source_npy = source_npy[source_sel] 34 | source_feats_h = source_feats_h[source_sel] 35 | 36 | after_vote = [source_npy, target_npy, source_feats_h, target_feats_h] 37 | return after_vote 38 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | easydict==1.9 2 | h5py==3.5.0 3 | matplotlib==3.4.3 4 | nibabel==3.2.1 5 | numpy==1.21.3 6 | open3d==0.10.0.0 7 | scipy==1.7.1 8 | torch==1.8.1+cu111 9 | tqdm==4.62.3 10 | rich==12.5.1 11 | -------------------------------------------------------------------------------- /train.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import shutil 4 | import sys 5 | import torch 6 | from easydict import EasyDict as edict 7 | from tqdm import tqdm 8 | from torch.utils.tensorboard import SummaryWriter 9 | 10 | from data import get_dataset, get_dataloader 11 | from models import architectures, NgeNet 12 | from losses import Loss 13 | from utils import decode_config, setup_seed 14 | 15 | CUR = os.path.dirname(os.path.abspath(__file__)) 16 | 17 | 18 | def save_summary(writer, loss_dict, global_step, tag, lr=None): 19 | for k, v in loss_dict.items(): 20 | writer.add_scalar(f'{tag}/{k}', v, global_step) 21 | if lr is not None: 22 | writer.add_scalar('lr', lr, global_step) 23 | 24 | 25 | def main(): 26 | setup_seed(1234) 27 | config = decode_config(sys.argv[1]) 28 | config = edict(config) 29 | config.architecture = architectures[config.dataset] 30 | 31 | saved_path = config.exp_dir 32 | saved_ckpt_path = os.path.join(saved_path, 'checkpoints') 33 | saved_logs_path = os.path.join(saved_path, 'summary') 34 | os.makedirs(saved_path, exist_ok=True) 35 | os.makedirs(saved_ckpt_path, exist_ok=True) 36 | os.makedirs(saved_logs_path, exist_ok=True) 37 | shutil.copyfile(sys.argv[1], os.path.join(saved_path, f'{config.dataset}.yaml')) 38 | train_dataset, val_dataset = get_dataset(config.dataset, config) 39 | train_dataloader, neighborhood_limits = get_dataloader(config=config, 40 | dataset=train_dataset, 41 | batch_size=config.batch_size, 42 | num_workers=config.num_workers, 43 | shuffle=True, 44 | neighborhood_limits=None) 45 | val_dataloader, _ = get_dataloader(config=config, 46 | dataset=val_dataset, 47 | batch_size=config.batch_size, 48 | num_workers=config.num_workers, 49 | shuffle=False, 50 | neighborhood_limits=neighborhood_limits) 51 | 52 | print(neighborhood_limits) 53 | model = NgeNet(config).cuda() 54 | model_loss = Loss(config) 55 | 56 | if config.optimizer == 'SGD': 57 | optimizer = torch.optim.SGD( 58 | model.parameters(), 59 | lr=config.lr, 60 | momentum=config.momentum, 61 | weight_decay=config.weight_decay, 62 | ) 63 | elif config.optimizer == 'ADAM': 64 | optimizer = torch.optim.Adam( 65 | model.parameters(), 66 | lr=config.lr, 67 | betas=(0.9, 0.999), 68 | weight_decay=config.weight_decay, 69 | ) 70 | 71 | # create learning rate scheduler 72 | if config.scheduler == 'ExpLR': 73 | scheduler = torch.optim.lr_scheduler.ExponentialLR( 74 | optimizer, 75 | gamma=config.scheduler_gamma, 76 | ) 77 | elif config.scheduler == 'CosA': 78 | scheduler = torch.optim.lr_scheduler.CosineAnnealingWarmRestarts(optimizer, 79 | T_0=config.T_0, 80 | T_mult=config.T_mult, 81 | eta_min=config.eta_min, 82 | last_epoch=-1) 83 | else: 84 | raise ValueError 85 | 86 | 87 | writer = SummaryWriter(saved_logs_path) 88 | 89 | best_recall, best_recall_sum, best_circle_loss, best_loss = 0, 0, 1e8, 1e8 90 | w_saliency = config.w_saliency_loss 91 | w_saliency_update = False 92 | 93 | for epoch in range(config.max_epoch): 94 | print('=' * 20, epoch, '=' * 20) 95 | train_step, val_step = 0, 0 96 | for inputs in tqdm(train_dataloader): 97 | for k, v in inputs.items(): 98 | if isinstance(v, list): 99 | for i in range(len(v)): 100 | inputs[k][i] = inputs[k][i].cuda() 101 | else: 102 | inputs[k] = inputs[k].cuda() 103 | 104 | optimizer.zero_grad() 105 | 106 | batched_feats, batched_feats_m, batched_feats_l = model(inputs) 107 | stack_points = inputs['points'] 108 | stack_lengths = inputs['stacked_lengths'] 109 | feats_src = batched_feats[:stack_lengths[0][0]] 110 | feats_tgt = batched_feats[stack_lengths[0][0]:] 111 | feats_src_m = batched_feats_m[:stack_lengths[0][0]] 112 | feats_tgt_m = batched_feats_m[stack_lengths[0][0]:] 113 | feats_src_l = batched_feats_l[:stack_lengths[0][0]] 114 | feats_tgt_l = batched_feats_l[stack_lengths[0][0]:] 115 | 116 | coors = inputs['coors'][0] # list, [coors1, coors2, ..], preparation for batchsize > 1 117 | transf = inputs['transf'][0] # (1, 4, 4), preparation for batchsize > 1 118 | points_raw = inputs['batched_points_raw'] 119 | coords_src = points_raw[:stack_lengths[0][0]] 120 | coords_tgt = points_raw[stack_lengths[0][0]:] 121 | 122 | loss_dict = model_loss(coords_src=coords_src, 123 | coords_tgt=coords_tgt, 124 | feats_src=feats_src, 125 | feats_tgt=feats_tgt, 126 | feats_src_m=feats_src_m, 127 | feats_tgt_m=feats_tgt_m, 128 | feats_src_l=feats_src_l, 129 | feats_tgt_l=feats_tgt_l, 130 | coors=coors, 131 | transf=transf, 132 | w_saliency=w_saliency) 133 | 134 | loss = loss_dict['total_loss'] 135 | loss.backward() 136 | optimizer.step() 137 | 138 | global_step = epoch * len(train_dataloader) + train_step + 1 139 | 140 | if global_step % config.log_freq == 0: 141 | save_summary(writer, loss_dict, global_step, 'train', 142 | lr=optimizer.param_groups[0]['lr']) 143 | train_step += 1 144 | 145 | # This line of code reduces the training speed. 146 | # If GPU memory allows, it is recommended not to add this line of code or add this line after each epoch 147 | torch.cuda.empty_cache() 148 | scheduler.step() 149 | 150 | total_circle_loss, total_recall, total_loss, total_recall_sum = [], [], [], [] 151 | model.eval() 152 | with torch.no_grad(): 153 | for inputs in tqdm(val_dataloader): 154 | for k, v in inputs.items(): 155 | if isinstance(v, list): 156 | for i in range(len(v)): 157 | inputs[k][i] = inputs[k][i].cuda() 158 | else: 159 | inputs[k] = inputs[k].cuda() 160 | 161 | batched_feats, batched_feats_m, batched_feats_l = model(inputs) 162 | stack_points = inputs['points'] 163 | stack_lengths = inputs['stacked_lengths'] 164 | feats_src = batched_feats[:stack_lengths[0][0]] 165 | feats_tgt = batched_feats[stack_lengths[0][0]:] 166 | feats_src_m = batched_feats_m[:stack_lengths[0][0]] 167 | feats_tgt_m = batched_feats_m[stack_lengths[0][0]:] 168 | feats_src_l = batched_feats_l[:stack_lengths[0][0]] 169 | feats_tgt_l = batched_feats_l[stack_lengths[0][0]:] 170 | 171 | coors = inputs['coors'][0] # list, [coors1, coors2, ..], preparation for batchsize > 1 172 | transf = inputs['transf'][0] # (1, 4, 4), preparation for batchsize > 1 173 | points_raw = inputs['batched_points_raw'] 174 | coords_src = points_raw[:stack_lengths[0][0]] 175 | coords_tgt = points_raw[stack_lengths[0][0]:] 176 | 177 | loss_dict = model_loss(coords_src=coords_src, 178 | coords_tgt=coords_tgt, 179 | feats_src=feats_src, 180 | feats_tgt=feats_tgt, 181 | feats_src_m=feats_src_m, 182 | feats_tgt_m=feats_tgt_m, 183 | feats_src_l=feats_src_l, 184 | feats_tgt_l=feats_tgt_l, 185 | coors=coors, 186 | transf=transf, 187 | w_saliency=w_saliency) 188 | 189 | loss = loss_dict['circle_loss'] + loss_dict['circle_loss_m'] + loss_dict['circle_loss_l'] 190 | total_loss.append(loss.detach().cpu().numpy()) 191 | circle_loss = loss_dict['circle_loss'] 192 | total_circle_loss.append(circle_loss.detach().cpu().numpy()) 193 | recall = loss_dict['recall'] 194 | total_recall.append(recall.detach().cpu().numpy()) 195 | recall_sum = loss_dict['recall'] + loss_dict['recall_m'] + loss_dict['recall_l'] 196 | total_recall_sum.append(recall_sum.detach().cpu().numpy()) 197 | 198 | global_step = epoch * len(val_dataloader) + val_step + 1 199 | 200 | if global_step % config.log_freq == 0: 201 | save_summary(writer, loss_dict, global_step, 'val') 202 | val_step += 1 203 | 204 | # This line of code reduces the training speed. 205 | # If GPU memory allows, it is recommended not to add this line of code or add this line after each epoch 206 | torch.cuda.empty_cache() 207 | 208 | if np.mean(total_circle_loss) < best_circle_loss: 209 | best_circle_loss = np.mean(total_circle_loss) 210 | torch.save(model.state_dict(), os.path.join(saved_ckpt_path, 'best_loss.pth')) 211 | if np.mean(total_recall) > best_recall: 212 | best_recall = np.mean(total_recall) 213 | torch.save(model.state_dict(), os.path.join(saved_ckpt_path, 'best_recall.pth')) 214 | if not w_saliency_update and np.mean(total_recall) > 0.3: 215 | w_saliency_update = True 216 | w_saliency = 1 217 | 218 | model.train() 219 | 220 | 221 | if __name__ == '__main__': 222 | main() 223 | -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- 1 | from .o3d import npy2pcd, pcd2npy, vis_plys, format_lines, get_correspondences, \ 2 | voxel_ds, batch_grid_subsampling, batch_neighbors, get_yellow, get_blue, \ 3 | execute_global_registration, npy2feat, normal, read_cloud 4 | from .yaml import decode_config 5 | from .process import gather_points, square_dists, setup_seed, sample_and_group, angle, \ 6 | fmat, to_tensor 7 | -------------------------------------------------------------------------------- /utils/o3d.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import numpy as np 3 | import torch 4 | import open3d as o3d 5 | import random 6 | 7 | 8 | def read_cloud(path, rt='pcd'): 9 | if path.endswith('.ply') or path.endswith('.pcd'): 10 | pcd = o3d.io.read_point_cloud(path) 11 | elif path.endswith('.pth'): 12 | points = torch.load(path) 13 | pcd = npy2pcd(points) 14 | else: 15 | raise NotImplementedError 16 | if rt == 'pcd': 17 | return pcd 18 | elif rt == 'npy': 19 | return np.asarray(pcd.points) 20 | else: 21 | raise NotImplementedError 22 | 23 | 24 | def npy2pcd(npy): 25 | pcd = o3d.geometry.PointCloud() 26 | pcd.points = o3d.utility.Vector3dVector(npy) 27 | return pcd 28 | 29 | 30 | def pcd2npy(pcd): 31 | npy = np.array(pcd.points) 32 | return npy 33 | 34 | 35 | def get_blue(): 36 | """ 37 | Get color blue for rendering 38 | """ 39 | return [0, 0.651, 0.929] 40 | 41 | 42 | def get_yellow(): 43 | """ 44 | Get color yellow for rendering 45 | """ 46 | return [1, 0.706, 0] 47 | 48 | 49 | def npy2feat(npy): 50 | feats = o3d.registration.Feature() 51 | feats.data = npy.T 52 | return feats 53 | 54 | 55 | def normal(pcd, radius=0.1, max_nn=30, loc=(0, 0, 0)): 56 | pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn), 57 | fast_normal_computation=False) 58 | pcd.orient_normals_towards_camera_location(loc) 59 | return pcd 60 | 61 | 62 | def vis_plys(pcds, need_color=True): 63 | colors = [[1.0, 0, 0], 64 | [0, 1.0, 0], 65 | [0, 0, 1.0]] 66 | if need_color: 67 | for i, pcd in enumerate(pcds): 68 | color = colors[i] if i < 3 else [random.random() for _ in range(3)] 69 | pcd.paint_uniform_color(color) 70 | o3d.visualization.draw_geometries(pcds) 71 | 72 | 73 | def format_lines(points, lines, colors=None): 74 | ''' 75 | :param points: n x 3 76 | :param lines: m x 2 77 | :param colors: m x 3 78 | :return: 79 | ''' 80 | if colors is None: 81 | colors = [[1, 0, 0] for i in range(len(lines))] 82 | line_set = o3d.geometry.LineSet( 83 | points=o3d.utility.Vector3dVector(points), 84 | lines=o3d.utility.Vector2iVector(lines), 85 | ) 86 | line_set.colors = o3d.utility.Vector3dVector(colors) 87 | return line_set 88 | 89 | 90 | # the speed is slow here ? we should test it later. 91 | def get_correspondences(src_ply, tgt_ply, transf, search_radius, K=None): 92 | src_ply = copy.deepcopy(src_ply) 93 | src_ply.transform(transf) 94 | pcd_tree = o3d.geometry.KDTreeFlann(tgt_ply) 95 | src_npy = pcd2npy(src_ply) 96 | corrs = [] 97 | for i in range(src_npy.shape[0]): 98 | point = src_npy[i] 99 | [k, idx, _] = pcd_tree.search_radius_vector_3d(point, search_radius) 100 | if K is not None: 101 | idx = idx[:K] 102 | for j in idx: 103 | corrs.append([i, j]) 104 | return np.array(corrs) 105 | 106 | 107 | def voxel_ds(ply, voxel_size): 108 | return ply.voxel_down_sample(voxel_size=voxel_size) 109 | 110 | 111 | def batch_grid_subsampling(points, batches_len, sampleDl): 112 | ''' 113 | 114 | :param points: shape=(n, 3), n = n1 + n2 + ... + nk, k denotes batch size 115 | :param batches_len: shape=(k, ), values = (n1, n2, ..., nk) 116 | :param sampleDl: float, 117 | :return: 118 | ''' 119 | new_points, new_len = [], [] 120 | st = 0 121 | points = points.cpu().numpy() 122 | batches_len = batches_len.cpu().numpy() 123 | for i in range(len(batches_len)): 124 | l = batches_len[i] 125 | cur_points = points[st:st+l] 126 | cur_ply = npy2pcd(cur_points) 127 | cur_ply_ds = voxel_ds(cur_ply, voxel_size=sampleDl) 128 | cur_points_ds = pcd2npy(cur_ply_ds) 129 | new_points.append(cur_points_ds) 130 | new_len.append(len(cur_points_ds)) 131 | st += l 132 | return torch.from_numpy(np.vstack(new_points).astype(np.float32)), \ 133 | torch.from_numpy(np.array(new_len, dtype=np.int32)) 134 | 135 | 136 | def batch_neighbors(batch_queries, batch_supports, q_batches, s_batches, radius, max_nn): 137 | ''' 138 | # Open3d is too slow, so we won't use this method here. 139 | :param batch_queries: shape=(n, 3), n = n1 + n2 + ... + nk, k denotes batch size 140 | :param batch_supports: shape=(m, 3), m = m1 + m2 + ... + mk 141 | :param q_batches: shape=(k, ), values = (n1, n2, ..., nk) 142 | :param s_batches: shape=(k, ), values = (m1, m2, ..., mk) 143 | :param radius: float 144 | :return: shape=(n, max_nn) 145 | ''' 146 | inds = [] 147 | q_st, s_st = 0, 0 148 | for i in range(len(q_batches)): 149 | q_len, s_len = q_batches[i], s_batches[i] 150 | queries, supports = batch_queries[q_st:q_st+q_len], batch_supports[s_st:s_st+s_len] 151 | supports_ply = npy2pcd(supports) 152 | supports_tree = o3d.geometry.KDTreeFlann(supports_ply) 153 | for query_point in queries: 154 | [k, idx, _] = supports_tree.search_radius_vector_3d(query_point, radius) 155 | if k > max_nn: 156 | process_idx = list(idx)[:max_nn] 157 | else: 158 | process_idx = list(idx) + [len(batch_supports) - s_st] * (max_nn - k) 159 | inds.append(np.array(process_idx) + s_st) 160 | q_st += q_len 161 | s_st += s_len 162 | 163 | return np.array(inds).astype(np.int32) 164 | 165 | 166 | def execute_global_registration(source, target, source_feats, target_feats, voxel_size): 167 | distance_threshold = voxel_size 168 | result = o3d.registration.registration_ransac_based_on_feature_matching( 169 | source, target, source_feats, target_feats, distance_threshold, 170 | o3d.registration.TransformationEstimationPointToPoint(False), 3, [ 171 | o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9), 172 | o3d.registration.CorrespondenceCheckerBasedOnDistance( 173 | distance_threshold) 174 | ], o3d.registration.RANSACConvergenceCriteria(50000, 1000)) 175 | transformation = result.transformation 176 | estimate = copy.deepcopy(source) 177 | estimate.transform(transformation) 178 | return transformation, estimate 179 | -------------------------------------------------------------------------------- /utils/process.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import torch 4 | 5 | 6 | def setup_seed(seed): 7 | torch.backends.cudnn.deterministic = True 8 | torch.manual_seed(seed) 9 | torch.cuda.manual_seed_all(seed) 10 | np.random.seed(seed) 11 | 12 | 13 | def fmat(arr): 14 | return np.around(arr,3) 15 | 16 | 17 | def to_tensor(x, use_cuda): 18 | if use_cuda: 19 | return torch.tensor(x).cuda() 20 | else: 21 | return torch.tensor(x) 22 | 23 | 24 | def gather_points(points, inds): 25 | ''' 26 | 27 | :param points: shape=(B, N, C) 28 | :param inds: shape=(B, M) or shape=(B, M, K) 29 | :return: sampling points: shape=(B, M, C) or shape=(B, M, K, C) 30 | ''' 31 | device = points.device 32 | B, N, C = points.shape 33 | inds_shape = list(inds.shape) 34 | inds_shape[1:] = [1] * len(inds_shape[1:]) 35 | repeat_shape = list(inds.shape) 36 | repeat_shape[0] = 1 37 | batchlists = torch.arange(0, B, dtype=torch.long).to(device).reshape(inds_shape).repeat(repeat_shape) 38 | return points[batchlists, inds, :] 39 | 40 | 41 | def square_dists(points1, points2): 42 | ''' 43 | Calculate square dists between two group points 44 | :param points1: shape=(B, N, C) 45 | :param points2: shape=(B, M, C) 46 | :return: 47 | ''' 48 | B, N, C = points1.shape 49 | _, M, _ = points2.shape 50 | dists = torch.sum(torch.pow(points1, 2), dim=-1).view(B, N, 1) + \ 51 | torch.sum(torch.pow(points2, 2), dim=-1).view(B, 1, M) 52 | dists -= 2 * torch.matmul(points1, points2.permute(0, 2, 1)) 53 | dists = torch.clamp(dists, min=1e-8) 54 | return dists.float() 55 | 56 | 57 | def ball_query(xyz, new_xyz, radius, K, rt_density=False): 58 | ''' 59 | :param xyz: shape=(B, N, 3) 60 | :param new_xyz: shape=(B, M, 3) 61 | :param radius: int 62 | :param K: int, an upper limit samples 63 | :return: shape=(B, M, K) 64 | ''' 65 | device = xyz.device 66 | B, N, C = xyz.shape 67 | M = new_xyz.shape[1] 68 | grouped_inds = torch.arange(0, N, dtype=torch.long).to(device).view(1, 1, N).repeat(B, M, 1) 69 | dists = square_dists(new_xyz, xyz) 70 | grouped_inds[dists > radius ** 2] = N 71 | if rt_density: 72 | density = torch.sum(grouped_inds < N, dim=-1) 73 | density = density / N 74 | grouped_inds = torch.sort(grouped_inds, dim=-1)[0][:, :, :K] 75 | grouped_min_inds = grouped_inds[:, :, 0:1].repeat(1, 1, min(K, grouped_inds.size(2))) 76 | grouped_inds[grouped_inds == N] = grouped_min_inds[grouped_inds == N] 77 | if rt_density: 78 | return grouped_inds, density 79 | return grouped_inds 80 | 81 | 82 | def sample_and_group(xyz, points, M, radius, K, use_xyz=True, rt_density=False): 83 | ''' 84 | :param xyz: shape=(B, N, 3) 85 | :param points: shape=(B, N, C) 86 | :param M: int 87 | :param radius:float 88 | :param K: int 89 | :param use_xyz: bool, if True concat XYZ with local point features, otherwise just use point features 90 | :return: new_xyz, shape=(B, M, 3); new_points, shape=(B, M, K, C+3); 91 | group_inds, shape=(B, M, K); grouped_xyz, shape=(B, M, K, 3) 92 | ''' 93 | assert M < 0 94 | new_xyz = xyz 95 | if rt_density: 96 | grouped_inds, density = ball_query(xyz, new_xyz, radius, K, 97 | rt_density=True) 98 | else: 99 | grouped_inds = ball_query(xyz, new_xyz, radius, K, rt_density=False) 100 | grouped_xyz = gather_points(xyz, grouped_inds) 101 | grouped_xyz -= torch.unsqueeze(new_xyz, 2).repeat(1, 1, min(K, grouped_inds.size(2)), 1) 102 | if points is not None: 103 | grouped_points = gather_points(points, grouped_inds) 104 | if use_xyz: 105 | new_points = torch.cat((grouped_xyz.float(), grouped_points.float()), dim=-1) 106 | else: 107 | new_points = grouped_points 108 | else: 109 | new_points = grouped_xyz 110 | if rt_density: 111 | return new_xyz, new_points, grouped_inds, grouped_xyz, density 112 | return new_xyz, new_points, grouped_inds, grouped_xyz 113 | 114 | 115 | def angle(v1: torch.Tensor, v2: torch.Tensor): 116 | """Compute angle between 2 vectors 117 | For robustness, we use the same formulation as in PPFNet, i.e. 118 | angle(v1, v2) = atan2(cross(v1, v2), dot(v1, v2)). 119 | This handles the case where one of the vectors is 0.0, since torch.atan2(0.0, 0.0)=0.0 120 | Args: 121 | v1: (B, *, 3) 122 | v2: (B, *, 3) 123 | Returns: 124 | """ 125 | 126 | cross_prod = torch.stack([v1[..., 1] * v2[..., 2] - v1[..., 2] * v2[..., 1], 127 | v1[..., 2] * v2[..., 0] - v1[..., 0] * v2[..., 2], 128 | v1[..., 0] * v2[..., 1] - v1[..., 1] * v2[..., 0]], dim=-1) 129 | cross_prod_norm = torch.norm(cross_prod, dim=-1) 130 | dot_prod = torch.sum(v1 * v2, dim=-1) 131 | 132 | return torch.atan2(cross_prod_norm, dot_prod) 133 | -------------------------------------------------------------------------------- /utils/yaml.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | 3 | 4 | def decode_config(filepath): 5 | with open(filepath, 'r') as f: 6 | data = yaml.load(f, Loader=yaml.FullLoader) 7 | configs_dict = dict() 8 | for k, v in data.items(): 9 | for sub_k, sub_v in v.items(): 10 | configs_dict[sub_k] = sub_v 11 | return configs_dict 12 | --------------------------------------------------------------------------------