├── .gitignore ├── LICENSE ├── README.md ├── cad ├── camera.ply ├── drone.ply ├── ipb_car.ply └── kitti_car.ply ├── config ├── lidar_slam │ ├── run.yaml │ ├── run_apollo.yaml │ ├── run_demo.yaml │ ├── run_demo_sem.yaml │ ├── run_fast.yaml │ ├── run_hilti.yaml │ ├── run_ipbcar.yaml │ ├── run_kitti.yaml │ ├── run_kitti_color.yaml │ ├── run_kitti_mos.yaml │ ├── run_light.yaml │ ├── run_livox.yaml │ ├── run_mulran.yaml │ ├── run_ncd.yaml │ ├── run_ncd_128.yaml │ ├── run_ncd_128_m.yaml │ ├── run_ncd_128_s.yaml │ ├── run_ncd_recon.yaml │ └── run_vbr.yaml ├── pin_slam_ros.rviz └── rgbd_slam │ └── run_replica.yaml ├── dataset ├── converter │ ├── mulran_to_pin_format.py │ ├── ncd128_pose_converter.py │ ├── neuralrgbd_to_pin_format.py │ ├── replica_to_pin_format.py │ └── tum_to_pin_format.py ├── dataloaders │ ├── __init__.py │ ├── apollo.py │ ├── boreas.py │ ├── generic.py │ ├── helipr.py │ ├── kitti.py │ ├── kitti360.py │ ├── kitti_mot.py │ ├── kitti_raw.py │ ├── mcap.py │ ├── mulran.py │ ├── ncd.py │ ├── nclt.py │ ├── neuralrgbd.py │ ├── nuscenes.py │ ├── ouster.py │ ├── paris_luco.py │ ├── replica.py │ ├── rosbag.py │ └── tum.py ├── dataset_indexing.py └── slam_dataset.py ├── docker ├── build_docker.sh ├── cu117.Dockerfile └── start_docker.sh ├── eval ├── README.md ├── eval_kitti.ipynb ├── eval_mesh_utils.py ├── eval_mulran.ipynb ├── eval_ncd.ipynb ├── eval_ncd_128.ipynb ├── eval_replica.ipynb └── eval_traj_utils.py ├── gui ├── gui_utils.py └── slam_gui.py ├── model ├── __init__.py ├── decoder.py └── neural_points.py ├── pin_slam.py ├── pin_slam_ros.py ├── requirements.txt ├── scripts ├── convert_neuralrgbd.sh ├── convert_replica.sh ├── download_kitti_example.sh ├── download_replica.sh ├── module │ └── ply.py └── rosbag2ply.py ├── utils ├── __init__.py ├── config.py ├── data_sampler.py ├── loop_detector.py ├── loss.py ├── mapper.py ├── mesher.py ├── pgo.py ├── point_cloud2.py ├── semantic_kitti_utils.py ├── tools.py ├── tracker.py └── visualizer.py └── vis_pin_map.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | 162 | data 163 | data/ 164 | experiments 165 | experiments/ 166 | .vscode/ 167 | 168 | *.json 169 | 170 | 171 | .viewpoints/ 172 | screenshots/ 173 | 174 | bak 175 | TODO.md 176 | NOTE.md 177 | notebook_exp -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Photogrammetry & Robotics Bonn 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 | -------------------------------------------------------------------------------- /cad/camera.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/PIN_SLAM/2a2c6f7dc2b3b01efb283d430165cb80d26f835b/cad/camera.ply -------------------------------------------------------------------------------- /cad/drone.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/PIN_SLAM/2a2c6f7dc2b3b01efb283d430165cb80d26f835b/cad/drone.ply -------------------------------------------------------------------------------- /cad/ipb_car.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/PIN_SLAM/2a2c6f7dc2b3b01efb283d430165cb80d26f835b/cad/ipb_car.ply -------------------------------------------------------------------------------- /cad/kitti_car.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/PIN_SLAM/2a2c6f7dc2b3b01efb283d430165cb80d26f835b/cad/kitti_car.ply -------------------------------------------------------------------------------- /config/lidar_slam/run.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_pin" 3 | output_root: "./experiments" 4 | deskew: True 5 | process: 6 | max_range_m: 80.0 # maximum distance filter for each frame 7 | sampler: 8 | surface_sample_range_m: 0.25 9 | neuralpoints: 10 | voxel_size_m: 0.4 11 | search_alpha: 0.5 # increase when you want to be more robust to agressive motions 12 | continual: 13 | batch_size_new_sample: 1000 14 | pool_capacity: 1e7 15 | pool_filter_freq: 10 16 | tracker: 17 | source_vox_down_m: 0.6 18 | iter_n: 50 19 | valid_nn_k: 5 20 | pgo: 21 | map_context: True 22 | context_cosdist: 0.3 23 | optimizer: # mapper 24 | batch_size: 10000 25 | adaptive_iters: True 26 | eval: 27 | wandb_vis_on: False # log to wandb or not 28 | silence_log: True -------------------------------------------------------------------------------- /config/lidar_slam/run_apollo.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_apollo" 3 | output_root: "./experiments" 4 | pc_path: "./data/apollo/ColumbiaPark/2018-10-11/pcds" 5 | pose_path: "./data/apollo/ColumbiaPark/2018-10-11/gt_poses_kitti_format.txt" 6 | first_frame_ref: True 7 | process: 8 | min_range_m: 3.0 9 | max_range_m: 80.0 10 | vox_down_m: 0.08 11 | sampler: 12 | surface_sample_range_m: 0.25 13 | surface_sample_n: 4 14 | free_sample_begin_ratio: 0.5 15 | free_sample_end_dist_m: 1.0 16 | free_front_sample_n: 2 17 | neuralpoints: 18 | voxel_size_m: 0.4 19 | search_alpha: 0.5 20 | loss: 21 | sigma_sigmoid_m: 0.08 22 | loss_weight_on: True 23 | dist_weight_scale: 0.8 24 | continual: 25 | batch_size_new_sample: 1000 26 | pool_capacity: 2e7 27 | tracker: 28 | source_vox_down_m: 0.6 29 | iter_n: 50 30 | valid_nn_k: 5 31 | pgo: 32 | map_context: True 33 | pgo_freq_frame: 30 34 | context_cosdist: 0.3 35 | virtual_side_count: 10 36 | local_loop_dist_thre: 20.0 37 | optimizer: # mapper 38 | iters: 15 # iterations per frame 39 | batch_size: 16384 40 | eval: 41 | sensor_cad_path: ./cad/ipb_car.ply 42 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 43 | mesh_min_nn: 9 -------------------------------------------------------------------------------- /config/lidar_slam/run_demo.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "sanity_test" 3 | output_root: "./experiments" 4 | pc_path: "./data/kitti_example/sequences/00/velodyne" 5 | process: 6 | max_range_m: 60.0 7 | tracker: 8 | iter_n: 20 9 | eval: 10 | sensor_cad_path: ./cad/kitti_car.ply -------------------------------------------------------------------------------- /config/lidar_slam/run_demo_sem.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "sanity_test_sem" 3 | output_root: "./experiments" 4 | pc_path: "./data/kitti_example/sequences/00/velodyne" 5 | label_path: "./data/kitti_example/sequences/00/labels" 6 | semantic_on: True 7 | process: 8 | max_range_m: 60.0 9 | tracker: 10 | iter_n: 20 11 | eval: 12 | sensor_cad_path: ./cad/kitti_car.ply -------------------------------------------------------------------------------- /config/lidar_slam/run_fast.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_fast" 3 | output_root: "./experiments" 4 | process: 5 | max_range_m: 100.0 6 | vox_down_m: 0.1 7 | sampler: 8 | surface_sample_range_m: 0.35 9 | neuralpoints: 10 | voxel_size_m: 0.6 11 | continual: 12 | batch_size_new_sample: 800 13 | pool_capacity: 5e6 # 1e7 14 | pool_filter_freq: 10 15 | tracker: 16 | source_vox_down_m: 1.5 17 | iter_n: 15 18 | term_deg: 0.1 19 | pgo: 20 | pgo_freq_frame: 50 21 | context_cosdist: 0.25 22 | virtual_side_count: 0 23 | optimizer: # mappers 24 | batch_size: 4000 25 | adaptive_iters: True 26 | eval: 27 | mesh_freq_frame: 20 # reconstruct the mesh every x frames 28 | mesh_min_nn: 9 -------------------------------------------------------------------------------- /config/lidar_slam/run_hilti.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_hilti" 3 | output_root: "./experiments" 4 | pc_path: "./data/HILTI/cons2/ply" #rpg, lab, base1, base4, camp2, cons2 5 | deskew: True 6 | process: 7 | min_range_m: 1.0 8 | max_range_m: 60.0 9 | min_z_m: -10.0 10 | vox_down_m: 0.06 11 | adaptive_range_on: True 12 | sampler: 13 | surface_sample_range_m: 0.25 14 | surface_sample_n: 4 15 | free_sample_begin_ratio: 0.5 16 | free_sample_end_dist_m: 1.0 17 | free_front_sample_n: 2 18 | neuralpoints: 19 | voxel_size_m: 0.3 20 | search_alpha: 0.8 21 | loss: 22 | sigma_sigmoid_m: 0.08 23 | loss_weight_on: True 24 | dist_weight_scale: 0.8 25 | ekional_loss_on: True 26 | weight_e: 0.5 27 | continual: 28 | batch_size_new_sample: 1000 29 | pool_capacity: 2e7 30 | tracker: 31 | source_vox_down_m: 0.4 32 | iter_n: 100 33 | pgo: 34 | map_context: True 35 | pgo_freq_frame: 30 36 | optimizer: # mapper 37 | iters: 15 # iterations per frame 38 | batch_size: 16384 39 | adaptive_iters: True 40 | eval: 41 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 42 | mesh_min_nn: 18 -------------------------------------------------------------------------------- /config/lidar_slam/run_ipbcar.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_ipbcar" 3 | output_root: "./experiments" 4 | pc_path: "./data/ipb_car/01/ply" 5 | pose_path: "./data/ipb_car/01/poses.txt" 6 | deskew: True 7 | process: 8 | min_range_m: 3.0 9 | max_range_m: 80.0 10 | sampler: 11 | surface_sample_range_m: 0.25 12 | surface_sample_n: 4 13 | free_sample_begin_ratio: 0.5 14 | free_sample_end_dist_m: 1.0 15 | free_front_sample_n: 2 16 | neuralpoints: 17 | search_alpha: 0.5 18 | weighted_first: False 19 | loss: 20 | loss_weight_on: True 21 | dist_weight_scale: 0.8 22 | ekional_loss_on: True 23 | weight_e: 0.5 24 | continual: 25 | batch_size_new_sample: 3000 26 | pool_capacity: 2e7 27 | pool_filter_freq: 10 28 | tracker: 29 | iter_n: 100 30 | pgo: 31 | map_context: True 32 | pgo_freq_frame: 30 33 | virtual_side_count: 6 34 | optimizer: # mapper 35 | iters: 15 # iterations per frame 36 | batch_size: 16384 37 | eval: 38 | sensor_cad_path: ./cad/ipb_car.ply 39 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 40 | mesh_min_nn: 15 -------------------------------------------------------------------------------- /config/lidar_slam/run_kitti.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_kitti" 3 | output_root: "./experiments" 4 | pc_path: "./data/kitti/sequences/00/velodyne" 5 | pose_path: "./data/kitti/poses/00.txt" 6 | calib_path: "./data/kitti/sequences/00/calib.txt" 7 | kitti_correct: True 8 | correct_deg: 0.195 9 | process: 10 | min_range_m: 3.0 11 | max_range_m: 80.0 12 | vox_down_m: 0.08 13 | min_z_m: -3.5 14 | sampler: 15 | surface_sample_range_m: 0.25 16 | surface_sample_n: 4 17 | free_sample_begin_ratio: 0.3 18 | free_sample_end_dist_m: 1.0 19 | free_front_sample_n: 2 20 | neuralpoints: 21 | voxel_size_m: 0.4 22 | feature_dim: 8 23 | query_nn_k: 6 24 | search_alpha: 0.2 25 | weighted_first: False 26 | decoder: 27 | freeze_after_frame: 40 28 | loss: 29 | main_loss_type: bce 30 | sigma_sigmoid_m: 0.08 31 | loss_weight_on: True 32 | dist_weight_scale: 0.8 33 | ekional_loss_on: True 34 | weight_e: 0.5 35 | continual: 36 | batch_size_new_sample: 1000 37 | pool_capacity: 2e7 38 | tracker: 39 | source_vox_down_m: 0.6 40 | iter_n: 100 41 | GM_grad: 0.1 42 | GM_dist: 0.2 43 | pgo: 44 | pgo_freq_frame: 20 45 | context_cosdist: 0.3 46 | optimizer: # mapper 47 | batch_size: 16384 48 | eval: 49 | o3d_vis_on: False # visualize the mapping or not 50 | silence_log: True # output the logs or not 51 | sensor_cad_path: ./cad/kitti_car.ply 52 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 53 | mesh_min_nn: 9 -------------------------------------------------------------------------------- /config/lidar_slam/run_kitti_color.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_kitti_color" 3 | output_root: "./experiments" 4 | # in the kitti data folder, the `image_2` folder is required 5 | # please use kitti dataloader with -d flag 6 | # this config also support kitti360 dataset 7 | kitti_correct: True 8 | correct_deg: 0.195 9 | color_channel: 3 10 | color_map_on: True 11 | process: 12 | min_range_m: 3.0 13 | max_range_m: 80.0 14 | vox_down_m: 0.08 15 | min_z_m: -3.5 16 | sampler: 17 | surface_sample_range_m: 0.25 18 | surface_sample_n: 3 19 | free_sample_begin_ratio: 0.3 20 | free_sample_end_dist_m: 1.0 21 | free_front_sample_n: 4 # 2 22 | neuralpoints: 23 | voxel_size_m: 0.4 24 | feature_dim: 8 25 | query_nn_k: 6 26 | search_alpha: 0.2 27 | weighted_first: False 28 | decoder: 29 | freeze_after_frame: 40 30 | loss: 31 | main_loss_type: bce 32 | sigma_sigmoid_m: 0.08 33 | loss_weight_on: True 34 | dist_weight_scale: 0.8 35 | ekional_loss_on: True 36 | weight_e: 0.5 37 | continual: 38 | batch_size_new_sample: 4000 39 | pool_capacity: 2e7 40 | tracker: 41 | source_vox_down_m: 0.6 42 | iter_n: 100 43 | GM_grad: 0.1 44 | GM_dist: 0.2 45 | pgo: 46 | pgo_freq_frame: 20 47 | context_cosdist: 0.3 48 | optimizer: # mapper 49 | batch_size: 16384 50 | eval: 51 | o3d_vis_on: False # visualize the mapping or not 52 | silence_log: True # output the logs or not 53 | sensor_cad_path: ./cad/kitti_car.ply 54 | mesh_freq_frame: 20 # reconstruct the mesh every x frames 55 | mesh_min_nn: 9 -------------------------------------------------------------------------------- /config/lidar_slam/run_kitti_mos.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_kitti_mos" 3 | # you may test it with kitti_tracking data sequences: 4 | # wget https://www.ipb.uni-bonn.de/html/projects/kitti-tracking/post-processed/kitti-tracking.zip 5 | # you can use the same dynamic filtering parameter settings on other datasets 6 | output_root: "./experiments" 7 | pc_path: "./data/kitti/sequences/00/velodyne" 8 | pose_path: "./data/kitti/poses/00.txt" 9 | calib_path: "./data/kitti/sequences/00/calib.txt" 10 | kitti_correct: True 11 | correct_deg: 0.195 12 | process: 13 | min_range_m: 3.0 14 | max_range_m: 80.0 15 | vox_down_m: 0.08 16 | min_z_m: -3.5 17 | dynamic_filter_on: True 18 | dynamic_certainty_thre: 1.0 19 | dynamic_sdf_ratio_thre: 0.5 20 | dynamic_min_grad_norm_thre: 0.25 21 | sampler: 22 | surface_sample_range_m: 0.25 23 | surface_sample_n: 3 24 | free_sample_begin_ratio: 0.3 25 | free_sample_end_dist_m: 1.0 26 | free_front_sample_n: 4 # large value here for more free-space supervision 27 | neuralpoints: 28 | voxel_size_m: 0.4 29 | feature_dim: 8 30 | query_nn_k: 6 31 | search_alpha: 0.5 32 | weighted_first: False 33 | decoder: 34 | freeze_after_frame: 40 35 | loss: 36 | main_loss_type: bce 37 | sigma_sigmoid_m: 0.08 38 | loss_weight_on: True 39 | dist_weight_scale: 0.8 40 | ekional_loss_on: True 41 | weight_e: 0.5 42 | continual: 43 | batch_size_new_sample: 1000 44 | pool_capacity: 2e7 45 | tracker: 46 | source_vox_down_m: 0.6 47 | iter_n: 100 48 | GM_grad: 0.1 49 | GM_dist: 0.2 50 | pgo: 51 | pgo_freq_frame: 20 52 | context_cosdist: 0.3 53 | optimizer: # mapper 54 | batch_size: 16384 55 | eval: 56 | o3d_vis_on: False # visualize the mapping or not 57 | silence_log: True # output the logs or not 58 | sensor_cad_path: ./cad/kitti_car.ply 59 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 60 | mesh_min_nn: 9 -------------------------------------------------------------------------------- /config/lidar_slam/run_light.yaml: -------------------------------------------------------------------------------- 1 | # PIN-SLAM light 2 | setting: 3 | name: "test_light" 4 | output_root: "./experiments" 5 | pc_path: "./data/your_dataset/pointcloud" 6 | pose_path: "./data/your_dataset/poses.txt" 7 | process: 8 | min_range_m: 3.0 9 | max_range_m: 80.0 10 | vox_down_m: 0.12 11 | sampler: 12 | surface_sample_range_m: 0.35 13 | surface_sample_n: 2 14 | free_sample_begin_ratio: 0.5 15 | free_sample_end_dist_m: 1.0 16 | free_front_sample_n: 1 17 | neuralpoints: 18 | voxel_size_m: 0.4 19 | loss: 20 | sigma_sigmoid_m: 0.08 21 | loss_weight_on: True 22 | dist_weight_scale: 0.8 23 | continual: 24 | pool_capacity: 1e7 25 | pool_filter_freq: 10 26 | tracker: 27 | source_vox_down_m: 1.0 28 | iter_n: 20 29 | term_deg: 0.1 30 | valid_nn_k: 5 31 | pgo: 32 | pgo_freq_frame: 50 33 | context_cosdist: 0.25 34 | virtual_side_count: 0 35 | optimizer: # mappers 36 | batch_size: 8192 37 | adaptive_iters: True 38 | eval: 39 | o3d_vis_on: False # visualize the mapping or not 40 | silence_log: True # output the logs or not 41 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 42 | mesh_min_nn: 9 -------------------------------------------------------------------------------- /config/lidar_slam/run_livox.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_livox" 3 | output_root: "./experiments" 4 | pc_path: "./data/livox/HKU_ZYM/pcd" 5 | process: 6 | min_range_m: 0.5 7 | max_range_m: 30.0 8 | min_z_m: -30.0 9 | vox_down_m: 0.05 10 | sampler: 11 | surface_sample_range_m: 0.15 12 | surface_sample_n: 3 13 | free_sample_begin_ratio: 0.3 14 | free_sample_end_dist_m: 0.6 15 | free_front_sample_n: 3 16 | neuralpoints: 17 | voxel_size_m: 0.15 18 | query_nn_k: 8 19 | search_alpha: 0.5 20 | weighted_first: False 21 | loss: 22 | sigma_sigmoid_m: 0.08 23 | loss_weight_on: True 24 | dist_weight_scale: 0.5 25 | ekional_loss_on: True 26 | weight_e: 0.5 27 | numerical_grad_on: False 28 | continual: 29 | batch_size_cur_frame: 2000 30 | pool_capacity: 2e7 31 | tracker: 32 | source_vox_down_m: 0.2 33 | iter_n: 100 34 | optimizer: 35 | iters: 15 36 | batch_size: 8192 37 | eval: 38 | o3d_vis_on: False # visualize the mapping or not 39 | mesh_freq_frame: 20 # reconstruct the mesh every x frames 40 | mesh_min_nn: 15 -------------------------------------------------------------------------------- /config/lidar_slam/run_mulran.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_mulran" 3 | output_root: "./experiments" 4 | pc_path: "./data/MulRan/kaist01/Ouster" 5 | pose_path: "./data/MulRan/kaist01/poses.txt" 6 | deskew: True 7 | process: 8 | min_range_m: 3.0 9 | max_range_m: 80.0 10 | sampler: 11 | surface_sample_range_m: 0.3 12 | surface_sample_n: 4 13 | free_sample_begin_ratio: 0.5 14 | free_sample_end_dist_m: 1.0 15 | free_front_sample_n: 2 16 | neuralpoints: 17 | voxel_size_m: 0.4 18 | search_alpha: 0.8 19 | loss: 20 | loss_weight_on: True 21 | dist_weight_scale: 0.8 22 | ekional_loss_on: True 23 | weight_e: 0.5 24 | continual: 25 | batch_size_new_sample: 3000 26 | pool_capacity: 1e7 27 | tracker: 28 | source_vox_down_m: 0.6 29 | iter_n: 100 30 | eigenvalue_check: False 31 | GM_dist: 0.2 32 | valid_nn_k: 5 33 | pgo: 34 | map_context: True 35 | pgo_freq_frame: 30 36 | context_cosdist: 0.25 37 | virtual_side_count: 10 # added 38 | local_loop_dist_thre: 20.0 # added 39 | optimizer: # mapper 40 | iters: 15 # iterations per frame 41 | batch_size: 16384 42 | eval: 43 | o3d_vis_on: False # visualize the mapping or not 44 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 45 | mesh_min_nn: 18 -------------------------------------------------------------------------------- /config/lidar_slam/run_ncd.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_ncd" 3 | output_root: "./experiments" 4 | pc_path: "./data/ncd/01/bin" 5 | pose_path: "./data/ncd/01/poses.txt" 6 | process: 7 | min_range_m: 1.5 8 | max_range_m: 80.0 9 | min_z_m: -10.0 10 | vox_down_m: 0.1 11 | adaptive_range_on: True 12 | sampler: 13 | surface_sample_range_m: 0.35 14 | surface_sample_n: 3 15 | free_sample_begin_ratio: 0.3 16 | free_sample_end_dist_m: 1.2 17 | free_front_sample_n: 2 18 | neuralpoints: 19 | voxel_size_m: 0.4 20 | search_alpha: 0.8 21 | use_mid_ts: True 22 | loss: 23 | sigma_sigmoid_m: 0.1 24 | loss_weight_on: True 25 | dist_weight_scale: 1.0 26 | ekional_loss_on: True 27 | weight_e: 0.5 28 | continual: 29 | batch_size_new_sample: 1000 30 | pool_capacity: 2e7 31 | tracker: 32 | source_vox_down_m: 0.8 33 | iter_n: 100 34 | valid_nn_k: 5 35 | pgo: 36 | map_context: True 37 | context_cosdist: 0.25 38 | pgo_freq_frame: 30 39 | optimizer: # mapper 40 | iters: 15 41 | batch_size: 16384 42 | eval: 43 | o3d_vis_on: False # visualize the mapping or not 44 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 45 | mesh_min_nn: 18 -------------------------------------------------------------------------------- /config/lidar_slam/run_ncd_128.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_ncd_128_easy" 3 | output_root: "./experiments" 4 | pc_path: "./data/ncd_128/quad_e/ply" 5 | pose_path: "./data/ncd_128/quad_e/poses.txt" 6 | deskew: True 7 | process: 8 | min_range_m: 1.5 9 | max_range_m: 60.0 10 | min_z_m: -10.0 11 | vox_down_m: 0.08 12 | adaptive_range_on: True 13 | sampler: 14 | surface_sample_range_m: 0.25 15 | surface_sample_n: 4 16 | free_sample_begin_ratio: 0.5 17 | free_sample_end_dist_m: 1.2 18 | free_front_sample_n: 2 19 | neuralpoints: 20 | voxel_size_m: 0.4 21 | search_alpha: 0.5 22 | weighted_first: False 23 | loss: 24 | sigma_sigmoid_m: 0.08 25 | loss_weight_on: True 26 | dist_weight_scale: 0.8 27 | continual: 28 | batch_size_new_sample: 1000 29 | pool_capacity: 2e7 30 | tracker: 31 | source_vox_down_m: 0.6 32 | iter_n: 100 33 | valid_nn_k: 5 34 | pgo: 35 | map_context: True 36 | context_cosdist: 0.3 37 | min_loop_travel_ratio: 3.0 38 | optimizer: # mapper 39 | iters: 15 40 | batch_size: 16384 41 | adaptive_iters: True 42 | eval: 43 | o3d_vis_on: False # visualize the mapping or not 44 | silence_log: True 45 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 46 | mesh_min_nn: 15 -------------------------------------------------------------------------------- /config/lidar_slam/run_ncd_128_m.yaml: -------------------------------------------------------------------------------- 1 | setting: # used for middle scenes such as cloister 2 | name: "test_ncd_128_middle" 3 | output_root: "./experiments" 4 | pc_path: "./data/ncd_128/cloister/ply" 5 | pose_path: "./data/ncd_128/cloister/poses.txt" 6 | deskew: True 7 | process: 8 | min_range_m: 1.5 9 | max_range_m: 60.0 10 | min_z_m: -10.0 11 | vox_down_m: 0.08 12 | adaptive_range_on: True 13 | sampler: 14 | surface_sample_range_m: 0.35 15 | surface_sample_n: 4 16 | free_sample_begin_ratio: 0.5 17 | free_sample_end_dist_m: 1.2 18 | free_front_sample_n: 2 19 | neuralpoints: 20 | voxel_size_m: 0.4 21 | search_alpha: 0.5 22 | weighted_first: False 23 | loss: 24 | sigma_sigmoid_m: 0.1 25 | loss_weight_on: True 26 | dist_weight_scale: 0.8 27 | continual: 28 | batch_size_new_sample: 3000 29 | pool_capacity: 2e7 30 | tracker: 31 | source_vox_down_m: 0.6 32 | iter_n: 100 33 | valid_nn_k: 5 34 | pgo: 35 | map_context: True 36 | context_cosdist: 0.3 37 | min_loop_travel_ratio: 3.0 38 | optimizer: # mapper 39 | iters: 15 40 | batch_size: 16384 41 | adaptive_iters: True 42 | eval: 43 | o3d_vis_on: False # visualize the mapping or not 44 | silence_log: True 45 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 46 | mesh_min_nn: 15 -------------------------------------------------------------------------------- /config/lidar_slam/run_ncd_128_s.yaml: -------------------------------------------------------------------------------- 1 | setting: # used for small scenes such as stairs 2 | name: "test_ncd_128_small" 3 | output_root: "./experiments" 4 | pc_path: "./data/ncd_128/stairs/ply" 5 | pose_path: "./data/ncd_128/stairs/poses.txt" 6 | deskew: True 7 | process: 8 | min_range_m: 0.5 9 | max_range_m: 15.0 10 | min_z_m: -10.0 11 | vox_down_m: 0.05 12 | adaptive_range_on: True 13 | sampler: 14 | surface_sample_range_m: 0.15 15 | surface_sample_n: 3 16 | free_sample_begin_ratio: 0.3 17 | free_sample_end_dist_m: 0.6 18 | free_front_sample_n: 1 19 | neuralpoints: 20 | voxel_size_m: 0.15 21 | search_alpha: 0.5 22 | weighted_first: False 23 | loss: 24 | sigma_sigmoid_m: 0.05 25 | loss_weight_on: True 26 | dist_weight_scale: 0.5 27 | continual: 28 | batch_size_new_sample: 1000 29 | pool_capacity: 1e7 30 | tracker: 31 | source_vox_down_m: 0.2 32 | iter_n: 100 33 | pgo: 34 | map_context: True 35 | context_cosdist: 0.3 36 | min_loop_travel_ratio: 3.0 37 | optimizer: # mapper 38 | iters: 15 # iterations per frame 39 | batch_size: 16384 40 | adaptive_iters: True 41 | ba_freq_frame: 20 42 | lr_pose_ba: 1e-3 43 | eval: 44 | o3d_vis_on: False # visualize the mapping or not 45 | silence_log: True 46 | mesh_freq_frame: 50 47 | mesh_min_nn: 18 -------------------------------------------------------------------------------- /config/lidar_slam/run_ncd_recon.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_ncd_recon" 3 | output_root: "./experiments" 4 | pc_path: "data/ncd_example/quad/pcd" 5 | pose_path: "data/ncd_example/quad/poses.txt" 6 | deskew: False 7 | process: 8 | min_range_m: 1.5 9 | max_range_m: 80.0 10 | min_z_m: -10.0 11 | vox_down_m: 0.08 12 | adaptive_range_on: True 13 | sampler: 14 | surface_sample_range_m: 0.3 15 | surface_sample_n: 6 16 | free_sample_begin_ratio: 0.3 17 | free_sample_end_dist_m: 1.2 18 | free_front_sample_n: 2 19 | neuralpoints: 20 | voxel_size_m: 0.3 21 | search_alpha: 0.5 22 | use_mid_ts: True 23 | loss: 24 | sigma_sigmoid_m: 0.1 25 | loss_weight_on: True 26 | dist_weight_scale: 1.0 27 | ekional_loss_on: True 28 | weight_e: 0.5 29 | continual: 30 | batch_size_new_sample: 1000 31 | pool_capacity: 2e7 32 | tracker: 33 | source_vox_down_m: 0.8 34 | iter_n: 100 35 | valid_nn_k: 5 36 | pgo: 37 | map_context: True 38 | context_cosdist: 0.25 39 | pgo_freq_frame: 30 40 | optimizer: # mapper 41 | iters: 30 42 | batch_size: 16384 43 | eval: 44 | o3d_vis_on: False # visualize the mapping or not 45 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 46 | mesh_min_nn: 18 -------------------------------------------------------------------------------- /config/lidar_slam/run_vbr.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_vbr" 3 | output_root: "./experiments" 4 | pc_path: "./data/vbr/vbr_slam/colosseo/colosseo_train0/ply" 5 | pose_path: "./data/vbr/vbr_slam/colosseo/colosseo_train0/colosseo_train0_gt.txt" 6 | deskew: True 7 | process: 8 | min_range_m: 1.5 9 | max_range_m: 60.0 10 | min_z_m: -10.0 11 | vox_down_m: 0.08 12 | sampler: 13 | surface_sample_range_m: 0.25 14 | surface_sample_n: 4 15 | free_sample_begin_ratio: 0.5 16 | free_sample_end_dist_m: 1.2 17 | free_front_sample_n: 2 18 | neuralpoints: 19 | voxel_size_m: 0.3 20 | search_alpha: 0.5 21 | weighted_first: False 22 | loss: 23 | sigma_sigmoid_m: 0.08 24 | loss_weight_on: True 25 | dist_weight_scale: 0.8 26 | continual: 27 | batch_size_new_sample: 1000 28 | pool_capacity: 2e7 29 | pool_filter_freq: 10 30 | tracker: 31 | source_vox_down_m: 0.6 32 | iter_n: 50 33 | valid_nn_k: 5 34 | pgo: 35 | map_context: True 36 | context_cosdist: 0.3 37 | min_loop_travel_ratio: 3.0 38 | optimizer: # mapper 39 | iters: 15 40 | batch_size: 16384 41 | adaptive_iters: True 42 | eval: 43 | o3d_vis_on: False # visualize the mapping or not 44 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 45 | mesh_min_nn: 15 -------------------------------------------------------------------------------- /config/pin_slam_ros.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.7984189987182617 8 | Tree Height: 1101 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Name: Time 25 | SyncMode: 0 26 | SyncSource: Neural Point Map 27 | Preferences: 28 | PromptSaveOnExit: true 29 | Toolbars: 30 | toolButtonStyle: 2 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: false 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: false 52 | - Alpha: 1 53 | Class: rviz/Axes 54 | Enabled: true 55 | Length: 1 56 | Name: Axes 57 | Radius: 0.10000000149011612 58 | Reference Frame: range_sensor 59 | Show Trail: false 60 | Value: true 61 | - Alpha: 0.30000001192092896 62 | Autocompute Intensity Bounds: true 63 | Autocompute Value Bounds: 64 | Max Value: 4.691665172576904 65 | Min Value: -0.9005652070045471 66 | Value: true 67 | Axis: Z 68 | Channel Name: intensity 69 | Class: rviz/PointCloud2 70 | Color: 255; 255; 255 71 | Color Transformer: AxisColor 72 | Decay Time: 0 73 | Enabled: true 74 | Invert Rainbow: false 75 | Max Color: 255; 255; 255 76 | Min Color: 0; 0; 0 77 | Name: Neural Point Map 78 | Position Transformer: XYZ 79 | Queue Size: 10 80 | Selectable: true 81 | Size (Pixels): 5 82 | Size (m): 0.009999999776482582 83 | Style: Points 84 | Topic: /pin_slam/map/neural_points 85 | Unreliable: false 86 | Use Fixed Frame: true 87 | Use rainbow: true 88 | Value: true 89 | - Alpha: 1 90 | Autocompute Intensity Bounds: true 91 | Autocompute Value Bounds: 92 | Max Value: 10 93 | Min Value: -10 94 | Value: true 95 | Axis: Z 96 | Channel Name: intensity 97 | Class: rviz/PointCloud2 98 | Color: 239; 41; 41 99 | Color Transformer: FlatColor 100 | Decay Time: 0 101 | Enabled: true 102 | Invert Rainbow: false 103 | Max Color: 255; 255; 255 104 | Min Color: 0; 0; 0 105 | Name: Points for Registration 106 | Position Transformer: XYZ 107 | Queue Size: 10 108 | Selectable: true 109 | Size (Pixels): 3 110 | Size (m): 0.10000000149011612 111 | Style: Points 112 | Topic: /pin_slam/frame/registration 113 | Unreliable: false 114 | Use Fixed Frame: true 115 | Use rainbow: true 116 | Value: true 117 | - Alpha: 1 118 | Autocompute Intensity Bounds: true 119 | Autocompute Value Bounds: 120 | Max Value: 10 121 | Min Value: -10 122 | Value: true 123 | Axis: Z 124 | Channel Name: intensity 125 | Class: rviz/PointCloud2 126 | Color: 115; 210; 22 127 | Color Transformer: FlatColor 128 | Decay Time: 0 129 | Enabled: false 130 | Invert Rainbow: false 131 | Max Color: 255; 255; 255 132 | Min Color: 0; 0; 0 133 | Name: Points for Mapping 134 | Position Transformer: XYZ 135 | Queue Size: 10 136 | Selectable: true 137 | Size (Pixels): 3 138 | Size (m): 0.05000000074505806 139 | Style: Points 140 | Topic: /pin_slam/frame/mapping 141 | Unreliable: false 142 | Use Fixed Frame: true 143 | Use rainbow: true 144 | Value: false 145 | - Alpha: 1 146 | Buffer Length: 1 147 | Class: rviz/Path 148 | Color: 25; 255; 0 149 | Enabled: true 150 | Head Diameter: 0.30000001192092896 151 | Head Length: 0.20000000298023224 152 | Length: 0.30000001192092896 153 | Line Style: Billboards 154 | Line Width: 0.10000000149011612 155 | Name: Path 156 | Offset: 157 | X: 0 158 | Y: 0 159 | Z: 0 160 | Pose Color: 255; 85; 255 161 | Pose Style: None 162 | Queue Size: 10 163 | Radius: 0.029999999329447746 164 | Shaft Diameter: 0.10000000149011612 165 | Shaft Length: 0.10000000149011612 166 | Topic: /pin_slam/pin_path 167 | Unreliable: false 168 | Value: true 169 | Enabled: true 170 | Global Options: 171 | Background Color: 48; 48; 48 172 | Default Light: true 173 | Fixed Frame: map 174 | Frame Rate: 30 175 | Name: root 176 | Tools: 177 | - Class: rviz/Interact 178 | Hide Inactive Objects: true 179 | - Class: rviz/MoveCamera 180 | - Class: rviz/Select 181 | - Class: rviz/FocusCamera 182 | - Class: rviz/Measure 183 | - Class: rviz/SetInitialPose 184 | Theta std deviation: 0.2617993950843811 185 | Topic: /initialpose 186 | X std deviation: 0.5 187 | Y std deviation: 0.5 188 | - Class: rviz/SetGoal 189 | Topic: /move_base_simple/goal 190 | - Class: rviz/PublishPoint 191 | Single click: true 192 | Topic: /clicked_point 193 | Value: true 194 | Views: 195 | Current: 196 | Class: rviz/Orbit 197 | Distance: 62.76740264892578 198 | Enable Stereo Rendering: 199 | Stereo Eye Separation: 0.05999999865889549 200 | Stereo Focal Distance: 1 201 | Swap Stereo Eyes: false 202 | Value: false 203 | Field of View: 0.7853981852531433 204 | Focal Point: 205 | X: 0 206 | Y: 0 207 | Z: 0 208 | Focal Shape Fixed Size: false 209 | Focal Shape Size: 0.05000000074505806 210 | Invert Z Axis: false 211 | Name: Current View 212 | Near Clip Distance: 0.009999999776482582 213 | Pitch: 1.5697963237762451 214 | Target Frame: 215 | Yaw: 0.7853981852531433 216 | Saved: ~ 217 | Window Geometry: 218 | Displays: 219 | collapsed: false 220 | Height: 1505 221 | Hide Left Dock: false 222 | Hide Right Dock: false 223 | QMainWindow State: 000000ff00000000fd00000004000000000000022d00000545fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000005450000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000545fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000005450000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e700000005efc0100000002fb0000000800540069006d0065000000000000000e70000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007300000054500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 224 | Selection: 225 | collapsed: false 226 | Time: 227 | collapsed: false 228 | Tool Properties: 229 | collapsed: false 230 | Views: 231 | collapsed: false 232 | Width: 2772 233 | X: 606 234 | Y: 228 235 | -------------------------------------------------------------------------------- /config/rgbd_slam/run_replica.yaml: -------------------------------------------------------------------------------- 1 | setting: 2 | name: "test_replica" 3 | output_root: "./experiments" 4 | pc_path: "./data/Replica/room0/rgbd_ply" 5 | pose_path: "./data/Replica/room0/poses.txt" 6 | color_channel: 3 7 | first_frame_ref: False 8 | process: 9 | min_range_m: 0.05 10 | max_range_m: 10.0 11 | min_z_m: -10.0 12 | vox_down_m: 0.02 13 | sampler: 14 | surface_sample_range_m: 0.03 15 | surface_sample_n: 3 16 | free_sample_begin_ratio: 0.3 17 | free_sample_end_dist_m: 0.1 18 | free_front_sample_n: 1 19 | neuralpoints: 20 | voxel_size_m: 0.05 21 | loss: 22 | sigma_sigmoid_m: 0.01 23 | ekional_loss_on: True 24 | weight_e: 0.2 25 | continual: 26 | batch_size_new_sample: 4000 27 | pool_capacity: 2e7 28 | pool_filter_freq: 10 29 | tracker: 30 | photo_loss: True 31 | photo_weight: 0.01 32 | eigenvalue_check: False 33 | source_vox_down_m: 0.06 34 | iter_n: 50 35 | min_grad_norm: 0.4 36 | max_grad_norm: 2.5 37 | GM_grad: 0.3 38 | GM_dist: 0.05 39 | term_deg: 1e-3 40 | term_m: 1e-4 41 | optimizer: 42 | iters: 20 43 | batch_size: 16384 44 | ba_freq_frame: 20 45 | lr_pose_ba: 1e-3 46 | eval: 47 | mesh_freq_frame: 50 # reconstruct the mesh every x frames 48 | mesh_min_nn: 9 49 | sensor_cad_path: ./cad/camera.ply 50 | skip_top_voxel: 5 -------------------------------------------------------------------------------- /dataset/converter/mulran_to_pin_format.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 24 | # this file is adopted from kiss-icp 25 | 26 | import glob 27 | import os 28 | from pathlib import Path 29 | 30 | import numpy as np 31 | 32 | 33 | class MulranDataset: 34 | def __init__(self, data_dir: Path, *_, **__): 35 | self.sequence_id = os.path.basename(data_dir) 36 | self.sequence_dir = os.path.realpath(data_dir) 37 | self.velodyne_dir = os.path.join(self.sequence_dir, "Ouster/") 38 | 39 | print(self.sequence_dir) 40 | 41 | self.scan_files = sorted(glob.glob(self.velodyne_dir + "*.bin")) 42 | self.scan_timestamps = [int(os.path.basename(t).split(".")[0]) for t in self.scan_files] 43 | self.gt_poses = self.load_gt_poses(os.path.join(self.sequence_dir, "global_pose.csv")) 44 | 45 | write_kitti_format_poses(os.path.join(self.sequence_dir, "poses"), self.gt_poses) # then calib not needed, compared in lidar frame 46 | 47 | def __len__(self): 48 | return len(self.scan_files) 49 | 50 | def __getitem__(self, idx): 51 | return self.read_point_cloud(self.scan_files[idx]) 52 | 53 | def read_point_cloud(self, file_path: str): 54 | points = np.fromfile(file_path, dtype=np.float32).reshape((-1, 4))[:, :3] 55 | timestamps = self.get_timestamps() 56 | if points.shape[0] != timestamps.shape[0]: 57 | # MulRan has some broken point clouds, just fallback to no timestamps 58 | return points.astype(np.float64), np.ones(points.shape[0]) 59 | return points.astype(np.float64), timestamps 60 | 61 | @staticmethod 62 | def get_timestamps(): 63 | H = 64 64 | W = 1024 65 | return (np.floor(np.arange(H * W) / H) / W).reshape(-1, 1) 66 | 67 | def load_gt_poses(self, poses_file: str): 68 | """MuRan has more poses than scans, therefore we need to match 1-1 timestamp with pose""" 69 | 70 | def read_csv(poses_file: str): 71 | poses = np.loadtxt(poses_file, delimiter=",") 72 | timestamps = poses[:, 0] 73 | poses = poses[:, 1:] 74 | n = poses.shape[0] 75 | poses = np.concatenate( 76 | (poses, np.zeros((n, 3), dtype=np.float32), np.ones((n, 1), dtype=np.float32)), 77 | axis=1, 78 | ) 79 | poses = poses.reshape((n, 4, 4)) # [N, 4, 4] 80 | return poses, timestamps 81 | 82 | # Read the csv file 83 | poses, timestamps = read_csv(poses_file) 84 | # Extract only the poses that has a matching Ouster scan 85 | poses = poses[[np.argmin(abs(timestamps - t)) for t in self.scan_timestamps]] 86 | 87 | # print(poses) 88 | 89 | # Convert from global coordinate poses to local poses 90 | first_pose = poses[0, :, :] 91 | poses = np.linalg.inv(first_pose) @ poses 92 | 93 | T_lidar_to_base, T_base_to_lidar = self._get_calibration() 94 | return T_lidar_to_base @ poses @ T_base_to_lidar 95 | 96 | def _get_calibration(self): 97 | # Apply calibration obtainbed from calib_base2ouster.txt 98 | # T_lidar_to_base[:3, 3] = np.array([1.7042, -0.021, 1.8047]) 99 | # T_lidar_to_base[:3, :3] = tu_vieja.from_euler( 100 | # "xyz", [0.0001, 0.0003, 179.6654], degrees=True 101 | # ) 102 | T_lidar_to_base = np.array( 103 | [ 104 | [-9.9998295e-01, -5.8398386e-03, -5.2257060e-06, 1.7042000e00], 105 | [5.8398386e-03, -9.9998295e-01, 1.7758769e-06, -2.1000000e-02], 106 | [-5.2359878e-06, 1.7453292e-06, 1.0000000e00, 1.8047000e00], 107 | [0.0000000e00, 0.0000000e00, 0.0000000e00, 1.0000000e00], 108 | ] 109 | ) 110 | T_base_to_lidar = np.linalg.inv(T_lidar_to_base) 111 | return T_lidar_to_base, T_base_to_lidar 112 | 113 | 114 | def write_kitti_format_poses(filename: str, poses): 115 | def _to_kitti_format(poses: np.ndarray) -> np.ndarray: 116 | return np.array([np.concatenate((pose[0], pose[1], pose[2])) for pose in poses]) 117 | 118 | np.savetxt(fname=f"{filename}.txt", X=_to_kitti_format(poses)) 119 | 120 | # conduct the conversion 121 | loader = MulranDataset("./data/MulRan/kaist01") -------------------------------------------------------------------------------- /dataset/converter/ncd128_pose_converter.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import numpy as np 3 | from pyquaternion import Quaternion 4 | 5 | def read_tum_format_poses_csv(filename: str): 6 | poses = [] 7 | with open(filename, mode="r") as f: 8 | # reader = csv.reader(f, delimiter=' ') # split with space 9 | reader = csv.reader(f, delimiter=',') # split with comma 10 | # # get header and change timestamp label name 11 | header = next(reader) 12 | # header[0] = "ts" 13 | # Convert string odometry to numpy transfor matrices 14 | for row in reader: 15 | # print(row[2:5]) 16 | trans = np.array(row[2:5]) 17 | 18 | quat = Quaternion(np.array([row[8], row[5], row[6], row[7]])) # w, i, j, k 19 | 20 | rot = quat.rotation_matrix 21 | # Build numpy transform matrix 22 | odom_tf = np.eye(4) 23 | odom_tf[0:3, 3] = trans 24 | odom_tf[0:3, 0:3] = rot 25 | poses.append(odom_tf) 26 | 27 | return poses 28 | 29 | # copyright: Nacho et al. KISS-ICP 30 | def write_kitti_format_poses(filename: str, poses): 31 | def _to_kitti_format(poses: np.ndarray) -> np.ndarray: 32 | return np.array([np.concatenate((pose[0], pose[1], pose[2])) for pose in poses]) 33 | 34 | np.savetxt(fname=f"{filename}.txt", X=_to_kitti_format(poses)) 35 | 36 | 37 | in_file = "./data/ncd_128/math_e/gt-state-easy.csv" 38 | out_file = "./data/ncd_128/math_e/poses" 39 | 40 | poses = read_tum_format_poses_csv(in_file) 41 | write_kitti_format_poses(out_file, poses) -------------------------------------------------------------------------------- /dataset/converter/neuralrgbd_to_pin_format.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # @file neuralrgbd_to_pin_format.py 3 | # @author Yue Pan [yue.pan@igg.uni-bonn.de] 4 | # Copyright (c) 2024 Yue Pan, all rights reserved 5 | 6 | import open3d as o3d 7 | from tqdm import tqdm 8 | import argparse 9 | import os 10 | import numpy as np 11 | import json 12 | import sys 13 | import glob 14 | import re 15 | 16 | sys.path.append('./') # the script is launched in the root folder, required for import visualizer 17 | from utils.visualizer import MapVisualizer 18 | 19 | # icl living room data is a bit different 20 | def neuralrgbd_to_pin_format(args): 21 | 22 | if args.vis_on: 23 | vis = MapVisualizer() 24 | 25 | color_paths = [os.path.join(args.input_root, 'images', f) for f in sorted(os.listdir(os.path.join(args.input_root, 'images')), key=alphanum_key) if f.endswith('png')] 26 | # depth_paths = [os.path.join(args.input_root, 'depth_filtered', f) for f in sorted(os.listdir(os.path.join(args.input_root, 'depth_filtered')), key=alphanum_key) if f.endswith('png')] 27 | depth_paths = [os.path.join(args.input_root, 'depth', f) for f in sorted(os.listdir(os.path.join(args.input_root, 'depth')), key=alphanum_key) if f.endswith('png')] 28 | 29 | # get pose 30 | all_gt_poses, valid_gt_poses = load_poses(os.path.join(args.input_root, 'poses.txt')) 31 | 32 | pose_kitti_format_path = os.path.join(args.output_root, "poses_pin.txt") 33 | 34 | write_poses_kitti_format(all_gt_poses, pose_kitti_format_path) 35 | 36 | if args.down_sample: 37 | ply_path = os.path.join(args.output_root, "rgbd_down_ply") 38 | else: 39 | # ply_path = os.path.join(args.output_root, "rgbd_ply") 40 | ply_path = os.path.join(args.output_root, "rgbd_gt_ply") 41 | 42 | os.makedirs(ply_path, exist_ok=True) 43 | 44 | H = 480 45 | W = 640 46 | 47 | intrinsic = o3d.camera.PinholeCameraIntrinsic() 48 | 49 | focal = load_focal_length(args.intrinsic_file) 50 | print("Focal length:", focal) 51 | 52 | intrinsic.set_intrinsics(height=H, 53 | width=W, 54 | fx=focal, 55 | fy=focal, 56 | cx=(W-1.)/2., 57 | cy=(H-1.)/2.) 58 | 59 | depth_scale = 1000. 60 | # use this extrinsic matrix to rotate the image since frames captured with RealSense camera are upside down # really? 61 | extrinsic = np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) 62 | # extrinsic = np.eye(4) 63 | 64 | print("Camera Intrinsics:") 65 | print(intrinsic.intrinsic_matrix) 66 | 67 | map_pcd = o3d.geometry.PointCloud() 68 | 69 | frame_count = 0 70 | for color_path, depth_path in tqdm(zip(color_paths, depth_paths)): 71 | 72 | frame_id_str = f'{frame_count:06d}' 73 | cur_filename = frame_id_str+".ply" 74 | cur_path = os.path.join(ply_path, cur_filename) 75 | 76 | print(frame_id_str) 77 | print(color_path) 78 | print(depth_path) 79 | 80 | im_color = o3d.io.read_image(color_path) 81 | im_depth = o3d.io.read_image(depth_path) 82 | im_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(im_color, im_depth, depth_scale=depth_scale, 83 | depth_trunc=args.max_depth_m, convert_rgb_to_intensity=False) 84 | 85 | im_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(im_rgbd, intrinsic, extrinsic) 86 | 87 | if args.down_sample: 88 | im_pcd = im_pcd.random_down_sample(sampling_ratio=0.2) # TODO change here 89 | 90 | o3d.io.write_point_cloud(cur_path, im_pcd) 91 | 92 | if args.vis_on: 93 | im_pcd = im_pcd.transform(all_gt_poses[frame_count]) 94 | map_pcd += im_pcd 95 | map_pcd = map_pcd.voxel_down_sample(0.05) 96 | vis.update_pointcloud(map_pcd) 97 | 98 | frame_count+=1 99 | 100 | print("The Neural RGBD dataset in our format has been saved at %s", args.output_root) 101 | 102 | if args.vis_on: 103 | vis.stop() 104 | 105 | def load_focal_length(filepath): 106 | file = open(filepath, "r") 107 | return float(file.readline()) 108 | 109 | def load_poses(path): 110 | file = open(path, "r") 111 | lines = file.readlines() 112 | file.close() 113 | poses = [] 114 | valid = [] 115 | lines_per_matrix = 4 116 | for i in range(0, len(lines), lines_per_matrix): 117 | if 'nan' in lines[i]: 118 | valid.append(False) 119 | poses.append(np.eye(4, 4, dtype=np.float32).tolist()) 120 | else: 121 | valid.append(True) 122 | pose_floats = np.array([[float(x) for x in line.split()] for line in lines[i:i+lines_per_matrix]]) 123 | poses.append(pose_floats) 124 | 125 | return poses, valid 126 | 127 | def write_poses_kitti_format(poses_mat, posefile): 128 | poses_vec = [] 129 | for pose_mat in poses_mat: 130 | pose_vec= pose_mat.flatten()[0:12] 131 | poses_vec.append(pose_vec) 132 | np.savetxt(posefile, poses_vec, delimiter=' ') 133 | 134 | def alphanum_key(s): 135 | """ Turn a string into a list of string and number chunks. 136 | "z23a" -> ["z", 23, "a"] 137 | """ 138 | return [int(x) if x.isdigit() else x for x in re.split('([0-9]+)', s)] 139 | 140 | def str2bool(v): 141 | if isinstance(v, bool): 142 | return v 143 | if v.lower() in ('yes', 'true', 't', 'y', '1'): 144 | return True 145 | elif v.lower() in ('no', 'false', 'f', 'n', '0'): 146 | return False 147 | else: 148 | raise argparse.ArgumentTypeError('Boolean value expected.') 149 | 150 | if __name__ == "__main__": 151 | 152 | parser = argparse.ArgumentParser() 153 | parser.add_argument('--input_root', help="base path for the input data") 154 | parser.add_argument('--output_root', help="path for outputing the kitti format data") 155 | parser.add_argument('--intrinsic_file', help="path to the camera intrinsic json file", default=None) 156 | parser.add_argument('--max_depth_m', type=float, default=10.0, help="maximum depth to be used") 157 | parser.add_argument('--vis_on', type=str2bool, nargs='?', default=False) 158 | parser.add_argument('--down_sample', type=str2bool, nargs='?', default=False) 159 | args = parser.parse_args() 160 | 161 | neuralrgbd_to_pin_format(args) -------------------------------------------------------------------------------- /dataset/converter/replica_to_pin_format.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # @file replica_to_pin_format.py 3 | # @author Yue Pan [yue.pan@igg.uni-bonn.de] 4 | # Copyright (c) 2024 Yue Pan, all rights reserved 5 | 6 | import open3d as o3d 7 | from tqdm import tqdm 8 | import argparse 9 | import os 10 | import numpy as np 11 | import json 12 | import sys 13 | import glob 14 | 15 | sys.path.append('./') # the script is launched in the root folder, required for import visualizer 16 | from utils.visualizer import MapVisualizer 17 | 18 | def replica_to_pin_format(args): 19 | 20 | if args.vis_on: 21 | vis = MapVisualizer() 22 | 23 | color_paths = sorted(glob.glob(f'{args.input_root}/results/frame*.jpg')) 24 | depth_paths = sorted(glob.glob(f'{args.input_root}/results/depth*.png')) 25 | poses = load_poses(os.path.join(args.input_root, 'traj.txt'), len(color_paths)) 26 | 27 | # get pose 28 | pose_kitti_format_path = os.path.join(args.output_root, "poses.txt") 29 | 30 | write_poses_kitti_format(poses, pose_kitti_format_path) 31 | 32 | if args.down_sample: 33 | ply_path = os.path.join(args.output_root, "rgbd_down_ply") 34 | else: 35 | ply_path = os.path.join(args.output_root, "rgbd_ply") 36 | 37 | os.makedirs(ply_path, exist_ok=True) 38 | 39 | intrinsic = o3d.camera.PinholeCameraIntrinsic() 40 | with open(args.intrinsic_file, 'r') as infile: # load intrinsic json file 41 | cam = json.load(infile)["camera"] 42 | intrinsic.set_intrinsics(height=cam["h"], 43 | width=cam["w"], 44 | fx=cam["fx"], 45 | fy=cam["fy"], 46 | cx=cam["cx"], 47 | cy=cam["cy"]) 48 | 49 | depth_scale = cam["scale"] 50 | extrinsic = np.eye(4) 51 | 52 | print("Camera Intrinsics:") 53 | print(intrinsic.intrinsic_matrix) 54 | 55 | map_pcd = o3d.geometry.PointCloud() 56 | 57 | frame_count = 0 58 | for color_path, depth_path in tqdm(zip(color_paths, depth_paths)): 59 | 60 | frame_id_str = f'{frame_count:06d}' 61 | cur_filename = frame_id_str+".ply" 62 | cur_path = os.path.join(ply_path, cur_filename) 63 | 64 | print(frame_id_str) 65 | print(color_path) 66 | print(depth_path) 67 | 68 | im_color = o3d.io.read_image(color_path) 69 | im_depth = o3d.io.read_image(depth_path) 70 | im_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(im_color, im_depth, depth_scale=depth_scale, 71 | depth_trunc=args.max_depth_m, convert_rgb_to_intensity=False) 72 | 73 | im_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(im_rgbd, intrinsic) 74 | 75 | if args.down_sample: 76 | im_pcd = im_pcd.random_down_sample(sampling_ratio=0.1) # TODO change here 77 | 78 | o3d.io.write_point_cloud(cur_path, im_pcd) 79 | 80 | if args.vis_on: 81 | im_pcd = im_pcd.transform(poses[frame_count]) 82 | if not args.down_sample: 83 | im_pcd = im_pcd.random_down_sample(sampling_ratio=0.1) 84 | map_pcd += im_pcd 85 | map_pcd = map_pcd.voxel_down_sample(0.03) 86 | vis.update_pointcloud(map_pcd) 87 | 88 | frame_count+=1 89 | 90 | print("The Replica RGBD dataset in our format has been saved at %s", args.output_root) 91 | 92 | if args.vis_on: 93 | vis.stop() 94 | 95 | def load_poses(path, frame_num): 96 | poses = [] 97 | with open(path, "r") as f: 98 | lines = f.readlines() 99 | for i in range(frame_num): 100 | line = lines[i] 101 | c2w = np.array(list(map(float, line.split()))).reshape(4, 4) 102 | # c2w[:3, 1] *= -1 103 | # c2w[:3, 2] *= -1 104 | # c2w[:3, 3] *= self.sc_factor 105 | poses.append(c2w) 106 | 107 | return poses 108 | 109 | def write_poses_kitti_format(poses_mat, posefile): 110 | poses_vec = [] 111 | for pose_mat in poses_mat: 112 | pose_vec= pose_mat.flatten()[0:12] 113 | poses_vec.append(pose_vec) 114 | np.savetxt(posefile, poses_vec, delimiter=' ') 115 | 116 | 117 | def str2bool(v): 118 | if isinstance(v, bool): 119 | return v 120 | if v.lower() in ('yes', 'true', 't', 'y', '1'): 121 | return True 122 | elif v.lower() in ('no', 'false', 'f', 'n', '0'): 123 | return False 124 | else: 125 | raise argparse.ArgumentTypeError('Boolean value expected.') 126 | 127 | if __name__ == "__main__": 128 | 129 | parser = argparse.ArgumentParser() 130 | parser.add_argument('--input_root', help="base path for the input data") 131 | parser.add_argument('--output_root', help="path for outputing the kitti format data") 132 | parser.add_argument('--intrinsic_file', help="path to the camera intrinsic json file", default=None) 133 | parser.add_argument('--max_depth_m', type=float, default=10.0, help="maximum depth to be used") 134 | parser.add_argument('--vis_on', type=str2bool, nargs='?', default=False) 135 | parser.add_argument('--down_sample', type=str2bool, nargs='?', default=False) 136 | args = parser.parse_args() 137 | 138 | replica_to_pin_format(args) -------------------------------------------------------------------------------- /dataset/converter/tum_to_pin_format.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # @file tum_to_pin_format.py 3 | # @author Yue Pan [yue.pan@igg.uni-bonn.de] 4 | # Copyright (c) 2024 Yue Pan, all rights reserved 5 | 6 | import open3d as o3d 7 | from tqdm import tqdm 8 | import argparse 9 | import os 10 | import numpy as np 11 | import json 12 | import sys 13 | 14 | sys.path.append('./') # the script is launched in the root folder, required for import visualizer 15 | from utils.visualizer import MapVisualizer 16 | 17 | def tum_to_pin_format(args): 18 | 19 | if args.vis_on: 20 | vis = MapVisualizer() 21 | 22 | # get pose 23 | pose_kitti_format_path = os.path.join(args.output_root, "poses.txt") 24 | 25 | color_paths, depth_paths, poses = loadtum(args.input_root, switch_axis=args.switch_axis) 26 | 27 | write_poses_kitti_format(poses, pose_kitti_format_path) 28 | 29 | if args.down_sample: 30 | ply_path = os.path.join(args.output_root, "rgbd_down_ply") 31 | else: 32 | ply_path = os.path.join(args.output_root, "rgbd_ply") 33 | 34 | os.makedirs(ply_path, exist_ok=True) 35 | 36 | intrinsic = o3d.camera.PinholeCameraIntrinsic() 37 | if args.intrinsic_file is None: # default one 38 | intrinsic = o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault) 39 | # fx = 525, fy = 525, cx = 319.5, cy = 239.5 as the default value 40 | else: 41 | with open(args.intrinsic_file, 'r') as infile: # load intrinsic json file 42 | cam = json.load(infile)["camera"] 43 | intrinsic.set_intrinsics(height=cam["h"], 44 | width=cam["w"], 45 | fx=cam["fx"], 46 | fy=cam["fy"], 47 | cx=cam["cx"], 48 | cy=cam["cy"]) 49 | 50 | print("Camera Intrinsics:") 51 | print(intrinsic.intrinsic_matrix) 52 | 53 | map_pcd = o3d.geometry.PointCloud() 54 | 55 | frame_count = 0 56 | for color_path, depth_path in tqdm(zip(color_paths, depth_paths)): 57 | 58 | frame_id_str = f'{frame_count:06d}' 59 | cur_filename = frame_id_str+".ply" 60 | cur_path = os.path.join(ply_path, cur_filename) 61 | 62 | print(frame_id_str) 63 | print(color_path) 64 | print(depth_path) 65 | 66 | im_color = o3d.io.read_image(color_path) 67 | im_depth = o3d.io.read_image(depth_path) 68 | im_rgbd = o3d.geometry.RGBDImage.create_from_tum_format(im_color, im_depth, convert_rgb_to_intensity=False) 69 | 70 | im_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(im_rgbd, intrinsic) 71 | 72 | if args.down_sample: 73 | im_pcd = im_pcd.random_down_sample(sampling_ratio=0.2) # TODO change here 74 | 75 | o3d.io.write_point_cloud(cur_path, im_pcd) 76 | 77 | if args.vis_on: 78 | im_pcd = im_pcd.transform(poses[frame_count]) 79 | # down_pcd = down_pcd.random_down_sample(sampling_ratio=0.1) 80 | map_pcd += im_pcd 81 | map_pcd = map_pcd.voxel_down_sample(0.02) 82 | vis.update_pointcloud(map_pcd) 83 | 84 | frame_count+=1 85 | 86 | print("The TUM RGBD dataset in our format has been saved at %s", args.output_root) 87 | 88 | if args.vis_on: 89 | vis.stop() 90 | 91 | def loadtum(datapath, frame_rate=-1, switch_axis=False): 92 | """ read video data in tum-rgbd format """ 93 | if os.path.isfile(os.path.join(datapath, 'groundtruth.txt')): 94 | pose_list = os.path.join(datapath, 'groundtruth.txt') 95 | elif os.path.isfile(os.path.join(datapath, 'pose.txt')): 96 | pose_list = os.path.join(datapath, 'pose.txt') 97 | 98 | image_list = os.path.join(datapath, 'rgb.txt') 99 | depth_list = os.path.join(datapath, 'depth.txt') 100 | 101 | image_data = parse_list(image_list) 102 | depth_data = parse_list(depth_list) 103 | pose_data = parse_list(pose_list, skiprows=1) 104 | pose_vecs = pose_data[:, 1:].astype(np.float64) 105 | 106 | tstamp_image = image_data[:, 0].astype(np.float64) 107 | tstamp_depth = depth_data[:, 0].astype(np.float64) 108 | tstamp_pose = pose_data[:, 0].astype(np.float64) 109 | associations = associate_frames( 110 | tstamp_image, tstamp_depth, tstamp_pose) 111 | 112 | indicies = [0] 113 | for i in range(1, len(associations)): 114 | t0 = tstamp_image[associations[indicies[-1]][0]] 115 | t1 = tstamp_image[associations[i][0]] 116 | if t1 - t0 > 1.0 / frame_rate: 117 | indicies += [i] 118 | 119 | images, poses, depths = [], [], [] 120 | for ix in indicies: 121 | (i, j, k) = associations[ix] 122 | images += [os.path.join(datapath, image_data[i, 1])] 123 | depths += [os.path.join(datapath, depth_data[j, 1])] 124 | c2w = pose_matrix_from_quaternion(pose_vecs[k]) 125 | # if switch_axis: 126 | # c2w[:3, 1] *= -1 127 | # c2w[:3, 2] *= -1 128 | poses += [c2w] 129 | 130 | return images, depths, poses # as file path and pose 131 | 132 | 133 | def pose_matrix_from_quaternion(pvec): 134 | """ convert 4x4 pose matrix to (t, q) """ 135 | from scipy.spatial.transform import Rotation 136 | pose = np.eye(4) 137 | pose[:3, :3] = Rotation.from_quat(pvec[3:]).as_matrix() # rotation 138 | pose[:3, 3] = pvec[:3] # translation 139 | return pose 140 | 141 | def associate_frames(tstamp_image, tstamp_depth, tstamp_pose, max_dt=0.08): 142 | """ pair images, depths, and poses """ 143 | associations = [] 144 | for i, t in enumerate(tstamp_image): 145 | if tstamp_pose is None: 146 | j = np.argmin(np.abs(tstamp_depth - t)) 147 | if (np.abs(tstamp_depth[j] - t) < max_dt): 148 | associations.append((i, j)) 149 | 150 | else: 151 | j = np.argmin(np.abs(tstamp_depth - t)) 152 | k = np.argmin(np.abs(tstamp_pose - t)) 153 | 154 | if (np.abs(tstamp_depth[j] - t) < max_dt) and \ 155 | (np.abs(tstamp_pose[k] - t) < max_dt): 156 | associations.append((i, j, k)) 157 | 158 | return associations 159 | 160 | def parse_list(filepath, skiprows=0): 161 | """ read list data """ 162 | data = np.loadtxt(filepath, delimiter=' ', 163 | dtype=np.unicode_, skiprows=skiprows) 164 | return data 165 | 166 | def write_poses_kitti_format(poses_mat, posefile): 167 | poses_vec = [] 168 | for pose_mat in poses_mat: 169 | pose_vec= pose_mat.flatten()[0:12] 170 | poses_vec.append(pose_vec) 171 | np.savetxt(posefile, poses_vec, delimiter=' ') 172 | 173 | 174 | def str2bool(v): 175 | if isinstance(v, bool): 176 | return v 177 | if v.lower() in ('yes', 'true', 't', 'y', '1'): 178 | return True 179 | elif v.lower() in ('no', 'false', 'f', 'n', '0'): 180 | return False 181 | else: 182 | raise argparse.ArgumentTypeError('Boolean value expected.') 183 | 184 | if __name__ == "__main__": 185 | 186 | parser = argparse.ArgumentParser() 187 | parser.add_argument('--input_root', help="base path for the input data") 188 | parser.add_argument('--output_root', help="path for outputing the kitti format data") 189 | parser.add_argument('--intrinsic_file', help="path to the camera intrinsic json file", default=None) 190 | parser.add_argument('--crop_edge', type=int, default=0.0, help="edge pixels for cropping") 191 | parser.add_argument('--vis_on', type=str2bool, nargs='?', default=False) 192 | parser.add_argument('--down_sample', type=str2bool, nargs='?', default=False) 193 | parser.add_argument('--switch_axis', type=str2bool, nargs='?', default=False) 194 | args = parser.parse_args() 195 | 196 | tum_to_pin_format(args) -------------------------------------------------------------------------------- /dataset/dataloaders/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | from pathlib import Path 24 | from typing import Dict, List 25 | 26 | 27 | def supported_file_extensions(): 28 | return [ 29 | "bin", 30 | "pcd", 31 | "ply", 32 | "xyz", 33 | "obj", 34 | "ctm", 35 | "off", 36 | "stl", 37 | ] 38 | 39 | 40 | def sequence_dataloaders(): 41 | # TODO: automatically infer this 42 | return ["kitti", "kitti_raw", "nuscenes", "helipr", "replica", "tum", "neuralrgbd"] 43 | 44 | 45 | def available_dataloaders() -> List: 46 | import os.path 47 | import pkgutil 48 | 49 | pkgpath = os.path.dirname(__file__) 50 | return [name for _, name, _ in pkgutil.iter_modules([pkgpath])] 51 | 52 | 53 | def jumpable_dataloaders(): 54 | _jumpable_dataloaders = available_dataloaders() 55 | _jumpable_dataloaders.remove("mcap") 56 | _jumpable_dataloaders.remove("ouster") 57 | _jumpable_dataloaders.remove("rosbag") 58 | return _jumpable_dataloaders 59 | 60 | 61 | def dataloader_types() -> Dict: 62 | import ast 63 | import importlib 64 | 65 | dataloaders = available_dataloaders() 66 | _types = {} 67 | for dataloader in dataloaders: 68 | script = importlib.util.find_spec(f".{dataloader}", __name__).origin 69 | with open(script) as f: 70 | tree = ast.parse(f.read(), script) 71 | classes = [cls for cls in tree.body if isinstance(cls, ast.ClassDef)] 72 | _types[dataloader] = classes[0].name # assuming there is only 1 class 73 | return _types 74 | 75 | 76 | def dataset_factory(dataloader: str, data_dir: Path, *args, **kwargs): 77 | import importlib 78 | 79 | dataloader_type = dataloader_types()[dataloader] 80 | module = importlib.import_module(f".{dataloader}", __name__) 81 | assert hasattr(module, dataloader_type), f"{dataloader_type} is not defined in {module}" 82 | dataset = getattr(module, dataloader_type) 83 | return dataset(data_dir=data_dir, *args, **kwargs) 84 | -------------------------------------------------------------------------------- /dataset/dataloaders/apollo.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import glob 24 | import importlib 25 | import os 26 | import sys 27 | from pathlib import Path 28 | 29 | import natsort 30 | import numpy as np 31 | import open3d as o3d 32 | from pyquaternion import Quaternion 33 | 34 | 35 | class ApolloDataset: 36 | def __init__(self, data_dir: Path, *_, **__): 37 | self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/pcds/*.pcd")) 38 | self.gt_poses = self.read_poses(f"{data_dir}/poses/gt_poses.txt") 39 | self.sequence_id = os.path.basename(data_dir) 40 | 41 | def __len__(self): 42 | return len(self.scan_files) 43 | 44 | def __getitem__(self, idx): 45 | points = self.get_scan(self.scan_files[idx]) 46 | point_ts = self.get_timestamps(points) 47 | frame_data = {"points": points, "point_ts": point_ts} 48 | 49 | return frame_data 50 | 51 | def get_scan(self, scan_file: str): 52 | points = np.asarray(o3d.io.read_point_cloud(scan_file).points, dtype=np.float64) 53 | return points.astype(np.float64) 54 | 55 | @staticmethod 56 | def read_poses(file): 57 | data = np.loadtxt(file) 58 | _, _, translations, qxyzw = np.split(data, [1, 2, 5], axis=1) 59 | rotations = np.array( 60 | [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in qxyzw] 61 | ) 62 | poses = np.zeros([rotations.shape[0], 4, 4]) 63 | poses[:, :3, -1] = translations 64 | poses[:, :3, :3] = rotations 65 | poses[:, -1, -1] = 1 66 | # Convert from global coordinate poses to local poses 67 | first_pose = poses[0, :, :] 68 | return np.linalg.inv(first_pose) @ poses 69 | 70 | # velodyne lidar 71 | @staticmethod 72 | def get_timestamps(points): 73 | x = points[:, 0] 74 | y = points[:, 1] 75 | yaw = -np.arctan2(y, x) 76 | timestamps = 0.5 * (yaw / np.pi + 1.0) 77 | return timestamps -------------------------------------------------------------------------------- /dataset/dataloaders/boreas.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 2024 Yue Pan 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | import glob 25 | import os 26 | from pathlib import Path 27 | 28 | import natsort 29 | import numpy as np 30 | 31 | 32 | class BoreasDataset: 33 | def __init__(self, data_dir: Path, *_, **__): 34 | self.root_dir = os.path.realpath(data_dir) 35 | self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/lidar/*.bin")) 36 | self.gt_poses = self.load_poses(f"{data_dir}/applanix/lidar_poses.csv") 37 | self.sequence_id = os.path.basename(data_dir) 38 | assert len(self.scan_files) == self.gt_poses.shape[0] 39 | 40 | def __len__(self): 41 | return len(self.scan_files) 42 | 43 | def __getitem__(self, idx): 44 | points, point_ts = self.read_point_cloud(self.scan_files[idx]) 45 | frame_data = {"points": points, "point_ts": point_ts} 46 | return frame_data 47 | 48 | def read_point_cloud(self, scan_file: str): 49 | points = np.fromfile(scan_file, dtype=np.float32).reshape((-1, 6))[:, :3] 50 | return points.astype(np.float64), self.get_timestamps(points) 51 | 52 | def load_poses(self, poses_file): 53 | data = np.loadtxt(poses_file, delimiter=",", skiprows=1) 54 | n, m = data.shape 55 | t, x, y, z, vx, vy, vz, r, p, ya, wz, wy, wx = data[0, :] 56 | first_pose = self.get_transformation_matrix(x, y, z, ya, p, r) 57 | poses = np.empty((n, 4, 4), dtype=np.float32) 58 | poses[0, :, :] = np.identity(4, dtype=np.float32) 59 | for i in range(n): 60 | t, x, y, z, vx, vy, vz, r, p, ya, wz, wy, wx = data[i, :] 61 | current_pose = self.get_transformation_matrix(x, y, z, ya, p, r) 62 | poses[i, :, :] = np.linalg.inv(first_pose) @ current_pose 63 | return poses 64 | 65 | @staticmethod 66 | def get_timestamps(points): 67 | x = points[:, 0] 68 | y = points[:, 1] 69 | yaw = -np.arctan2(y, x) 70 | timestamps = 0.5 * (yaw / np.pi + 1.0) 71 | return timestamps 72 | 73 | @staticmethod 74 | def get_transformation_matrix(x, y, z, yaw, pitch, roll): 75 | T_enu_sensor = np.identity(4, dtype=np.float64) 76 | R_yaw = np.array( 77 | [[np.cos(yaw), np.sin(yaw), 0], [-np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]], 78 | dtype=np.float64, 79 | ) 80 | R_pitch = np.array( 81 | [[np.cos(pitch), 0, -np.sin(pitch)], [0, 1, 0], [np.sin(pitch), 0, np.cos(pitch)]], 82 | dtype=np.float64, 83 | ) 84 | R_roll = np.array( 85 | [[1, 0, 0], [0, np.cos(roll), np.sin(roll)], [0, -np.sin(roll), np.cos(roll)]], 86 | dtype=np.float64, 87 | ) 88 | C_enu_sensor = R_roll @ R_pitch @ R_yaw 89 | T_enu_sensor[:3, :3] = C_enu_sensor 90 | r_sensor_enu_in_enu = np.array([x, y, z]).reshape(3, 1) 91 | T_enu_sensor[:3, 3:] = r_sensor_enu_in_enu 92 | return T_enu_sensor 93 | -------------------------------------------------------------------------------- /dataset/dataloaders/generic.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 2024 Yue Pan 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | import os 25 | import sys 26 | from pathlib import Path 27 | 28 | import natsort 29 | import numpy as np 30 | 31 | from dataset.dataloaders import supported_file_extensions 32 | 33 | 34 | class GenericDataset: 35 | def __init__(self, data_dir: Path, *_, **__): 36 | # Config stuff 37 | self.sequence_id = os.path.basename(os.path.abspath(data_dir)) 38 | self.scans_dir = os.path.join(os.path.realpath(data_dir), "") 39 | self.scan_files = np.array( 40 | natsort.natsorted( 41 | [ 42 | os.path.join(self.scans_dir, fn) 43 | for fn in os.listdir(self.scans_dir) 44 | if any(fn.endswith(ext) for ext in supported_file_extensions()) 45 | ] 46 | ), 47 | dtype=str, 48 | ) 49 | if len(self.scan_files) == 0: 50 | raise ValueError(f"Tried to read point cloud files in {self.scans_dir} but none found") 51 | self.file_extension = self.scan_files[0].split(".")[-1] 52 | if self.file_extension not in supported_file_extensions(): 53 | raise ValueError(f"Supported formats are: {supported_file_extensions()}") 54 | 55 | # Obtain the pointcloud reader for the given data folder 56 | self._read_point_cloud = self._get_point_cloud_reader() 57 | 58 | def __len__(self): 59 | return len(self.scan_files) 60 | 61 | def __getitem__(self, idx): 62 | points = self.read_point_cloud(self.scan_files[idx]) 63 | frame_data = {"points": points} 64 | return frame_data 65 | 66 | def read_point_cloud(self, file_path: str): 67 | points = self._read_point_cloud(file_path) 68 | return points.astype(np.float64) 69 | 70 | def _get_point_cloud_reader(self): 71 | """Attempt to guess with try/catch blocks which is the best point cloud reader to use for 72 | the given dataset folder. Supported readers so far are: 73 | - np.fromfile 74 | - trimesh.load 75 | - PyntCloud 76 | - open3d[optional] 77 | """ 78 | # This is easy, the old KITTI format 79 | if self.file_extension == "bin": 80 | print("[WARNING] Reading .bin files, the only format supported is the KITTI format") 81 | return lambda file: np.fromfile(file, dtype=np.float32).reshape((-1, 4))[:, :3] 82 | 83 | first_scan_file = self.scan_files[0] 84 | 85 | # first try trimesh 86 | try: 87 | import trimesh 88 | 89 | trimesh.load(first_scan_file) 90 | return lambda file: np.asarray(trimesh.load(file).vertices) 91 | except: 92 | pass 93 | 94 | # then try pyntcloud 95 | try: 96 | from pyntcloud import PyntCloud 97 | 98 | PyntCloud.from_file(first_scan_file) 99 | return lambda file: PyntCloud.from_file(file).points[["x", "y", "z"]].to_numpy() 100 | except: 101 | pass 102 | 103 | # lastly with open3d 104 | try: 105 | import open3d as o3d 106 | 107 | o3d.io.read_point_cloud(first_scan_file) 108 | return lambda file: np.asarray(o3d.io.read_point_cloud(file).points, dtype=np.float64) 109 | except: 110 | print("[ERROR], File format not supported") 111 | sys.exit(1) 112 | -------------------------------------------------------------------------------- /dataset/dataloaders/helipr.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Benedikt Mersch, Saurabh Gupta, Ignacio Vizzo, 4 | # Tiziano Guadagnino, Cyrill Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import importlib 24 | import os 25 | import struct 26 | import sys 27 | from pathlib import Path 28 | 29 | import natsort 30 | import numpy as np 31 | from pyquaternion import Quaternion 32 | 33 | from dataset.dataloaders import supported_file_extensions 34 | 35 | class HeLiPRDataset: 36 | def __init__(self, data_dir: Path, sequence: str, *_, **__): 37 | 38 | self.sequence_id = sequence 39 | self.sequence_dir = os.path.join(data_dir, "LiDAR", self.sequence_id) 40 | scan_files = os.listdir(self.sequence_dir) 41 | scan_timestamps = [int(Path(file).stem) for file in scan_files] 42 | 43 | pose_file = os.path.join(data_dir, "LiDAR_GT", f"{self.sequence_id}_gt.txt") 44 | pose_timestamps, poses = self.read_poses(pose_file) 45 | 46 | # Match number of scans with number of references poses available 47 | self.gt_poses = np.array( 48 | [pose for time, pose in zip(pose_timestamps, poses) if time in scan_timestamps] 49 | ).reshape(-1, 4, 4) 50 | 51 | scan_files = [file for file in scan_files if int(Path(file).stem) in pose_timestamps] 52 | 53 | self.scan_files = np.array( 54 | natsort.natsorted( 55 | [ 56 | os.path.join(self.sequence_dir, fn) 57 | for fn in os.listdir(self.sequence_dir) 58 | if any(fn.endswith(ext) for ext in supported_file_extensions()) 59 | ] 60 | ), 61 | dtype=str, 62 | ) 63 | if len(self.scan_files) == 0: 64 | raise ValueError(f"Tried to read point cloud files in {data_dir} but none found") 65 | 66 | # Obtain the pointcloud reader for the given data folder 67 | if self.sequence_id == "Avia": 68 | self.format_string = "fffBBBL" 69 | self.intensity_channel = None 70 | self.time_channel = 6 71 | elif self.sequence_id == "Aeva": 72 | self.format_string = "ffffflBf" 73 | self.format_string_no_intensity = "ffffflB" 74 | self.intensity_channel = 7 75 | self.time_channel = 5 76 | elif self.sequence_id == "Ouster": 77 | self.format_string = "ffffIHHH" 78 | self.intensity_channel = 3 79 | self.time_channel = 4 80 | elif self.sequence_id == "Velodyne": 81 | self.format_string = "ffffHf" 82 | self.intensity_channel = 3 83 | self.time_channel = 5 84 | else: 85 | print("[ERROR] Unsupported LiDAR Type") 86 | sys.exit() 87 | 88 | def __len__(self): 89 | return len(self.scan_files) 90 | 91 | def __getitem__(self, idx): 92 | data = self.get_data(idx) 93 | if data is None: 94 | return None 95 | points = self.read_point_cloud(data) 96 | timestamps = self.read_timestamps(data) 97 | frame_data = {"points": points, "point_ts": timestamps} 98 | return frame_data 99 | 100 | def read_poses(self, pose_file: str): 101 | gt = np.loadtxt(pose_file, delimiter=" ") 102 | time = gt[:, 0] 103 | xyz = gt[:, 1:4] 104 | # xyz[:, 2] *= -1 # FIXME (added) 105 | rotations = np.array( 106 | [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in gt[:, 4:]] 107 | ) 108 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(len(gt), axis=0) 109 | poses[:, :3, :3] = rotations 110 | poses[:, :3, 3] = xyz 111 | return time, poses 112 | 113 | def get_data(self, idx: int): 114 | file_path = self.scan_files[idx] 115 | list_lines = [] 116 | 117 | # Special case, see https://github.com/minwoo0611/HeLiPR-File-Player/blob/e8d95e390454ece1415ae9deb51515f63730c10a/src/ROSThread.cpp#L632 118 | if self.sequence_id == "Aeva" and int(Path(file_path).stem) <= 1691936557946849179: 119 | self.intensity_channel = None 120 | format_string = self.format_string_no_intensity 121 | else: 122 | format_string = self.format_string 123 | 124 | chunk_size = struct.calcsize(f"={format_string}") 125 | with open(file_path, "rb") as f: 126 | binary = f.read() 127 | offset = 0 128 | while offset < len(binary) - chunk_size: # chunk_size issue fixed # have problem when reading 16716th frame of Town03 Ouster 129 | list_lines.append(struct.unpack_from(f"={format_string}", binary, offset)) 130 | offset += chunk_size 131 | if len(list_lines) > 0: 132 | data = np.stack(list_lines) 133 | return data 134 | else: 135 | print(f"Error: No data found for file {file_path}") 136 | return None 137 | 138 | def read_timestamps(self, data: np.ndarray) -> np.ndarray: 139 | time = data[:, self.time_channel] 140 | return (time - time.min()) / (time.max() - time.min()) 141 | 142 | def read_point_cloud(self, data: np.ndarray) -> np.ndarray: 143 | return data[:, :3] 144 | -------------------------------------------------------------------------------- /dataset/dataloaders/mcap.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import importlib 24 | import os 25 | import sys 26 | 27 | from natsort import natsorted 28 | 29 | class McapDataloader: 30 | def __init__(self, data_dir: str, topic: str, *_, **__): 31 | """Standalone .mcap dataloader withouth any ROS distribution.""" 32 | # Conditional imports to avoid injecting dependencies for non mcap users 33 | 34 | try: 35 | self.make_reader = importlib.import_module("mcap.reader").make_reader 36 | self.read_ros2_messages = importlib.import_module("mcap_ros2.reader").read_ros2_messages 37 | except ModuleNotFoundError: 38 | print("mcap plugins not installed: 'pip install mcap-ros2-support'") 39 | exit(1) 40 | 41 | from utils.point_cloud2 import read_point_cloud 42 | 43 | # Handle both single file and directory inputs 44 | if os.path.isfile(data_dir): 45 | self.mcap_files = [data_dir] 46 | elif os.path.isdir(data_dir): 47 | self.mcap_files = natsorted([ 48 | os.path.join(data_dir, f) for f in os.listdir(data_dir) 49 | if f.endswith('.mcap') 50 | ]) 51 | assert len(self.mcap_files) > 0, f"No .mcap files found in directory: {data_dir}" 52 | if len(self.mcap_files) > 1: 53 | print("Reading multiple .mcap files in directory:") 54 | print("\n".join([os.path.basename(path) for path in self.mcap_files])) 55 | else: 56 | raise ValueError(f"Input path {data_dir} is neither a file nor directory") 57 | 58 | # Initialize with first file 59 | self.current_file_idx = 0 60 | self.sequence_id = os.path.basename(self.mcap_files[0]).split(".")[0] 61 | self._initialize_current_file(self.mcap_files[0], topic) 62 | 63 | self.read_point_cloud = read_point_cloud 64 | 65 | # Calculate total number of scans across all files 66 | self.total_scans = self._calculate_total_scans(topic) 67 | 68 | def _initialize_current_file(self, mcap_file: str, topic: str): 69 | """Initialize readers for a new mcap file.""" 70 | if hasattr(self, 'bag'): 71 | del self.bag 72 | self.bag = self.make_reader(open(mcap_file, "rb")) 73 | self.summary = self.bag.get_summary() 74 | self.topic = self.check_topic(topic) 75 | self.n_scans = self._get_n_scans() 76 | self.msgs = self.read_ros2_messages(mcap_file, topics=topic) 77 | self.current_scan = 0 78 | 79 | def __del__(self): 80 | if hasattr(self, "bag"): 81 | del self.bag 82 | 83 | def _calculate_total_scans(self, topic: str) -> int: 84 | """Calculate total number of scans across all mcap files.""" 85 | total = 0 86 | for file in self.mcap_files: 87 | bag = self.make_reader(open(file, "rb")) 88 | summary = bag.get_summary() 89 | total += sum( 90 | count 91 | for (id, count) in summary.statistics.channel_message_counts.items() 92 | if summary.channels[id].topic == topic 93 | ) 94 | del bag # Clean up the reader 95 | return total 96 | 97 | def __getitem__(self, idx): 98 | # Check if we need to move to next file 99 | while self.current_scan >= self.n_scans: 100 | self.current_file_idx += 1 101 | if self.current_file_idx >= len(self.mcap_files): 102 | raise IndexError("Index out of range") 103 | self._initialize_current_file(self.mcap_files[self.current_file_idx], self.topic) 104 | 105 | msg = next(self.msgs).ros_msg 106 | self.current_scan += 1 107 | points, point_ts = self.read_point_cloud(msg) 108 | frame_data = {"points": points, "point_ts": point_ts} 109 | return frame_data 110 | 111 | def __len__(self): 112 | return self.total_scans 113 | 114 | def _get_n_scans(self) -> int: 115 | return sum( 116 | count 117 | for (id, count) in self.summary.statistics.channel_message_counts.items() 118 | if self.summary.channels[id].topic == self.topic 119 | ) 120 | 121 | def check_topic(self, topic: str) -> str: 122 | # Extract schema id from the .mcap file that encodes the PointCloud2 msg 123 | schema_id = [ 124 | schema.id 125 | for schema in self.summary.schemas.values() 126 | if schema.name == "sensor_msgs/msg/PointCloud2" 127 | ][0] 128 | 129 | point_cloud_topics = [ 130 | channel.topic 131 | for channel in self.summary.channels.values() 132 | if channel.schema_id == schema_id 133 | ] 134 | 135 | def print_available_topics_and_exit(): 136 | print("Select from the following topics:") 137 | print(50 * "-") 138 | for t in point_cloud_topics: 139 | print(f"{t}") 140 | print(50 * "-") 141 | sys.exit(1) 142 | 143 | if topic and topic in point_cloud_topics: 144 | return topic 145 | # when user specified the topic check that exists 146 | if topic and topic not in point_cloud_topics: 147 | print( 148 | f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' 149 | "Specify the correct topic name by python pin_slam.py path/to/config/file.yaml mcap your/topic ... ..." 150 | ) 151 | print_available_topics_and_exit() 152 | if len(point_cloud_topics) > 1: 153 | print( 154 | "Multiple sensor_msgs/msg/PointCloud2 topics available." 155 | "Specify the correct topic name by python pin_slam.py path/to/config/file.yaml mcap your/topic ... ..." 156 | ) 157 | print_available_topics_and_exit() 158 | 159 | if len(point_cloud_topics) == 0: 160 | print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") 161 | if len(point_cloud_topics) == 1: 162 | return point_cloud_topics[0] 163 | -------------------------------------------------------------------------------- /dataset/dataloaders/mulran.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 2024 Yue Pan 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | import glob 25 | import os 26 | from pathlib import Path 27 | 28 | import numpy as np 29 | 30 | 31 | class MulranDataset: 32 | def __init__(self, data_dir: Path, *_, **__): 33 | self.sequence_id = os.path.basename(data_dir) 34 | self.sequence_dir = os.path.realpath(data_dir) 35 | self.velodyne_dir = os.path.join(self.sequence_dir, "Ouster/") 36 | 37 | self.scan_files = sorted(glob.glob(self.velodyne_dir + "*.bin")) 38 | self.scan_timestamps = [int(os.path.basename(t).split(".")[0]) for t in self.scan_files] 39 | self.gt_poses = self.load_gt_poses(os.path.join(self.sequence_dir, "global_pose.csv")) 40 | 41 | def __len__(self): 42 | return len(self.scan_files) 43 | 44 | def __getitem__(self, idx): 45 | points, point_ts = self.read_point_cloud(self.scan_files[idx]) 46 | frame_data = {"points": points, "point_ts": point_ts} 47 | return frame_data 48 | 49 | def read_point_cloud(self, file_path: str): 50 | points = np.fromfile(file_path, dtype=np.float32).reshape((-1, 4))[:, :3] 51 | timestamps = self.get_timestamps() 52 | if points.shape[0] != timestamps.shape[0]: 53 | # MulRan has some broken point clouds, just fallback to no timestamps 54 | return points.astype(np.float64), np.ones(points.shape[0]) 55 | return points.astype(np.float64), timestamps 56 | 57 | @staticmethod 58 | def get_timestamps(): 59 | H = 64 60 | W = 1024 61 | return (np.floor(np.arange(H * W) / H) / W).reshape(-1, 1) 62 | 63 | def load_gt_poses(self, poses_file: str): 64 | """MulRan has more poses than scans, therefore we need to match 1-1 timestamp with pose""" 65 | 66 | def read_csv(poses_file: str): 67 | poses = np.loadtxt(poses_file, delimiter=",") 68 | timestamps = poses[:, 0] 69 | poses = poses[:, 1:] 70 | n = poses.shape[0] 71 | poses = np.concatenate( 72 | (poses, np.zeros((n, 3), dtype=np.float32), np.ones((n, 1), dtype=np.float32)), 73 | axis=1, 74 | ) 75 | poses = poses.reshape((n, 4, 4)) # [N, 4, 4] 76 | return poses, timestamps 77 | 78 | # Read the csv file 79 | poses, timestamps = read_csv(poses_file) 80 | # Extract only the poses that has a matching Ouster scan 81 | poses = poses[[np.argmin(abs(timestamps - t)) for t in self.scan_timestamps]] 82 | 83 | # Convert from global coordinate poses to local poses 84 | first_pose = poses[0, :, :] 85 | poses = np.linalg.inv(first_pose) @ poses 86 | 87 | T_lidar_to_base, T_base_to_lidar = self._get_calibration() 88 | return T_lidar_to_base @ poses @ T_base_to_lidar 89 | 90 | def _get_calibration(self): 91 | # Apply calibration obtainbed from calib_base2ouster.txt 92 | # T_lidar_to_base[:3, 3] = np.array([1.7042, -0.021, 1.8047]) 93 | # T_lidar_to_base[:3, :3] = tu_vieja.from_euler( 94 | # "xyz", [0.0001, 0.0003, 179.6654], degrees=True 95 | # ) 96 | T_lidar_to_base = np.array( 97 | [ 98 | [-9.9998295e-01, -5.8398386e-03, -5.2257060e-06, 1.7042000e00], 99 | [5.8398386e-03, -9.9998295e-01, 1.7758769e-06, -2.1000000e-02], 100 | [-5.2359878e-06, 1.7453292e-06, 1.0000000e00, 1.8047000e00], 101 | [0.0000000e00, 0.0000000e00, 0.0000000e00, 1.0000000e00], 102 | ] 103 | ) 104 | T_base_to_lidar = np.linalg.inv(T_lidar_to_base) 105 | return T_lidar_to_base, T_base_to_lidar 106 | -------------------------------------------------------------------------------- /dataset/dataloaders/ncd.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import importlib 24 | import os 25 | import re 26 | from pathlib import Path 27 | 28 | import numpy as np 29 | from pyquaternion import Quaternion 30 | 31 | 32 | class NewerCollegeDataset: 33 | def __init__(self, data_dir: Path, *_, **__): 34 | try: 35 | self.PyntCloud = importlib.import_module("pyntcloud").PyntCloud 36 | except ModuleNotFoundError: 37 | print(f'Newer College requires pnytccloud: "pip install pyntcloud"') 38 | # can also use open3d 39 | 40 | self.data_source = os.path.join(data_dir, "") 41 | self.scan_folder = os.path.join(self.data_source, "raw_format/ouster_scan") 42 | self.pose_file = os.path.join(self.data_source, "ground_truth/registered_poses.csv") 43 | self.sequence_id = os.path.basename(data_dir) 44 | 45 | # Load scan files and poses 46 | self.scan_files = self.get_pcd_filenames(self.scan_folder) 47 | self.gt_poses = self.load_gt_poses(self.pose_file) 48 | 49 | # Visualization Options 50 | self.use_global_visualizer = True 51 | 52 | def __len__(self): 53 | return len(self.scan_files) 54 | 55 | def __getitem__(self, idx): 56 | file_path = os.path.join(self.scan_folder, self.scan_files[idx]) 57 | points, point_ts = self.getitem(file_path) 58 | frame_data = {"points": points, "point_ts": point_ts} 59 | return frame_data 60 | 61 | def getitem(self, scan_file: str): 62 | points = self.PyntCloud.from_file(scan_file).points[["x", "y", "z"]].to_numpy() 63 | timestamps = self.get_timestamps() 64 | if points.shape[0] != timestamps.shape[0]: 65 | return points.astype(np.float64), np.ones(points.shape[0]) 66 | return points.astype(np.float64), timestamps 67 | 68 | @staticmethod 69 | def get_timestamps(): 70 | H = 64 71 | W = 1024 72 | return (np.floor(np.arange(H * W) / H) / W).reshape(-1, 1) 73 | 74 | @staticmethod 75 | def get_pcd_filenames(scans_folder): 76 | # cloud_1583836591_182590976.pcd 77 | regex = re.compile("^cloud_(\d*_\d*)") 78 | 79 | def get_cloud_timestamp(pcd_filename): 80 | m = regex.search(pcd_filename) 81 | secs, nsecs = m.groups()[0].split("_") 82 | return int(secs) * int(1e9) + int(nsecs) 83 | 84 | return sorted(os.listdir(scans_folder), key=get_cloud_timestamp) 85 | 86 | @staticmethod 87 | def load_gt_poses(file_path: str): 88 | """Taken from pyLiDAR-SLAM/blob/master/slam/dataset/nhcd_dataset.py""" 89 | ground_truth_df = np.genfromtxt(str(file_path), delimiter=",", dtype=np.float64) 90 | xyz = ground_truth_df[:, 2:5] 91 | rotations = np.array( 92 | [ 93 | Quaternion(x=x, y=y, z=z, w=w).rotation_matrix 94 | for x, y, z, w in ground_truth_df[:, 5:] 95 | ] 96 | ) 97 | 98 | num_poses = rotations.shape[0] 99 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) 100 | poses[:, :3, :3] = rotations 101 | poses[:, :3, 3] = xyz 102 | 103 | T_CL = np.eye(4, dtype=np.float32) 104 | T_CL[:3, :3] = Quaternion(x=0, y=0, z=0.924, w=0.383).rotation_matrix 105 | T_CL[:3, 3] = np.array([-0.084, -0.025, 0.050], dtype=np.float32) 106 | poses = np.einsum("nij,jk->nik", poses, T_CL) 107 | poses = np.einsum("ij,njk->nik", np.linalg.inv(poses[0]), poses) 108 | return poses 109 | -------------------------------------------------------------------------------- /dataset/dataloaders/nclt.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # Copyright (c) 2024 Yue Pan 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | import os 25 | import sys 26 | from pathlib import Path 27 | 28 | import numpy as np 29 | 30 | 31 | class NCLTDataset: 32 | """Adapted from PyLidar-SLAM""" 33 | 34 | def __init__(self, data_dir: Path, *_, **__): 35 | self.sequence_id = os.path.basename(data_dir) 36 | self.sequence_dir = os.path.join(os.path.realpath(data_dir), "") 37 | self.scans_dir = os.path.join(self.sequence_dir, "velodyne_sync") 38 | scan_files = np.array(sorted(os.listdir(str(self.scans_dir))), dtype=str) 39 | poses_file = os.path.realpath( 40 | os.path.join( 41 | self.sequence_dir, 42 | "..", 43 | f"ground_truth/groundtruth_{self.sequence_id}.csv", 44 | ) 45 | ) 46 | gt_data = np.loadtxt(poses_file, delimiter=",") 47 | self.timestamps, timestamp_filter = self.load_valid_timestamps(gt_data, scan_files) 48 | self.scan_files = scan_files[timestamp_filter] 49 | self.gt_poses = self.load_gt_poses(gt_data) 50 | 51 | # calibrations: https://robots.engin.umich.edu/nclt/nclt.pdf 52 | self.z_body_vel = -0.957 # unit: m 53 | # x_body_vel = [0.002, -0.004, -0.957, 0.807, 0.166, -90.703] 54 | 55 | # Visualization Options 56 | self.use_global_visualizer = True 57 | 58 | def __len__(self): 59 | return len(self.scan_files) 60 | 61 | def __getitem__(self, idx): 62 | points = self.read_point_cloud(os.path.join(self.scans_dir, self.scan_files[idx])) 63 | frame_data = {"points": points} 64 | return frame_data 65 | 66 | def read_point_cloud(self, file_path: str): 67 | def _convert(x_s, y_s, z_s): 68 | # Copied from http://robots.engin.umich.edu/nclt/python/read_vel_sync.py 69 | scaling = 0.005 70 | offset = -100.0 71 | 72 | x = x_s * scaling + offset 73 | y = y_s * scaling + offset 74 | z = z_s * scaling + offset 75 | return x, y, z 76 | 77 | binary = np.fromfile(file_path, dtype=np.int16) 78 | x = np.ascontiguousarray(binary[::4]) 79 | y = np.ascontiguousarray(binary[1::4]) 80 | z = np.ascontiguousarray(binary[2::4]) 81 | x = x.astype(np.float32).reshape(-1, 1) 82 | y = y.astype(np.float32).reshape(-1, 1) 83 | z = z.astype(np.float32).reshape(-1, 1) 84 | x, y, z = _convert(x, y, z) # already motion compensated and in the body frame 85 | 86 | # Flip to have z pointing up 87 | # and convert to velodyne frame 88 | # for the original code, the point cloud are in the body frame 89 | points = np.concatenate([x, -y, -z+self.z_body_vel], axis=1) 90 | return points.astype(np.float64) 91 | 92 | @staticmethod 93 | def load_valid_timestamps(gt_data: np.ndarray, scan_files: np.ndarray): 94 | # Ground truth timestamps and LiDARs don't match, interpolate 95 | gt_t = gt_data[:, 0] 96 | # Limit the sequence to timestamps for which a ground truth exists 97 | timestamps = np.array( 98 | [os.path.basename(file).split(".")[0] for file in scan_files], dtype=np.int64 99 | ) 100 | filter_ = (timestamps > np.min(gt_t)) * (timestamps < np.max(gt_t)) 101 | return timestamps[filter_], filter_ 102 | 103 | def load_gt_poses(self, gt_data: np.ndarray): 104 | try: 105 | from scipy import interpolate 106 | from scipy.spatial.transform import Rotation 107 | except ImportError: 108 | print('NCLT dataloader requires scipy: "pip install scipy"') 109 | sys.exit(1) 110 | 111 | inter = interpolate.interp1d(gt_data[:, 0], gt_data[:, 1:], kind="nearest", axis=0) 112 | 113 | # Limit the sequence to timestamps for which a ground truth exists 114 | gt = inter(self.timestamps) 115 | gt_tr = gt[:, :3] 116 | gt_euler = gt[:, 3:][:, [2, 1, 0]] 117 | gt_rot = Rotation.from_euler("ZYX", gt_euler).as_matrix() 118 | 119 | gt = np.eye(4, dtype=np.float32).reshape(1, 4, 4).repeat(gt.shape[0], axis=0) 120 | gt[:, :3, :3] = gt_rot 121 | gt[:, :3, 3] = gt_tr 122 | 123 | gt = np.einsum( 124 | "nij,jk->nik", 125 | gt, 126 | np.array( 127 | [ 128 | [1.0, 0.0, 0.0, 0.0], 129 | [0.0, -1.0, 0.0, 0.0], 130 | [0.0, 0.0, -1.0, 0.0], 131 | [0.0, 0.0, 0.0, 1.0], 132 | ], 133 | dtype=np.float32, 134 | ), 135 | ) 136 | gt = np.einsum( 137 | "ij,njk->nik", 138 | np.array( 139 | [ 140 | [1.0, 0.0, 0.0, 0.0], 141 | [0.0, -1.0, 0.0, 0.0], 142 | [0.0, 0.0, -1.0, 0.0], 143 | [0.0, 0.0, 0.0, 1.0], 144 | ], 145 | dtype=np.float32, 146 | ), 147 | gt, 148 | ) 149 | 150 | return gt -------------------------------------------------------------------------------- /dataset/dataloaders/neuralrgbd.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Yue Pan 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 | 23 | import importlib 24 | import os 25 | from pathlib import Path 26 | from natsort import natsorted 27 | import numpy as np 28 | 29 | 30 | class NeuralRGBDDataset: 31 | def __init__(self, data_dir: Path, sequence: str, *_, **__): 32 | try: 33 | self.o3d = importlib.import_module("open3d") 34 | except ModuleNotFoundError as err: 35 | print(f'open3d is not installed on your system, run "pip install open3d"') 36 | exit(1) 37 | 38 | sequence_dir = os.path.join(data_dir, sequence) 39 | rgb_folder_path = os.path.join(sequence_dir, 'images') 40 | depth_folder_path = os.path.join(sequence_dir, 'depth') 41 | 42 | rgb_frames_name = natsorted(os.listdir(rgb_folder_path)) 43 | depth_frames_name = natsorted(os.listdir(depth_folder_path)) 44 | 45 | self.rgb_frames = [os.path.join(rgb_folder_path, f) for f in rgb_frames_name if f.endswith('png')] 46 | self.depth_frames = [os.path.join(depth_folder_path, f) for f in depth_frames_name if f.endswith('png')] 47 | 48 | self.pose_path = os.path.join(sequence_dir, 'poses.txt') 49 | self.gt_poses, _ = self.load_poses(self.pose_path) 50 | 51 | H, W = 480, 640 52 | focal_file_path = os.path.join(sequence_dir, "focal.txt") 53 | focal_file = open(focal_file_path, "r") 54 | focal_length = float(focal_file.readline()) 55 | 56 | self.fx = focal_length 57 | self.fy = focal_length 58 | self.cx = (W-1)/2.0 59 | self.cy = (H-1)/2.0 60 | 61 | self.K_mat = np.eye(3) 62 | self.K_mat[0,0]=self.fx 63 | self.K_mat[1,1]=self.fy 64 | self.K_mat[0,2]=self.cx 65 | self.K_mat[1,2]=self.cy 66 | 67 | self.K_mats = {"cam": self.K_mat} 68 | 69 | self.intrinsic = self.o3d.camera.PinholeCameraIntrinsic() 70 | self.intrinsic.set_intrinsics(height=H, 71 | width=W, 72 | fx=self.fx, 73 | fy=self.fy, 74 | cx=self.cx, 75 | cy=self.cy) 76 | 77 | self.extrinsic = np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) 78 | 79 | self.T_l_c = self.extrinsic.astype(np.float64) 80 | self.T_c_l = np.linalg.inv(self.T_l_c) 81 | 82 | self.T_c_l_mats = {"cam": self.T_c_l} 83 | 84 | self.depth_scale = 1000.0 85 | self.max_depth_m = 10.0 86 | self.down_sample_on = False 87 | self.rand_down_rate = 0.1 88 | 89 | self.load_img = False 90 | 91 | def __len__(self): 92 | return len(self.depth_frames) 93 | 94 | def load_poses(self, path): 95 | file = open(path, "r") 96 | lines = file.readlines() 97 | file.close() 98 | poses = [] 99 | valid = [] 100 | lines_per_matrix = 4 101 | for i in range(0, len(lines), lines_per_matrix): 102 | if 'nan' in lines[i]: 103 | valid.append(False) 104 | poses.append(np.eye(4, 4, dtype=np.float32).tolist()) 105 | else: 106 | valid.append(True) 107 | pose_floats = np.array([[float(x) for x in line.split()] for line in lines[i:i+lines_per_matrix]]) 108 | poses.append(pose_floats) 109 | poses = np.array(poses) 110 | valid = np.array(valid) 111 | return poses, valid 112 | 113 | def __getitem__(self, idx): 114 | 115 | rgb_image = self.o3d.io.read_image(self.rgb_frames[idx]) 116 | depth_image = self.o3d.io.read_image(self.depth_frames[idx]) 117 | rgbd_image = self.o3d.geometry.RGBDImage.create_from_color_and_depth(rgb_image, 118 | depth_image, 119 | depth_scale=self.depth_scale, 120 | depth_trunc=self.max_depth_m, 121 | convert_rgb_to_intensity=False) 122 | 123 | pcd = self.o3d.geometry.PointCloud.create_from_rgbd_image( 124 | rgbd_image, self.intrinsic, self.extrinsic) 125 | if self.down_sample_on: 126 | pcd = pcd.random_down_sample(sampling_ratio=self.rand_down_rate) 127 | 128 | points_xyz = np.array(pcd.points, dtype=np.float64) 129 | points_rgb = np.array(pcd.colors, dtype=np.float64) 130 | points_xyzrgb = np.hstack((points_xyz, points_rgb)) 131 | frame_data = {"points": points_xyzrgb} 132 | 133 | if self.load_img: 134 | rgb_image = np.array(rgb_image) 135 | rgb_image_dict = {"cam": rgb_image} 136 | frame_data["img"] = rgb_image_dict 137 | 138 | return frame_data -------------------------------------------------------------------------------- /dataset/dataloaders/nuscenes.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 2024 Yue Pan 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | import importlib 25 | import os 26 | import sys 27 | from pathlib import Path 28 | from typing import List 29 | 30 | import numpy as np 31 | 32 | 33 | class NuScenesDataset: 34 | def __init__(self, data_dir: Path, sequence: str, *_, **__): 35 | try: 36 | importlib.import_module("nuscenes") 37 | except ModuleNotFoundError: 38 | print("nuscenes-devkit is not installed on your system") 39 | print('run "pip install nuscenes-devkit"') 40 | sys.exit(1) 41 | 42 | # TODO: If someone needs more splits from nuScenes expose this 2 parameters 43 | # nusc_version: str = "v1.0-trainval" 44 | # split: str = "train" 45 | nusc_version: str = "v1.0-mini" 46 | split: str = "mini_train" 47 | self.lidar_name: str = "LIDAR_TOP" 48 | 49 | # Lazy loading 50 | from nuscenes.nuscenes import NuScenes 51 | from nuscenes.utils.splits import create_splits_logs 52 | 53 | self.sequence_id = str(sequence).zfill(4) 54 | 55 | self.nusc = NuScenes(dataroot=str(data_dir), version=nusc_version) 56 | self.scene_name = f"scene-{self.sequence_id}" 57 | if self.scene_name not in [s["name"] for s in self.nusc.scene]: 58 | print(f'[ERROR] Sequence "{self.sequence_id}" not available scenes') 59 | print("\nAvailable scenes:") 60 | self.nusc.list_scenes() 61 | sys.exit(1) 62 | 63 | # Load nuScenes read from file inside dataloader module 64 | self.load_point_cloud = importlib.import_module( 65 | "nuscenes.utils.data_classes" 66 | ).LidarPointCloud.from_file 67 | 68 | # Get assignment of scenes to splits. 69 | split_logs = create_splits_logs(split, self.nusc) 70 | 71 | # Use only the samples from the current split. 72 | scene_token = self._get_scene_token(split_logs) 73 | self.lidar_tokens = self._get_lidar_tokens(scene_token) 74 | self.gt_poses = self._load_poses() 75 | 76 | def __len__(self): 77 | return len(self.lidar_tokens) 78 | 79 | def __getitem__(self, idx): 80 | 81 | points = self.read_point_cloud(self.lidar_tokens[idx]) 82 | point_ts = self.get_timestamps(points) 83 | frame_data = {"points": points, "point_ts": point_ts} 84 | # add imgs (TODO) 85 | 86 | return frame_data 87 | 88 | # velodyne lidar 89 | @staticmethod 90 | def get_timestamps(points): 91 | x = points[:, 0] 92 | y = points[:, 1] 93 | yaw = -np.arctan2(y, x) 94 | timestamps = 0.5 * (yaw / np.pi + 1.0) 95 | return timestamps 96 | 97 | def read_point_cloud(self, token: str): 98 | filename = self.nusc.get("sample_data", token)["filename"] 99 | pcl = self.load_point_cloud(os.path.join(self.nusc.dataroot, filename)) 100 | points = pcl.points.T[:, :3] 101 | return points.astype(np.float64) 102 | 103 | def _load_poses(self) -> np.ndarray: 104 | from nuscenes.utils.geometry_utils import transform_matrix 105 | from pyquaternion import Quaternion 106 | 107 | poses = np.empty((len(self), 4, 4), dtype=np.float32) 108 | for i, lidar_token in enumerate(self.lidar_tokens): 109 | sd_record_lid = self.nusc.get("sample_data", lidar_token) 110 | cs_record_lid = self.nusc.get( 111 | "calibrated_sensor", sd_record_lid["calibrated_sensor_token"] 112 | ) 113 | ep_record_lid = self.nusc.get("ego_pose", sd_record_lid["ego_pose_token"]) 114 | 115 | car_to_velo = transform_matrix( 116 | cs_record_lid["translation"], 117 | Quaternion(cs_record_lid["rotation"]), 118 | ) 119 | pose_car = transform_matrix( 120 | ep_record_lid["translation"], 121 | Quaternion(ep_record_lid["rotation"]), 122 | ) 123 | 124 | poses[i:, :] = pose_car @ car_to_velo 125 | 126 | # Convert from global coordinate poses to local poses 127 | first_pose = poses[0, :, :] 128 | poses = np.linalg.inv(first_pose) @ poses 129 | return poses 130 | 131 | def _get_scene_token(self, split_logs: List[str]) -> str: 132 | """ 133 | Convenience function to get the samples in a particular split. 134 | :param split_logs: A list of the log names in this split. 135 | :return: The list of samples. 136 | """ 137 | scene_tokens = [s["token"] for s in self.nusc.scene if s["name"] == self.scene_name][0] 138 | scene = self.nusc.get("scene", scene_tokens) 139 | log = self.nusc.get("log", scene["log_token"]) 140 | return scene["token"] if log["logfile"] in split_logs else "" 141 | 142 | def _get_lidar_tokens(self, scene_token: str) -> List[str]: 143 | # Get records from DB. 144 | scene_rec = self.nusc.get("scene", scene_token) 145 | start_sample_rec = self.nusc.get("sample", scene_rec["first_sample_token"]) 146 | sd_rec = self.nusc.get("sample_data", start_sample_rec["data"][self.lidar_name]) 147 | 148 | # Make list of frames 149 | cur_sd_rec = sd_rec 150 | sd_tokens = [cur_sd_rec["token"]] 151 | while cur_sd_rec["next"] != "": 152 | cur_sd_rec = self.nusc.get("sample_data", cur_sd_rec["next"]) 153 | sd_tokens.append(cur_sd_rec["token"]) 154 | return sd_tokens 155 | -------------------------------------------------------------------------------- /dataset/dataloaders/ouster.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # Copyright (c) 2023 Pavlo Bashmakov 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import os 26 | from typing import Optional 27 | 28 | import numpy as np 29 | 30 | 31 | class OusterDataloader: 32 | """Ouster pcap dataloader""" 33 | 34 | def __init__( 35 | self, 36 | data_dir: str, 37 | meta: Optional[str] = None, 38 | *_, 39 | **__, 40 | ): 41 | """Create Ouster pcap dataloader to read scans from a pcap file. 42 | 43 | Ouster pcap can be recorded with a `tcpdump` command or programmatically. 44 | Pcap file should contain raw lidar_packets and `meta` file (i.e. metadata.json) 45 | should be a corresponding sensor metadata stored at the time of pcap recording. 46 | 47 | 48 | NOTE: It's critical to have a metadata json stored in the same recording session 49 | as a pcap file, because pcap reader checks the `init_id` field in the UDP 50 | lidar_packets and expects it to match `initialization_id` 51 | in the metadata json, packets with different `init_id` just skipped. 52 | 53 | Metadata json can be obtainer with Ouster SDK: 54 | See examples here https://static.ouster.dev/sdk-docs/python/examples/basics-sensor.html#obtaining-sensor-metadata 55 | 56 | or with Sensor HTTP API endpoint GET /api/v1/sensor/metadata directly: 57 | See doc for details https://static.ouster.dev/sensor-docs/image_route1/image_route2/common_sections/API/http-api-v1.html#get-api-v1-sensor-metadata 58 | 59 | Args: 60 | data_dir: path to a pcap file (not a directory) 61 | meta: path to a metadata json file that should be recorded together with 62 | a pcap file. If `meta` is not provided attempts to find the best matching 63 | json file with the longest commong prefix of the pcap file (`data_dir`) in 64 | the same directory. 65 | """ 66 | 67 | try: 68 | from ouster.sdk import client, open_source 69 | except ImportError: 70 | print(f'ouster-sdk is not installed on your system, run "pip install ouster-sdk"') 71 | exit(1) 72 | 73 | assert os.path.isfile(data_dir), "Ouster pcap dataloader expects an existing PCAP file" 74 | 75 | # we expect `data_dir` param to be a path to the .pcap file, so rename for clarity 76 | pcap_file = data_dir 77 | 78 | print("Indexing Ouster pcap to count the scans number ...") 79 | source = open_source(str(pcap_file), meta=[meta] if meta else [], index=True) 80 | 81 | # since we import ouster-sdk's client module locally, we keep reference 82 | # to it locally as well 83 | self._client = client 84 | 85 | self.data_dir = os.path.dirname(data_dir) 86 | 87 | # lookup table for 2D range image projection to a 3D point cloud 88 | self._xyz_lut = client.XYZLut(source.metadata) 89 | 90 | self._pcap_file = str(data_dir) 91 | 92 | self._scans_num = len(source) 93 | print(f"Ouster pcap total scans number: {self._scans_num}") 94 | 95 | # frame timestamps array 96 | self._timestamps = np.linspace(0, self._scans_num, self._scans_num, endpoint=False) 97 | 98 | self._source = source 99 | 100 | def __getitem__(self, idx): 101 | scan = self._source[idx] 102 | 103 | self._timestamps[idx] = 1e-9 * scan.timestamp[0] 104 | 105 | timestamps = np.tile(np.linspace(0, 1.0, scan.w, endpoint=False), (scan.h, 1)) 106 | 107 | # filtering our zero returns makes it substantially faster 108 | sel_flag = scan.field(self._client.ChanField.RANGE) != 0 109 | xyz = self._xyz_lut(scan)[sel_flag] 110 | timestamps = timestamps[sel_flag] 111 | 112 | frame_data = {"points": xyz, "point_ts": timestamps} 113 | return frame_data 114 | 115 | def get_frames_timestamps(self): 116 | return self._timestamps 117 | 118 | def __len__(self): 119 | return self._scans_num -------------------------------------------------------------------------------- /dataset/dataloaders/paris_luco.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import glob 24 | import os 25 | from pathlib import Path 26 | 27 | import numpy as np 28 | from plyfile import PlyData 29 | 30 | 31 | class ParisLucoDataset: 32 | def __init__(self, data_dir: Path, *_, **__): 33 | # Config stuff 34 | self.sequence_id = os.path.basename(data_dir) 35 | self.sequence_dir = os.path.realpath(data_dir) 36 | self.velodyne_dir = os.path.join(self.sequence_dir, "frames/") 37 | self.scan_files = sorted(glob.glob(self.velodyne_dir + "*.ply")) 38 | self.gt_poses = self.load_gt_poses(os.path.join(self.sequence_dir, "gt_traj_lidar.txt")) 39 | 40 | def __len__(self): 41 | return len(self.scan_files) 42 | 43 | def __getitem__(self, idx): 44 | return self.read_point_cloud(self.scan_files[idx]) 45 | 46 | def read_point_cloud(self, file_path: str): 47 | plydata = PlyData.read(file_path) 48 | x = np.asarray(plydata.elements[0].data["x"]).reshape(-1, 1) 49 | y = np.asarray(plydata.elements[0].data["y"]).reshape(-1, 1) 50 | z = np.asarray(plydata.elements[0].data["z"]).reshape(-1, 1) 51 | points = np.concatenate([x, y, z], axis=1) 52 | timestamps = np.asarray(plydata.elements[0].data["timestamp"]) 53 | timestamps = timestamps / np.max(timestamps) 54 | return points.astype(np.float64), timestamps 55 | 56 | def load_gt_poses(self, file_path): 57 | gt_poses = [] 58 | for xyz in np.loadtxt(file_path): 59 | T = np.eye(4) 60 | T[:3, 3] = xyz 61 | gt_poses.append(T) 62 | return np.array(gt_poses).reshape(-1, 4, 4) 63 | 64 | def apply_calibration(self, poses): 65 | """ParisLucoDataset only has a x, y, z trajectory, so we must will em all""" 66 | new_poses = [] 67 | for pose in poses: 68 | T = pose.copy() 69 | T[:3, :3] = np.eye(3) 70 | new_poses.append(T) 71 | return new_poses 72 | -------------------------------------------------------------------------------- /dataset/dataloaders/replica.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # Copyright (c) 2024 Yue Pan 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import importlib 26 | import glob 27 | import os 28 | from pathlib import Path 29 | 30 | import numpy as np 31 | 32 | # The Replica dataset is a commonly used synthetic RGB-D dataset 33 | # It can be downloaded from: https://cvg-data.inf.ethz.ch/nice-slam/data/Replica.zip 34 | # Or use the script: sh scripts/download_replica.sh 35 | 36 | class ReplicaDataset: 37 | def __init__(self, data_dir: Path, sequence: str, *_, **__): 38 | try: 39 | self.o3d = importlib.import_module("open3d") 40 | except ModuleNotFoundError as err: 41 | print(f'open3d is not installed on your system, run "pip install open3d"') 42 | exit(1) 43 | 44 | sequence_dir = os.path.join(data_dir, sequence) 45 | 46 | self.img_dir = os.path.join(sequence_dir, "results/") 47 | self.rgb_frames = sorted(glob.glob(self.img_dir + '*.jpg')) 48 | self.depth_frames = sorted(glob.glob(self.img_dir + '*.png')) 49 | 50 | self.poses_fn = os.path.join(sequence_dir, "traj.txt") 51 | self.gt_poses = self.load_poses(self.poses_fn) 52 | 53 | self.intrinsic = self.o3d.camera.PinholeCameraIntrinsic() 54 | # From cam_params.json, shared by all sequences 55 | 56 | self.fx = 600.0 57 | self.fy = 600.0 58 | self.cx = 599.5 59 | self.cy = 339.5 60 | 61 | self.K_mat = np.eye(3) 62 | self.K_mat[0,0]=self.fx 63 | self.K_mat[1,1]=self.fy 64 | self.K_mat[0,2]=self.cx 65 | self.K_mat[1,2]=self.cy 66 | 67 | self.K_mats = {"cam": self.K_mat} 68 | 69 | self.T_l_c = np.eye(4) 70 | self.T_c_l = np.linalg.inv(self.T_l_c) 71 | 72 | self.T_c_l_mats = {"cam": self.T_c_l} 73 | 74 | self.intrinsic.set_intrinsics(height=680, 75 | width=1200, 76 | fx=self.fx, 77 | fy=self.fy, 78 | cx=self.cx, 79 | cy=self.cy) 80 | 81 | self.depth_scale = 6553.5 82 | self.max_depth_m = 10.0 83 | self.down_sample_on = False 84 | self.rand_down_rate = 0.1 85 | 86 | self.load_img = False 87 | 88 | def __len__(self): 89 | return len(self.depth_frames) 90 | 91 | def load_poses(self, poses_file: str) -> np.ndarray: 92 | poses = np.loadtxt(poses_file, delimiter=" ") 93 | n = poses.shape[0] 94 | return poses.reshape((n, 4, 4)) 95 | 96 | def __getitem__(self, idx): 97 | rgb_image = self.o3d.io.read_image(self.rgb_frames[idx]) 98 | depth_image = self.o3d.io.read_image(self.depth_frames[idx]) 99 | rgbd_image = self.o3d.geometry.RGBDImage.create_from_color_and_depth(rgb_image, 100 | depth_image, 101 | depth_scale=self.depth_scale, 102 | depth_trunc=self.max_depth_m, 103 | convert_rgb_to_intensity=False) 104 | 105 | pcd = self.o3d.geometry.PointCloud.create_from_rgbd_image( 106 | rgbd_image, self.intrinsic) 107 | if self.down_sample_on: 108 | pcd = pcd.random_down_sample(sampling_ratio=self.rand_down_rate) 109 | 110 | points_xyz = np.array(pcd.points, dtype=np.float64) 111 | points_rgb = np.array(pcd.colors, dtype=np.float64) 112 | points_xyzrgb = np.hstack((points_xyz, points_rgb)) 113 | frame_data = {"points": points_xyzrgb} 114 | 115 | if self.load_img: 116 | rgb_image = np.array(rgb_image) 117 | rgb_image_dict = {"cam": rgb_image} 118 | frame_data["img"] = rgb_image_dict 119 | 120 | return frame_data -------------------------------------------------------------------------------- /dataset/dataloaders/rosbag.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 2024 Yue Pan 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | import glob 25 | import os 26 | import sys 27 | from pathlib import Path 28 | from typing import Sequence 29 | 30 | import natsort 31 | 32 | 33 | class RosbagDataset: 34 | def __init__(self, data_dir: Path, topic: str, *_, **__): 35 | """ROS1 / ROS2 bagfile dataloader. 36 | 37 | It can take either one ROS2 bag file or one or more ROS1 bag files belonging to a split bag. 38 | The reader will replay ROS1 split bags in correct timestamp order. 39 | 40 | TODO: Merge mcap and rosbag dataloaders into 1 41 | """ 42 | try: 43 | from rosbags.highlevel import AnyReader 44 | except ModuleNotFoundError: 45 | print('rosbags library not installed, run "pip install -U rosbags"') 46 | sys.exit(1) 47 | 48 | from utils.point_cloud2 import read_point_cloud 49 | 50 | self.read_point_cloud = read_point_cloud 51 | if data_dir.is_file(): 52 | self.sequence_id = os.path.basename(data_dir).split(".")[0] 53 | self.bag = AnyReader([data_dir]) 54 | else: 55 | bagfiles = [Path(path) for path in glob.glob(os.path.join(data_dir, "*.bag"))] 56 | if len(bagfiles) > 0: 57 | self.sequence_id = os.path.basename(bagfiles[0]).split(".")[0] 58 | self.bag = AnyReader(bagfiles) 59 | else: 60 | self.sequence_id = os.path.basename(data_dir).split(".")[0] 61 | self.bag = AnyReader([data_dir]) 62 | 63 | if len(self.bag.paths) > 1: 64 | print("Reading multiple .bag files in directory:") 65 | print("\n".join(natsort.natsorted([path.name for path in self.bag.paths]))) 66 | 67 | self.bag.open() 68 | self.topic = self.check_topic(topic) 69 | self.n_scans = self.bag.topics[self.topic].msgcount 70 | 71 | # limit connections to selected topic 72 | connections = [x for x in self.bag.connections if x.topic == self.topic] 73 | self.msgs = self.bag.messages(connections=connections) 74 | self.timestamps = [] 75 | 76 | def __del__(self): 77 | if hasattr(self, "bag"): 78 | self.bag.close() 79 | 80 | def __len__(self): 81 | return self.n_scans 82 | 83 | def __getitem__(self, idx): 84 | connection, timestamp, rawdata = next(self.msgs) 85 | self.timestamps.append(self.to_sec(timestamp)) 86 | msg = self.bag.deserialize(rawdata, connection.msgtype) 87 | 88 | points, point_ts = self.read_point_cloud(msg) 89 | frame_data = {"points": points, "point_ts": point_ts} 90 | 91 | return frame_data 92 | 93 | @staticmethod 94 | def to_sec(nsec: int): 95 | return float(nsec) / 1e9 96 | 97 | def get_frames_timestamps(self) -> list: 98 | return self.timestamps 99 | 100 | def check_topic(self, topic: str) -> str: 101 | # Extract all PointCloud2 msg topics from the bagfile 102 | point_cloud_topics = [ 103 | topic[0] 104 | for topic in self.bag.topics.items() 105 | if topic[1].msgtype == "sensor_msgs/msg/PointCloud2" 106 | ] 107 | 108 | def print_available_topics_and_exit(): 109 | print("Select from the following topics:") 110 | print(50 * "-") 111 | for t in point_cloud_topics: 112 | print(f"{t}") 113 | print(50 * "-") 114 | sys.exit(1) 115 | 116 | if topic and topic in point_cloud_topics: 117 | return topic 118 | # when user specified the topic check that exists 119 | if topic and topic not in point_cloud_topics: 120 | print( 121 | f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' 122 | "Specify the correct topic name by python pin_slam.py path/to/config/file.yaml rosbag your/topic ... ..." 123 | ) 124 | print_available_topics_and_exit() 125 | if len(point_cloud_topics) > 1: 126 | print( 127 | "Multiple sensor_msgs/msg/PointCloud2 topics available." 128 | "Specify the correct topic name by python pin_slam.py path/to/config/file.yaml rosbag your/topic ... ..." 129 | ) 130 | print_available_topics_and_exit() 131 | 132 | if len(point_cloud_topics) == 0: 133 | print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") 134 | if len(point_cloud_topics) == 1: 135 | return point_cloud_topics[0] 136 | -------------------------------------------------------------------------------- /dataset/dataloaders/tum.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # Copyright (c) 2024 Yue Pan 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | import importlib 25 | import os 26 | from pathlib import Path 27 | 28 | import numpy as np 29 | 30 | class TUMDataset: 31 | def __init__(self, data_dir: Path, sequence: str, *_, **__): 32 | try: 33 | self.o3d = importlib.import_module("open3d") 34 | except ModuleNotFoundError as err: 35 | print(f'open3d is not installed on your system, run "pip install open3d"') 36 | exit(1) 37 | 38 | sequence_dir = os.path.join(data_dir, sequence) 39 | 40 | self.rgb_frames, self.depth_frames, self.gt_poses = self.loadtum(sequence_dir) 41 | 42 | self.intrinsic = self.o3d.camera.PinholeCameraIntrinsic() 43 | H, W = 480, 640 44 | 45 | if "freiburg1" in sequence: 46 | fx, fy, cx, cy = 517.3, 516.5, 318.6, 255.3 47 | elif "freiburg2" in sequence: 48 | fx, fy, cx, cy = 520.9, 521.0, 325.1, 249.7 49 | elif "freiburg3" in sequence: 50 | fx, fy, cx, cy = 535.4, 539.2, 320.1, 247.6 51 | else: # default 52 | fx, fy, cx, cy = 525.0, 525.0, 319.5, 239.5 53 | 54 | self.intrinsic.set_intrinsics(height=H, 55 | width=W, 56 | fx=fx, 57 | fy=fy, 58 | cx=cx, 59 | cy=cy) 60 | 61 | self.K_mat = np.eye(3) 62 | self.K_mat[0,0]=fx 63 | self.K_mat[1,1]=fy 64 | self.K_mat[0,2]=cx 65 | self.K_mat[1,2]=cy 66 | 67 | self.K_mats = {"cam": self.K_mat} 68 | 69 | self.T_l_c = np.eye(4) 70 | self.T_c_l = np.linalg.inv(self.T_l_c) 71 | 72 | self.T_c_l_mats = {"cam": self.T_c_l} 73 | 74 | self.down_sample_on = False 75 | self.rand_down_rate = 0.1 76 | 77 | self.load_img = False 78 | 79 | def __len__(self): 80 | return len(self.depth_frames) 81 | 82 | def loadtum(self, datapath, frame_rate=-1): 83 | """ read video data in tum-rgbd format """ 84 | if os.path.isfile(os.path.join(datapath, 'groundtruth.txt')): 85 | pose_list = os.path.join(datapath, 'groundtruth.txt') 86 | 87 | image_list = os.path.join(datapath, 'rgb.txt') 88 | depth_list = os.path.join(datapath, 'depth.txt') 89 | 90 | def parse_list(filepath, skiprows=0): 91 | """ read list data """ 92 | data = np.loadtxt(filepath, delimiter=' ', 93 | dtype=np.unicode_, skiprows=skiprows) 94 | return data 95 | 96 | def associate_frames(tstamp_image, tstamp_depth, tstamp_pose, max_dt=0.08): 97 | """ pair images, depths, and poses """ 98 | associations = [] 99 | for i, t in enumerate(tstamp_image): 100 | if tstamp_pose is None: 101 | j = np.argmin(np.abs(tstamp_depth - t)) 102 | if (np.abs(tstamp_depth[j] - t) < max_dt): 103 | associations.append((i, j)) 104 | else: 105 | j = np.argmin(np.abs(tstamp_depth - t)) 106 | k = np.argmin(np.abs(tstamp_pose - t)) 107 | 108 | if (np.abs(tstamp_depth[j] - t) < max_dt) and \ 109 | (np.abs(tstamp_pose[k] - t) < max_dt): 110 | associations.append((i, j, k)) 111 | return associations 112 | 113 | def pose_matrix_from_quaternion(pvec): 114 | """ convert 4x4 pose matrix to (t, q) """ 115 | from scipy.spatial.transform import Rotation 116 | pose = np.eye(4) 117 | pose[:3, :3] = Rotation.from_quat(pvec[3:]).as_matrix() # rotation 118 | pose[:3, 3] = pvec[:3] # translation 119 | return pose 120 | 121 | image_data = parse_list(image_list) 122 | depth_data = parse_list(depth_list) 123 | pose_data = parse_list(pose_list, skiprows=1) 124 | pose_vecs = pose_data[:, 1:].astype(np.float64) 125 | 126 | tstamp_image = image_data[:, 0].astype(np.float64) 127 | tstamp_depth = depth_data[:, 0].astype(np.float64) 128 | tstamp_pose = pose_data[:, 0].astype(np.float64) 129 | associations = associate_frames( 130 | tstamp_image, tstamp_depth, tstamp_pose) 131 | 132 | indicies = [0] 133 | for i in range(1, len(associations)): 134 | t0 = tstamp_image[associations[indicies[-1]][0]] 135 | t1 = tstamp_image[associations[i][0]] 136 | if t1 - t0 > 1.0 / frame_rate: 137 | indicies += [i] 138 | 139 | images, poses, depths = [], [], [] 140 | for ix in indicies: 141 | (i, j, k) = associations[ix] 142 | images += [os.path.join(datapath, image_data[i, 1])] 143 | depths += [os.path.join(datapath, depth_data[j, 1])] 144 | c2w = pose_matrix_from_quaternion(pose_vecs[k]) 145 | poses += [c2w] 146 | 147 | poses = np.array(poses) 148 | 149 | return images, depths, poses 150 | 151 | def __getitem__(self, idx): 152 | 153 | im_color = self.o3d.io.read_image(self.rgb_frames[idx]) 154 | im_depth = self.o3d.io.read_image(self.depth_frames[idx]) 155 | rgbd_image = self.o3d.geometry.RGBDImage.create_from_tum_format(im_color, 156 | im_depth, convert_rgb_to_intensity=False) 157 | 158 | pcd = self.o3d.geometry.PointCloud.create_from_rgbd_image( 159 | rgbd_image, 160 | self.intrinsic 161 | ) 162 | if self.down_sample_on: 163 | pcd = pcd.random_down_sample(sampling_ratio=self.rand_down_rate) 164 | 165 | points_xyz = np.array(pcd.points, dtype=np.float64) 166 | points_rgb = np.array(pcd.colors, dtype=np.float64) 167 | points_xyzrgb = np.hstack((points_xyz, points_rgb)) 168 | 169 | frame_data = {"points": points_xyzrgb} 170 | 171 | if self.load_img: 172 | rgb_image = np.array(self.rgb_frames[idx]) 173 | rgb_image_dict = {"cam": rgb_image} 174 | frame_data["img"] = rgb_image_dict 175 | 176 | return frame_data -------------------------------------------------------------------------------- /dataset/dataset_indexing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # @file dataset_indexing.py 3 | # @author Yue Pan [yue.pan@igg.uni-bonn.de] 4 | # Copyright (c) 2024 Yue Pan, all rights reserved 5 | 6 | import os 7 | 8 | from utils.config import Config 9 | 10 | def set_dataset_path(config: Config, dataset_name: str = '', seq: str = ''): 11 | 12 | if seq is None: 13 | seq = '' 14 | 15 | config.name = config.name + '_' + dataset_name + '_' + seq.replace("/", "") 16 | 17 | # recommended 18 | if config.use_dataloader: 19 | config.data_loader_name = dataset_name 20 | config.data_loader_seq = seq 21 | print('Using data loader for specific dataset or specific input data format') 22 | # from dataset.dataloaders import available_dataloaders 23 | # print('Available dataloaders:', available_dataloaders()) 24 | 25 | # not recommended 26 | else: 27 | if dataset_name == "kitti": 28 | base_path = config.pc_path.rsplit('/', 3)[0] 29 | config.pc_path = os.path.join(base_path, 'sequences', seq, "velodyne") # input point cloud folder 30 | pose_file_name = seq + '.txt' 31 | config.pose_path = os.path.join(base_path, 'poses', pose_file_name) # input pose file 32 | config.calib_path = os.path.join(base_path, 'sequences', seq, "calib.txt") # input calib file (to sensor frame) 33 | config.label_path = os.path.join(base_path, 'sequences', seq, "labels") # input point-wise label path, for semantic mapping (optional) 34 | config.kitti_correction_on = True 35 | config.correction_deg = 0.195 36 | elif dataset_name == "mulran": 37 | config.name = config.name + "_mulran_" + seq 38 | base_path = config.pc_path.rsplit('/', 2)[0] 39 | config.pc_path = os.path.join(base_path, seq, "Ouster") # input point cloud folder 40 | config.pose_path = os.path.join(base_path, seq, "poses.txt") # input pose file (for this you need to do the conversion from global_pose.csv to poses.txt in kitti format by yourself. Otherwise, use Mulran data loader with the -d flag) 41 | elif dataset_name == "kitti_carla": 42 | config.name = config.name + "_kitti_carla_" + seq 43 | base_path = config.pc_path.rsplit('/', 3)[0] 44 | config.pc_path = os.path.join(base_path, seq, "generated", "frames") # input point cloud folder 45 | config.pose_path = os.path.join(base_path, seq, "generated", "poses.txt") # input pose file 46 | config.calib_path = os.path.join(base_path, seq, "generated", "calib.txt") # input calib file (to sensor frame) 47 | elif dataset_name == "ncd": 48 | config.name = config.name + "_ncd_" + seq 49 | base_path = config.pc_path.rsplit('/', 2)[0] 50 | config.pc_path = os.path.join(base_path, seq, "bin") # input point cloud folder 51 | config.pose_path = os.path.join(base_path, seq, "poses.txt") # input pose file 52 | config.calib_path = os.path.join(base_path, seq, "calib.txt") # input calib file (to sensor frame) 53 | elif dataset_name == "ncd128": 54 | config.name = config.name + "_ncd128_" + seq 55 | base_path = config.pc_path.rsplit('/', 2)[0] 56 | config.pc_path = os.path.join(base_path, seq, "ply") # input point cloud folder 57 | config.pose_path = os.path.join(base_path, seq, "poses.txt") # input pose file 58 | elif dataset_name == "ipbcar": 59 | config.name = config.name + "_ipbcar_" + seq 60 | base_path = config.pc_path.rsplit('/', 2)[0] 61 | config.pc_path = os.path.join(base_path, seq, "ouster") # input point cloud folder 62 | config.pose_path = os.path.join(base_path, seq, "poses.txt") # input pose file 63 | config.calib_path = os.path.join(base_path, seq, "calib.txt") # input calib file (to sensor frame) 64 | elif dataset_name == "hilti": 65 | config.name = config.name + "_hilti_" + seq 66 | base_path = config.pc_path.rsplit('/', 2)[0] 67 | config.pc_path = os.path.join(base_path, seq, "ply") # input point cloud folder 68 | # config.pose_path = os.path.join(base_path, seq, "poses.txt") # input pose file 69 | elif dataset_name == "m2dgr": 70 | config.name = config.name + "_m2dgr_" + seq 71 | base_path = config.pc_path.rsplit('/', 2)[0] 72 | config.pc_path = os.path.join(base_path, seq, "points") # input point cloud folder 73 | config.pose_path = os.path.join(base_path, seq, "poses.txt") # input pose file 74 | elif dataset_name == "replica": 75 | # you need to preprocess replica dataset to pin format using scripts/convert_replica.sh 76 | # otherwise, use: python pin_slam.py ./config/rgbd_slam/run_replica.yaml replica room0 -i data/Replica/ -vsmd 77 | config.name = config.name + "_replica_" + seq 78 | base_path = config.pc_path.rsplit('/', 2)[0] 79 | config.pc_path = os.path.join(base_path, seq, "rgbd_down_ply") # input point cloud folder 80 | #config.pc_path = os.path.join(base_path, seq, "rgbd_ply") # input point cloud folder 81 | config.pose_path = os.path.join(base_path, seq, "poses.txt") # input pose file 82 | else: 83 | print('The specified dataset has not been supported yet, try using specific data loader by adding -d flag.') 84 | -------------------------------------------------------------------------------- /docker/build_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "Build docker" 4 | 5 | sudo docker build -f cu117.Dockerfile -t pinslam:localbuild . 6 | 7 | echo "docker successfully build!" 8 | -------------------------------------------------------------------------------- /docker/cu117.Dockerfile: -------------------------------------------------------------------------------- 1 | FROM nvidia/cuda:11.7.1-base-ubuntu20.04 2 | 3 | # Set environment variables 4 | ENV DEBIAN_FRONTEND=noninteractive 5 | ENV NVENCODE_CFLAGS="-I/usr/local/cuda/include" 6 | ENV CV_VERSION=4.2.0 7 | ENV DEBIAN_FRONTEND=noninteractive 8 | ENV TORCH_CUDA_ARCH_LIST="3.5;5.0;6.0;6.1;7.0;7.5;8.0;8.6+PTX" 9 | 10 | # Install python3.10 on Ubuntu 20.04 using deadsnakes PPA 11 | RUN apt-get update && \ 12 | apt-get install software-properties-common -y && \ 13 | add-apt-repository ppa:deadsnakes/ppa && \ 14 | apt-get update && apt-get install -y \ 15 | python3.10 && \ 16 | update-alternatives --install /usr/bin/python3 python3 /usr/bin/python3.10 1 17 | 18 | # Get all dependencies 19 | RUN apt-get update && apt-get install -y \ 20 | git zip unzip libssl-dev libcairo2-dev lsb-release libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev software-properties-common \ 21 | build-essential cmake pkg-config libapr1-dev autoconf automake libtool curl libc6 libboost-all-dev debconf libomp5 libstdc++6 \ 22 | libqt5core5a libqt5xml5 libqt5gui5 libqt5widgets5 libqt5concurrent5 libqt5opengl5 libcap2 libusb-1.0-0 libatk-adaptor neovim \ 23 | python3-tornado python3-dev python3-numpy python3-virtualenv libpcl-dev libgoogle-glog-dev libgflags-dev libatlas-base-dev \ 24 | libsuitesparse-dev python3-pcl pcl-tools libgtk2.0-dev libavcodec-dev libavformat-dev libswscale-dev libtbb2 libtbb-dev libjpeg-dev \ 25 | libpng-dev libtiff-dev libdc1394-22-dev xfce4-terminal \ 26 | libclang-dev \ 27 | libatk-bridge2.0 \ 28 | libfontconfig1-dev \ 29 | libfreetype6-dev \ 30 | libglib2.0-dev \ 31 | libgtk-3-dev \ 32 | libssl-dev \ 33 | libxcb-render0-dev \ 34 | libxcb-shape0-dev \ 35 | libxcb-xfixes0-dev \ 36 | libxkbcommon-dev \ 37 | patchelf \ 38 | wget &&\ 39 | rm -rf /var/lib/apt/lists/* 40 | 41 | # OpenCV with CUDA support 42 | WORKDIR /opencv 43 | RUN git clone https://github.com/opencv/opencv.git -b $CV_VERSION &&\ 44 | git clone https://github.com/opencv/opencv_contrib.git -b $CV_VERSION 45 | 46 | # While using OpenCV 4.2.0 we have to apply some fixes to ensure that CUDA is fully supported, thanks @https://github.com/gismo07 for this fix 47 | RUN mkdir opencvfix && cd opencvfix &&\ 48 | git clone https://github.com/opencv/opencv.git -b 4.5.2 &&\ 49 | cd opencv/cmake &&\ 50 | cp -r FindCUDA /opencv/opencv/cmake/ &&\ 51 | cp FindCUDA.cmake /opencv/opencv/cmake/ &&\ 52 | cp FindCUDNN.cmake /opencv/opencv/cmake/ &&\ 53 | cp OpenCVDetectCUDA.cmake /opencv/opencv/cmake/ 54 | 55 | WORKDIR /opencv/opencv/build 56 | 57 | RUN cmake -D CMAKE_BUILD_TYPE=RELEASE \ 58 | -D CMAKE_INSTALL_PREFIX=/usr/local \ 59 | -D OPENCV_GENERATE_PKGCONFIG=ON \ 60 | -D BUILD_EXAMPLES=OFF \ 61 | -D INSTALL_PYTHON_EXAMPLES=OFF \ 62 | -D INSTALL_C_EXAMPLES=OFF \ 63 | -D PYTHON_EXECUTABLE=$(which python2) \ 64 | -D PYTHON3_EXECUTABLE=$(which python3) \ 65 | -D PYTHON3_INCLUDE_DIR=$(python3 -c "from distutils.sysconfig import get_python_inc; print(get_python_inc())") \ 66 | -D PYTHON3_PACKAGES_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib())") \ 67 | -D BUILD_opencv_python2=ON \ 68 | -D BUILD_opencv_python3=ON \ 69 | -D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules/ \ 70 | -D WITH_GSTREAMER=ON \ 71 | -D WITH_CUDA=ON \ 72 | -D ENABLE_PRECOMPILED_HEADERS=OFF \ 73 | .. &&\ 74 | make -j$(nproc) &&\ 75 | make install &&\ 76 | ldconfig &&\ 77 | rm -rf /opencv 78 | 79 | WORKDIR / 80 | ENV OpenCV_DIR=/usr/share/OpenCV 81 | 82 | RUN curl -sS https://bootstrap.pypa.io/get-pip.py | python3 83 | RUN python3 -m pip install --upgrade pip 84 | RUN python3 -m pip install --upgrade setuptools 85 | 86 | # PyTorch for CUDA 11.7 87 | RUN python3 -m pip install torch==2.0.0 torchvision==0.15.0 torchaudio==2.0.0 --index-url https://download.pytorch.org/whl/cu117 88 | 89 | # PIN SLAM 90 | RUN mkdir -p /src 91 | WORKDIR /src/ 92 | RUN git clone https://github.com/PRBonn/PIN_SLAM.git 93 | WORKDIR /src/PIN_SLAM 94 | 95 | RUN python3 -m pip install --upgrade pip 96 | RUN python3 -m pip install --upgrade properties 97 | # Fix uninstallation-issue with Blinker 1.4 98 | RUN python3 -m pip install --ignore-installed blinker 99 | RUN python3 -m pip install -r ./requirements.txt 100 | 101 | # Load the KITTI data 102 | RUN mkdir -p /src/PIN_SLAM/data 103 | WORKDIR /src/PIN_SLAM/data 104 | 105 | RUN wget -O kitti_example.tar.gz -c https://uni-bonn.sciebo.de/s/Ycl28f1Cppghvjm/download 106 | RUN tar -xvf kitti_example.tar.gz 107 | RUN rm kitti_example.tar.gz 108 | 109 | # Fix unknown ownership 110 | RUN git config --global --add safe.directory '*' 111 | 112 | # Finish installation 113 | WORKDIR /src/PIN_SLAM 114 | 115 | RUN apt-get clean 116 | 117 | ENV NVIDIA_VISIBLE_DEVICES="all" \ 118 | OpenCV_DIR=/usr/share/OpenCV \ 119 | NVIDIA_DRIVER_CAPABILITIES="video,compute,utility,graphics" \ 120 | LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/lib:/usr/lib:/usr/local/lib \ 121 | QT_GRAPHICSSYSTEM="native" 122 | -------------------------------------------------------------------------------- /docker/start_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | xhost local:root && docker run -it --rm -e SDL_VIDEODRIVER=x11 -e DISPLAY=$DISPLAY --env='DISPLAY' --ipc host --privileged --network host -p 8080:8081 --gpus all \ 4 | -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ 5 | -v /your_data_storage_directory_here:/storage/ \ 6 | pinslam:localbuild xfce4-terminal --title=PIN-SLAM 7 | -------------------------------------------------------------------------------- /eval/README.md: -------------------------------------------------------------------------------- 1 | # PIN-SLAM Evaluation 2 | 3 | In this folder, you can find a few Python notebooks to run the evaluation of PIN-SLAM on typical public datasets without much effort. Note that you'd better checkout to the v1.0.0 branch of PIN-SLAM to have the results closer to the paper. 4 | 5 | This file is inspired by [KISS-ICP](https://github.com/PRBonn/kiss-icp/tree/main/eval). 6 | 7 | ## Datasets evaluated 8 | 9 | **NOTE:** In this folder, we add the empty notebooks to use for running experiments on your 10 | local machine. If you want to see the results of these experiments you can click on any link below: 11 | 12 | - [KITTI Odometry Dataset (random seed 42)](https://nbviewer.org/github/YuePanEdward/PIN_evaluation/blob/main/eval_kitti.ipynb) 13 | - [KITTI Odometry Dataset (random seed 2024)](https://nbviewer.org/github/YuePanEdward/PIN_evaluation/blob/main/eval_kitti_2024.ipynb) 14 | - [MulRan Dataset (random seed 42)](https://nbviewer.org/github/YuePanEdward/PIN_evaluation/blob/main/eval_mulran.ipynb) 15 | - [Newer College Dataset (random seed 42)](https://nbviewer.org/github/YuePanEdward/PIN_evaluation/blob/main/eval_ncd.ipynb) 16 | - [Newer College Dataset 2021 extension (random seed 42)](https://nbviewer.org/github/YuePanEdward/PIN_evaluation/blob/main/eval_ncd_128.ipynb) 17 | - [Replica RGB-D Dataset (random seed 42)](https://nbviewer.org/github/YuePanEdward/PIN_evaluation/blob/main/eval_replica.ipynb) 18 | 19 | ## Public benchmark evaluation 20 | - [KITTI Odometry Dataset](https://www.cvlibs.net/datasets/kitti/eval_odometry.php) 21 | - [VBR SLAM Benchmark](https://rvp-group.net/slam-benchmark.html) -------------------------------------------------------------------------------- /eval/eval_kitti.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# PIN-SLAM running on the KITTI-Odometry benchmark dataset\n", 8 | "The aim of this notebook is to serve as a reproducible entry point for the experiments outlined in the paper.\n", 9 | "\n", 10 | "This notebook is inspired by [KISS-ICP](https://nbviewer.org/github/nachovizzo/kiss-icp/blob/main/evaluation/kitti.ipynb).\n", 11 | "\n", 12 | "Please download the dataset from [here](https://www.cvlibs.net/datasets/kitti/eval_odometry.php).\n", 13 | "\n", 14 | "To directly run this notebook without changing path, you need to put it in the parent directory of `eval`.\n", 15 | "\n", 16 | "Note: the results are slightly different using different random seeds and different hardwares." 17 | ] 18 | }, 19 | { 20 | "cell_type": "code", 21 | "execution_count": null, 22 | "metadata": {}, 23 | "outputs": [], 24 | "source": [ 25 | "import pin_slam\n", 26 | "from eval.eval_traj_utils import get_metrics, mean_metrics\n", 27 | "import torch\n", 28 | "from IPython.display import display_markdown\n", 29 | "\n", 30 | "print('Device used:', torch.cuda.get_device_name(0))\n", 31 | "\n", 32 | "metrics_dicts = []\n", 33 | "for seq in range(11):\n", 34 | " seq_str = f'{seq:02d}'\n", 35 | " print('Now evaluate sequence '+ seq_str)\n", 36 | " seq_results = pin_slam.run_pin_slam('./config/lidar_slam/run_kitti.yaml', 'kitti', seq_str)\n", 37 | " metrics_dict = get_metrics(seq_results)\n", 38 | " metrics_dicts.append(metrics_dict)\n", 39 | "\n", 40 | "metric_mean = mean_metrics(metrics_dicts)\n", 41 | "table_results = f\"# Experiment Results (KITTI dataset) \\n|Metric|Value|\\n|-:|:-|\\n\"\n", 42 | "for metric, result in metric_mean.items():\n", 43 | " table_results += f\"|{metric}|{result:.2f}|\\n\"\n", 44 | "display_markdown(table_results, raw=True)" 45 | ] 46 | } 47 | ], 48 | "metadata": { 49 | "interpreter": { 50 | "hash": "916dbcbb3f70747c44a77c7bcd40155683ae19c65e1c03b4aa3499c5328201f1" 51 | }, 52 | "kernelspec": { 53 | "display_name": "Python 3.8.10 64-bit", 54 | "language": "python", 55 | "name": "python3" 56 | }, 57 | "language_info": { 58 | "codemirror_mode": { 59 | "name": "ipython", 60 | "version": 3 61 | }, 62 | "file_extension": ".py", 63 | "mimetype": "text/x-python", 64 | "name": "python", 65 | "nbconvert_exporter": "python", 66 | "pygments_lexer": "ipython3", 67 | "version": "3.8.19" 68 | }, 69 | "orig_nbformat": 4 70 | }, 71 | "nbformat": 4, 72 | "nbformat_minor": 2 73 | } 74 | -------------------------------------------------------------------------------- /eval/eval_mulran.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# PIN-SLAM running on MulRan dataset\n", 8 | "The aim of this notebook is to serve as a reproducible entry point for the experiments outlined in the paper.\n", 9 | "\n", 10 | "To directly run this notebook without changing path, you need to put it in the parent directory of `eval`.\n", 11 | "\n", 12 | "Please download the dataset from [here](https://sites.google.com/view/mulran-pr/download) and use this [script](https://github.com/PRBonn/PIN_SLAM/blob/main/dataset/converter/mulran_to_pin_format.py) to generate the reference pose file. \n", 13 | "\n", 14 | "Note: the results are slightly different using different random seeds and different hardwares.\n", 15 | "\n" 16 | ] 17 | }, 18 | { 19 | "cell_type": "code", 20 | "execution_count": null, 21 | "metadata": {}, 22 | "outputs": [], 23 | "source": [ 24 | "import pin_slam\n", 25 | "from eval.eval_traj_utils import get_metrics, mean_metrics\n", 26 | "import torch\n", 27 | "from IPython.display import display_markdown\n", 28 | "\n", 29 | "print('Device used:', torch.cuda.get_device_name(0))\n", 30 | "\n", 31 | "metrics_dicts = []\n", 32 | "seq_list = ['kaist01', 'kaist02', 'kaist03', 'dcc01', 'dcc02', 'dcc03', 'riverside01', 'riverside02', 'riverside03']\n", 33 | "for seq_str in seq_list:\n", 34 | " print('Now evaluate sequence '+ seq_str)\n", 35 | " seq_results = pin_slam.run_pin_slam('./config/lidar_slam/run_mulran.yaml', 'mulran', seq_str)\n", 36 | " metrics_dict = get_metrics(seq_results)\n", 37 | " metrics_dicts.append(metrics_dict)\n", 38 | "\n", 39 | "metric_mean = mean_metrics(metrics_dicts)\n", 40 | "table_results = f\"# Experiment Results (MulRan dataset) \\n|Metric|Value|\\n|-:|:-|\\n\"\n", 41 | "for metric, result in metric_mean.items():\n", 42 | " table_results += f\"|{metric}|{result:.2f}|\\n\"\n", 43 | "display_markdown(table_results, raw=True)" 44 | ] 45 | } 46 | ], 47 | "metadata": { 48 | "interpreter": { 49 | "hash": "23ab7127a0bd204976f8870b5ba7b53bf3b662fb0566b25d6a446c21e6a7408b" 50 | }, 51 | "kernelspec": { 52 | "display_name": "Python 3.8.17 ('pin')", 53 | "language": "python", 54 | "name": "python3" 55 | }, 56 | "language_info": { 57 | "codemirror_mode": { 58 | "name": "ipython", 59 | "version": 3 60 | }, 61 | "file_extension": ".py", 62 | "mimetype": "text/x-python", 63 | "name": "python", 64 | "nbconvert_exporter": "python", 65 | "pygments_lexer": "ipython3", 66 | "version": "3.8.19" 67 | }, 68 | "orig_nbformat": 4 69 | }, 70 | "nbformat": 4, 71 | "nbformat_minor": 2 72 | } 73 | -------------------------------------------------------------------------------- /eval/eval_ncd.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# PIN-SLAM running on Newer College dataset\n", 8 | "The aim of this notebook is to serve as a reproducible entry point for the experiments outlined in the paper.\n", 9 | "\n", 10 | "To directly run this notebook without changing path, you need to put it in the parent directory of `eval`.\n", 11 | "\n", 12 | "Please download the dataset from [here](https://drive.google.com/drive/folders/15lTH5osZzZlDpcW7oXfR_2t8TNssNARS). Then you can use this [script](https://github.com/PRBonn/PIN_SLAM/blob/main/scripts/rosbag2ply.py) to extract point cloud frames from the rosbag and use this [script](https://github.com/PRBonn/PIN_SLAM/blob/main/dataset/converter/ncd128_pose_converter.py) to convert the reference pose to the KITTI format. \n", 13 | "\n", 14 | "Note: the results are slightly different using different random seeds and different hardwares.\n", 15 | "\n" 16 | ] 17 | }, 18 | { 19 | "cell_type": "code", 20 | "execution_count": null, 21 | "metadata": {}, 22 | "outputs": [], 23 | "source": [ 24 | "import pin_slam\n", 25 | "from eval.eval_traj_utils import get_metrics, mean_metrics\n", 26 | "import torch\n", 27 | "from IPython.display import display_markdown\n", 28 | "\n", 29 | "print('Device used:', torch.cuda.get_device_name(0))\n", 30 | "\n", 31 | "metrics_dicts = []\n", 32 | "seq_list = ['01', '02']\n", 33 | "for seq_str in seq_list:\n", 34 | " print('Now evaluate sequence '+ seq_str)\n", 35 | " seq_results = pin_slam.run_pin_slam('./config/lidar_slam/run_ncd.yaml', 'ncd', seq_str)\n", 36 | " metrics_dict = get_metrics(seq_results)\n", 37 | " metrics_dicts.append(metrics_dict)\n", 38 | "\n", 39 | "metric_mean = mean_metrics(metrics_dicts)\n", 40 | "table_results = f\"# Experiment Results (Newer College dataset) \\n|Metric|Value|\\n|-:|:-|\\n\"\n", 41 | "for metric, result in metric_mean.items():\n", 42 | " table_results += f\"|{metric}|{result:.2f}|\\n\"\n", 43 | "display_markdown(table_results, raw=True)" 44 | ] 45 | } 46 | ], 47 | "metadata": { 48 | "interpreter": { 49 | "hash": "23ab7127a0bd204976f8870b5ba7b53bf3b662fb0566b25d6a446c21e6a7408b" 50 | }, 51 | "kernelspec": { 52 | "display_name": "Python 3.8.17 ('pin')", 53 | "language": "python", 54 | "name": "python3" 55 | }, 56 | "language_info": { 57 | "codemirror_mode": { 58 | "name": "ipython", 59 | "version": 3 60 | }, 61 | "file_extension": ".py", 62 | "mimetype": "text/x-python", 63 | "name": "python", 64 | "nbconvert_exporter": "python", 65 | "pygments_lexer": "ipython3", 66 | "version": "3.8.19" 67 | }, 68 | "orig_nbformat": 4 69 | }, 70 | "nbformat": 4, 71 | "nbformat_minor": 2 72 | } 73 | -------------------------------------------------------------------------------- /eval/eval_ncd_128.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# PIN-SLAM running on Newer College dataset 2021 extension (NCD-128)\n", 8 | "The aim of this notebook is to serve as a reproducible entry point for the experiments outlined in the paper.\n", 9 | "\n", 10 | "To directly run this notebook without changing path, you need to put it in the parent directory of `eval`.\n", 11 | "\n", 12 | "Please download the dataset from [here](https://drive.google.com/drive/folders/15lTH5osZzZlDpcW7oXfR_2t8TNssNARS). Then you can use this [script](https://github.com/PRBonn/PIN_SLAM/blob/main/scripts/rosbag2ply.py) to extract point cloud frames from the rosbag and use this [script](https://github.com/PRBonn/PIN_SLAM/blob/main/dataset/converter/ncd128_pose_converter.py) to convert the reference pose to the KITTI format. \n", 13 | "\n", 14 | "Note: the results are slightly different using different random seeds and different hardwares.\n", 15 | "\n" 16 | ] 17 | }, 18 | { 19 | "cell_type": "code", 20 | "execution_count": null, 21 | "metadata": {}, 22 | "outputs": [], 23 | "source": [ 24 | "import os\n", 25 | "import torch\n", 26 | "from IPython.display import display_markdown\n", 27 | "import pin_slam\n", 28 | "from eval.eval_traj_utils import get_metrics, mean_metrics\n", 29 | "\n", 30 | "print('Device used:', torch.cuda.get_device_name(0))\n", 31 | "\n", 32 | "metrics_dicts = []\n", 33 | "seq_list = ['quad_e', 'math_e', 'underground_e', 'cloister', 'stairs']\n", 34 | "config_list = ['run_ncd_128', 'run_ncd_128', 'run_ncd_128', 'run_ncd_128_m', 'run_ncd_128_s']\n", 35 | "for (seq_str, config_str) in zip(seq_list, config_list):\n", 36 | " print('Now evaluate sequence '+ seq_str)\n", 37 | " seq_results = pin_slam.run_pin_slam(os.path.join('./config/lidar_slam', config_str + '.yaml'), 'ncd128', seq_str)\n", 38 | " metrics_dict = get_metrics(seq_results)\n", 39 | " metrics_dicts.append(metrics_dict)\n", 40 | "\n", 41 | "metric_mean = mean_metrics(metrics_dicts)\n", 42 | "table_results = f\"# Experiment Results (Newer College 128 dataset) \\n|Metric|Value|\\n|-:|:-|\\n\"\n", 43 | "for metric, result in metric_mean.items():\n", 44 | " table_results += f\"|{metric}|{result:.2f}|\\n\"\n", 45 | "display_markdown(table_results, raw=True)" 46 | ] 47 | } 48 | ], 49 | "metadata": { 50 | "interpreter": { 51 | "hash": "23ab7127a0bd204976f8870b5ba7b53bf3b662fb0566b25d6a446c21e6a7408b" 52 | }, 53 | "kernelspec": { 54 | "display_name": "Python 3.8.17 ('pin')", 55 | "language": "python", 56 | "name": "python3" 57 | }, 58 | "language_info": { 59 | "codemirror_mode": { 60 | "name": "ipython", 61 | "version": 3 62 | }, 63 | "file_extension": ".py", 64 | "mimetype": "text/x-python", 65 | "name": "python", 66 | "nbconvert_exporter": "python", 67 | "pygments_lexer": "ipython3", 68 | "version": "3.8.19" 69 | }, 70 | "orig_nbformat": 4 71 | }, 72 | "nbformat": 4, 73 | "nbformat_minor": 2 74 | } 75 | -------------------------------------------------------------------------------- /eval/eval_replica.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# PIN-SLAM running on the Replica RGB-D dataset\n", 8 | "The aim of this notebook is to serve as a reproducible entry point for the experiments outlined in the paper.\n", 9 | "\n", 10 | "To directly run this notebook without changing path, you need to put it in the parent directory of `eval`.\n", 11 | "\n", 12 | "Please download the dataset by this [script](https://github.com/PRBonn/PIN_SLAM/blob/main/scripts/download_replica.sh) and use this [script](https://github.com/PRBonn/PIN_SLAM/blob/main/scripts/convert_replica.sh) to convert to the desired format for PIN-SLAM. \n", 13 | "\n", 14 | "Note: the results are slightly different using different random seeds and different hardwares.\n", 15 | "\n" 16 | ] 17 | }, 18 | { 19 | "cell_type": "code", 20 | "execution_count": null, 21 | "metadata": {}, 22 | "outputs": [], 23 | "source": [ 24 | "import pin_slam\n", 25 | "import torch\n", 26 | "from IPython.display import display_markdown\n", 27 | "\n", 28 | "print('Device used:', torch.cuda.get_device_name(0))\n", 29 | "\n", 30 | "rmse_list = []\n", 31 | "seq_list = ['room0', 'room1', 'room2', 'office0', 'office1', 'office2', 'office3', 'office4']\n", 32 | "for seq_str in seq_list:\n", 33 | " print('Now evaluate sequence '+ seq_str)\n", 34 | " seq_results = pin_slam.run_pin_slam('./config/rgbd_slam/run_replica.yaml', 'replica', seq_str)\n", 35 | " rmse_list.append(seq_results[0]['Absoulte Trajectory Error [m]'])\n", 36 | "\n", 37 | "rmse_mean_cm = sum(rmse_list)/len(rmse_list)*100.0\n", 38 | "table_results = f\"# Experiment Results (Replica dataset) \\n|Metric|Value|\\n|-:|:-|\\n\"\n", 39 | "rmse_key = 'Absoulte Trajectory Error [cm]'\n", 40 | "table_results += f\"|{rmse_key}|{rmse_mean_cm:.2f}|\\n\"\n", 41 | "display_markdown(table_results, raw=True)\n" 42 | ] 43 | } 44 | ], 45 | "metadata": { 46 | "interpreter": { 47 | "hash": "23ab7127a0bd204976f8870b5ba7b53bf3b662fb0566b25d6a446c21e6a7408b" 48 | }, 49 | "kernelspec": { 50 | "display_name": "Python 3.8.17 ('pin')", 51 | "language": "python", 52 | "name": "python3" 53 | }, 54 | "language_info": { 55 | "codemirror_mode": { 56 | "name": "ipython", 57 | "version": 3 58 | }, 59 | "file_extension": ".py", 60 | "mimetype": "text/x-python", 61 | "name": "python", 62 | "nbconvert_exporter": "python", 63 | "pygments_lexer": "ipython3", 64 | "version": "3.8.19" 65 | }, 66 | "orig_nbformat": 4 67 | }, 68 | "nbformat": 4, 69 | "nbformat_minor": 2 70 | } 71 | -------------------------------------------------------------------------------- /gui/gui_utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # @file gui_utils.py 3 | # @author Yue Pan [yue.pan@igg.uni-bonn.de] 4 | 5 | # This GUI is built on top of the great work of MonoGS (https://github.com/muskie82/MonoGS/blob/main/gui/gui_utils.py) 6 | 7 | import queue 8 | 9 | from utils.tools import feature_pca_torch 10 | from model.neural_points import NeuralPoints 11 | 12 | 13 | class VisPacket: 14 | def __init__( 15 | self, 16 | frame_id = None, 17 | finish=False, 18 | current_pointcloud_xyz=None, 19 | current_pointcloud_rgb=None, 20 | mesh_verts=None, 21 | mesh_faces=None, 22 | mesh_verts_rgb=None, 23 | odom_poses=None, 24 | gt_poses=None, 25 | slam_poses=None, 26 | travel_dist=None, 27 | gpu_mem_usage_gb=None, 28 | cur_fps=None, 29 | slam_finished=False, 30 | ): 31 | self.has_neural_points = False 32 | 33 | self.neural_points_data = None 34 | 35 | self.frame_id = frame_id 36 | 37 | self.add_scan(current_pointcloud_xyz, current_pointcloud_rgb) 38 | 39 | self.add_mesh(mesh_verts, mesh_faces, mesh_verts_rgb) 40 | 41 | self.add_traj(odom_poses, gt_poses, slam_poses) 42 | 43 | self.sdf_slice_xyz = None 44 | self.sdf_slice_rgb = None 45 | 46 | self.sdf_pool_xyz = None 47 | self.sdf_pool_rgb = None 48 | 49 | self.travel_dist = travel_dist 50 | self.gpu_mem_usage_gb = gpu_mem_usage_gb 51 | self.cur_fps = cur_fps 52 | self.slam_finished = slam_finished 53 | 54 | self.finish = finish 55 | 56 | # the sorrounding map is also added here 57 | def add_neural_points_data(self, neural_points: NeuralPoints, only_local_map: bool = True, 58 | pca_color_on: bool = True): 59 | 60 | if neural_points is not None: 61 | self.has_neural_points = True 62 | self.neural_points_data = {} 63 | self.neural_points_data["count"] = neural_points.count() 64 | self.neural_points_data["local_count"] = neural_points.local_count() 65 | self.neural_points_data["map_memory_mb"] = neural_points.cur_memory_mb 66 | self.neural_points_data["resolution"] = neural_points.resolution 67 | 68 | if only_local_map: 69 | self.neural_points_data["position"] = neural_points.local_neural_points 70 | self.neural_points_data["orientation"] = neural_points.local_point_orientations 71 | self.neural_points_data["geo_feature"] = neural_points.local_geo_features.detach() 72 | if neural_points.color_on: 73 | self.neural_points_data["color_feature"] = neural_points.local_color_features.detach() 74 | self.neural_points_data["ts"] = neural_points.local_point_ts_update 75 | self.neural_points_data["stability"] = neural_points.local_point_certainties 76 | 77 | if pca_color_on: 78 | local_geo_feature_3d, _ = feature_pca_torch((self.neural_points_data["geo_feature"])[:-1], principal_components=neural_points.geo_feature_pca, down_rate=17) 79 | self.neural_points_data["color_pca_geo"] = local_geo_feature_3d 80 | 81 | if neural_points.color_on: 82 | local_color_feature_3d, _ = feature_pca_torch((self.neural_points_data["color_feature"])[:-1], principal_components=neural_points.color_feature_pca, down_rate=17) 83 | self.neural_points_data["color_pca_color"] = local_color_feature_3d 84 | 85 | else: 86 | self.neural_points_data["position"] = neural_points.neural_points 87 | self.neural_points_data["orientation"] = neural_points.point_orientations 88 | self.neural_points_data["geo_feature"] = neural_points.geo_features 89 | if neural_points.color_on: 90 | self.neural_points_data["color_feature"] = neural_points.color_features 91 | self.neural_points_data["ts"] = neural_points.point_ts_update 92 | self.neural_points_data["stability"] = neural_points.point_certainties 93 | if neural_points.local_mask is not None: 94 | self.neural_points_data["local_mask"] = neural_points.local_mask[:-1] 95 | 96 | if pca_color_on: 97 | geo_feature_3d, _ = feature_pca_torch(neural_points.geo_features[:-1], principal_components=neural_points.geo_feature_pca, down_rate=97) 98 | self.neural_points_data["color_pca_geo"] = geo_feature_3d 99 | 100 | if neural_points.color_on: 101 | color_feature_3d, _ = feature_pca_torch(neural_points.color_features[:-1], principal_components=neural_points.color_feature_pca, down_rate=97) 102 | self.neural_points_data["color_pca_color"] = color_feature_3d 103 | 104 | 105 | def add_scan(self, current_pointcloud_xyz=None, current_pointcloud_rgb=None): 106 | self.current_pointcloud_xyz = current_pointcloud_xyz 107 | self.current_pointcloud_rgb = current_pointcloud_rgb 108 | 109 | # TODO: add normal later 110 | 111 | def add_sdf_slice(self, sdf_slice_xyz=None, sdf_slice_rgb=None): 112 | self.sdf_slice_xyz = sdf_slice_xyz 113 | self.sdf_slice_rgb = sdf_slice_rgb 114 | 115 | def add_sdf_training_pool(self, sdf_pool_xyz=None, sdf_pool_rgb=None): 116 | self.sdf_pool_xyz = sdf_pool_xyz 117 | self.sdf_pool_rgb = sdf_pool_rgb 118 | 119 | def add_mesh(self, mesh_verts=None, mesh_faces=None, mesh_verts_rgb=None): 120 | self.mesh_verts = mesh_verts 121 | self.mesh_faces = mesh_faces 122 | self.mesh_verts_rgb = mesh_verts_rgb 123 | 124 | def add_traj(self, odom_poses=None, gt_poses=None, slam_poses=None, loop_edges=None): 125 | 126 | self.odom_poses = odom_poses 127 | self.gt_poses = gt_poses 128 | self.slam_poses = slam_poses 129 | 130 | if slam_poses is None: 131 | self.slam_poses = odom_poses 132 | 133 | self.loop_edges = loop_edges 134 | 135 | 136 | def get_latest_queue(q): 137 | message = None 138 | while True: 139 | try: 140 | message_latest = q.get_nowait() 141 | if message is not None: 142 | del message 143 | message = message_latest 144 | except queue.Empty: 145 | if q.empty(): 146 | break 147 | return message 148 | 149 | 150 | class ControlPacket: 151 | flag_pause = False 152 | flag_vis = True 153 | flag_mesh = False 154 | flag_sdf = False 155 | flag_global = False 156 | flag_source = False 157 | mc_res_m = 0.2 158 | mesh_min_nn = 10 159 | mesh_freq_frame = 50 160 | sdf_freq_frame = 50 161 | sdf_slice_height = 0.2 162 | sdf_res_m = 0.2 163 | cur_frame_id = 0 164 | 165 | class ParamsGUI: 166 | def __init__( 167 | self, 168 | q_main2vis=None, 169 | q_vis2main=None, 170 | config=None, 171 | local_map_default_on: bool = True, 172 | robot_default_on: bool = True, 173 | mesh_default_on: bool = False, 174 | sdf_default_on: bool = False, 175 | neural_point_map_default_on: bool = False, 176 | neural_point_color_default_mode: int = 1, # 1: geo feature pca, 2: photo feature pca, 3: time, 4: height 177 | neural_point_vis_down_rate: int = 1, 178 | ): 179 | self.q_main2vis = q_main2vis 180 | self.q_vis2main = q_vis2main 181 | self.config = config 182 | 183 | self.robot_default_on = robot_default_on 184 | self.neural_point_map_default_on = neural_point_map_default_on 185 | self.mesh_default_on = mesh_default_on 186 | self.sdf_default_on = sdf_default_on 187 | self.local_map_default_on = local_map_default_on 188 | self.neural_point_color_default_mode = neural_point_color_default_mode 189 | self.neural_point_vis_down_rate = neural_point_vis_down_rate 190 | 191 | -------------------------------------------------------------------------------- /model/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/PIN_SLAM/2a2c6f7dc2b3b01efb283d430165cb80d26f835b/model/__init__.py -------------------------------------------------------------------------------- /model/decoder.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # @file decoder.py 3 | # @author Yue Pan [yue.pan@igg.uni-bonn.de] 4 | # Copyright (c) 2024 Yue Pan, all rights reserved 5 | 6 | 7 | import torch 8 | import torch.nn as nn 9 | import torch.nn.functional as F 10 | 11 | from utils.config import Config 12 | 13 | 14 | class Decoder(nn.Module): 15 | def __init__( 16 | self, 17 | config: Config, 18 | hidden_dim, 19 | hidden_level, 20 | out_dim, 21 | is_time_conditioned=False, 22 | ): 23 | 24 | super().__init__() 25 | 26 | self.out_dim = out_dim 27 | self.use_leaky_relu = config.mlp_leaky_relu 28 | bias_on = config.mlp_bias_on 29 | 30 | # default not used 31 | if config.use_gaussian_pe: 32 | position_dim = config.pos_input_dim + 2 * config.pos_encoding_band 33 | else: 34 | position_dim = config.pos_input_dim * (2 * config.pos_encoding_band + 1) 35 | 36 | feature_dim = config.feature_dim 37 | input_dim = feature_dim + position_dim 38 | 39 | # default not used 40 | if is_time_conditioned: 41 | input_layer_count += 1 42 | 43 | # predict sdf (now it anyway only predict sdf without further sigmoid 44 | # Initializa the structure of shared MLP 45 | layers = [] 46 | for i in range(hidden_level): 47 | if i == 0: 48 | layers.append(nn.Linear(input_dim, hidden_dim, bias_on)) 49 | else: 50 | layers.append(nn.Linear(hidden_dim, hidden_dim, bias_on)) 51 | self.layers = nn.ModuleList(layers) 52 | self.lout = nn.Linear(hidden_dim, out_dim, bias_on) 53 | 54 | self.sdf_scale = 1.0 55 | if config.main_loss_type == "bce": 56 | self.sdf_scale = config.logistic_gaussian_ratio * config.sigma_sigmoid_m 57 | 58 | self.to(config.device) 59 | # torch.cuda.empty_cache() 60 | 61 | def mlp(self, features): 62 | # linear (feature_dim -> hidden_dim) 63 | # relu 64 | # linear (hidden_dim -> hidden_dim) 65 | # relu 66 | # linear (hidden_dim -> 1) 67 | for k, l in enumerate(self.layers): 68 | if k == 0: 69 | if self.use_leaky_relu: 70 | h = F.leaky_relu(l(features)) 71 | else: 72 | h = F.relu(l(features)) 73 | else: 74 | if self.use_leaky_relu: 75 | h = F.leaky_relu(l(h)) 76 | else: 77 | h = F.relu(l(h)) 78 | out = self.lout(h) 79 | return out 80 | 81 | # predict the sdf (opposite sign to the actual sdf) 82 | # unit is already m 83 | def sdf(self, features): 84 | out = self.mlp(features).squeeze(1) * self.sdf_scale 85 | return out 86 | 87 | def time_conditionded_sdf(self, features, ts): 88 | nn_k = features.shape[1] 89 | ts_nn_k = ts.repeat(nn_k).view(-1, nn_k, 1) 90 | time_conditioned_feature = torch.cat((features, ts_nn_k), dim=-1) 91 | out = self.sdf(time_conditioned_feature) 92 | return out 93 | 94 | # predict the occupancy probability 95 | def occupancy(self, features): 96 | out = torch.sigmoid(self.sdf(features) / -self.sdf_scale) # to [0, 1] 97 | return out 98 | 99 | # predict the probabilty of each semantic label 100 | def sem_label_prob(self, features): 101 | out = F.log_softmax(self.mlp(features), dim=-1) 102 | return out 103 | 104 | def sem_label(self, features): 105 | out = torch.argmax(self.sem_label_prob(features), dim=1) 106 | return out 107 | 108 | # def regress_color(self, features): 109 | # out = torch.clamp(self.mlp(features), 0.0, 1.0) 110 | # return out 111 | 112 | def regress_color(self, features): 113 | out = torch.sigmoid(self.mlp(features)) # sigmoid map to [0,1] 114 | return out -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.26.4 2 | evo==1.28.0 3 | gnupg==2.3.1 4 | gtsam==4.2 5 | laspy==2.5.3 6 | natsort==8.1.0 7 | open3d==0.19.0 8 | pycryptodomex==3.20.0 9 | pypose==0.6.8 10 | pyquaternion==0.9.9 11 | rich==12.5.1 12 | roma==1.5.0 13 | rospkg==1.5.1 14 | scikit-image==0.25.2 15 | dtyper==2.5.1 16 | wandb==0.19.0 17 | opencv-python -------------------------------------------------------------------------------- /scripts/convert_neuralrgbd.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | root_path=./data/neural_rgbd_data 4 | 5 | sequence_name=icl_living_room 6 | #sequence_name=breakfast_room 7 | #sequence_name=staircase 8 | #sequence_name=green_room 9 | #sequence_name=complete_kitchen 10 | #sequence_name=whiteroom 11 | #sequence_name=grey_white_room 12 | #sequence_name=morning_apartment 13 | #sequence_name=kitchen 14 | #sequence_name=thin_geometry 15 | 16 | base_path=${root_path}/${sequence_name} 17 | 18 | command="python3 ./dataset/converter/neuralrgbd_to_pin_format.py 19 | --input_root ${base_path} 20 | --output_root ${base_path} 21 | --intrinsic_file ${base_path}/focal.txt 22 | --vis_on False 23 | --down_sample False" 24 | 25 | echo "Convert Neural RGBD dataset to our format" 26 | eval $command 27 | echo "Done." -------------------------------------------------------------------------------- /scripts/convert_replica.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | root_path=./data/Replica 4 | 5 | sequence_name=room0 6 | 7 | base_path=${root_path}/${sequence_name} 8 | 9 | command="python3 ./dataset/converter/replica_to_pin_format.py 10 | --input_root ${base_path} 11 | --output_root ${base_path} 12 | --intrinsic_file ${root_path}/cam_params.json 13 | --vis_on False 14 | --down_sample True" 15 | 16 | echo "Convert Replica RGBD dataset to our format" 17 | eval $command 18 | echo "Done." -------------------------------------------------------------------------------- /scripts/download_kitti_example.sh: -------------------------------------------------------------------------------- 1 | echo Creating the dataset path... 2 | 3 | mkdir -p data 4 | cd data 5 | 6 | echo Downloading KITTI odometry dataset, sequence 00 subset, first 100 frames ... 7 | wget -O kitti_example.tar.gz -c https://uni-bonn.sciebo.de/s/Ycl28f1Cppghvjm/download 8 | 9 | echo Extracting dataset... 10 | tar -xvf kitti_example.tar.gz 11 | 12 | rm kitti_example.tar.gz 13 | 14 | cd ../.. -------------------------------------------------------------------------------- /scripts/download_replica.sh: -------------------------------------------------------------------------------- 1 | echo Creating the dataset path... 2 | 3 | mkdir -p data 4 | cd data 5 | 6 | # you can also download the Replica.zip manually through 7 | # provided by iMap and Nice-SLAM 8 | echo Downloading Replica dataset... 9 | wget https://cvg-data.inf.ethz.ch/nice-slam/data/Replica.zip 10 | 11 | echo Extracting dataset... 12 | unzip Replica.zip 13 | 14 | rm Replica.zip 15 | 16 | cd ../.. 17 | -------------------------------------------------------------------------------- /scripts/module/ply.py: -------------------------------------------------------------------------------- 1 | # Copyright: jedeschaud 2 | # Basic libs 3 | import numpy as np 4 | import sys 5 | 6 | # Define PLY types 7 | ply_dtypes = dict([ 8 | (b'char', 'i1'), 9 | (b'int8', 'i1'), 10 | (b'uchar', 'b1'), 11 | (b'uchar', 'u1'), 12 | (b'uint8', 'u1'), 13 | (b'short', 'i2'), 14 | (b'int16', 'i2'), 15 | (b'ushort', 'u2'), 16 | (b'uint16', 'u2'), 17 | (b'int', 'i4'), 18 | (b'int32', 'i4'), 19 | (b'uint', 'u4'), 20 | (b'uint32', 'u4'), 21 | (b'float', 'f4'), 22 | (b'float32', 'f4'), 23 | (b'double', 'f8'), 24 | (b'float64', 'f8') 25 | ]) 26 | 27 | 28 | # Numpy reader format 29 | valid_formats = {'ascii': '', 'binary_big_endian': '>', 'binary_little_endian': '<'} 30 | 31 | 32 | def parse_header(plyfile, ext): 33 | # Variables 34 | line = [] 35 | properties = [] 36 | num_points = None 37 | 38 | while b'end_header' not in line and line != b'': 39 | line = plyfile.readline() 40 | 41 | if b'element' in line: 42 | line = line.split() 43 | num_points = int(line[2]) 44 | 45 | elif b'property' in line: 46 | line = line.split() 47 | properties.append((line[2].decode(), ext + ply_dtypes[line[1]])) 48 | 49 | return num_points, properties 50 | 51 | 52 | def read_ply(filename): 53 | """ 54 | Read ".ply" files 55 | 56 | Parameters 57 | ---------- 58 | filename : string 59 | the name of the file to read. 60 | 61 | Returns 62 | ------- 63 | result : array 64 | data stored in the file 65 | 66 | Examples 67 | -------- 68 | Store data in file 69 | 70 | >>> points = np.random.rand(5, 3) 71 | >>> values = np.random.randint(2, size=10) 72 | >>> write_ply('example.ply', [points, values], ['x', 'y', 'z', 'values']) 73 | 74 | Read the file 75 | 76 | >>> data = read_ply('example.ply') 77 | >>> values = data['values'] 78 | array([0, 0, 1, 1, 0]) 79 | 80 | >>> points = np.vstack((data['x'], data['y'], data['z'])).T 81 | array([[ 0.466 0.595 0.324] 82 | [ 0.538 0.407 0.654] 83 | [ 0.850 0.018 0.988] 84 | [ 0.395 0.394 0.363] 85 | [ 0.873 0.996 0.092]]) 86 | 87 | """ 88 | 89 | with open(filename, 'rb') as plyfile: 90 | # Check if the file start with ply 91 | if b'ply' not in plyfile.readline(): 92 | raise ValueError('The file does not start whith the word ply') 93 | 94 | # get binary_little/big or ascii 95 | fmt = plyfile.readline().split()[1].decode() 96 | if fmt == "ascii": 97 | raise ValueError('The file is not binary') 98 | 99 | # get extension for building the numpy dtypes 100 | ext = valid_formats[fmt] 101 | 102 | # Parse header 103 | num_points, properties = parse_header(plyfile, ext) 104 | 105 | # Get data 106 | data = np.fromfile(plyfile, dtype=properties, count=num_points) 107 | 108 | return data 109 | 110 | 111 | def header_properties(field_list, field_names): 112 | # List of lines to write 113 | lines = [] 114 | 115 | # First line describing element vertex 116 | lines.append('element vertex %d' % field_list[0].shape[0]) 117 | 118 | # Properties lines 119 | i = 0 120 | for fields in field_list: 121 | for field in fields.T: 122 | lines.append('property %s %s' % (field.dtype.name, field_names[i])) 123 | i += 1 124 | 125 | return lines 126 | 127 | 128 | def write_ply(filename, field_list, field_names): 129 | """ 130 | Write ".ply" files 131 | 132 | Parameters 133 | ---------- 134 | filename : string 135 | the name of the file to which the data is saved. A '.ply' extension will be appended to the 136 | file name if it does no already have one. 137 | 138 | field_list : list, tuple, numpy array 139 | the fields to be saved in the ply file. Either a numpy array, a list of numpy arrays or a 140 | tuple of numpy arrays. Each 1D numpy array and each column of 2D numpy arrays are considered 141 | as one field. 142 | 143 | field_names : list 144 | the name of each fields as a list of strings. Has to be the same length as the number of 145 | fields. 146 | 147 | Examples 148 | -------- 149 | >>> points = np.random.rand(10, 3) 150 | >>> write_ply('example1.ply', points, ['x', 'y', 'z']) 151 | 152 | >>> values = np.random.randint(2, size=10) 153 | >>> write_ply('example2.ply', [points, values], ['x', 'y', 'z', 'values']) 154 | 155 | >>> colors = np.random.randint(255, size=(10,3), dtype=np.uint8) 156 | >>> field_names = ['x', 'y', 'z', 'red', 'green', 'blue', values'] 157 | >>> write_ply('example3.ply', [points, colors, values], field_names) 158 | 159 | """ 160 | 161 | # Format list input to the right form 162 | field_list = list(field_list) if (type(field_list) == list or type(field_list) == tuple) else list((field_list,)) 163 | for i, field in enumerate(field_list): 164 | if field is None: 165 | print('WRITE_PLY ERROR: a field is None') 166 | return False 167 | elif field.ndim > 2: 168 | print('WRITE_PLY ERROR: a field have more than 2 dimensions') 169 | return False 170 | elif field.ndim < 2: 171 | field_list[i] = field.reshape(-1, 1) 172 | 173 | # check all fields have the same number of data 174 | n_points = [field.shape[0] for field in field_list] 175 | if not np.all(np.equal(n_points, n_points[0])): 176 | print('wrong field dimensions') 177 | return False 178 | 179 | # Check if field_names and field_list have same nb of column 180 | n_fields = np.sum([field.shape[1] for field in field_list]) 181 | if (n_fields != len(field_names)): 182 | print(n_fields, 'fields instead of', len(field_names)) 183 | return False 184 | 185 | # Add extension if not there 186 | if not filename.endswith('.ply'): 187 | filename += '.ply' 188 | 189 | # open in text mode to write the header 190 | with open(filename, 'w') as plyfile: 191 | 192 | # First magical word 193 | header = ['ply'] 194 | 195 | # Encoding format 196 | header.append('format binary_' + sys.byteorder + '_endian 1.0') 197 | 198 | # Points properties description 199 | header.extend(header_properties(field_list, field_names)) 200 | 201 | # End of header 202 | header.append('end_header') 203 | 204 | # Write all lines 205 | for line in header: 206 | plyfile.write("%s\n" % line) 207 | 208 | 209 | # open in binary/append to use tofile 210 | with open(filename, 'ab') as plyfile: 211 | 212 | # Create a structured array 213 | i = 0 214 | type_list = [] 215 | 216 | for fields in field_list: 217 | for field in fields.T: 218 | type_list += [(field_names[i], field.dtype.str)] 219 | i += 1 220 | data = np.empty(field_list[0].shape[0], dtype=type_list) 221 | i = 0 222 | for fields in field_list: 223 | for field in fields.T: 224 | data[field_names[i]] = field 225 | i += 1 226 | 227 | data.tofile(plyfile) 228 | 229 | return True -------------------------------------------------------------------------------- /scripts/rosbag2ply.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import sensor_msgs.point_cloud2 as pc2 3 | 4 | import os 5 | import argparse 6 | import numpy as np 7 | import open3d as o3d 8 | 9 | from module import ply 10 | 11 | def rosbag2ply(args): 12 | 13 | os.makedirs(args.output_folder, 0o755, exist_ok=True) 14 | 15 | if args.output_pcd: 16 | output_folder_pcd = args.output_folder+"_pcd" 17 | os.makedirs(output_folder_pcd, 0o755, exist_ok=True) 18 | 19 | 20 | begin_flag = False 21 | 22 | print('Start extraction') 23 | in_bag = rosbag.Bag(args.input_bag) 24 | for topic, msg, t in in_bag.read_messages(): 25 | 26 | if not begin_flag: 27 | print(topic) 28 | 29 | if topic == args.topic: 30 | gen = pc2.read_points(msg, skip_nans=True) 31 | data = list(gen) 32 | array = np.array(data) 33 | 34 | # NOTE: point cloud array: x,y,z,intensity,timestamp,ring,others... 35 | # could be different for some other rosbags 36 | # print(array[:, :6]) 37 | 38 | timestamps = array[:, 4] # for hilti, vbr, and others 39 | # timestamps = array[:, 5] # for m2dgrmm 40 | # print(timestamps) 41 | 42 | if not begin_flag: 43 | shift_timestamp = timestamps[0] 44 | begin_flag = True 45 | 46 | timestamps_shifted = timestamps - shift_timestamp 47 | # print(timestamps_shifted) 48 | 49 | field_names = ['x','y','z','intensity','timestamp'] 50 | ply_file_path = os.path.join(args.output_folder, str(t)+".ply") 51 | 52 | if ply.write_ply(ply_file_path, [array[:,:4], timestamps_shifted], field_names): 53 | print("Export : "+ply_file_path) 54 | else: 55 | print('ply.write_ply() failed') 56 | 57 | if args.output_pcd: 58 | pcd = o3d.geometry.PointCloud() 59 | pcd.points = o3d.utility.Vector3dVector(array[:, :3]) 60 | pcd_file_path = os.path.join(output_folder_pcd, str(t)+".pcd") 61 | o3d.io.write_point_cloud(pcd_file_path, pcd) 62 | 63 | 64 | if __name__ == "__main__": 65 | 66 | parser = argparse.ArgumentParser() 67 | parser.add_argument('-i','--input_bag', help="path to the input rosbag") 68 | parser.add_argument('-o','--output_folder', help="path for output foler") 69 | parser.add_argument('-t','--topic', help="name of the point cloud topic used in the rosbag", default="/hesai/pandar_points") 70 | parser.add_argument('-p','--output_pcd', action='store_true', help='Also output the pcd file') 71 | args = parser.parse_args() 72 | print("usage: python3 rosbag2ply.py -i [path to input rosbag] -o [path to point cloud output folder] -t [name of point cloud rostopic]") 73 | 74 | rosbag2ply(args) 75 | -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/PIN_SLAM/2a2c6f7dc2b3b01efb283d430165cb80d26f835b/utils/__init__.py -------------------------------------------------------------------------------- /utils/loss.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # @file loss.py 3 | # @author Yue Pan [yue.pan@igg.uni-bonn.de] 4 | # Copyright (c) 2024 Yue Pan, all rights reserved 5 | 6 | import torch 7 | import torch.nn as nn 8 | 9 | 10 | def sdf_diff_loss(pred, label, weight, scale=1.0, l2_loss=True): 11 | count = pred.shape[0] 12 | diff = pred - label 13 | diff_m = diff / scale # so it's still in m unit 14 | if l2_loss: 15 | loss = (weight * (diff_m**2)).sum() / count # l2 loss 16 | else: 17 | loss = (weight * torch.abs(diff_m)).sum() / count # l1 loss 18 | return loss 19 | 20 | 21 | def sdf_l1_loss(pred, label): 22 | loss = torch.abs(pred - label) 23 | return loss.mean() 24 | 25 | 26 | def sdf_l2_loss(pred, label): 27 | loss = (pred - label) ** 2 28 | return loss.mean() 29 | 30 | 31 | def color_diff_loss(pred, label, weight, weighted=False, l2_loss=False): 32 | diff = pred - label 33 | if not weighted: 34 | weight = 1.0 35 | else: 36 | weight = weight.unsqueeze(1) 37 | if l2_loss: 38 | loss = (weight * (diff**2)).mean() 39 | else: 40 | loss = (weight * torch.abs(diff)).mean() 41 | return loss 42 | 43 | 44 | # used by our approach 45 | def sdf_bce_loss(pred, label, sigma, weight, weighted=False, bce_reduction="mean"): 46 | """ Calculate the binary cross entropy (BCE) loss for SDF supervision 47 | Args: 48 | pred (torch.tenosr): batch of predicted SDF values 49 | label (torch.tensor): batch of the target SDF values 50 | sigma (float): scale factor for the sigmoid function 51 | weight (torch.tenosr): batch of the per-sample weight 52 | weighted (bool, optional): apply the weight or not 53 | bce_reduction (string, optional): specifies the reduction to apply to the output 54 | Returns: 55 | loss (torch.tensor): BCE loss for the batch 56 | """ 57 | if weighted: 58 | loss_bce = nn.BCEWithLogitsLoss(reduction=bce_reduction, weight=weight) 59 | else: 60 | loss_bce = nn.BCEWithLogitsLoss(reduction=bce_reduction) 61 | label_op = torch.sigmoid(label / sigma) # occupancy prob 62 | loss = loss_bce(pred / sigma, label_op) 63 | return loss 64 | 65 | 66 | # the loss divised by Starry Zhong 67 | def sdf_zhong_loss(pred, label, trunc_dist=None, weight=None, weighted=False): 68 | if not weighted: 69 | weight = 1.0 70 | else: 71 | weight = weight 72 | loss = torch.zeros_like(label, dtype=label.dtype, device=label.device) 73 | middle_point = label / 2.0 74 | middle_point_abs = torch.abs(middle_point) 75 | shift_difference_abs = torch.abs(pred - middle_point) 76 | mask = shift_difference_abs > middle_point_abs 77 | loss[mask] = (shift_difference_abs - middle_point_abs)[ 78 | mask 79 | ] # not masked region simply has a loss of zero, masked region L1 loss 80 | if trunc_dist is not None: 81 | surface_mask = torch.abs(label) < trunc_dist 82 | loss[surface_mask] = torch.abs(pred - label)[surface_mask] 83 | loss *= weight 84 | return loss.mean() 85 | 86 | 87 | # not used 88 | def smooth_sdf_loss(pred, label, delta=20.0, weight=None, weighted=False): 89 | if not weighted: 90 | weight = 1.0 91 | else: 92 | weight = weight 93 | sign_factors = torch.ones_like(label, dtype=label.dtype, device=label.device) 94 | sign_factors[label < 0.0] = -1.0 95 | sign_loss = -sign_factors * delta * pred / 2.0 96 | no_loss = torch.zeros_like(pred, dtype=pred.dtype, device=pred.device) 97 | truncated_loss = sign_factors * delta * (pred / 2.0 - label) 98 | losses = torch.stack((sign_loss, no_loss, truncated_loss), dim=0) 99 | final_loss = torch.logsumexp(losses, dim=0) 100 | final_loss = ((2.0 / delta) * final_loss * weight).mean() 101 | return final_loss 102 | 103 | # deprecated 104 | def ray_estimation_loss(x, y, d_meas): # for each ray 105 | # x as depth 106 | # y as sdf prediction 107 | # d_meas as measured depth 108 | 109 | # regard each sample as a ray 110 | mat_A = torch.vstack((x, torch.ones_like(x))).transpose(0, 1) 111 | vec_b = y.view(-1, 1) 112 | least_square_estimate = torch.linalg.lstsq(mat_A, vec_b).solution 113 | 114 | a = least_square_estimate[0] # -> -1 (added in ekional loss term) 115 | b = least_square_estimate[1] 116 | 117 | d_estimate = torch.clamp(-b / a, min=1.0, max=40.0) # -> d 118 | 119 | d_error = torch.abs(d_estimate - d_meas) 120 | 121 | return d_error 122 | 123 | # deprecated 124 | def ray_rendering_loss(x, y, d_meas): # for each ray [should run in batch] 125 | # x as depth 126 | # y as occ.prob. prediction 127 | x = x.squeeze(1) 128 | sort_x, indices = torch.sort(x) 129 | sort_y = y[indices] 130 | 131 | w = torch.ones_like(y) 132 | for i in range(sort_x.shape[0]): 133 | w[i] = sort_y[i] 134 | for j in range(i): 135 | w[i] *= 1.0 - sort_y[j] 136 | 137 | d_render = (w * x).sum() 138 | 139 | d_error = torch.abs(d_render - d_meas) 140 | 141 | return d_error 142 | 143 | # deprecated 144 | def batch_ray_rendering_loss(x, y, d_meas, neus_on=True): # for all rays in a batch 145 | # x as depth [ray number * sample number] 146 | # y as prediction (the alpha in volume rendering) [ray number * sample number] 147 | # d_meas as measured depth [ray number] 148 | # w as the raywise weight [ray number] 149 | # neus_on determine if using the loss defined in NEUS 150 | 151 | sort_x, indices = torch.sort(x, 1) # for each row 152 | sort_y = torch.gather(y, 1, indices) # for each row 153 | 154 | if neus_on: 155 | neus_alpha = (sort_y[:, 1:] - sort_y[:, 0:-1]) / (1.0 - sort_y[:, 0:-1] + 1e-10) 156 | # avoid dividing by 0 (nan) 157 | # print(neus_alpha) 158 | alpha = torch.clamp(neus_alpha, min=0.0, max=1.0) 159 | else: 160 | alpha = sort_y 161 | 162 | one_minus_alpha = torch.ones_like(alpha) - alpha + 1e-10 163 | 164 | cum_mat = torch.cumprod(one_minus_alpha, 1) 165 | 166 | weights = cum_mat / one_minus_alpha * alpha 167 | 168 | weights_x = weights * sort_x[:, 0 : alpha.shape[1]] 169 | 170 | d_render = torch.sum(weights_x, 1) 171 | 172 | d_error = torch.abs(d_render - d_meas) 173 | 174 | d_error_mean = torch.mean(d_error) 175 | 176 | return d_error_mean 177 | -------------------------------------------------------------------------------- /utils/point_cloud2.py: -------------------------------------------------------------------------------- 1 | # Copyright 2008 Willow Garage, Inc. 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """ 30 | This file is based on https://github.com/ros2/common_interfaces/blob/4bac182a0a582b5e6b784d9fa9f0dabc1aca4d35/sensor_msgs_py/sensor_msgs_py/point_cloud2.py 31 | All rights reserved to the original authors: Tim Field and Florian Vahl. 32 | """ 33 | 34 | import sys 35 | from typing import Iterable, List, Optional, Tuple 36 | 37 | import numpy as np 38 | 39 | try: 40 | from rosbags.typesys.types import sensor_msgs__msg__PointCloud2 as PointCloud2 41 | from rosbags.typesys.types import sensor_msgs__msg__PointField as PointField 42 | except ImportError as e: 43 | raise ImportError('rosbags library not installed, run "pip install -U rosbags"') from e 44 | 45 | 46 | _DATATYPES = {} 47 | _DATATYPES[PointField.INT8] = np.dtype(np.int8) 48 | _DATATYPES[PointField.UINT8] = np.dtype(np.uint8) 49 | _DATATYPES[PointField.INT16] = np.dtype(np.int16) 50 | _DATATYPES[PointField.UINT16] = np.dtype(np.uint16) 51 | _DATATYPES[PointField.INT32] = np.dtype(np.int32) 52 | _DATATYPES[PointField.UINT32] = np.dtype(np.uint32) 53 | _DATATYPES[PointField.FLOAT32] = np.dtype(np.float32) 54 | _DATATYPES[PointField.FLOAT64] = np.dtype(np.float64) 55 | 56 | DUMMY_FIELD_PREFIX = "unnamed_field" 57 | 58 | 59 | def read_point_cloud(msg: PointCloud2) -> Tuple[np.ndarray, np.ndarray]: 60 | """ 61 | Extract poitns and timestamps from a PointCloud2 message. 62 | 63 | :return: Tuple of [points, timestamps] 64 | points: array of x, y z points, shape: (N, 3) 65 | timestamps: array of per-pixel timestamps, shape: (N,) 66 | """ 67 | field_names = ["x", "y", "z"] 68 | t_field = None 69 | for field in msg.fields: 70 | if field.name in ["t", "timestamp", "time", "ts", "timestamps"]: 71 | t_field = field.name 72 | field_names.append(t_field) 73 | break 74 | 75 | points_structured = read_points(msg, field_names=field_names) 76 | points = np.column_stack( 77 | [points_structured["x"], points_structured["y"], points_structured["z"]] 78 | ) 79 | 80 | # Remove nan if any 81 | points = points[~np.any(np.isnan(points), axis=1)] 82 | 83 | if t_field: 84 | timestamps = points_structured[t_field].astype(np.float64) 85 | min_timestamp = np.min(timestamps) 86 | max_timestamp = np.max(timestamps) 87 | if min_timestamp == max_timestamp: 88 | timestamps = None 89 | else: 90 | timestamps = (timestamps - min_timestamp) / (max_timestamp - min_timestamp) # normalized to 0-1 91 | else: 92 | timestamps = None 93 | return points.astype(np.float64), timestamps 94 | 95 | 96 | def read_points( 97 | cloud: PointCloud2, 98 | field_names: Optional[List[str]] = None, 99 | uvs: Optional[Iterable] = None, 100 | reshape_organized_cloud: bool = False, 101 | ) -> np.ndarray: 102 | """ 103 | Read points from a sensor_msgs.PointCloud2 message. 104 | :param cloud: The point cloud to read from sensor_msgs.PointCloud2. 105 | :param field_names: The names of fields to read. If None, read all fields. 106 | (Type: Iterable, Default: None) 107 | :param uvs: If specified, then only return the points at the given 108 | coordinates. (Type: Iterable, Default: None) 109 | :param reshape_organized_cloud: Returns the array as an 2D organized point cloud if set. 110 | :return: Structured NumPy array containing all points. 111 | """ 112 | # Cast bytes to numpy array 113 | points = np.ndarray( 114 | shape=(cloud.width * cloud.height,), 115 | dtype=dtype_from_fields(cloud.fields, point_step=cloud.point_step), 116 | buffer=cloud.data, 117 | ) 118 | 119 | # Keep only the requested fields 120 | if field_names is not None: 121 | assert all( 122 | field_name in points.dtype.names for field_name in field_names 123 | ), "Requests field is not in the fields of the PointCloud!" 124 | # Mask fields 125 | points = points[list(field_names)] 126 | 127 | # Swap array if byte order does not match 128 | if bool(sys.byteorder != "little") != bool(cloud.is_bigendian): 129 | points = points.byteswap(inplace=True) 130 | 131 | # Select points indexed by the uvs field 132 | if uvs is not None: 133 | # Don't convert to numpy array if it is already one 134 | if not isinstance(uvs, np.ndarray): 135 | uvs = np.fromiter(uvs, int) 136 | # Index requested points 137 | points = points[uvs] 138 | 139 | # Cast into 2d array if cloud is 'organized' 140 | if reshape_organized_cloud and cloud.height > 1: 141 | points = points.reshape(cloud.width, cloud.height) 142 | 143 | return points 144 | 145 | 146 | def dtype_from_fields(fields: Iterable[PointField], point_step: Optional[int] = None) -> np.dtype: 147 | """ 148 | Convert a Iterable of sensor_msgs.msg.PointField messages to a np.dtype. 149 | :param fields: The point cloud fields. 150 | (Type: iterable of sensor_msgs.msg.PointField) 151 | :param point_step: Point step size in bytes. Calculated from the given fields by default. 152 | (Type: optional of integer) 153 | :returns: NumPy datatype 154 | """ 155 | # Create a lists containing the names, offsets and datatypes of all fields 156 | field_names = [] 157 | field_offsets = [] 158 | field_datatypes = [] 159 | for i, field in enumerate(fields): 160 | # Datatype as numpy datatype 161 | datatype = _DATATYPES[field.datatype] 162 | # Name field 163 | if field.name == "": 164 | name = f"{DUMMY_FIELD_PREFIX}_{i}" 165 | else: 166 | name = field.name 167 | # Handle fields with count > 1 by creating subfields with a suffix consiting 168 | # of "_" followed by the subfield counter [0 -> (count - 1)] 169 | assert field.count > 0, "Can't process fields with count = 0." 170 | for a in range(field.count): 171 | # Add suffix if we have multiple subfields 172 | if field.count > 1: 173 | subfield_name = f"{name}_{a}" 174 | else: 175 | subfield_name = name 176 | assert subfield_name not in field_names, "Duplicate field names are not allowed!" 177 | field_names.append(subfield_name) 178 | # Create new offset that includes subfields 179 | field_offsets.append(field.offset + a * datatype.itemsize) 180 | field_datatypes.append(datatype.str) 181 | 182 | # Create dtype 183 | dtype_dict = {"names": field_names, "formats": field_datatypes, "offsets": field_offsets} 184 | if point_step is not None: 185 | dtype_dict["itemsize"] = point_step 186 | return np.dtype(dtype_dict) 187 | -------------------------------------------------------------------------------- /utils/semantic_kitti_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class LabelDataConverter: 5 | """Convert .label binary data to instance id and rgb""" 6 | 7 | def __init__(self, labelscan): 8 | 9 | self.convertdata(labelscan) 10 | 11 | def convertdata(self, labelscan): 12 | 13 | self.semantic_id = [] 14 | self.rgb_id = [] 15 | self.instance_id = [] 16 | self.rgb_arr_id = [] 17 | 18 | for counting in range(len(labelscan)): 19 | 20 | sem_id = int(labelscan[counting]) & 0xFFFF # lower 16 bit 21 | rgb, rgb_arr = self.get_sem_rgb(sem_id) 22 | instance_id = int(labelscan[counting]) >> 16 # higher 16 bit 23 | # rgb = self.get_random_rgb(instance_id) 24 | 25 | # print("Sem label:", sem_id, "Ins label:", instance_id, "Color:", hex(rgb)) 26 | # print(hex(rgb)) 27 | # instance label is given in each semantic label 28 | 29 | self.semantic_id.append(sem_id) 30 | self.rgb_id.append(rgb) 31 | self.rgb_arr_id.append(rgb_arr) 32 | self.instance_id.append(instance_id) 33 | 34 | 35 | def get_random_rgb(n): 36 | n = ((n ^ n >> 15) * 2246822519) & 0xFFFFFFFF 37 | n = ((n ^ n >> 13) * 3266489917) & 0xFFFFFFFF 38 | n = (n ^ n >> 16) >> 8 39 | print(n) 40 | return tuple(n.to_bytes(3, "big")) 41 | 42 | 43 | sem_kitti_learning_map = { 44 | 0: 0, # "unlabeled" 45 | 1: 0, # "outlier" mapped to "unlabeled" --------------------------mapped 46 | 10: 1, # "car" 47 | 11: 2, # "bicycle" 48 | 13: 5, # "bus" mapped to "other-vehicle" --------------------------mapped 49 | 15: 3, # "motorcycle" 50 | 16: 5, # "on-rails" mapped to "other-vehicle" ---------------------mapped 51 | 18: 4, # "truck" 52 | 20: 5, # "other-vehicle" 53 | 30: 6, # "person" 54 | 31: 7, # "bicyclist" 55 | 32: 8, # "motorcyclist" 56 | 40: 9, # "road" 57 | 44: 10, # "parking" 58 | 48: 11, # "sidewalk" 59 | 49: 12, # "other-ground" 60 | 50: 13, # "building" 61 | 51: 14, # "fence" 62 | 52: 20, # "other-structure" mapped to "unlabeled" ------------------mapped 63 | 60: 9, # "lane-marking" to "road" ---------------------------------mapped 64 | 70: 15, # "vegetation" 65 | 71: 16, # "trunk" 66 | 72: 17, # "terrain" 67 | 80: 18, # "pole" 68 | 81: 19, # "traffic-sign" 69 | 99: 20, # "other-object" to "unlabeled" ----------------------------mapped 70 | 252: 1, # "moving-car" 71 | 253: 7, # "moving-bicyclist" 72 | 254: 6, # "moving-person" 73 | 255: 8, # "moving-motorcyclist" 74 | 256: 5, # "moving-on-rails" mapped to "moving-other-vehicle" ------mapped 75 | 257: 5, # "moving-bus" mapped to "moving-other-vehicle" -----------mapped 76 | 258: 4, # "moving-truck" 77 | 259: 5, # "moving-other-vehicle" 78 | } 79 | 80 | 81 | def sem_map_function(value): 82 | return sem_kitti_learning_map.get(value, value) 83 | 84 | 85 | sem_kitti_labels = { 86 | 0: "unlabeled", 87 | 1: "car", 88 | 2: "bicycle", 89 | 3: "motorcycle", 90 | 4: "truck", 91 | 5: "other-vehicle", 92 | 6: "person", 93 | 7: "bicyclist", 94 | 8: "motorcyclist", 95 | 9: "road", 96 | 10: "parking", 97 | 11: "sidewalk", 98 | 12: "other-ground", 99 | 13: "building", 100 | 14: "fence", 101 | 15: "vegetation", 102 | 16: "trunk", 103 | 17: "terrain", 104 | 18: "pole", 105 | 19: "traffic-sign", 106 | 20: "others", 107 | } 108 | 109 | sem_kitti_color_map = { # rgb 110 | 0: [255, 255, 255], 111 | 1: [100, 150, 245], 112 | 2: [100, 230, 245], 113 | 3: [30, 60, 150], 114 | 4: [80, 30, 180], 115 | 5: [0, 0, 255], 116 | 6: [255, 30, 30], 117 | 7: [255, 40, 200], 118 | 8: [150, 30, 90], 119 | 9: [255, 0, 255], 120 | 10: [255, 150, 255], 121 | 11: [75, 0, 75], 122 | 12: [175, 0, 75], 123 | 13: [255, 200, 0], 124 | 14: [255, 120, 50], 125 | 15: [0, 175, 0], 126 | 16: [135, 60, 0], 127 | 17: [150, 240, 80], 128 | 18: [255, 240, 150], 129 | 19: [255, 0, 0], 130 | 20: [30, 30, 30], 131 | } 132 | -------------------------------------------------------------------------------- /vis_pin_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # @file vis_pin_map.py 3 | # @author Yue Pan [yue.pan@igg.uni-bonn.de] 4 | # Copyright (c) 2024 Yue Pan, all rights reserved 5 | 6 | import glob 7 | import os 8 | import sys 9 | import time 10 | 11 | import numpy as np 12 | import open3d as o3d 13 | import torch 14 | import torch.multiprocessing as mp 15 | import dtyper as typer 16 | from rich import print 17 | 18 | from model.decoder import Decoder 19 | from model.neural_points import NeuralPoints 20 | from utils.config import Config 21 | from utils.mesher import Mesher 22 | from utils.tools import setup_experiment, split_chunks, load_decoders, remove_gpu_cache 23 | 24 | from gui import slam_gui 25 | from gui.gui_utils import ParamsGUI, VisPacket 26 | 27 | 28 | ''' 29 | load the pin-map and do the reconstruction 30 | ''' 31 | 32 | app = typer.Typer(add_completion=False, rich_markup_mode="rich", context_settings={"help_option_names": ["-h", "--help"]}) 33 | 34 | docstring = f""" 35 | :round_pushpin: Inspect the PIN Map \n 36 | 37 | [bold green]Examples: [/bold green] 38 | 39 | # Inspect the PIN Map stored in a mapping result folder, showing both the neural points and the mesh reconstructed with a certain marching cubes resolution 40 | $ python3 vis_pin_map.ply :open_file_folder: -m 41 | 42 | # Additionally, you can specify the cropped point cloud file and the output mesh file 43 | $ python3 vis_pin_map.ply :open_file_folder: -m -c :page_facing_up: -o :page_facing_up: 44 | 45 | """ 46 | 47 | @app.command(help=docstring) 48 | def vis_pin_map( 49 | result_folder: str = typer.Argument(..., help='Path to the result folder'), 50 | mesh_res_m: float = typer.Option(None, '--mesh_res_m', '-m', help='Resolution of the mesh in meters'), 51 | cropped_ply_filename: str = typer.Option("neural_points.ply", '--cropped_ply_filename', '-c', help='Path to the cropped point cloud file'), 52 | output_mesh_filename: str = typer.Option(None, '--output_mesh_filename', '-o', help='Path to the output mesh file'), 53 | mc_nn: int = typer.Option(9, '--mc_nn', '-n', help='Minimum number of neighbors for SDF querying for marching cubes'), 54 | o3d_vis_on: bool = typer.Option(True, '--visualize_on', '-v', help='Turn on the visualizer'), 55 | ): 56 | 57 | config = Config() 58 | 59 | yaml_files = glob.glob(f"{result_folder}/*.yaml") 60 | if len(yaml_files) > 1: # Check if there is exactly one YAML file 61 | sys.exit("There are multiple YAML files. Please handle accordingly.") 62 | elif len(yaml_files) == 0: # If no YAML files are found 63 | sys.exit("No YAML files found in the specified path.") 64 | config.load(yaml_files[0]) 65 | config.model_path = os.path.join(result_folder, "model", "pin_map.pth") 66 | 67 | print("[bold green]Load and inspect PIN Map[/bold green]","📍" ) 68 | 69 | run_path = setup_experiment(config, sys.argv, debug_mode=True) 70 | 71 | mp.set_start_method("spawn") # don't forget this 72 | 73 | # initialize the mlp decoder 74 | geo_mlp = Decoder(config, config.geo_mlp_hidden_dim, config.geo_mlp_level, 1) 75 | sem_mlp = Decoder(config, config.sem_mlp_hidden_dim, config.sem_mlp_level, config.sem_class_count + 1) if config.semantic_on else None 76 | color_mlp = Decoder(config, config.color_mlp_hidden_dim, config.color_mlp_level, config.color_channel) if config.color_on else None 77 | 78 | mlp_dict = {} 79 | mlp_dict["sdf"] = geo_mlp 80 | mlp_dict["semantic"] = sem_mlp 81 | mlp_dict["color"] = color_mlp 82 | 83 | # initialize the neural point features 84 | neural_points: NeuralPoints = NeuralPoints(config) 85 | 86 | # Load the map 87 | loaded_model = torch.load(config.model_path) 88 | neural_points = loaded_model["neural_points"] 89 | load_decoders(loaded_model, mlp_dict) 90 | neural_points.temporal_local_map_on = False 91 | neural_points.recreate_hash(neural_points.neural_points[0], torch.eye(3).cuda(), False, False) 92 | neural_points.compute_feature_principle_components(down_rate = 59) 93 | print("PIN Map loaded") 94 | 95 | # mesh reconstructor 96 | mesher = Mesher(config, neural_points, mlp_dict) 97 | 98 | mesh_on = (mesh_res_m is not None) 99 | if mesh_on: 100 | config.mc_res_m = mesh_res_m 101 | config.mesh_min_nn = mc_nn 102 | 103 | q_main2vis = q_vis2main = None 104 | if o3d_vis_on: 105 | # communicator between the processes 106 | q_main2vis = mp.Queue() 107 | q_vis2main = mp.Queue() 108 | 109 | params_gui = ParamsGUI( 110 | q_main2vis=q_main2vis, 111 | q_vis2main=q_vis2main, 112 | config=config, 113 | local_map_default_on=False, 114 | mesh_default_on=mesh_on, 115 | neural_point_map_default_on=config.neural_point_map_default_on, 116 | ) 117 | gui_process = mp.Process(target=slam_gui.run, args=(params_gui,)) 118 | gui_process.start() 119 | time.sleep(3) # second 120 | 121 | cur_mesh = None 122 | out_mesh_path = None 123 | 124 | if mesh_on: 125 | cropped_ply_path = os.path.join(result_folder, "map", cropped_ply_filename) 126 | if os.path.exists(cropped_ply_path): 127 | cropped_pc = o3d.io.read_point_cloud(cropped_ply_path) 128 | print("Load region for meshing from {}".format(cropped_ply_path)) 129 | 130 | else: 131 | cropped_pc = neural_points.get_neural_points_o3d(query_global=True, random_down_ratio=23) 132 | 133 | mesh_aabb = cropped_pc.get_axis_aligned_bounding_box() 134 | chunks_aabb = split_chunks(cropped_pc, mesh_aabb, mesh_res_m*300) 135 | 136 | if output_mesh_filename is not None: 137 | out_mesh_path = os.path.join(result_folder, "mesh", output_mesh_filename) 138 | print("Output the mesh to: ", out_mesh_path) 139 | 140 | mc_cm_str = str(round(mesh_res_m*1e2)) 141 | print("Reconstructing the mesh with resolution {} cm".format(mc_cm_str)) 142 | cur_mesh = mesher.recon_aabb_collections_mesh(chunks_aabb, mesh_res_m, out_mesh_path, False, config.semantic_on, 143 | config.color_on, filter_isolated_mesh=True, mesh_min_nn=mc_nn) 144 | print("Reconstructing the global mesh done") 145 | 146 | remove_gpu_cache() 147 | 148 | if o3d_vis_on: 149 | 150 | while True: 151 | if not q_vis2main.empty(): 152 | q_vis2main.get() 153 | 154 | packet_to_vis: VisPacket = VisPacket(slam_finished=True) 155 | 156 | if not neural_points.is_empty(): 157 | packet_to_vis.add_neural_points_data(neural_points, only_local_map=False, pca_color_on=True) 158 | 159 | if cur_mesh is not None: 160 | packet_to_vis.add_mesh(np.array(cur_mesh.vertices, dtype=np.float64), np.array(cur_mesh.triangles), np.array(cur_mesh.vertex_colors, dtype=np.float64)) 161 | cur_mesh = None 162 | 163 | q_main2vis.put(packet_to_vis) 164 | time.sleep(1.0) 165 | 166 | 167 | if __name__ == "__main__": 168 | app() --------------------------------------------------------------------------------