├── .github └── FUNDING.yml ├── .gitignore ├── README.md ├── dev.sh ├── docs ├── install.md ├── prepare_dataset.md └── running.md ├── mtgs ├── __init__.py ├── config │ ├── 3DGS.py │ ├── MTGS.py │ ├── MTGS_deformable.py │ ├── WildGaussians.py │ └── nuplan_dataparser.py ├── custom_viewer │ ├── __init__.py │ ├── control_panel.py │ ├── export_panel.py │ ├── render_panel.py │ ├── render_state_machine.py │ ├── server │ │ ├── __init__.py │ │ └── viewer_elements.py │ ├── utils.py │ ├── viewer.py │ └── viewer_elements.py ├── dataset │ ├── __init__.py │ ├── custom_datamanager.py │ ├── custom_dataset.py │ ├── nuplan_dataparser.py │ └── utils │ │ ├── __init__.py │ │ ├── dataloader.py │ │ └── sampler.py ├── scene_model │ ├── __init__.py │ ├── custom_pipeline.py │ ├── custom_trainer.py │ ├── gaussian_model │ │ ├── __init__.py │ │ ├── deformable_node.py │ │ ├── multi_color_gaussian_splatting.py │ │ ├── rigid_node.py │ │ ├── skybox_gaussian_splatting.py │ │ ├── utils.py │ │ └── vanilla_gaussian_splatting.py │ ├── module │ │ ├── __init__.py │ │ └── appearance.py │ └── mtgs_scene_graph.py ├── tools │ ├── batch_exp │ │ ├── export_eval_images.py │ │ ├── mtgs_tasks.py │ │ ├── run_base_benchmarking.py │ │ └── run_single_road_block.py │ ├── render.py │ └── run_viewer.py └── utils │ ├── __init__.py │ ├── camera_utils.py │ ├── dinov2.py │ ├── geometric_loss.py │ ├── nuplan_pointcloud.py │ ├── pnsr.py │ └── ssim.py ├── nuplan_scripts ├── __init__.py ├── ba_multi_traversal.py ├── collect_raw_data.py ├── configs │ ├── common │ │ └── nuplan_path.yml │ └── mtgs_exp │ │ ├── road_block-331220_4690660_331190_4690710.yml │ │ ├── road_block-365000_144000_365100_144080.yml │ │ ├── road_block-365530_143960_365630_144060.yml │ │ ├── road_block-587400_4475700_587480_4475800.yml │ │ ├── road_block-587640_4475600_587710_4475660.yml │ │ └── road_block-587860_4475510_587910_4475570.yml ├── export_videos.py ├── filter_trajectory.py ├── generate_dense_depth.py ├── generate_semantic_mask.py ├── lidar_registration_multi_traversal.py ├── misc │ ├── generate_configs_from_navsim_filter.py │ └── generate_nuplan_log_info.py ├── nuplan_video_processing.py ├── preprocess.sh ├── preprocess_stage_1.sh ├── preprocess_stage_2.sh ├── preview.py ├── stack_RGB_point_cloud.py └── utils │ ├── __init__.py │ ├── camera_utils.py │ ├── colmap_utils │ ├── align_model.py │ ├── bundle_adjustment.py │ ├── database.py │ ├── gen_colmap_db.py │ ├── point_triangulator.py │ └── read_write_model.py │ ├── config.py │ ├── constants.py │ ├── nuplan_utils_custom.py │ ├── stack_point_cloud_utils.py │ └── video_scene_dict_tools.py ├── pyproject.toml ├── requirements.txt ├── requirements_data.txt └── thirdparty ├── UniDepth ├── .gitignore ├── LICENSE ├── README.md ├── configs │ ├── config_v1_cnvnxtl.json │ ├── config_v1_vitl14.json │ ├── config_v2_vitl14.json │ └── config_v2_vits14.json ├── hubconf.py ├── pyproject.toml ├── requirements.txt ├── scripts │ └── demo.py └── unidepth │ ├── layers │ ├── __init__.py │ ├── activation.py │ ├── attention.py │ ├── convnext.py │ ├── drop_path.py │ ├── layer_scale.py │ ├── mlp.py │ ├── nystrom_attention.py │ ├── positional_encoding.py │ └── upsample.py │ ├── models │ ├── __init__.py │ ├── backbones │ │ ├── __init__.py │ │ ├── convnext.py │ │ ├── convnext2.py │ │ ├── dinov2.py │ │ └── metadinov2 │ │ │ ├── __init__.py │ │ │ ├── attention.py │ │ │ ├── block.py │ │ │ ├── dino_head.py │ │ │ ├── drop_path.py │ │ │ ├── layer_scale.py │ │ │ ├── mlp.py │ │ │ ├── patch_embed.py │ │ │ └── swiglu_ffn.py │ ├── encoder.py │ ├── unidepthv1 │ │ ├── __init__.py │ │ ├── decoder.py │ │ └── unidepthv1.py │ └── unidepthv2 │ │ ├── __init__.py │ │ ├── decoder.py │ │ ├── decoder_old.py │ │ ├── export.py │ │ └── unidepthv2.py │ ├── ops │ ├── __init__.py │ ├── losses.py │ └── scheduler.py │ └── utils │ ├── __init__.py │ ├── constants.py │ ├── distributed.py │ ├── ema_torch.py │ ├── evaluation_depth.py │ ├── geometric.py │ ├── misc.py │ ├── positional_embedding.py │ ├── sht.py │ └── visualization.py └── kiss-icp ├── .clang-format ├── .cmake-format.yaml ├── .gitignore ├── .pre-commit-config.yaml ├── CITATION.cff ├── LICENSE ├── Makefile ├── README.md ├── config ├── README.md ├── advanced.yaml └── basic.yaml ├── cpp ├── COLCON_IGNORE └── kiss_icp │ ├── 3rdparty │ ├── eigen │ │ ├── LICENSE │ │ ├── eigen.cmake │ │ └── eigen.patch │ ├── find_dependencies.cmake │ ├── sophus │ │ ├── LICENSE │ │ ├── sophus.cmake │ │ └── sophus.patch │ ├── tbb │ │ ├── LICENSE │ │ └── tbb.cmake │ └── tsl_robin │ │ ├── LICENSE │ │ └── tsl_robin.cmake │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── cmake │ └── CompilerOptions.cmake │ ├── core │ ├── CMakeLists.txt │ ├── Deskew.cpp │ ├── Deskew.hpp │ ├── Preprocessing.cpp │ ├── Preprocessing.hpp │ ├── Registration.cpp │ ├── Registration.hpp │ ├── Threshold.cpp │ ├── Threshold.hpp │ ├── VoxelHashMap.cpp │ ├── VoxelHashMap.hpp │ ├── VoxelUtils.cpp │ └── VoxelUtils.hpp │ ├── metrics │ ├── CMakeLists.txt │ ├── Metrics.cpp │ └── Metrics.hpp │ └── pipeline │ ├── CMakeLists.txt │ ├── KissICP.cpp │ └── KissICP.hpp ├── python ├── CMakeLists.txt ├── COLCON_IGNORE ├── LICENSE ├── MANIFEST.in ├── README.md ├── kiss_icp │ ├── __init__.py │ ├── config │ │ ├── __init__.py │ │ ├── config.py │ │ └── parser.py │ ├── datasets │ │ ├── __init__.py │ │ ├── apollo.py │ │ ├── boreas.py │ │ ├── generic.py │ │ ├── helipr.py │ │ ├── kitti.py │ │ ├── kitti_raw.py │ │ ├── mcap.py │ │ ├── mtgs.py │ │ ├── mulran.py │ │ ├── ncd.py │ │ ├── nclt.py │ │ ├── nuscenes.py │ │ ├── ouster.py │ │ ├── paris_luco.py │ │ ├── rosbag.py │ │ └── tum.py │ ├── deskew.py │ ├── kiss_icp.py │ ├── mapping.py │ ├── metrics.py │ ├── pipeline.py │ ├── preprocess.py │ ├── pybind │ │ ├── .gitignore │ │ ├── CMakeLists.txt │ │ ├── kiss_icp_pybind.cpp │ │ └── stl_vector_eigen.h │ ├── registration.py │ ├── threshold.py │ ├── tools │ │ ├── __init__.py │ │ ├── cmd.py │ │ ├── pipeline_results.py │ │ ├── point_cloud2.py │ │ ├── progress_bar.py │ │ └── visualizer.py │ └── voxelization.py ├── pyproject.toml └── tests │ └── test_kiss_icp.py └── ros ├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch └── odometry.launch.py ├── package.xml ├── rviz └── kiss_icp.rviz └── src ├── OdometryServer.cpp ├── OdometryServer.hpp └── Utils.hpp /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | 3 | github: [OpenDriveLab] # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2] 4 | patreon: # Replace with a single Patreon username 5 | open_collective: # Replace with a single Open Collective username 6 | ko_fi: # Replace with a single Ko-fi username 7 | tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel 8 | community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry 9 | liberapay: # Replace with a single Liberapay username 10 | issuehunt: # Replace with a single IssueHunt username 11 | lfx_crowdfunding: # Replace with a single LFX Crowdfunding project-name e.g., cloud-foundry 12 | polar: # Replace with a single Polar username 13 | buy_me_a_coffee: # Replace with a single Buy Me a Coffee username 14 | custom: # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2'] 15 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | 132 | # Custom 133 | ckpts 134 | data 135 | work_dirs 136 | .DS_Store 137 | 138 | outputs 139 | experiments 140 | renders -------------------------------------------------------------------------------- /dev.sh: -------------------------------------------------------------------------------- 1 | export CODEBASE="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 2 | cd $CODEBASE; 3 | conda activate mtgs; 4 | export PYTHONPATH=\"./\":$PYTHONPATH; 5 | 6 | function mtgs_dev_setup(){ 7 | tmp_var=$1; 8 | tmp_var=${tmp_var%.py} 9 | tmp_var=${tmp_var//\//.} 10 | export NERFSTUDIO_DATAPARSER_CONFIGS='nuplan=mtgs.config.nuplan_dataparser:nuplan_dataparser'; 11 | export NERFSTUDIO_METHOD_CONFIGS='mtgs-dev='${tmp_var}':method'; 12 | echo '-------------------------------------'; 13 | echo 'ENV UPDATED with following ENV VARS.'; 14 | echo ' NERFSTUDIO_DATAPARSER_CONFIGS = '${NERFSTUDIO_DATAPARSER_CONFIGS}; 15 | echo ' NERFSTUDIO_METHOD_CONFIGS = '${NERFSTUDIO_METHOD_CONFIGS}; 16 | echo '-------------------------------------' 17 | } 18 | echo 'INIT SETUP DONE.' 19 | 20 | alias mtgs_setup="mtgs_dev_setup" 21 | alias mtgs_train="ns-train mtgs-dev" 22 | alias mtgs_render="python mtgs/tools/render.py interpolate --load-config" 23 | alias mtgs_viewer="python tmgs/tools/run_viewer.py --viewer.camera-frustum-scale 0.3 --viewer.default_composite_depth False --viewer.max_num_display_images 500 --load-config" 24 | -------------------------------------------------------------------------------- /docs/install.md: -------------------------------------------------------------------------------- 1 | # Installation Instructions of MTGS 2 | 3 | ## Prerequisites 4 | 5 | You must have a CUDA-enabled NVIDIA video card. The code is tested on Ubuntu 20.04 and CUDA 11.8. You may need at least 40 GB of GPU memory and 64 GB of RAM to run the code. 6 | 7 | ## Dependencies 8 | 9 | The repository is highly dependent on [nerfstudio](https://github.com/nerfstudio-project/nerfstudio) and [gsplat](https://github.com/nerfstudio-project/gsplat). 10 | 11 | ```bash 12 | # clone the repository 13 | git clone https://github.com/OpenDriveLab/MTGS.git 14 | cd MTGS/ 15 | 16 | # create a conda environment 17 | conda create --name mtgs -y python=3.9 18 | conda activate mtgs 19 | 20 | # tiny-cuda-nn requires >gcc 9.0 21 | conda install -c conda-forge gxx=9.5.0 22 | 23 | # if you do not have cuda 11.8 locally, install it with the following command 24 | conda install -c "nvidia/label/cuda-11.8.0" cuda-toolkit 25 | 26 | # install torch and torchvision 27 | pip install torch==2.0.1+cu118 torchvision==0.15.2+cu118 --extra-index-url https://download.pytorch.org/whl/cu118 28 | 29 | # install the requirements 30 | pip install -r requirements.txt 31 | ``` 32 | 33 | ## Install the dependencies of nuplan data processing 34 | 35 | If you want to process the nuplan data by yourself, you need to first install the [nuplan-devkit](https://github.com/motional/nuplan-devkit) with version 1.2.0 and [colmap](https://github.com/colmap/colmap) with version 3.11.1. 36 | 37 | Then, you need to install the customized version of [kiss-icp](https://github.com/PRBonn/kiss-icp) and [UniDepth](https://github.com/lpiccinelli-eth/UniDepth) in this repository. 38 | 39 | ```bash 40 | # Install the requirements for nuplan data processing 41 | pip install -r requirements_data.txt 42 | 43 | # Install kiss-icp. 44 | # this is a customized version with some changes, based on kiss-icp 1.0.0 45 | cd thirdparty/kiss-icp 46 | make editable 47 | 48 | # Install UniDepth. 49 | # The requirements are aligned with MTGS. 50 | pip install -e thirdparty/UniDepth 51 | ``` 52 | -------------------------------------------------------------------------------- /mtgs/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/mtgs/__init__.py -------------------------------------------------------------------------------- /mtgs/config/nuplan_dataparser.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | from nerfstudio.plugins.registry_dataparser import DataParserSpecification 7 | from mtgs.dataset.nuplan_dataparser import NuplanDataParserConfig 8 | 9 | nuplan_dataparser = DataParserSpecification(config=NuplanDataParserConfig()) 10 | -------------------------------------------------------------------------------- /mtgs/custom_viewer/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 the Regents of the University of California, Nerfstudio Team and contributors. All rights reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | -------------------------------------------------------------------------------- /mtgs/custom_viewer/server/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 the Regents of the University of California, Nerfstudio Team and contributors. All rights reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /mtgs/custom_viewer/server/viewer_elements.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 the Regents of the University of California, Nerfstudio Team and contributors. All rights reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Shim for viewer_elements.py, which has been moved. 16 | 17 | Resolves issues like: https://github.com/ayaanzhaque/instruct-nerf2nerf/pull/88 18 | """ 19 | from ..viewer_elements import * # noqa 20 | -------------------------------------------------------------------------------- /mtgs/dataset/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/mtgs/dataset/__init__.py -------------------------------------------------------------------------------- /mtgs/dataset/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/mtgs/dataset/utils/__init__.py -------------------------------------------------------------------------------- /mtgs/dataset/utils/sampler.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import random 7 | import numpy as np 8 | 9 | class CameraSampler: 10 | 11 | def __init__(self, dataset, in_order: bool = False): 12 | self.dataset = dataset 13 | self.unseen_cameras = [i for i in range(len(self.dataset))] 14 | self.in_order = in_order 15 | 16 | def get_next_image_idx(self) -> int: 17 | 18 | if self.in_order: 19 | image_idx = self.unseen_cameras.pop(0) 20 | else: 21 | image_idx = self.unseen_cameras.pop(random.randint(0, len(self.unseen_cameras) - 1)) 22 | if len(self.unseen_cameras) == 0: 23 | self.unseen_cameras = [i for i in range(len(self.dataset))] 24 | 25 | return image_idx 26 | 27 | class MultiTraversalBalancedSampler: 28 | 29 | def __init__(self, dataset, **kwargs): 30 | self.dataset = dataset 31 | self.dataparser_outputs = self.dataset._dataparser_outputs 32 | 33 | self.traversals = set(self.dataparser_outputs.travel_ids) 34 | 35 | travel_ids = np.array(self.dataparser_outputs.travel_ids) 36 | self.traversal_counts = { 37 | traversal: (travel_ids == traversal).sum() for traversal in self.traversals} 38 | self.traversal_indices = { 39 | traversal: (np.where(travel_ids == traversal)[0]).tolist() for traversal in self.traversals 40 | } 41 | 42 | self.unseen_traversals = list(self.traversals) 43 | self.unseen_per_traversal_images = { 44 | traversal: self.traversal_indices[traversal].copy() for traversal in self.traversals 45 | } 46 | 47 | def get_next_traversal(self): 48 | traversal = self.unseen_traversals.pop(random.randint(0, len(self.unseen_traversals) - 1)) 49 | if len(self.unseen_traversals) == 0: 50 | self.unseen_traversals = list(self.traversals) 51 | return traversal 52 | 53 | def get_next_image_idx(self): 54 | travel_id = self.get_next_traversal() 55 | image_idx = self.unseen_per_traversal_images[travel_id].pop(random.randint(0, len(self.unseen_per_traversal_images[travel_id]) - 1)) 56 | if len(self.unseen_per_traversal_images[travel_id]) == 0: 57 | self.unseen_per_traversal_images[travel_id] = self.traversal_indices[travel_id].copy() 58 | return image_idx 59 | -------------------------------------------------------------------------------- /mtgs/scene_model/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/mtgs/scene_model/__init__.py -------------------------------------------------------------------------------- /mtgs/scene_model/gaussian_model/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/mtgs/scene_model/gaussian_model/__init__.py -------------------------------------------------------------------------------- /mtgs/scene_model/module/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/mtgs/scene_model/module/__init__.py -------------------------------------------------------------------------------- /mtgs/scene_model/module/appearance.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | from dataclasses import dataclass, field 7 | from typing import Optional, Type 8 | 9 | import torch 10 | import torch.nn as nn 11 | 12 | from jaxtyping import Float, Int 13 | from torch import Tensor, nn 14 | 15 | from nerfstudio.configs.base_config import InstantiateConfig 16 | 17 | @dataclass 18 | class GSAppearanceModelConfig(InstantiateConfig): 19 | """Configuration for Gaussian Splatting AppearanceModel""" 20 | _target: Type = field(default_factory=lambda: GSAppearanceModel) 21 | appearance_embedding_dim: int = 32 22 | 23 | class GSAppearanceModel(nn.Module): 24 | """Appearance model for Gaussian Splatting. This is a dummy class that does nothing.""" 25 | config: GSAppearanceModelConfig 26 | 27 | def __init__( 28 | self, 29 | config: GSAppearanceModelConfig, 30 | num_cameras: int 31 | ) -> None: 32 | super().__init__() 33 | self.config = config 34 | 35 | def get_param_groups(self, param_groups: dict) -> None: 36 | """Get camera optimizer parameters""" 37 | pass 38 | 39 | def forward( 40 | self, 41 | image: torch.Tensor, 42 | indices: Int[Tensor, "camera_indices"] 43 | ): 44 | return image 45 | 46 | @dataclass 47 | class LearnableExposureRGBModelConfig(GSAppearanceModelConfig): 48 | """Configuration for VastAppearanceModel""" 49 | _target: Type = field(default_factory=lambda: LearnableExposureRGBModel) 50 | 51 | class LearnableExposureRGBModel(GSAppearanceModel): 52 | """Decoupled Appearance Modeling in VastGaussian: Vast 3D Gaussians for Large Scene Reconstruction""" 53 | config: LearnableExposureRGBModelConfig 54 | 55 | def __init__( 56 | self, 57 | config: LearnableExposureRGBModelConfig, 58 | num_cameras: int, 59 | # v_adjust: torch.Tensor 60 | ) -> None: 61 | 62 | super().__init__(config, num_cameras) 63 | 64 | self.exposure_factor = nn.Parameter( 65 | torch.eye(3, 4, dtype=torch.float32, requires_grad=True)[None].repeat(num_cameras, 1, 1) 66 | ) 67 | 68 | def get_param_groups(self, param_groups: dict) -> None: 69 | """Get camera optimizer parameters""" 70 | opt_params = list(self.parameters()) 71 | param_groups["appearance"] = opt_params 72 | 73 | def forward(self, 74 | image: Float[Tensor, "image"], 75 | indices: Optional[Int[Tensor, "camera_indices"]] = None, 76 | ): 77 | # image shape: W x H x 3 78 | if indices is None: 79 | return image 80 | 81 | exposure_factor = self.exposure_factor[indices[0]] 82 | 83 | # exposure_factor = exposure_factor * self.v_adjust.data[indices] 84 | optimized_image = image.matmul(exposure_factor[:3, :3]) + exposure_factor[None, None, :3, 3] 85 | optimized_image = torch.clamp(optimized_image, 0, 1) 86 | 87 | return optimized_image 88 | -------------------------------------------------------------------------------- /mtgs/tools/batch_exp/export_eval_images.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import os 7 | import argparse 8 | import yaml 9 | import torch 10 | from pathlib import Path 11 | from mtgs.tools.batch_exp.mtgs_tasks import tasks_registry 12 | 13 | if __name__ == '__main__': 14 | parser = argparse.ArgumentParser() 15 | parser.add_argument('--ns-config', type=str, default='mtgs/config/MTGS.py') 16 | parser.add_argument('--output-dir', type=str, default='experiments/main_mt/MTGS') 17 | parser.add_argument('--task-name', type=str, default='main_mt') 18 | args = parser.parse_args() 19 | 20 | # We hack the enviroment variable to register the method and dataparser to nerfstudio. 21 | # NOTE: the method name is `mtgs` here, not guaranteed to be the same as in the config. 22 | module_name = args.ns_config.replace('.py', '').replace('/', '.') 23 | os.environ["NERFSTUDIO_DATAPARSER_CONFIGS"] = "nuplan=mtgs.config.nuplan_dataparser:nuplan_dataparser" 24 | os.environ["NERFSTUDIO_METHOD_CONFIGS"] = 'mtgs=' + module_name + ':method' 25 | from nerfstudio.utils.eval_utils import eval_load_checkpoint 26 | 27 | def update_config(config): 28 | config.pipeline.image_saving_mode = "sequential" 29 | config.pipeline.datamanager.dataparser.eval_2hz = True 30 | # disable loading to speed up. 31 | config.pipeline.datamanager.load_mask = False 32 | config.pipeline.datamanager.load_custom_masks = () 33 | config.pipeline.datamanager.load_instance_masks = False 34 | config.pipeline.datamanager.load_semantic_masks_from = False 35 | config.pipeline.datamanager.load_lidar_depth = False 36 | config.pipeline.datamanager.load_pseudo_depth = False 37 | 38 | config.pipeline.model.output_depth_during_training = False 39 | config.pipeline.model.predict_normals = False 40 | config.pipeline.model.color_corrected_metrics = False 41 | config.pipeline.model.lpips_metric = False 42 | config.pipeline.model.dinov2_metric = False 43 | return config 44 | 45 | def eval_setup(config_path, test_mode, update_config_callback): 46 | # load save config 47 | config = yaml.load(config_path.read_text(), Loader=yaml.Loader) 48 | 49 | if update_config_callback is not None: 50 | config = update_config_callback(config) 51 | 52 | # load checkpoints from wherever they were saved 53 | config.load_dir = config.get_checkpoint_dir() 54 | 55 | # setup pipeline (which includes the DataManager) 56 | device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 57 | pipeline = config.pipeline.setup(device=device, test_mode=test_mode) 58 | pipeline.eval() 59 | 60 | # load checkpointed information 61 | checkpoint_path, step = eval_load_checkpoint(config, pipeline) 62 | 63 | return config, pipeline, checkpoint_path, step 64 | 65 | 66 | tasks = tasks_registry[args.task_name] 67 | for task in tasks: 68 | load_config = Path(args.output_dir) / task["config"].split('/')[-1].split('.')[0] / 'config.yml' 69 | image_output_path = Path(args.output_dir) / 'rendered_images' / task["config"].split('/')[-1].split('.')[0] 70 | config, pipeline, checkpoint_path, _ = eval_setup(load_config, update_config_callback=update_config) 71 | metrics_dict = pipeline.get_average_eval_image_metrics( 72 | output_path=image_output_path, 73 | get_std=False 74 | ) 75 | -------------------------------------------------------------------------------- /mtgs/utils/__init__.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.distance import cdist 3 | 4 | def chamfer_distance(gt: np.ndarray, pred: np.ndarray) -> float: 5 | r""" 6 | Calculate Chamfer distance. 7 | 8 | Parameters 9 | ---------- 10 | gt : np.ndarray 11 | Curve of (G, N) shape, 12 | where G is the number of data points, 13 | and N is the number of dimmensions. 14 | pred : np.ndarray 15 | Curve of (P, N) shape, 16 | where P is the number of points, 17 | and N is the number of dimmensions. 18 | 19 | Returns 20 | ------- 21 | float 22 | Chamfer distance 23 | 24 | """ 25 | assert gt.ndim == pred.ndim == 2 and gt.shape[1] == pred.shape[1] 26 | if (gt[0] == gt[-1]).all(): 27 | gt = gt[:-1] 28 | dist_mat = cdist(pred, gt) 29 | 30 | dist_pred = dist_mat.min(-1).mean() 31 | dist_gt = dist_mat.min(0).mean() 32 | 33 | return (dist_pred + dist_gt) / 2 34 | -------------------------------------------------------------------------------- /nuplan_scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/nuplan_scripts/__init__.py -------------------------------------------------------------------------------- /nuplan_scripts/collect_raw_data.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import argparse 7 | import os 8 | import shutil 9 | from tqdm import tqdm 10 | from torch.utils.data import Dataset, DataLoader 11 | 12 | from nuplan_scripts.utils.config import load_config, RoadBlockConfig 13 | from nuplan_scripts.utils.video_scene_dict_tools import VideoScene 14 | from nuplan_scripts.utils.constants import NUPLAN_SENSOR_ROOT 15 | 16 | 17 | class CollectRawData: 18 | 19 | class PseudoDataset(Dataset): 20 | def __init__(self, frame_infos, video_scene: VideoScene): 21 | self.frame_infos = frame_infos 22 | self.video_scene = video_scene 23 | 24 | def __getitem__(self, idx): 25 | frame_info = self.frame_infos[idx] 26 | 27 | raw_lidar_path = os.path.join(self.video_scene.raw_lidar_path, frame_info['lidar_path']) 28 | if not os.path.exists(raw_lidar_path): 29 | os.makedirs(os.path.dirname(raw_lidar_path), exist_ok=True) 30 | shutil.copy2( 31 | os.path.join(NUPLAN_SENSOR_ROOT, frame_info['lidar_path']), 32 | raw_lidar_path 33 | ) 34 | 35 | for cam_info in frame_info['cams'].values(): 36 | raw_path = os.path.join(self.video_scene.raw_image_path, cam_info['data_path']) 37 | if os.path.exists(raw_path): 38 | continue 39 | os.makedirs(os.path.dirname(raw_path), exist_ok=True) 40 | shutil.copy2( 41 | os.path.join(NUPLAN_SENSOR_ROOT, cam_info['data_path']), 42 | raw_path 43 | ) 44 | 45 | return frame_info 46 | 47 | def __len__(self): 48 | return len(self.frame_infos) 49 | 50 | @staticmethod 51 | def run( 52 | video_scene: VideoScene, 53 | video_scene_dict, 54 | num_workers): 55 | 56 | total_frame_infos = [] 57 | for video_token in video_scene_dict: 58 | frame_infos = video_scene_dict[video_token]['frame_infos'] 59 | total_frame_infos.extend(frame_infos) 60 | 61 | dataset = CollectRawData.PseudoDataset( 62 | frame_infos=total_frame_infos, 63 | video_scene=video_scene 64 | ) 65 | 66 | dataloader = DataLoader( 67 | dataset, 68 | batch_size=1, 69 | shuffle=False, 70 | num_workers=num_workers, 71 | collate_fn=lambda x: x, pin_memory=False, drop_last=False) 72 | 73 | for _ in tqdm(dataloader, desc='processing images', ncols=120, leave=False): 74 | pass 75 | 76 | if __name__ == '__main__': 77 | parser = argparse.ArgumentParser() 78 | parser.add_argument('--config', type=str, required=True) 79 | parser.add_argument('--num_workers', type=int, default=0) 80 | args = parser.parse_args() 81 | 82 | config: RoadBlockConfig = load_config(args.config) 83 | 84 | if config.collect_raw: 85 | video_scene = VideoScene(config) 86 | video_scene_dict = video_scene.load_pickle(video_scene.pickle_path) 87 | 88 | CollectRawData.run( 89 | video_scene=video_scene, 90 | video_scene_dict=video_scene_dict, 91 | num_workers=args.num_workers 92 | ) 93 | -------------------------------------------------------------------------------- /nuplan_scripts/configs/common/nuplan_path.yml: -------------------------------------------------------------------------------- 1 | NUPLAN_DATA_ROOT: "./data/nuplan/dataset/nuplan-v1.1" 2 | NUPLAN_DB_FILES: "./data/nuplan/dataset/nuplan-v1.1/splits/trainval" 3 | NUPLAN_SENSOR_ROOT: "./data/nuplan/dataset/nuplan-v1.1/sensor_blobs" 4 | NUPLAN_MAPS_ROOT: "./data/nuplan/dataset/maps" 5 | NUPLAN_MAP_VERSION: "nuplan-maps-v1.0" 6 | -------------------------------------------------------------------------------- /nuplan_scripts/configs/mtgs_exp/road_block-331220_4690660_331190_4690710.yml: -------------------------------------------------------------------------------- 1 | !!python/object:nuplan_scripts.utils.config.RoadBlockConfig 2 | city: us-ma-boston 3 | data_root: ./data/MTGS 4 | interval: 1 5 | reconstruct_buffer: 0 6 | expand_buffer: 0 7 | exclude_bad_registration: false 8 | use_colmap_ba: false 9 | collect_raw: true 10 | road_block: !!python/tuple 11 | - 331120 12 | - 4690660 13 | - 331190 14 | - 4690710 15 | road_block_name: road_block-331220_4690660_331190_4690710 16 | selected_videos: !!python/tuple 17 | - 0 18 | - 1 19 | - 2 20 | - 6 21 | - 7 22 | - 8 23 | -------------------------------------------------------------------------------- /nuplan_scripts/configs/mtgs_exp/road_block-365000_144000_365100_144080.yml: -------------------------------------------------------------------------------- 1 | !!python/object:nuplan_scripts.utils.config.RoadBlockConfig 2 | city: sg-one-north 3 | data_root: ./data/MTGS 4 | interval: 1 5 | reconstruct_buffer: 0 6 | expand_buffer: 0 7 | exclude_bad_registration: false 8 | use_colmap_ba: false 9 | collect_raw: true 10 | road_block: !!python/tuple 11 | - 365000 12 | - 144000 13 | - 365100 14 | - 144080 15 | road_block_name: road_block-365000_144000_365100_144080 16 | selected_videos: !!python/tuple 17 | - 3 18 | - 4 19 | - 5 -------------------------------------------------------------------------------- /nuplan_scripts/configs/mtgs_exp/road_block-365530_143960_365630_144060.yml: -------------------------------------------------------------------------------- 1 | !!python/object:nuplan_scripts.utils.config.RoadBlockConfig 2 | city: sg-one-north 3 | data_root: ./data/MTGS 4 | interval: 1 5 | reconstruct_buffer: 0 6 | expand_buffer: 0 7 | exclude_bad_registration: false 8 | use_colmap_ba: false 9 | collect_raw: true 10 | road_block: !!python/tuple 11 | - 365530 12 | - 143960 13 | - 365630 14 | - 144060 15 | road_block_name: road_block-365530_143960_365630_144060 16 | selected_videos: !!python/tuple 17 | - 1 18 | - 2 19 | - 5 20 | - 6 -------------------------------------------------------------------------------- /nuplan_scripts/configs/mtgs_exp/road_block-587400_4475700_587480_4475800.yml: -------------------------------------------------------------------------------- 1 | !!python/object:nuplan_scripts.utils.config.RoadBlockConfig 2 | city: us-pa-pittsburgh-hazelwood 3 | data_root: ./data/MTGS 4 | interval: 1 5 | reconstruct_buffer: 0 6 | expand_buffer: 0 7 | exclude_bad_registration: false 8 | use_colmap_ba: false 9 | collect_raw: true 10 | road_block: !!python/tuple 11 | - 587400 12 | - 4475700 13 | - 587480 14 | - 4475800 15 | road_block_name: road_block-587400_4475700_587480_4475800 16 | selected_videos: !!python/tuple 17 | - 0 18 | - 2 19 | - 23 20 | - 31 -------------------------------------------------------------------------------- /nuplan_scripts/configs/mtgs_exp/road_block-587640_4475600_587710_4475660.yml: -------------------------------------------------------------------------------- 1 | !!python/object:nuplan_scripts.utils.config.RoadBlockConfig 2 | city: us-pa-pittsburgh-hazelwood 3 | data_root: ./data/MTGS 4 | interval: 1 5 | reconstruct_buffer: 0 6 | expand_buffer: 0 7 | exclude_bad_registration: false 8 | use_colmap_ba: false 9 | collect_raw: true 10 | road_block: !!python/tuple 11 | - 587640 12 | - 4475600 13 | - 587710 14 | - 4475660 15 | road_block_name: road_block-587640_4475600_587710_4475660 16 | selected_videos: !!python/tuple 17 | - 0 18 | - 2 19 | - 3 20 | - 23 21 | - 30 22 | - 41 23 | -------------------------------------------------------------------------------- /nuplan_scripts/configs/mtgs_exp/road_block-587860_4475510_587910_4475570.yml: -------------------------------------------------------------------------------- 1 | !!python/object:nuplan_scripts.utils.config.RoadBlockConfig 2 | city: us-pa-pittsburgh-hazelwood 3 | data_root: ./data/MTGS 4 | interval: 1 5 | reconstruct_buffer: 0 6 | expand_buffer: 0 7 | exclude_bad_registration: false 8 | use_colmap_ba: false 9 | collect_raw: true 10 | road_block: !!python/tuple 11 | - 587860 12 | - 4475510 13 | - 587910 14 | - 4475570 15 | road_block_name: road_block-587860_4475510_587910_4475570 16 | selected_videos: !!python/tuple 17 | - 1 18 | - 4 19 | - 12 20 | - 16 21 | - 33 22 | - 35 -------------------------------------------------------------------------------- /nuplan_scripts/generate_semantic_mask.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import os 7 | import argparse 8 | 9 | 10 | import cv2 11 | import numpy as np 12 | from PIL import Image 13 | 14 | import torch 15 | 16 | from transformers import Mask2FormerImageProcessor, Mask2FormerForUniversalSegmentation 17 | from accelerate import Accelerator 18 | from accelerate.utils import tqdm 19 | 20 | from nuplan_scripts.utils.config import load_config, RoadBlockConfig 21 | from nuplan_scripts.utils.video_scene_dict_tools import VideoScene 22 | from nuplan_scripts.utils.constants import NUPLAN_SENSOR_ROOT 23 | 24 | model_path = "facebook/mask2former-swin-large-cityscapes-semantic" 25 | 26 | if __name__ == '__main__': 27 | parser = argparse.ArgumentParser() 28 | parser.add_argument('--config', type=str, required=True) 29 | args = parser.parse_args() 30 | 31 | config: RoadBlockConfig = load_config(args.config) 32 | 33 | video_scene = VideoScene(config) 34 | video_scene_dict = video_scene.load_pickle(video_scene.pickle_path) 35 | 36 | total_cams = [] 37 | for video_token in video_scene_dict: 38 | frame_infos = video_scene_dict[video_token]['frame_infos'] 39 | # generate semantic mask for stacking RGB point cloud 40 | frame_infos = [info for info in frame_infos if info.get('skipped', False) != 'low_velocity'] 41 | for info in frame_infos: 42 | total_cams.extend(list(info['cams'].values())) 43 | 44 | processor = Mask2FormerImageProcessor(do_resize=False) 45 | model = Mask2FormerForUniversalSegmentation.from_pretrained(model_path) 46 | distributed_state = Accelerator() 47 | 48 | distributed_state.prepare_model(model, evaluation_mode=True) 49 | model.eval() 50 | # model.to(distributed_state.device).eval() 51 | 52 | pbar = tqdm(total=len(total_cams), ncols=120, desc="Generating semantic masks") 53 | 54 | with distributed_state.split_between_processes(total_cams) as partial_frames: 55 | for cam_info in partial_frames: 56 | image_path = os.path.join(NUPLAN_SENSOR_ROOT, cam_info['data_path']) 57 | mask_path = os.path.join( 58 | video_scene.raw_mask_path, 59 | video_scene.mask_suffix_cityscape, 60 | cam_info['data_path']).replace('.jpg', '.png') 61 | 62 | image = Image.open(image_path) 63 | input = processor(images=image, return_tensors="pt").pixel_values.to(distributed_state.device) 64 | with torch.no_grad(): 65 | outputs = model(input) 66 | predicted_segmentation_map = processor.post_process_semantic_segmentation(outputs, target_sizes=[image.size[::-1]])[0] 67 | predicted_segmentation_map = predicted_segmentation_map.cpu().numpy().astype(np.uint8) 68 | 69 | os.makedirs(os.path.dirname(mask_path), exist_ok=True) 70 | cv2.imwrite(mask_path, predicted_segmentation_map) 71 | 72 | pbar.update(distributed_state.num_processes) 73 | -------------------------------------------------------------------------------- /nuplan_scripts/misc/generate_nuplan_log_info.py: -------------------------------------------------------------------------------- 1 | """ 2 | Used to generate the mapping from log name to lidar pc token. 3 | This file is designed to run with OpenScene data, which means log with sensor data only. 4 | If you don't have one, you can rewrite it with nuplan log_db, but will be slow. 5 | """ 6 | import argparse 7 | import os 8 | import jsonlines 9 | import pickle 10 | from pathlib import Path 11 | from tqdm import tqdm 12 | import numpy as np 13 | 14 | if __name__ == '__main__': 15 | parser = argparse.ArgumentParser() 16 | parser.add_argument('--openscene_dataroot', type=str, required=True) 17 | parser.add_argument('--output_dir', type=str, required=True) 18 | args = parser.parse_args() 19 | 20 | splits = ['trainval', 'test'] 21 | files = [] 22 | for split in splits: 23 | metadata_folder_path = Path(args.openscene_dataroot) / 'meta_datas' / split 24 | for file in metadata_folder_path.iterdir(): 25 | if file.is_file(): 26 | files.append(file) 27 | 28 | log2lidar_pc_token = {} 29 | for filename in tqdm(files, ncols=120): 30 | with filename.open('rb') as f: 31 | data_infos = pickle.load(f) 32 | log_name = filename.name[:-4] 33 | 34 | trajectory = np.asarray([info['ego2global_translation'] for info in data_infos])[:, :2] 35 | trajectory = np.round(trajectory, 2).tolist() 36 | 37 | log2lidar_pc_token[log_name] = { 38 | "log_name": log_name, 39 | "log_token": data_infos[0]['log_token'], 40 | "city": data_infos[0]['map_location'], 41 | "split": filename.parent.name, 42 | "lidar_pc_tokens": [info['token'] for info in data_infos], 43 | "trajectory": trajectory 44 | } 45 | 46 | with jsonlines.open(os.path.join(args.output_dir, 'nuplan_log_infos.jsonl'), 'w') as writer: 47 | writer.write_all(log2lidar_pc_token.values()) 48 | -------------------------------------------------------------------------------- /nuplan_scripts/preprocess.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CONFIG=$1 4 | NUM_WORKERS=$2 5 | NUM_GPUS=$3 6 | STEP=${4:-1} 7 | PORT=${5:-29500} 8 | 9 | print_banner() { 10 | echo "==================================" 11 | echo "Executing: $1" 12 | echo "==================================" 13 | } 14 | 15 | run_command() { 16 | if [ $STEP -le $1 ]; then 17 | print_banner "$2" 18 | eval "$2" 19 | if [ $? -ne 0 ]; then 20 | exit 1 21 | fi 22 | fi 23 | } 24 | 25 | command_list=( 26 | "python -m nuplan_scripts.nuplan_video_processing --config $CONFIG --prefilter --num_workers $NUM_WORKERS" 27 | 28 | "python -m nuplan_scripts.export_videos --config $CONFIG --num_workers $NUM_WORKERS" 29 | 30 | "python -m nuplan_scripts.collect_raw_data --config $CONFIG --num_workers $NUM_WORKERS" 31 | 32 | "accelerate launch --num_processes $NUM_GPUS --main_process_port $PORT -m nuplan_scripts.generate_semantic_mask --config $CONFIG" 33 | 34 | "python -m nuplan_scripts.lidar_registration_multi_traversal --config $CONFIG" 35 | 36 | "python -m nuplan_scripts.ba_multi_traversal --config $CONFIG" 37 | 38 | "accelerate launch --num_processes $NUM_GPUS --main_process_port $PORT -m nuplan_scripts.generate_dense_depth --config $CONFIG --num_workers $NUM_WORKERS" 39 | 40 | "python -m nuplan_scripts.stack_RGB_point_cloud --config $CONFIG --num_workers $NUM_WORKERS" 41 | ) 42 | 43 | for i in $(seq 1 ${#command_list[@]}); do 44 | run_command $i "${command_list[$i - 1]}" 45 | done 46 | -------------------------------------------------------------------------------- /nuplan_scripts/preprocess_stage_1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CONFIG=$1 4 | NUM_WORKERS=$2 5 | 6 | print_banner() { 7 | echo "==================================" 8 | echo "Executing: $1" 9 | echo "==================================" 10 | } 11 | 12 | run_command() { 13 | print_banner "$1" 14 | eval "$1" 15 | if [ $? -ne 0 ]; then 16 | exit 1 17 | fi 18 | } 19 | 20 | run_command "python -m nuplan_scripts.nuplan_video_processing --config $CONFIG --num_workers $NUM_WORKERS" 21 | 22 | run_command "python -m nuplan_scripts.export_videos --config $CONFIG --num_workers $NUM_WORKERS" 23 | -------------------------------------------------------------------------------- /nuplan_scripts/preprocess_stage_2.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CONFIG=$1 4 | NUM_WORKERS=$2 5 | NUM_GPUS=$3 6 | PORT=${4:-29500} 7 | 8 | print_banner() { 9 | echo "==================================" 10 | echo "Executing: $1" 11 | echo "==================================" 12 | } 13 | 14 | run_command() { 15 | print_banner "$1" 16 | eval "$1" 17 | if [ $? -ne 0 ]; then 18 | exit 1 19 | fi 20 | } 21 | 22 | run_command "python -m nuplan_scripts.filter_trajectory --config $CONFIG" 23 | 24 | run_command "python -m nuplan_scripts.collect_raw_data --config $CONFIG --num_workers $NUM_WORKERS" 25 | 26 | run_command "accelerate launch --num_processes $NUM_GPUS --main_process_port $PORT -m nuplan_scripts.generate_semantic_mask --config $CONFIG" 27 | 28 | run_command "python -m nuplan_scripts.lidar_registration_multi_traversal --config $CONFIG" 29 | 30 | run_command "python -m nuplan_scripts.ba_multi_traversal --config $CONFIG" 31 | 32 | run_command "accelerate launch --num_processes $NUM_GPUS --main_process_port $PORT -m nuplan_scripts.generate_dense_depth --config $CONFIG --num_workers $NUM_WORKERS" 33 | 34 | run_command "python -m nuplan_scripts.stack_RGB_point_cloud --config $CONFIG --num_workers $NUM_WORKERS" 35 | -------------------------------------------------------------------------------- /nuplan_scripts/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/nuplan_scripts/utils/__init__.py -------------------------------------------------------------------------------- /nuplan_scripts/utils/colmap_utils/align_model.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import numpy as np 7 | 8 | def compute_transformation_matrix_with_scaling(source_points, target_points): 9 | assert source_points.shape == target_points.shape 10 | 11 | centroid_source = np.mean(source_points, axis=0) 12 | centroid_target = np.mean(target_points, axis=0) 13 | 14 | source_centered = source_points - centroid_source 15 | target_centered = target_points - centroid_target 16 | 17 | H = source_centered.T @ target_centered 18 | 19 | U, S, Vt = np.linalg.svd(H) 20 | 21 | R = Vt.T @ U.T 22 | 23 | if np.linalg.det(R) < 0: 24 | Vt[-1, :] *= -1 25 | R = Vt.T @ U.T 26 | scale = np.sum(S) / np.sum(source_centered ** 2) 27 | 28 | t = centroid_target.T - (R * scale) @ centroid_source.T 29 | 30 | return scale, R, t 31 | 32 | def compute_transformation_matrix_without_scaling(source_points, target_points): 33 | assert source_points.shape == target_points.shape 34 | 35 | centroid_source = np.mean(source_points, axis=0) 36 | centroid_target = np.mean(target_points, axis=0) 37 | 38 | source_centered = source_points - centroid_source 39 | target_centered = target_points - centroid_target 40 | 41 | H = source_centered.T @ target_centered 42 | 43 | U, S, Vt = np.linalg.svd(H) 44 | 45 | R = Vt.T @ U.T 46 | 47 | if np.linalg.det(R) < 0: 48 | Vt[-1, :] *= -1 49 | R = Vt.T @ U.T 50 | 51 | t = centroid_target.T - R @ centroid_source.T 52 | 53 | return R, t 54 | -------------------------------------------------------------------------------- /nuplan_scripts/utils/colmap_utils/gen_colmap_db.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import argparse 7 | import numpy as np 8 | from pyquaternion import Quaternion 9 | 10 | from nuplan_scripts.utils.colmap_utils.database import COLMAPDatabase 11 | 12 | def create_colmap_database(colmap_path, input_model='sparse_model', camera_type='OPENCV'): 13 | camera_type = 4 if camera_type == 'OPENCV' else 1 14 | 15 | colmap_db = COLMAPDatabase.connect(f"{colmap_path}/database.db") 16 | colmap_db.create_tables() 17 | for cameras in open(f"{colmap_path}/{input_model}/cameras.txt", "r").readlines(): 18 | cameras = cameras.strip().split() 19 | colmap_db.add_camera( 20 | camera_id=int(cameras[0]), 21 | model=camera_type, # 1 for PINHOLE, 4 for OPENCV 22 | width=int(cameras[2]), 23 | height=int(cameras[3]), 24 | params=cameras[4:], 25 | prior_focal_length=True 26 | ) 27 | for image in open(f"{colmap_path}/{input_model}/images.txt", "r").readlines()[::2]: 28 | image = image.strip().split() 29 | colmap_db.add_image( 30 | name=image[-1], 31 | camera_id=int(image[-2]), 32 | image_id=int(image[0]) 33 | ) 34 | 35 | prior_position = -Quaternion(*image[1:5]).rotation_matrix.T @ np.array(image[5:8], dtype=np.float64) 36 | colmap_db.add_pose_prior( 37 | image_id=int(image[0]), 38 | position=prior_position, 39 | coordinate_system=1 # 1 for CARTESIAN 40 | ) 41 | 42 | colmap_db.commit() 43 | colmap_db.close() 44 | 45 | if __name__ == '__main__': 46 | parser = argparse.ArgumentParser() 47 | parser.add_argument('--colmap_path', type=str, help='Input colmap dir', required=True) 48 | parser.add_argument('--input_model', type=str, default='sparse_model') 49 | args = parser.parse_args() 50 | 51 | create_colmap_database(args.colmap_path, args.input_model) 52 | -------------------------------------------------------------------------------- /nuplan_scripts/utils/colmap_utils/point_triangulator.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import os 7 | import sys 8 | import shutil 9 | import subprocess 10 | import argparse 11 | import platform 12 | 13 | def run_point_triangulator( 14 | colmap_path, 15 | input_model='sparse_model', 16 | output_model='sfm_model', 17 | num_workers=8 18 | ): 19 | 20 | colmap_exe = "colmap.bat" if platform.system() == "Windows" else "colmap" 21 | 22 | print("extracting features...") 23 | colmap_feature_extractor_args = [ 24 | colmap_exe, "feature_extractor", 25 | "--database_path", f"{colmap_path}/database.db", 26 | "--image_path", f"{colmap_path}/images", 27 | ] 28 | if os.path.exists(f"{colmap_path}/masks"): 29 | colmap_feature_extractor_args += ["--ImageReader.mask_path", f"{colmap_path}/masks"] 30 | colmap_feature_extractor_args += ["--SiftExtraction.num_threads", f"{num_workers}"] 31 | 32 | try: 33 | subprocess.run(colmap_feature_extractor_args, check=True) 34 | except subprocess.CalledProcessError as e: 35 | print(f"Error executing colmap feature_extractor: {e}") 36 | sys.exit(1) 37 | 38 | 39 | print("feature matching...") 40 | colmap_matches_importer_args = [ 41 | colmap_exe, "matches_importer", 42 | "--database_path", f"{colmap_path}/database.db", 43 | "--match_list_path", f"{colmap_path}/image_pairs.txt", 44 | "--SiftMatching.num_threads", f"{num_workers}" 45 | ] 46 | 47 | try: 48 | subprocess.run(colmap_matches_importer_args, check=True) 49 | except subprocess.CalledProcessError as e: 50 | print(f"Error executing colmap matches_importer: {e}") 51 | sys.exit(1) 52 | 53 | if os.path.exists(os.path.join(colmap_path, output_model)): 54 | shutil.rmtree(os.path.join(colmap_path, output_model)) 55 | 56 | os.makedirs(os.path.join(colmap_path, output_model, "0"), exist_ok=True) 57 | 58 | colmap_point_triangulator_args = [ 59 | colmap_exe, "point_triangulator", 60 | "--refine_intrinsics", "1", 61 | "--Mapper.num_threads", f"{num_workers}", 62 | "--Mapper.ba_global_function_tolerance", "0.000001", 63 | "--Mapper.ba_global_max_num_iterations", "30", 64 | "--Mapper.ba_global_max_refinements", "3", 65 | ] 66 | 67 | # point triangulation 68 | try: 69 | subprocess.run(colmap_point_triangulator_args + [ 70 | "--database_path", f"{colmap_path}/database.db", 71 | "--image_path", f"{colmap_path}/images", 72 | "--input_path", f"{colmap_path}/{input_model}", 73 | "--output_path", f"{colmap_path}/{output_model}/0", 74 | ], check=True) 75 | except subprocess.CalledProcessError as e: 76 | print(f"Error executing colmap_point_triangulator_args: {e}") 77 | sys.exit(1) 78 | 79 | 80 | if __name__ == '__main__': 81 | 82 | parser = argparse.ArgumentParser() 83 | parser.add_argument('--colmap_path', type=str, help='Input colmap dir', required=True) 84 | parser.add_argument('--input_model', type=str, default='sparse_model') 85 | parser.add_argument('--output_model', type=str, default='sfm_model') 86 | parser.add_argument('--num_workers', type=int, default=8) 87 | args = parser.parse_args() 88 | 89 | colmap_path = args.colmap_path 90 | 91 | run_point_triangulator(colmap_path, args.input_model, args.output_model, args.num_workers) 92 | -------------------------------------------------------------------------------- /nuplan_scripts/utils/config.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import os 7 | import importlib 8 | import yaml 9 | 10 | from dataclasses import dataclass, field 11 | from pathlib import Path 12 | from typing import List, Literal, Tuple, Union 13 | 14 | 15 | class BaseConfig: 16 | 17 | def save_config(self, path: str): 18 | path = Path(path) 19 | path.write_text(yaml.dump(self), "utf8") 20 | 21 | @staticmethod 22 | def load_from(path: str): 23 | path = Path(path) 24 | return yaml.load(path.read_text(), Loader=yaml.Loader) 25 | 26 | def __str__(self): 27 | lines = [self.__class__.__name__ + ":"] 28 | for key, val in vars(self).items(): 29 | if isinstance(val, Tuple): 30 | flattened_val = "[" 31 | for item in val: 32 | flattened_val += str(item) + "\n" 33 | flattened_val = flattened_val.rstrip("\n") 34 | val = flattened_val + "]" 35 | lines += f"{key}: {str(val)}".split("\n") 36 | return "\n ".join(lines) 37 | 38 | @dataclass 39 | class RoadBlockConfig(BaseConfig): 40 | 41 | road_block_name: str 42 | 43 | road_block: Tuple 44 | 45 | city: Literal['sg-one-north', 'us-ma-boston', 'us-na-las-vegas-strip', 'us-pa-pittsburgh-hazelwood'] 46 | 47 | data_root: str = "./data/MTGS" 48 | 49 | interval: int = 1 50 | """interval = 1 -> 10Hz """ 51 | 52 | expand_buffer: int = 0 53 | """ 54 | expand the trajectory by expand_buffer meters 55 | those lidar_pcs will be used for lidar registration and stacking lidar points 56 | """ 57 | 58 | reconstruct_buffer: int = 0 59 | """ 60 | buffer reconstruct_buffer meters for better in range reconstruction. 61 | those frames will be used for reconstruction, including caching images, lidar, semantic map, etc. 62 | """ 63 | 64 | selected_videos: Tuple = field(default_factory=tuple) 65 | """tuple of video_idx or dict of video_idx and start_frame, end_frame """ 66 | 67 | split: Literal['trainval', 'test', 'all'] = 'trainval' 68 | """data source of nuplan split""" 69 | 70 | collect_raw: bool = False 71 | """ 72 | collect raw data from nuplan sensor root to data_root 73 | """ 74 | 75 | exclude_bad_registration: bool = True 76 | """ 77 | exclude videos with bad lidar registration result 78 | """ 79 | 80 | use_colmap_ba: bool = False 81 | """ 82 | use colmap bundle adjustment to refine the registration result 83 | """ 84 | 85 | @dataclass 86 | class FrmaeCentralConfig(RoadBlockConfig): 87 | central_log: str = "" 88 | """nuplan log name""" 89 | 90 | central_tokens: List[str] = field(default_factory=list) 91 | """list of central tokens""" 92 | 93 | multi_traversal_mode: Literal['reconstruction', 'off'] = 'off' 94 | 95 | def load_config(config_path: str) -> Union[RoadBlockConfig, FrmaeCentralConfig]: 96 | 97 | if config_path.endswith('.py'): 98 | module_name = os.path.splitext(os.path.basename(config_path))[0] 99 | spec = importlib.util.spec_from_file_location(module_name, config_path) 100 | config = importlib.util.module_from_spec(spec) 101 | spec.loader.exec_module(config) 102 | config = config.config 103 | elif config_path.endswith('.yml') or config_path.endswith('.yaml'): 104 | config = BaseConfig.load_from(config_path) 105 | else: 106 | raise NotImplementedError(f"not supported config file suffix: .{config_path.split('.')[-1]}") 107 | 108 | return config 109 | -------------------------------------------------------------------------------- /nuplan_scripts/utils/constants.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------------------------------------------------# 2 | # MTGS: Multi-Traversal Gaussian Splatting (https://arxiv.org/abs/2503.12552) # 3 | # Source code: https://github.com/OpenDriveLab/MTGS # 4 | # Copyright (c) OpenDriveLab. All rights reserved. # 5 | #-------------------------------------------------------------------------------# 6 | import os 7 | import yaml 8 | import pytz 9 | from rich.console import Console 10 | 11 | CONSOLE = Console(width=120) 12 | os.environ['TQDM_NCOLS'] = '120' 13 | 14 | # load nuplan config 15 | nuplan_config_path = os.path.join(os.path.dirname(__file__), '..', 'configs', 'common', 'nuplan_path.yml') 16 | nuplan_config = yaml.load(open(nuplan_config_path, 'r'), Loader=yaml.FullLoader) 17 | NUPLAN_DATA_ROOT = nuplan_config['NUPLAN_DATA_ROOT'] 18 | NUPLAN_DB_FILES = nuplan_config['NUPLAN_DB_FILES'] 19 | NUPLAN_SENSOR_ROOT = nuplan_config['NUPLAN_SENSOR_ROOT'] 20 | NUPLAN_MAP_VERSION = nuplan_config['NUPLAN_MAP_VERSION'] 21 | NUPLAN_MAPS_ROOT = nuplan_config['NUPLAN_MAPS_ROOT'] 22 | 23 | cityscape_label = { 24 | 'road': 0, 25 | 'sidewalk': 1, 26 | 'building': 2, 27 | 'wall': 3, 28 | 'fence': 4, 29 | 'pole': 5, 30 | 'traffic light': 6, 31 | 'traffic sign': 7, 32 | 'vegetation': 8, 33 | 'terrain': 9, 34 | 'sky': 10, 35 | 'person': 11, 36 | 'rider': 12, 37 | 'car': 13, 38 | 'truck': 14, 39 | 'bus': 15, 40 | 'train': 16, 41 | 'motorcycle': 17, 42 | 'bicycle': 18 43 | } 44 | 45 | cityscape_colormap = { 46 | 'person': (220, 20, 60), # Red 47 | 'car': (0, 0, 142), # Dark blue 48 | 'truck': (0, 0, 70), # Darker blue 49 | 'bus': (0, 60, 100), # Blue 50 | 'rider': (255, 0, 0), # Bright red 51 | 'motorcycle': (0, 0, 230), # Bright blue 52 | 'bicycle': (119, 11, 32), # Dark red 53 | } 54 | 55 | NUPLAN_ACCEPTABLE_CITYSCAPE_LABELS = { 56 | "vehicle": [13, 14, 15], 57 | "bicycle": [12, 17, 18], 58 | "pedestrian": [11], 59 | "traffic cone": [5], 60 | "barrier": [], 61 | "construction zone sign": [], 62 | "generic object": [], 63 | "background": [] 64 | } 65 | 66 | NUPLAN_TIMEZONE = { 67 | 'us-ma-boston': pytz.timezone('America/New_York'), 68 | 'sg-one-north': pytz.timezone('Asia/Singapore'), 69 | 'us-nv-las-vegas-strip': pytz.timezone('America/Los_Angeles'), 70 | 'us-pa-pittsburgh-hazelwood': pytz.timezone('US/Eastern'), 71 | } 72 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "mtgs" 3 | description = "Official code for MTGS: Multi-Traversal Gaussian Splatting." 4 | version = "1.0.0" 5 | dependencies = [] 6 | readme = "README.md" 7 | license = {text="Apache 2.0"} 8 | 9 | [tool.setuptools.packages.find] 10 | include = ["mtgs*"] 11 | 12 | [tool.ruff] 13 | line-length = 120 14 | lint.ignore = [ 15 | "E501", # Line too long. 16 | "F722", # Forward annotation false positive from jaxtyping. Should be caught by pyright. 17 | "F821", # Forward annotation false positive from jaxtyping. Should be caught by pyright. 18 | "PLR2004", # Magic value used in comparison. 19 | "PLR0915", # Too many statements. 20 | "PLR0913", # Too many arguments. 21 | "PLC0414", # Import alias does not rename variable. (this is used for exporting names) 22 | "PLC1901", # Use falsey strings. 23 | "PLR5501", # Use `elif` instead of `else if`. 24 | "PLR0911", # Too many return statements. 25 | "PLR0912", # Too many branches. 26 | "PLW0603", # Globa statement updates are discouraged. 27 | "PLW2901", # For loop variable overwritten. 28 | "PLR1730", # Replace if statement with min/max 29 | "PLC0206", # Extracting value from dictionary without calling `.items()` 30 | ] 31 | 32 | [project.entry-points.'nerfstudio.method_configs'] 33 | mtgs = "mtgs.config.MTGS:method" 34 | 35 | [project.entry-points.'nerfstudio.dataparser_configs'] 36 | nuplan = "mtgs.config.nuplan_dataparser:nuplan_dataparser" 37 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.26.3 2 | tensorly==0.8.1 3 | rawpy==0.21.0 4 | kornia==0.7.2 5 | setuptools==69.5.1 6 | urllib3==1.26.20 7 | tyro==0.8.4 8 | typeguard==2.13.3 9 | jaxtyping==0.2.30 10 | ninja==1.11.1.1 11 | git+https://github.com/NVlabs/tiny-cuda-nn/#subdirectory=bindings/torch 12 | git+https://github.com/nerfstudio-project/gsplat.git@v1.4.0 # install gsplat from source to pre-compile it, instead of JIT. 13 | nerfstudio==1.1.5 -------------------------------------------------------------------------------- /requirements_data.txt: -------------------------------------------------------------------------------- 1 | numpy==1.26.3 2 | tensorly==0.8.1 3 | rawpy==0.21.0 4 | kornia==0.7.2 5 | setuptools==69.5.1 6 | xformers==0.0.22 7 | transformers==4.44.0 8 | accelerate==0.32.0 9 | einops==0.7.0 10 | timm==1.0.9 11 | streamlit==1.38.0 12 | urllib3==1.26.20 13 | tyro==0.8.4 14 | typeguard==2.13.3 15 | jaxtyping==0.2.30 16 | ninja==1.11.1.1 17 | prettytable==3.10.2 18 | jsonlines==4.0.0 -------------------------------------------------------------------------------- /thirdparty/UniDepth/.gitignore: -------------------------------------------------------------------------------- 1 | # don't upload macOS folder info 2 | *.DS_Store 3 | 4 | #python 5 | *.pyc 6 | __pycache__/ 7 | 8 | #scripts 9 | *.sh 10 | 11 | # package 12 | unidepth.egg-info 13 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/configs/config_v1_cnvnxtl.json: -------------------------------------------------------------------------------- 1 | { 2 | "generic": { 3 | "seed": 13 4 | }, 5 | "training": { 6 | }, 7 | "data": { 8 | "image_shape": [462, 616] 9 | }, 10 | "model": { 11 | "name": "UniDepthV1", 12 | "num_heads": 8, 13 | "expansion": 4, 14 | "pixel_decoder": { 15 | "hidden_dim": 512, 16 | "depths": [3, 2, 1], 17 | "dropout": 0.0 18 | }, 19 | "pixel_encoder": { 20 | "name": "convnext_large", 21 | "pretrained": null 22 | } 23 | } 24 | } -------------------------------------------------------------------------------- /thirdparty/UniDepth/configs/config_v1_vitl14.json: -------------------------------------------------------------------------------- 1 | { 2 | "generic": { 3 | "seed": 13 4 | }, 5 | "training": {}, 6 | "data": { 7 | "image_shape": [462, 616] 8 | }, 9 | "model": { 10 | "name": "UniDepthV1", 11 | "num_heads": 8, 12 | "expansion": 4, 13 | "pixel_decoder": { 14 | "hidden_dim": 512, 15 | "depths": [3, 2, 1], 16 | "dropout": 0.0 17 | }, 18 | "pixel_encoder": { 19 | "name": "dinov2_vitl14", 20 | "pretrained": null 21 | } 22 | } 23 | } -------------------------------------------------------------------------------- /thirdparty/UniDepth/configs/config_v2_vitl14.json: -------------------------------------------------------------------------------- 1 | { 2 | "generic": { 3 | "seed": 13, 4 | "deterministic": true 5 | }, 6 | "training": {}, 7 | "data": { 8 | "image_shape": [420, 560], 9 | "shape_constraints": { 10 | "ratio_bounds": [0.66, 2.0], 11 | "pixels_bounds": [1400, 2400], 12 | "patch_size": 14 13 | } 14 | }, 15 | "model": { 16 | "name": "UniDepthV2", 17 | "num_heads": 8, 18 | "expansion": 4, 19 | "pixel_decoder": { 20 | "hidden_dim": 512, 21 | "depths": [6, 0, 0], 22 | "dropout": 0.0 23 | }, 24 | "pixel_encoder": { 25 | "name": "dinov2_vitl14", 26 | "pretrained": null, 27 | "use_norm": true, 28 | "stacking_fn": "last", 29 | "output_idx": [21,22,23,24] 30 | } 31 | } 32 | } -------------------------------------------------------------------------------- /thirdparty/UniDepth/configs/config_v2_vits14.json: -------------------------------------------------------------------------------- 1 | { 2 | "generic": { 3 | "seed": 13, 4 | "deterministic": true 5 | }, 6 | "training": {}, 7 | "data": { 8 | "image_shape": [420, 560], 9 | "shape_constraints": { 10 | "ratio_bounds": [0.66, 2.0], 11 | "pixels_bounds": [1400, 2400], 12 | "patch_size": 14 13 | } 14 | }, 15 | "model": { 16 | "name": "UniDepthV2", 17 | "num_heads": 8, 18 | "expansion": 4, 19 | "pixel_decoder": { 20 | "hidden_dim": 512, 21 | "depths": [6, 0, 0], 22 | "dropout": 0.0 23 | }, 24 | "pixel_encoder": { 25 | "name": "dinov2_vits14", 26 | "pretrained": null, 27 | "use_norm": true, 28 | "stacking_fn": "last", 29 | "output_idx": [9,10,11,12] 30 | } 31 | } 32 | } -------------------------------------------------------------------------------- /thirdparty/UniDepth/hubconf.py: -------------------------------------------------------------------------------- 1 | dependencies = ["torch", "huggingface_hub"] 2 | 3 | import os 4 | import json 5 | 6 | import torch 7 | import huggingface_hub 8 | 9 | from unidepth.models import UniDepthV1, UniDepthV2 10 | 11 | 12 | MAP_VERSIONS = { 13 | "v1": UniDepthV1, 14 | "v2": UniDepthV2 15 | } 16 | 17 | BACKBONES = { 18 | "v1": ["vitl14", "cnvnxtl"], 19 | "v2": ["vitl14", "vits14"] 20 | } 21 | 22 | 23 | def UniDepth(version="v2", backbone="vitl14", pretrained=True): 24 | assert version in MAP_VERSIONS.keys(), f"version must be one of {list(MAP_VERSIONS.keys())}" 25 | assert backbone in BACKBONES[version], f"backbone for current version ({version}) must be one of {list(BACKBONES[version])}" 26 | repo_dir = os.path.dirname(os.path.realpath(__file__)) 27 | with open(os.path.join(repo_dir, "configs", f"config_{version}_{backbone}.json")) as f: 28 | config = json.load(f) 29 | 30 | model = MAP_VERSIONS[version](config) 31 | if pretrained: 32 | path = huggingface_hub.hf_hub_download(repo_id=f"lpiccinelli/unidepth-{version}-{backbone}", filename=f"pytorch_model.bin", repo_type="model") 33 | info = model.load_state_dict(torch.load(path), strict=False) 34 | print(f"UniDepth_{version}_{backbone} is loaded with:") 35 | print(f"\t missing keys: {info.missing_keys}") 36 | print(f"\t additional keys: {info.unexpected_keys}") 37 | 38 | return model 39 | 40 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [tool.pyright] 6 | include = ["unidepth"] 7 | 8 | [project] 9 | name = "unidepth" 10 | version = "0.1" 11 | authors = [{name = "Luigi Piccinelli", email = "lpiccinelli@ethz.ch"}] 12 | description = "UniDepth: Universal Monocular Metric Depth Estimation" 13 | readme = "README.md" 14 | license = { text="Creatives Common BY-NC 4.0 license"} 15 | requires-python = ">=3.9.0" 16 | 17 | [tool.setuptools.package-data] 18 | "*" = ["py.typed"] 19 | 20 | [tool.setuptools.packages.find] 21 | include = ["unidepth*"] 22 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/requirements.txt: -------------------------------------------------------------------------------- 1 | appdirs 2 | attrs 3 | black 4 | blosc2 5 | certifi==2022.12.7 6 | charset-normalizer 7 | click 8 | contourpy 9 | cycler 10 | docker-pycreds 11 | einops==0.7.0 12 | filelock 13 | flake8==7.0.0 14 | flake8-bugbear==24.2.6 15 | flake8-comprehensions==3.14.0 16 | fonttools 17 | fsspec 18 | fvcore==0.1.5.post20221221 19 | gitdb 20 | GitPython 21 | h5py>=3.10.0 22 | huggingface-hub>=0.22.0 23 | idna 24 | imageio 25 | imath 26 | iopath 27 | isort 28 | Jinja2 29 | jmespath 30 | kiwisolver 31 | MarkupSafe 32 | matplotlib 33 | mccabe 34 | mpmath 35 | msgpack 36 | mypy-extensions 37 | ndindex 38 | networkx 39 | ninja 40 | numexpr 41 | numpy<2.0.0 42 | opencv-python 43 | OpenEXR 44 | packaging 45 | pandas 46 | pathspec 47 | pillow==10.3.0 48 | platformdirs 49 | portalocker 50 | protobuf==3.20.3 51 | psutil 52 | py-cpuinfo 53 | pycodestyle 54 | pyflakes 55 | pyparsing 56 | python-dateutil 57 | pytz 58 | PyYAML 59 | requests 60 | safetensors 61 | scipy 62 | sentry-sdk 63 | setproctitle 64 | six 65 | smmap 66 | sympy 67 | tables 68 | tabulate 69 | termcolor 70 | timm==1.0.9 71 | triton==2.0.0 72 | typing_extensions 73 | tzdata==2024.1 74 | urllib3==1.26.13 75 | yacs -------------------------------------------------------------------------------- /thirdparty/UniDepth/scripts/demo.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from PIL import Image 4 | 5 | from unidepth.models import UniDepthV1, UniDepthV2 6 | from unidepth.utils import colorize, image_grid 7 | 8 | 9 | def demo(model): 10 | rgb = np.array(Image.open("assets/demo/rgb.png")) 11 | rgb_torch = torch.from_numpy(rgb).permute(2, 0, 1) 12 | intrinsics_torch = torch.from_numpy(np.load("assets/demo/intrinsics.npy")) 13 | 14 | # predict 15 | predictions = model.infer(rgb_torch, intrinsics_torch) 16 | 17 | # get GT and pred 18 | depth_pred = predictions["depth"].squeeze().cpu().numpy() 19 | depth_gt = np.array(Image.open("assets/demo/depth.png")).astype(float) / 1000.0 20 | 21 | # compute error, you have zero divison where depth_gt == 0.0 22 | depth_arel = np.abs(depth_gt - depth_pred) / depth_gt 23 | depth_arel[depth_gt == 0.0] = 0.0 24 | 25 | # colorize 26 | depth_pred_col = colorize(depth_pred, vmin=0.01, vmax=10.0, cmap="magma_r") 27 | depth_gt_col = colorize(depth_gt, vmin=0.01, vmax=10.0, cmap="magma_r") 28 | depth_error_col = colorize(depth_arel, vmin=0.0, vmax=0.2, cmap="coolwarm") 29 | 30 | # save image with pred and error 31 | artifact = image_grid([rgb, depth_gt_col, depth_pred_col, depth_error_col], 2, 2) 32 | Image.fromarray(artifact).save("assets/demo/output.png") 33 | 34 | print("Available predictions:", list(predictions.keys())) 35 | print(f"ARel: {depth_arel[depth_gt > 0].mean() * 100:.2f}%") 36 | 37 | 38 | if __name__ == "__main__": 39 | print("Torch version:", torch.__version__) 40 | name = "unidepth-v2-vitl14" 41 | # model = UniDepthV1.from_pretrained("lpiccinelli/unidepth-v1-vitl14") 42 | model = UniDepthV2.from_pretrained(f"lpiccinelli/{name}") 43 | 44 | # set resolution level (only V2) 45 | # model.resolution_level = 0 46 | 47 | # set interpolation mode (only V2) 48 | # model.interpolation_mode = "bilinear" 49 | 50 | device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 51 | model = model.to(device) 52 | demo(model) 53 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/layers/__init__.py: -------------------------------------------------------------------------------- 1 | from .activation import GEGLU, SwiGLU 2 | from .attention import AttentionBlock, AttentionDecoderBlock 3 | from .convnext import CvnxtBlock 4 | from .mlp import MLP 5 | from .nystrom_attention import NystromBlock 6 | from .positional_encoding import PositionEmbeddingSine 7 | from .upsample import (ConvUpsample, ConvUpsampleShuffle, 8 | ConvUpsampleShuffleResidual) 9 | 10 | __all__ = [ 11 | "SwiGLU", 12 | "GEGLU", 13 | "CvnxtBlock", 14 | "AttentionBlock", 15 | "NystromBlock", 16 | "PositionEmbeddingSine", 17 | "ConvUpsample", 18 | "MLP", 19 | "ConvUpsampleShuffle", 20 | "AttentionDecoderBlock", 21 | "ConvUpsampleShuffleResidual", 22 | ] 23 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/layers/activation.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.functional as F 4 | 5 | 6 | class SwiGLU(nn.Module): 7 | def forward(self, x: torch.Tensor) -> torch.Tensor: 8 | x, gates = x.chunk(2, dim=-1) 9 | return x * F.silu(gates) 10 | 11 | 12 | class GEGLU(nn.Module): 13 | def forward(self, x: torch.Tensor) -> torch.Tensor: 14 | x, gates = x.chunk(2, dim=-1) 15 | return x * F.gelu(gates) 16 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/layers/convnext.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | 4 | 5 | class CvnxtBlock(nn.Module): 6 | def __init__( 7 | self, 8 | dim, 9 | kernel_size=7, 10 | layer_scale=1.0, 11 | expansion=4, 12 | dilation=1, 13 | padding_mode: str = "zeros", 14 | ): 15 | super().__init__() 16 | self.dwconv = nn.Conv2d( 17 | dim, 18 | dim, 19 | kernel_size=kernel_size, 20 | padding=dilation * (kernel_size - 1) // 2, 21 | groups=dim, 22 | dilation=dilation, 23 | padding_mode=padding_mode, 24 | ) # depthwise conv 25 | self.norm = nn.LayerNorm(dim) 26 | self.pwconv1 = nn.Linear(dim, expansion * dim) 27 | self.act = nn.GELU() 28 | self.pwconv2 = nn.Linear(expansion * dim, dim) 29 | self.gamma = ( 30 | nn.Parameter(layer_scale * torch.ones((dim))) if layer_scale > 0.0 else 1.0 31 | ) 32 | 33 | def forward(self, x): 34 | input = x 35 | x = self.dwconv(x) 36 | x = x.permute(0, 2, 3, 1) # (N, C, H, W) -> (N, H, W, C) 37 | x = self.norm(x) 38 | x = self.pwconv1(x) 39 | x = self.act(x) 40 | x = self.pwconv2(x) 41 | 42 | x = self.gamma * x 43 | x = input + x.permute(0, 3, 1, 2) # (N, H, W, C) -> (N, C, H, W) 44 | return x 45 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/layers/drop_path.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | 4 | 5 | def drop_path(x: torch.Tensor, drop_prob: float = 0.0, training: bool = False): 6 | if drop_prob == 0.0 or not training: 7 | return x 8 | keep_prob = 1 - drop_prob 9 | shape = (x.shape[0],) + (1,) * ( 10 | x.ndim - 1 11 | ) # work with diff dim tensors, not just 2D ConvNets 12 | random_tensor = x.new_empty(shape).bernoulli_(keep_prob) 13 | if keep_prob > 0.0: 14 | random_tensor.div_(keep_prob) 15 | output = x * random_tensor 16 | return output 17 | 18 | 19 | class DropPath(nn.Module): 20 | def __init__(self, drop_prob=None): 21 | super(DropPath, self).__init__() 22 | self.drop_prob = drop_prob 23 | 24 | def forward(self, x): 25 | return drop_path(x, self.drop_prob, self.training) 26 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/layers/layer_scale.py: -------------------------------------------------------------------------------- 1 | from typing import Union 2 | 3 | import torch 4 | import torch.nn as nn 5 | 6 | 7 | class LayerScale(nn.Module): 8 | def __init__( 9 | self, 10 | dim: int, 11 | init_values: Union[float, torch.Tensor] = 1e-5, 12 | inplace: bool = False, 13 | ) -> None: 14 | super().__init__() 15 | self.inplace = inplace 16 | self.gamma = nn.Parameter(init_values * torch.ones(dim)) 17 | 18 | def forward(self, x: torch.Tensor) -> torch.Tensor: 19 | return x.mul_(self.gamma) if self.inplace else x * self.gamma 20 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/layers/mlp.py: -------------------------------------------------------------------------------- 1 | from typing import Optional 2 | 3 | import torch 4 | import torch.nn as nn 5 | 6 | from unidepth.utils.misc import default 7 | 8 | from .activation import SwiGLU 9 | 10 | 11 | class MLP(nn.Module): 12 | def __init__( 13 | self, 14 | input_dim: int, 15 | expansion: int = 4, 16 | dropout: float = 0.0, 17 | gated: bool = False, 18 | output_dim: Optional[int] = None, 19 | ): 20 | super().__init__() 21 | if gated: 22 | expansion = int(expansion * 2 / 3) 23 | hidden_dim = int(input_dim * expansion) 24 | output_dim = default(output_dim, input_dim) 25 | self.norm = nn.LayerNorm(input_dim) 26 | self.proj1 = nn.Linear(input_dim, hidden_dim) 27 | self.proj2 = nn.Linear(hidden_dim, output_dim) 28 | self.act = nn.GELU() if not gated else SwiGLU() 29 | self.dropout = nn.Dropout(dropout) if dropout > 0.0 else nn.Identity() 30 | 31 | def forward(self, x: torch.Tensor) -> torch.Tensor: 32 | x = self.norm(x) 33 | x = self.proj1(x) 34 | x = self.act(x) 35 | x = self.proj2(x) 36 | x = self.dropout(x) 37 | return x 38 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/layers/nystrom_attention.py: -------------------------------------------------------------------------------- 1 | from functools import partial 2 | from typing import Optional 3 | 4 | import torch 5 | import torch.nn as nn 6 | import torch.nn.functional as F 7 | from einops import rearrange 8 | from xformers.components.attention import NystromAttention 9 | 10 | from .attention import AttentionBlock 11 | 12 | 13 | class NystromBlock(AttentionBlock): 14 | def __init__( 15 | self, 16 | dim: int, 17 | num_heads: int = 4, 18 | expansion: int = 4, 19 | dropout: float = 0.0, 20 | cosine: bool = False, 21 | gated: bool = False, 22 | layer_scale: float = 1.0, 23 | context_dim: Optional[int] = None, 24 | ): 25 | super().__init__( 26 | dim=dim, 27 | num_heads=num_heads, 28 | expansion=expansion, 29 | dropout=dropout, 30 | cosine=cosine, 31 | gated=gated, 32 | layer_scale=layer_scale, 33 | context_dim=context_dim, 34 | ) 35 | self.attention_fn = NystromAttention( 36 | num_landmarks=128, num_heads=num_heads, dropout=dropout 37 | ) 38 | 39 | def attn( 40 | self, 41 | x: torch.Tensor, 42 | attn_bias: Optional[torch.Tensor] = None, 43 | context: Optional[torch.Tensor] = None, 44 | pos_embed: Optional[torch.Tensor] = None, 45 | pos_embed_context: Optional[torch.Tensor] = None, 46 | rope: Optional[nn.Module] = None, 47 | ) -> torch.Tensor: 48 | x = self.norm_attnx(x) 49 | context = self.norm_attnctx(context) 50 | k, v = rearrange( 51 | self.kv(context), "b n (kv h d) -> b n h d kv", h=self.num_heads, kv=2 52 | ).unbind(dim=-1) 53 | q = rearrange(self.q(x), "b n (h d) -> b n h d", h=self.num_heads) 54 | 55 | if rope is not None: 56 | q = rope(q) 57 | k = rope(k) 58 | else: 59 | if pos_embed is not None: 60 | pos_embed = rearrange( 61 | pos_embed, "b n (h d) -> b n h d", h=self.num_heads 62 | ) 63 | q = q + pos_embed 64 | if pos_embed_context is not None: 65 | pos_embed_context = rearrange( 66 | pos_embed_context, "b n (h d) -> b n h d", h=self.num_heads 67 | ) 68 | k = k + pos_embed_context 69 | 70 | if self.cosine: 71 | q, k = map(partial(F.normalize, p=2, dim=-1), (q, k)) # cosine sim 72 | x = self.attention_fn(q, k, v, key_padding_mask=attn_bias) 73 | x = rearrange(x, "b n h d -> b n (h d)") 74 | x = self.out(x) 75 | return x 76 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/__init__.py: -------------------------------------------------------------------------------- 1 | from .unidepthv1 import UniDepthV1 2 | from .unidepthv2 import UniDepthV2 3 | 4 | __all__ = [ 5 | "UniDepthV1", 6 | "UniDepthV2", 7 | ] 8 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/__init__.py: -------------------------------------------------------------------------------- 1 | from .convnext import ConvNeXt 2 | from .convnext2 import ConvNeXtV2 3 | from .dinov2 import _make_dinov2_model 4 | 5 | __all__ = [ 6 | "ConvNeXt", 7 | "ConvNeXtV2", 8 | "_make_dinov2_model", 9 | ] 10 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/metadinov2/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # All rights reserved. 3 | # 4 | # This source code is licensed under the license found in the 5 | # LICENSE file in the root directory of this source tree. 6 | 7 | from .attention import Attention, MemEffAttention 8 | from .block import NestedTensorBlock 9 | from .dino_head import DINOHead 10 | from .mlp import Mlp 11 | from .patch_embed import PatchEmbed 12 | from .swiglu_ffn import SwiGLUFFN, SwiGLUFFNFused 13 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/metadinov2/attention.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # All rights reserved. 3 | # 4 | # This source code is licensed under the license found in the 5 | # LICENSE file in the root directory of this source tree. 6 | 7 | # References: 8 | # https://github.com/facebookresearch/dino/blob/master/vision_transformer.py 9 | # https://github.com/rwightman/pytorch-image-models/tree/master/timm/models/vision_transformer.py 10 | 11 | import logging 12 | 13 | import torch.nn as nn 14 | from torch import Tensor 15 | 16 | logger = logging.getLogger("dinov2") 17 | 18 | 19 | try: 20 | from xformers.ops import fmha, memory_efficient_attention, unbind 21 | 22 | XFORMERS_AVAILABLE = True 23 | except ImportError: 24 | logger.warning("xFormers not available") 25 | XFORMERS_AVAILABLE = False 26 | 27 | 28 | class Attention(nn.Module): 29 | def __init__( 30 | self, 31 | dim: int, 32 | num_heads: int = 8, 33 | qkv_bias: bool = False, 34 | proj_bias: bool = True, 35 | attn_drop: float = 0.0, 36 | proj_drop: float = 0.0, 37 | ) -> None: 38 | super().__init__() 39 | self.num_heads = num_heads 40 | head_dim = dim // num_heads 41 | self.scale = head_dim**-0.5 42 | 43 | self.qkv = nn.Linear(dim, dim * 3, bias=qkv_bias) 44 | self.attn_drop = nn.Dropout(attn_drop) 45 | self.proj = nn.Linear(dim, dim, bias=proj_bias) 46 | self.proj_drop = nn.Dropout(proj_drop) 47 | 48 | def forward(self, x: Tensor) -> Tensor: 49 | B, N, C = x.shape 50 | qkv = ( 51 | self.qkv(x) 52 | .reshape(B, N, 3, self.num_heads, C // self.num_heads) 53 | .permute(2, 0, 3, 1, 4) 54 | ) 55 | 56 | q, k, v = qkv[0] * self.scale, qkv[1], qkv[2] 57 | attn = q @ k.transpose(-2, -1) 58 | 59 | attn = attn.softmax(dim=-1) 60 | attn = self.attn_drop(attn) 61 | 62 | x = (attn @ v).transpose(1, 2).reshape(B, N, C) 63 | x = self.proj(x) 64 | x = self.proj_drop(x) 65 | return x 66 | 67 | 68 | class MemEffAttention(Attention): 69 | def forward(self, x: Tensor, attn_bias=None) -> Tensor: 70 | if not XFORMERS_AVAILABLE: 71 | assert attn_bias is None, "xFormers is required for nested tensors usage" 72 | return super().forward(x) 73 | 74 | B, N, C = x.shape 75 | qkv = self.qkv(x).reshape(B, N, 3, self.num_heads, C // self.num_heads) 76 | 77 | q, k, v = unbind(qkv, 2) 78 | 79 | x = memory_efficient_attention(q, k, v, attn_bias=attn_bias) 80 | x = x.reshape([B, N, C]) 81 | 82 | x = self.proj(x) 83 | x = self.proj_drop(x) 84 | return x 85 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/metadinov2/dino_head.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # All rights reserved. 3 | # 4 | # This source code is licensed under the license found in the 5 | # LICENSE file in the root directory of this source tree. 6 | 7 | import torch 8 | import torch.nn as nn 9 | from torch.nn.init import trunc_normal_ 10 | from torch.nn.utils import weight_norm 11 | 12 | 13 | class DINOHead(nn.Module): 14 | def __init__( 15 | self, 16 | in_dim, 17 | out_dim, 18 | use_bn=False, 19 | nlayers=3, 20 | hidden_dim=2048, 21 | bottleneck_dim=256, 22 | mlp_bias=True, 23 | ): 24 | super().__init__() 25 | nlayers = max(nlayers, 1) 26 | self.mlp = _build_mlp( 27 | nlayers, 28 | in_dim, 29 | bottleneck_dim, 30 | hidden_dim=hidden_dim, 31 | use_bn=use_bn, 32 | bias=mlp_bias, 33 | ) 34 | self.apply(self._init_weights) 35 | self.last_layer = weight_norm(nn.Linear(bottleneck_dim, out_dim, bias=False)) 36 | self.last_layer.weight_g.data.fill_(1) 37 | 38 | def _init_weights(self, m): 39 | if isinstance(m, nn.Linear): 40 | trunc_normal_(m.weight, std=0.02) 41 | if isinstance(m, nn.Linear) and m.bias is not None: 42 | nn.init.constant_(m.bias, 0) 43 | 44 | def forward(self, x): 45 | x = self.mlp(x) 46 | eps = 1e-6 if x.dtype == torch.float16 else 1e-12 47 | x = nn.functional.normalize(x, dim=-1, p=2, eps=eps) 48 | x = self.last_layer(x) 49 | return x 50 | 51 | 52 | def _build_mlp( 53 | nlayers, in_dim, bottleneck_dim, hidden_dim=None, use_bn=False, bias=True 54 | ): 55 | if nlayers == 1: 56 | return nn.Linear(in_dim, bottleneck_dim, bias=bias) 57 | else: 58 | layers = [nn.Linear(in_dim, hidden_dim, bias=bias)] 59 | if use_bn: 60 | layers.append(nn.BatchNorm1d(hidden_dim)) 61 | layers.append(nn.GELU()) 62 | for _ in range(nlayers - 2): 63 | layers.append(nn.Linear(hidden_dim, hidden_dim, bias=bias)) 64 | if use_bn: 65 | layers.append(nn.BatchNorm1d(hidden_dim)) 66 | layers.append(nn.GELU()) 67 | layers.append(nn.Linear(hidden_dim, bottleneck_dim, bias=bias)) 68 | return nn.Sequential(*layers) 69 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/metadinov2/drop_path.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # All rights reserved. 3 | # 4 | # This source code is licensed under the license found in the 5 | # LICENSE file in the root directory of this source tree. 6 | 7 | # References: 8 | # https://github.com/facebookresearch/dino/blob/master/vision_transformer.py 9 | # https://github.com/rwightman/pytorch-image-models/tree/master/timm/layers/drop.py 10 | 11 | 12 | import torch.nn as nn 13 | 14 | 15 | def drop_path(x, drop_prob: float = 0.0, training: bool = False): 16 | if drop_prob == 0.0 or not training: 17 | return x 18 | keep_prob = 1 - drop_prob 19 | shape = (x.shape[0],) + (1,) * ( 20 | x.ndim - 1 21 | ) # work with diff dim tensors, not just 2D ConvNets 22 | random_tensor = x.new_empty(shape).bernoulli_(keep_prob) 23 | if keep_prob > 0.0: 24 | random_tensor.div_(keep_prob) 25 | output = x * random_tensor 26 | return output 27 | 28 | 29 | class DropPath(nn.Module): 30 | """Drop paths (Stochastic Depth) per sample (when applied in main path of residual blocks).""" 31 | 32 | def __init__(self, drop_prob=None): 33 | super(DropPath, self).__init__() 34 | self.drop_prob = drop_prob 35 | 36 | def forward(self, x): 37 | return drop_path(x, self.drop_prob, self.training) 38 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/metadinov2/layer_scale.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # All rights reserved. 3 | # 4 | # This source code is licensed under the license found in the 5 | # LICENSE file in the root directory of this source tree. 6 | 7 | # Modified from: https://github.com/huggingface/pytorch-image-models/blob/main/timm/models/vision_transformer.py#L103-L110 8 | 9 | from typing import Union 10 | 11 | import torch 12 | import torch.nn as nn 13 | from torch import Tensor 14 | 15 | 16 | class LayerScale(nn.Module): 17 | def __init__( 18 | self, 19 | dim: int, 20 | init_values: Union[float, Tensor] = 1e-5, 21 | inplace: bool = False, 22 | ) -> None: 23 | super().__init__() 24 | self.inplace = inplace 25 | self.gamma = nn.Parameter(init_values * torch.ones(dim)) 26 | 27 | def forward(self, x: Tensor) -> Tensor: 28 | return x.mul_(self.gamma) if self.inplace else x * self.gamma 29 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/metadinov2/mlp.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # All rights reserved. 3 | # 4 | # This source code is licensed under the license found in the 5 | # LICENSE file in the root directory of this source tree. 6 | 7 | # References: 8 | # https://github.com/facebookresearch/dino/blob/master/vision_transformer.py 9 | # https://github.com/rwightman/pytorch-image-models/tree/master/timm/layers/mlp.py 10 | 11 | 12 | from typing import Callable, Optional 13 | 14 | from torch import Tensor, nn 15 | 16 | 17 | class Mlp(nn.Module): 18 | def __init__( 19 | self, 20 | in_features: int, 21 | hidden_features: Optional[int] = None, 22 | out_features: Optional[int] = None, 23 | act_layer: Callable[..., nn.Module] = nn.GELU, 24 | drop: float = 0.0, 25 | bias: bool = True, 26 | ) -> None: 27 | super().__init__() 28 | out_features = out_features or in_features 29 | hidden_features = hidden_features or in_features 30 | self.fc1 = nn.Linear(in_features, hidden_features, bias=bias) 31 | self.act = act_layer() 32 | self.fc2 = nn.Linear(hidden_features, out_features, bias=bias) 33 | self.drop = nn.Dropout(drop) 34 | 35 | def forward(self, x: Tensor) -> Tensor: 36 | x = self.fc1(x) 37 | x = self.act(x) 38 | x = self.drop(x) 39 | x = self.fc2(x) 40 | x = self.drop(x) 41 | return x 42 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/metadinov2/patch_embed.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # All rights reserved. 3 | # 4 | # This source code is licensed under the license found in the 5 | # LICENSE file in the root directory of this source tree. 6 | 7 | # References: 8 | # https://github.com/facebookresearch/dino/blob/master/vision_transformer.py 9 | # https://github.com/rwightman/pytorch-image-models/tree/master/timm/layers/patch_embed.py 10 | 11 | from typing import Callable, Optional, Tuple, Union 12 | 13 | import torch.nn as nn 14 | from torch import Tensor 15 | 16 | 17 | def make_2tuple(x): 18 | if isinstance(x, tuple): 19 | assert len(x) == 2 20 | return x 21 | 22 | assert isinstance(x, int) 23 | return (x, x) 24 | 25 | 26 | class PatchEmbed(nn.Module): 27 | """ 28 | 2D image to patch embedding: (B,C,H,W) -> (B,N,D) 29 | 30 | Args: 31 | img_size: Image size. 32 | patch_size: Patch token size. 33 | in_chans: Number of input image channels. 34 | embed_dim: Number of linear projection output channels. 35 | norm_layer: Normalization layer. 36 | """ 37 | 38 | def __init__( 39 | self, 40 | img_size: Union[int, Tuple[int, int]] = 224, 41 | patch_size: Union[int, Tuple[int, int]] = 16, 42 | in_chans: int = 3, 43 | embed_dim: int = 768, 44 | norm_layer: Optional[Callable] = None, 45 | flatten_embedding: bool = True, 46 | ) -> None: 47 | super().__init__() 48 | 49 | image_HW = make_2tuple(img_size) 50 | patch_HW = make_2tuple(patch_size) 51 | patch_grid_size = ( 52 | image_HW[0] // patch_HW[0], 53 | image_HW[1] // patch_HW[1], 54 | ) 55 | 56 | self.img_size = image_HW 57 | self.patch_size = patch_HW 58 | self.patches_resolution = patch_grid_size 59 | self.num_patches = patch_grid_size[0] * patch_grid_size[1] 60 | 61 | self.in_chans = in_chans 62 | self.embed_dim = embed_dim 63 | 64 | self.flatten_embedding = flatten_embedding 65 | 66 | self.proj = nn.Conv2d( 67 | in_chans, embed_dim, kernel_size=patch_HW, stride=patch_HW 68 | ) 69 | self.norm = norm_layer(embed_dim) if norm_layer else nn.Identity() 70 | 71 | def forward(self, x: Tensor) -> Tensor: 72 | _, _, H, W = x.shape 73 | patch_H, patch_W = self.patch_size 74 | 75 | assert ( 76 | H % patch_H == 0 77 | ), f"Input image height {H} is not a multiple of patch height {patch_H}" 78 | assert ( 79 | W % patch_W == 0 80 | ), f"Input image width {W} is not a multiple of patch width: {patch_W}" 81 | 82 | x = self.proj(x) # B C H W 83 | H, W = x.size(2), x.size(3) 84 | x = x.flatten(2).transpose(1, 2) # B HW C 85 | x = self.norm(x) 86 | if not self.flatten_embedding: 87 | x = x.reshape(-1, H, W, self.embed_dim) # B H W C 88 | return x 89 | 90 | def flops(self) -> float: 91 | Ho, Wo = self.patches_resolution 92 | flops = ( 93 | Ho 94 | * Wo 95 | * self.embed_dim 96 | * self.in_chans 97 | * (self.patch_size[0] * self.patch_size[1]) 98 | ) 99 | if self.norm is not None: 100 | flops += Ho * Wo * self.embed_dim 101 | return flops 102 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/backbones/metadinov2/swiglu_ffn.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Meta Platforms, Inc. and affiliates. 2 | # All rights reserved. 3 | # 4 | # This source code is licensed under the license found in the 5 | # LICENSE file in the root directory of this source tree. 6 | 7 | from typing import Callable, Optional 8 | 9 | import torch.nn.functional as F 10 | from torch import Tensor, nn 11 | 12 | 13 | class SwiGLUFFN(nn.Module): 14 | def __init__( 15 | self, 16 | in_features: int, 17 | hidden_features: Optional[int] = None, 18 | out_features: Optional[int] = None, 19 | act_layer: Callable[..., nn.Module] = None, 20 | drop: float = 0.0, 21 | bias: bool = True, 22 | ) -> None: 23 | super().__init__() 24 | out_features = out_features or in_features 25 | hidden_features = hidden_features or in_features 26 | self.w12 = nn.Linear(in_features, 2 * hidden_features, bias=bias) 27 | self.w3 = nn.Linear(hidden_features, out_features, bias=bias) 28 | 29 | def forward(self, x: Tensor) -> Tensor: 30 | x12 = self.w12(x) 31 | x1, x2 = x12.chunk(2, dim=-1) 32 | hidden = F.silu(x1) * x2 33 | return self.w3(hidden) 34 | 35 | 36 | try: 37 | from xformers.ops import SwiGLU 38 | 39 | XFORMERS_AVAILABLE = True 40 | except ImportError: 41 | SwiGLU = SwiGLUFFN 42 | XFORMERS_AVAILABLE = False 43 | 44 | 45 | class SwiGLUFFNFused(SwiGLU): 46 | def __init__( 47 | self, 48 | in_features: int, 49 | hidden_features: Optional[int] = None, 50 | out_features: Optional[int] = None, 51 | act_layer: Callable[..., nn.Module] = None, 52 | drop: float = 0.0, 53 | bias: bool = True, 54 | ) -> None: 55 | out_features = out_features or in_features 56 | hidden_features = hidden_features or in_features 57 | hidden_features = (int(hidden_features * 2 / 3) + 7) // 8 * 8 58 | super().__init__( 59 | in_features=in_features, 60 | hidden_features=hidden_features, 61 | out_features=out_features, 62 | bias=bias, 63 | ) 64 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/unidepthv1/__init__.py: -------------------------------------------------------------------------------- 1 | from .unidepthv1 import UniDepthV1 2 | 3 | __all__ = [ 4 | "UniDepthV1", 5 | ] 6 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/models/unidepthv2/__init__.py: -------------------------------------------------------------------------------- 1 | from .unidepthv2 import UniDepthV2 2 | 3 | __all__ = [ 4 | "UniDepthV2", 5 | ] 6 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/ops/__init__.py: -------------------------------------------------------------------------------- 1 | from .losses import MSE, SelfCons, SILog 2 | from .scheduler import CosineScheduler 3 | 4 | __all__ = [ 5 | "SILog", 6 | "MSE", 7 | "SelfCons", 8 | "CosineScheduler", 9 | ] 10 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/ops/scheduler.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Luigi Piccinelli 3 | Licensed under the CC-BY NC 4.0 license (http://creativecommons.org/licenses/by-nc/4.0/) 4 | """ 5 | 6 | import numpy as np 7 | 8 | 9 | class CosineScheduler(object): 10 | def __init__( 11 | self, 12 | optimizer, 13 | warmup_iters, 14 | total_iters, 15 | key, 16 | overwrite=False, 17 | init_value=None, 18 | base_value=None, 19 | final_value=None, 20 | step_init=-1, 21 | ): 22 | super().__init__() 23 | self.iter = step_init 24 | self.overwrite = overwrite 25 | self.optimizer = optimizer 26 | self.base_value = base_value 27 | self.init_value = init_value 28 | self.final_value = final_value 29 | self.total_iters = total_iters 30 | self.warmup_iters = warmup_iters 31 | self.key = key 32 | self.schedulers = [ 33 | self.get_schedulers(group) for group in optimizer.param_groups 34 | ] 35 | 36 | def get_schedulers(self, group): 37 | init_value = group.get(self.key + "_init", self.init_value) 38 | base_value = group.get(self.key + "_base", self.base_value) 39 | final_value = group.get(self.key + "_final", self.final_value) 40 | warmup_iters = self.warmup_iters 41 | total_iters = self.total_iters 42 | if self.overwrite: 43 | final_value = self.final_value 44 | 45 | # normalize in 0,1, then apply function (power) and denormalize 46 | normalized_schedule = np.linspace(0, 1, warmup_iters, endpoint=True) 47 | normalized_schedule = np.power(normalized_schedule, 2) 48 | warmup_schedule = (base_value - init_value) * normalized_schedule + init_value 49 | 50 | # main scheduling 51 | iters = np.arange(total_iters - warmup_iters) 52 | schedule = final_value + 0.5 * (base_value - final_value) * ( 53 | 1 + np.cos(np.pi * iters / len(iters)) 54 | ) 55 | return np.concatenate((warmup_schedule, schedule)) 56 | 57 | def step(self): 58 | self.iter = self.iter + 1 59 | vals = self[self.iter] 60 | for group, val in zip(self.optimizer.param_groups, vals): 61 | if isinstance(group[self.key], (tuple, list)): 62 | val = (val, *group[self.key][1:]) 63 | group[self.key] = val 64 | 65 | def __getitem__(self, it): 66 | it = min(it, self.total_iters - 1) 67 | return [scheduler[it] for scheduler in self.schedulers] 68 | 69 | def get(self): 70 | return [group[self.key] for group in self.optimizer.param_groups] 71 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/utils/__init__.py: -------------------------------------------------------------------------------- 1 | from .distributed import (barrier, get_dist_info, get_rank, is_main_process, 2 | setup_multi_processes, setup_slurm, 3 | sync_tensor_across_gpus) 4 | from .evaluation_depth import DICT_METRICS, eval_depth 5 | from .geometric import spherical_zbuffer_to_euclidean, unproject_points 6 | from .misc import format_seconds, get_params, identity, remove_padding 7 | from .visualization import colorize, image_grid, log_train_artifacts 8 | 9 | __all__ = [ 10 | "eval_depth", 11 | "DICT_METRICS", 12 | "colorize", 13 | "image_grid", 14 | "log_train_artifacts", 15 | "format_seconds", 16 | "remove_padding", 17 | "get_params", 18 | "identity", 19 | "is_main_process", 20 | "setup_multi_processes", 21 | "setup_slurm", 22 | "sync_tensor_across_gpus", 23 | "barrier", 24 | "get_rank", 25 | "unproject_points", 26 | "spherical_zbuffer_to_euclidean", 27 | "validate", 28 | "get_dist_info", 29 | ] 30 | -------------------------------------------------------------------------------- /thirdparty/UniDepth/unidepth/utils/constants.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Luigi Piccinelli 3 | Licensed under the CC-BY NC 4.0 license (http://creativecommons.org/licenses/by-nc/4.0/) 4 | """ 5 | 6 | import math 7 | 8 | import torch 9 | 10 | OPENAI_DATASET_MEAN = (0.48145466, 0.4578275, 0.40821073) 11 | OPENAI_DATASET_STD = (0.26862954, 0.26130258, 0.27577711) 12 | IMAGENET_DATASET_MEAN = (0.485, 0.456, 0.406) 13 | IMAGENET_DATASET_STD = (0.229, 0.224, 0.225) 14 | DEPTH_BINS = torch.cat( 15 | ( 16 | torch.logspace(math.log10(0.1), math.log10(180.0), steps=512), 17 | torch.tensor([260.0]), 18 | ), 19 | dim=0, 20 | ) 21 | LOGERR_BINS = torch.linspace(-2, 2, steps=128 + 1) 22 | LINERR_BINS = torch.linspace(-50, 50, steps=256 + 1) 23 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | UseTab: Never 3 | IndentWidth: 4 4 | AccessModifierOffset: -4 5 | ColumnLimit: 100 6 | BinPackParameters: false 7 | SortIncludes: true 8 | Standard: c++17 9 | DerivePointerAlignment: false 10 | PointerAlignment: Right 11 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | enable_markup: false 2 | line_width: 120 3 | format: 4 | max_subgroups_hwrap: 5 5 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.4.0 4 | hooks: 5 | - id: trailing-whitespace 6 | exclude: \.patch$ 7 | - id: end-of-file-fixer 8 | exclude: \.patch$ 9 | - id: check-yaml 10 | - id: check-added-large-files 11 | - repo: https://github.com/pre-commit/mirrors-clang-format 12 | rev: v14.0.0 13 | hooks: 14 | - id: clang-format 15 | - repo: https://github.com/psf/black 16 | rev: 23.1.0 17 | hooks: 18 | - id: black 19 | args: [--config=python/pyproject.toml] 20 | - repo: https://github.com/ahans/cmake-format-precommit 21 | rev: 8e52fb6506f169dddfaa87f88600d765fca48386 22 | hooks: 23 | - id: cmake-format 24 | - repo: https://github.com/pycqa/isort 25 | rev: 5.12.0 26 | hooks: 27 | - id: isort 28 | args: ["--settings-path=python/pyproject.toml"] 29 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | preferred-citation: 3 | title: "KISS-ICP: In Defense of Point-to-Point ICP - Simple, Accurate, and Robust Registration If Done the Right Way" 4 | doi: "10.1109/LRA.2023.3236571" 5 | year: "2023" 6 | type: article 7 | journal: "IEEE Robotics and Automation Letters (RA-L)" 8 | url: https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/vizzo2023ral.pdf 9 | codeurl: https://github.com/PRBonn/kiss-icp 10 | authors: 11 | - family-names: Vizzo 12 | given-names: Ignacio 13 | - family-names: Guadagnino 14 | given-names: Tiziano 15 | - family-names: Mersch 16 | given-names: Benedikt 17 | - family-names: Wiesmann 18 | given-names: Louis 19 | - family-names: Behley 20 | given-names: Jens 21 | - family-names: Stachniss 22 | given-names: Cyrill 23 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/LICENSE: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/Makefile: -------------------------------------------------------------------------------- 1 | .PHONY: cpp 2 | 3 | install: 4 | @pip install --verbose ./python/ 5 | 6 | uninstall: 7 | @pip -v uninstall kiss_icp 8 | 9 | editable: 10 | @pip install scikit-build-core pyproject_metadata pathspec pybind11 ninja cmake 11 | @pip install --no-build-isolation -ve ./python/ 12 | 13 | test: 14 | @pytest -rA --verbose ./python/ 15 | 16 | cpp: 17 | @cmake -Bbuild cpp/kiss_icp/ 18 | @cmake --build build -j$(nproc --all) 19 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/config/README.md: -------------------------------------------------------------------------------- 1 | # KISS-ICP Config 2 | 3 | **NOTE:** You **DO NOT NEED** to use these configuration files to run the system. 4 | 5 | The configuration of our system is a sanitzed selection of parameters. In most of the cases it 6 | should be enough to use the default parameters "as is". 7 | 8 | ## Additional configuration for KISS-ICP 9 | 10 | You will need first need to install all the dependencies of the kiss_icp: 11 | 12 | ```sh 13 | pip install "kiss-icp[all]" 14 | ``` 15 | If you need extra tunning then you can use the optionally provided yaml files from this folder. You 16 | can override just 1 or all of the default parameters. To load this configuration you only need to 17 | instruct the command line interface: 18 | 19 | ```sh 20 | $ kiss_icp_pipeline --config /my_config.yaml 21 | ``` 22 | 23 | Big thanks to Markus Pielmeier who contributed PR #63 making this configuration module as flexible 24 | as possible. 25 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/config/advanced.yaml: -------------------------------------------------------------------------------- 1 | # NOTE: Please note that this is just an example of the advanced configuration for the KISS-ICP 2 | # pipeline and it's not really meant to use for any particular dataset, is just to expose other 3 | # options that are disabled by default 4 | out_dir: "results" 5 | 6 | data: 7 | deskew: True 8 | max_range: 100.0 # can be also changed in the CLI 9 | min_range: 0.0 10 | 11 | mapping: 12 | voxel_size: 1.0 # <- optional 13 | max_points_per_voxel: 20 14 | 15 | adaptive_threshold: 16 | fixed_threshold: 0.3 # <- optional, disables adaptive threshold 17 | # initial_threshold: 2.0 18 | # min_motion_th: 0.1 19 | 20 | registration: 21 | max_num_iterations: 500 # <- optional 22 | convergence_criterion: 0.0001 # <- optional 23 | max_num_threads: 0 # <- optional, 0 means automatic 24 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/config/basic.yaml: -------------------------------------------------------------------------------- 1 | out_dir: "results" 2 | 3 | data: 4 | deskew: False 5 | max_range: 100.0 # can be also changed in the CLI 6 | min_range: 5.0 7 | 8 | mapping: 9 | max_points_per_voxel: 20 10 | 11 | adaptive_threshold: 12 | initial_threshold: 2.0 13 | min_motion_th: 0.1 14 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/thirdparty/kiss-icp/cpp/COLCON_IGNORE -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/eigen/LICENSE: -------------------------------------------------------------------------------- 1 | Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links: 2 | http://www.mozilla.org/MPL/2.0/ 3 | http://www.mozilla.org/MPL/2.0/FAQ.html 4 | 5 | Some files contain third-party code under BSD or LGPL licenses, whence the other 6 | COPYING.* files here. 7 | 8 | All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later. 9 | For this reason, the COPYING.LGPL file contains the LGPL 2.1 text. 10 | 11 | If you want to guarantee that the Eigen code that you are #including is licensed 12 | under the MPL2 and possibly more permissive licenses (like BSD), #define this 13 | preprocessor symbol: 14 | EIGEN_MPL2_ONLY 15 | For example, with most compilers, you could add this to your project CXXFLAGS: 16 | -DEIGEN_MPL2_ONLY 17 | This will cause a compilation error to be generated if you #include any code that is 18 | LGPL licensed. 19 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/eigen/eigen.cmake: -------------------------------------------------------------------------------- 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 | set(EIGEN_BUILD_DOC OFF CACHE BOOL "Don't build Eigen docs") 25 | set(EIGEN_BUILD_TESTING OFF CACHE BOOL "Don't build Eigen tests") 26 | set(EIGEN_BUILD_PKGCONFIG OFF CACHE BOOL "Don't build Eigen pkg-config") 27 | set(EIGEN_BUILD_BLAS OFF CACHE BOOL "Don't build blas module") 28 | set(EIGEN_BUILD_LAPACK OFF CACHE BOOL "Don't build lapack module") 29 | 30 | include(FetchContent) 31 | FetchContent_Declare(eigen URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz 32 | PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/eigen.patch UPDATE_DISCONNECTED 1) 33 | FetchContent_GetProperties(eigen) 34 | if(NOT eigen_POPULATED) 35 | FetchContent_Populate(eigen) 36 | if(${CMAKE_VERSION} GREATER_EQUAL 3.25) 37 | add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) 38 | else() 39 | # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will 40 | # consider this 3rdparty headers as source code and fail due the -Werror flag. 41 | add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} EXCLUDE_FROM_ALL) 42 | get_target_property(eigen_include_dirs eigen INTERFACE_INCLUDE_DIRECTORIES) 43 | set_target_properties(eigen PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_include_dirs}") 44 | endif() 45 | endif() 46 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/eigen/eigen.patch: -------------------------------------------------------------------------------- 1 | commit cf82186416d04ea5df2a397d8fe09dc78d40ca65 2 | Author: Antonio Sánchez 3 | Date: Sat Mar 5 05:49:45 2022 +0000 4 | 5 | Adds new CMake Options for controlling build components. 6 | 7 | diff --git a/CMakeLists.txt b/CMakeLists.txt 8 | index de1c23e91..0af36a53a 100644 9 | --- a/CMakeLists.txt 10 | +++ b/CMakeLists.txt 11 | @@ -477,6 +477,9 @@ if(EIGEN_BUILD_TESTING) 12 | add_subdirectory(failtest) 13 | endif() 14 | 15 | +include(CMakeDetermineFortranCompiler) 16 | +option(EIGEN_BUILD_BLAS "Toggles the building of the Eigen Blas library" ${CMAKE_Fortran_COMPILER}) 17 | +option(EIGEN_BUILD_LAPACK "Toggles the building of the included Eigen LAPACK library" ${CMAKE_Fortran_COMPILER}) 18 | if(EIGEN_LEAVE_TEST_IN_ALL_TARGET) 19 | add_subdirectory(blas) 20 | add_subdirectory(lapack) 21 | @@ -611,6 +614,8 @@ set_target_properties (eigen PROPERTIES EXPORT_NAME Eigen) 22 | 23 | install (TARGETS eigen EXPORT Eigen3Targets) 24 | 25 | +option(EIGEN_BUILD_CMAKE_PACKAGE "Enables the creation of EigenConfig.cmake and related files" ON) 26 | +if(EIGEN_BUILD_CMAKE_PACKAGE) 27 | configure_package_config_file ( 28 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3Config.cmake.in 29 | ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake 30 | @@ -655,6 +660,7 @@ install (FILES ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake 31 | # Add uninstall target 32 | add_custom_target ( uninstall 33 | COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_SOURCE_DIR}/cmake/EigenUninstall.cmake) 34 | +endif() 35 | 36 | if (EIGEN_SPLIT_TESTSUITE) 37 | ei_split_testsuite("${EIGEN_SPLIT_TESTSUITE}") 38 | diff --git a/blas/CMakeLists.txt b/blas/CMakeLists.txt 39 | index 8d3cb86dc..c530957fb 100644 40 | --- a/blas/CMakeLists.txt 41 | +++ b/blas/CMakeLists.txt 42 | @@ -1,6 +1,7 @@ 43 | 44 | project(EigenBlas CXX) 45 | 46 | +if(EIGEN_BUILD_BLAS) 47 | include(CheckLanguage) 48 | check_language(Fortran) 49 | if(CMAKE_Fortran_COMPILER) 50 | @@ -59,4 +60,4 @@ if(EIGEN_BUILD_TESTING) 51 | endif() 52 | 53 | endif() 54 | - 55 | +endif() 56 | diff --git a/lapack/CMakeLists.txt b/lapack/CMakeLists.txt 57 | index c8ca64001..8d6d75401 100644 58 | --- a/lapack/CMakeLists.txt 59 | +++ b/lapack/CMakeLists.txt 60 | @@ -1,5 +1,7 @@ 61 | project(EigenLapack CXX) 62 | 63 | +if(EIGEN_BUILD_LAPACK AND EIGEN_BUILD_BLAS) 64 | + 65 | include(CheckLanguage) 66 | check_language(Fortran) 67 | if(CMAKE_Fortran_COMPILER) 68 | @@ -457,3 +459,6 @@ if(EXISTS ${eigen_full_path_to_testing_lapack}) 69 | 70 | endif() 71 | 72 | +elseif(EIGEN_BUILD_LAPACK AND NOT EIGEN_BUILD_BLAS) 73 | + message(FATAL_ERROR "EIGEN_BUILD_LAPACK requires EIGEN_BUILD_BLAS") 74 | +endif() #EIGEN_BUILD_LAPACK 75 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/find_dependencies.cmake: -------------------------------------------------------------------------------- 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 | # Silence timestamp warning 24 | if(CMAKE_VERSION VERSION_GREATER 3.24) 25 | cmake_policy(SET CMP0135 OLD) 26 | endif() 27 | 28 | function(find_external_dependency PACKAGE_NAME TARGET_NAME INCLUDED_CMAKE_PATH) 29 | string(TOUPPER ${PACKAGE_NAME} PACKAGE_NAME_UP) 30 | set(USE_FROM_SYSTEM_OPTION "USE_SYSTEM_${PACKAGE_NAME_UP}") 31 | if(${${USE_FROM_SYSTEM_OPTION}}) 32 | find_package(${PACKAGE_NAME} QUIET NO_MODULE) 33 | endif() 34 | if(NOT ${${USE_FROM_SYSTEM_OPTION}} OR NOT TARGET ${TARGET_NAME}) 35 | set(${USE_FROM_SYSTEM_OPTION} OFF PARENT_SCOPE) 36 | include(${INCLUDED_CMAKE_PATH}) 37 | endif() 38 | endfunction() 39 | 40 | find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake") 41 | find_external_dependency("Sophus" "Sophus::Sophus" "${CMAKE_CURRENT_LIST_DIR}/sophus/sophus.cmake") 42 | find_external_dependency("TBB" "TBB::tbb" "${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake") 43 | find_external_dependency("tsl-robin-map" "tsl::robin_map" "${CMAKE_CURRENT_LIST_DIR}/tsl_robin/tsl_robin.cmake") 44 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/sophus/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2008-2015 Jesse Beder. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | THE SOFTWAR 20 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/sophus/sophus.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # # Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Cyrill Stachniss, University of 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 | include(FetchContent) 23 | 24 | set(SOPHUS_USE_BASIC_LOGGING ON CACHE BOOL "Don't use fmt for Sophus libraru") 25 | set(BUILD_SOPHUS_TESTS OFF CACHE BOOL "Don't build Sophus tests") 26 | set(BUILD_SOPHUS_EXAMPLES OFF CACHE BOOL "Don't build Sophus Examples") 27 | 28 | FetchContent_Declare(sophus SYSTEM URL https://github.com/strasdat/Sophus/archive/refs/tags/1.22.10.tar.gz 29 | PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/sophus.patch UPDATE_DISCONNECTED 1) 30 | 31 | FetchContent_MakeAvailable(sophus) 32 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/sophus/sophus.patch: -------------------------------------------------------------------------------- 1 | diff --git a/sophus/common.hpp b/sophus/common.hpp 2 | index 5634d35b..56a1b590 100644 3 | --- a/sophus/common.hpp 4 | +++ b/sophus/common.hpp 5 | @@ -18,7 +18,7 @@ 6 | #define SOPHUS_FMT_CSTR(description, ...) description 7 | #define SOPHUS_FMT_STR(description, ...) std::string(description) 8 | #define SOPHUS_FMT_PRINT(description, ...) std::printf("%s\n", description) 9 | -#define SOPHUS_FMT_ARG(arg) 10 | +#define SOPHUS_FMT_ARG(arg) arg 11 | 12 | #else // !SOPHUS_USE_BASIC_LOGGING 13 | 14 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/tbb/tbb.cmake: -------------------------------------------------------------------------------- 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 | option(BUILD_SHARED_LIBS OFF) 24 | option(TBBMALLOC_BUILD OFF) 25 | option(TBB_EXAMPLES OFF) 26 | option(TBB_STRICT OFF) 27 | option(TBB_TEST OFF) 28 | 29 | include(FetchContent) 30 | FetchContent_Declare(tbb URL https://github.com/oneapi-src/oneTBB/archive/refs/tags/v2021.8.0.tar.gz) 31 | FetchContent_GetProperties(tbb) 32 | if(NOT tbb_POPULATED) 33 | FetchContent_Populate(tbb) 34 | if(${CMAKE_VERSION} GREATER_EQUAL 3.25) 35 | add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) 36 | else() 37 | # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will 38 | # consider this 3rdparty headers as source code and fail due the -Werror flag. 39 | add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} EXCLUDE_FROM_ALL) 40 | get_target_property(tbb_include_dirs tbb INTERFACE_INCLUDE_DIRECTORIES) 41 | set_target_properties(tbb PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${tbb_include_dirs}") 42 | endif() 43 | endif() 44 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/tsl_robin/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Thibaut Goetghebuer-Planchon 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 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/3rdparty/tsl_robin/tsl_robin.cmake: -------------------------------------------------------------------------------- 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 | include(FetchContent) 24 | FetchContent_Declare(tessil SYSTEM URL https://github.com/Tessil/robin-map/archive/refs/tags/v1.2.1.tar.gz) 25 | FetchContent_MakeAvailable(tessil) 26 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/CMakeLists.txt: -------------------------------------------------------------------------------- 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 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(kiss_icp_cpp VERSION 1.0.0 LANGUAGES CXX) 25 | 26 | # Setup build options 27 | option(USE_CCACHE "Build using Ccache if found on the path" ON) 28 | option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) 29 | option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) 30 | option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON) 31 | option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) 32 | 33 | # ccache setup 34 | if(USE_CCACHE) 35 | find_program(CCACHE_PATH ccache) 36 | if(CCACHE_PATH) 37 | set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) 38 | set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) 39 | message(STATUS "Using ccache: ${CCACHE_PATH}") 40 | endif() 41 | endif() 42 | 43 | # Set build type (repeat here for C++ only consumers) 44 | set(CMAKE_BUILD_TYPE Release) 45 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 46 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 47 | 48 | include(3rdparty/find_dependencies.cmake) 49 | include(cmake/CompilerOptions.cmake) 50 | 51 | add_subdirectory(core) 52 | add_subdirectory(metrics) 53 | add_subdirectory(pipeline) 54 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/LICENSE: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/README.md: -------------------------------------------------------------------------------- 1 | # KISS-ICP C++ Library 2 | 3 | ## How to build 4 | 5 | ```sh 6 | cmake -Bbuild 7 | cmake --build build -j$(nproc --all) 8 | ``` 9 | 10 | ## Dependencies 11 | 12 | The cmake build system should handle all dependencies for you. In case you have some particular 13 | requirements for the library dependencies, by default the build system will attempt to use the 14 | ones you have installed on your local system. 15 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/cmake/CompilerOptions.cmake: -------------------------------------------------------------------------------- 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 | function(set_global_target_properties target) 24 | target_compile_features(${target} PUBLIC cxx_std_17) 25 | target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) 26 | target_compile_options( 27 | ${target} 28 | PRIVATE # MSVC 29 | $<$:/W4> 30 | $<$:/WX> 31 | # Clang/AppleClang 32 | $<$:-fcolor-diagnostics> 33 | $<$:-Werror> 34 | $<$:-Wall> 35 | $<$:-Wextra> 36 | $<$:-Wconversion> 37 | $<$:-Wno-sign-conversion> 38 | # GNU 39 | $<$:-fdiagnostics-color=always> 40 | $<$:-Werror> 41 | $<$:-Wall> 42 | $<$:-Wextra> 43 | $<$:-pedantic> 44 | $<$:-Wcast-align> 45 | $<$:-Wcast-qual> 46 | $<$:-Wconversion> 47 | $<$:-Wdisabled-optimization> 48 | $<$:-Woverloaded-virtual>) 49 | set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) 50 | get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) 51 | target_include_directories(${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 52 | PUBLIC $ $) 53 | endfunction() 54 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/CMakeLists.txt: -------------------------------------------------------------------------------- 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 | add_library(kiss_icp_core STATIC) 24 | target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp VoxelUtils.cpp Preprocessing.cpp 25 | Threshold.cpp) 26 | target_link_libraries(kiss_icp_core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus) 27 | set_global_target_properties(kiss_icp_core) 28 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/Deskew.cpp: -------------------------------------------------------------------------------- 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 | #include "Deskew.hpp" 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace { 32 | /// TODO(Nacho) Explain what is the very important meaning of this param 33 | constexpr double mid_pose_timestamp{0.5}; 34 | } // namespace 35 | 36 | namespace kiss_icp { 37 | std::vector DeSkewScan(const std::vector &frame, 38 | const std::vector ×tamps, 39 | const Sophus::SE3d &delta) { 40 | const auto delta_pose = delta.log(); 41 | std::vector corrected_frame(frame.size()); 42 | // TODO(All): This tbb execution is ignoring the max_n_threads config value 43 | tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) { 44 | const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose); 45 | corrected_frame[i] = motion * frame[i]; 46 | }); 47 | return corrected_frame; 48 | } 49 | } // namespace kiss_icp 50 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/Deskew.hpp: -------------------------------------------------------------------------------- 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 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace kiss_icp { 30 | 31 | /// Compensate the frame by interpolating the delta pose 32 | std::vector DeSkewScan(const std::vector &frame, 33 | const std::vector ×tamps, 34 | const Sophus::SE3d &delta); 35 | 36 | } // namespace kiss_icp 37 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/Preprocessing.cpp: -------------------------------------------------------------------------------- 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 | #include "Preprocessing.hpp" 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "VoxelUtils.hpp" 33 | 34 | namespace kiss_icp { 35 | std::vector Preprocess(const std::vector &frame, 36 | double max_range, 37 | double min_range) { 38 | std::vector inliers; 39 | std::copy_if(frame.cbegin(), frame.cend(), std::back_inserter(inliers), [&](const auto &pt) { 40 | const double norm = pt.norm(); 41 | return norm < max_range && norm > min_range; 42 | }); 43 | return inliers; 44 | } 45 | 46 | } // namespace kiss_icp 47 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/Preprocessing.hpp: -------------------------------------------------------------------------------- 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 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | namespace kiss_icp { 29 | 30 | /// Crop the frame with max/min ranges 31 | std::vector Preprocess(const std::vector &frame, 32 | double max_range, 33 | double min_range); 34 | } // namespace kiss_icp 35 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/Registration.hpp: -------------------------------------------------------------------------------- 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 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "VoxelHashMap.hpp" 30 | 31 | namespace kiss_icp { 32 | 33 | struct Registration { 34 | explicit Registration(int max_num_iteration, double convergence_criterion, int max_num_threads); 35 | 36 | Sophus::SE3d AlignPointsToMap(const std::vector &frame, 37 | const VoxelHashMap &voxel_map, 38 | const Sophus::SE3d &initial_guess, 39 | const double max_correspondence_distance, 40 | const double kernel_scale); 41 | 42 | int max_num_iterations_; 43 | double convergence_criterion_; 44 | int max_num_threads_; 45 | }; 46 | } // namespace kiss_icp 47 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/Threshold.cpp: -------------------------------------------------------------------------------- 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 | #include "Threshold.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace kiss_icp { 30 | AdaptiveThreshold::AdaptiveThreshold(double initial_threshold, 31 | double min_motion_threshold, 32 | double max_range) 33 | : min_motion_threshold_(min_motion_threshold), 34 | max_range_(max_range), 35 | model_sse_(initial_threshold * initial_threshold), 36 | num_samples_(1) {} 37 | 38 | void AdaptiveThreshold::UpdateModelDeviation(const Sophus::SE3d ¤t_deviation) { 39 | const double model_error = [&]() { 40 | const double theta = Eigen::AngleAxisd(current_deviation.rotationMatrix()).angle(); 41 | const double delta_rot = 2.0 * max_range_ * std::sin(theta / 2.0); 42 | const double delta_trans = current_deviation.translation().norm(); 43 | return delta_trans + delta_rot; 44 | }(); 45 | if (model_error > min_motion_threshold_) { 46 | model_sse_ += model_error * model_error; 47 | num_samples_++; 48 | } 49 | } 50 | 51 | } // namespace kiss_icp 52 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/Threshold.hpp: -------------------------------------------------------------------------------- 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 | #pragma once 24 | 25 | #include 26 | 27 | namespace kiss_icp { 28 | 29 | struct AdaptiveThreshold { 30 | explicit AdaptiveThreshold(double initial_threshold, 31 | double min_motion_threshold, 32 | double max_range); 33 | 34 | /// Update the current belief of the deviation from the prediction model 35 | void UpdateModelDeviation(const Sophus::SE3d ¤t_deviation); 36 | 37 | /// Returns the KISS-ICP adaptive threshold used in registration 38 | inline double ComputeThreshold() const { return std::sqrt(model_sse_ / num_samples_); } 39 | 40 | // configurable parameters 41 | double min_motion_threshold_; 42 | double max_range_; 43 | 44 | // Local cache for ccomputation 45 | double model_sse_; 46 | int num_samples_; 47 | }; 48 | 49 | } // namespace kiss_icp 50 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/VoxelHashMap.hpp: -------------------------------------------------------------------------------- 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 | // NOTE: This implementation is heavily inspired in the original CT-ICP VoxelHashMap implementation, 25 | // although it was heavily modifed and drastically simplified, but if you are using this module you 26 | // should at least acknoowledge the work from CT-ICP by giving a star on GitHub 27 | #pragma once 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "VoxelUtils.hpp" 36 | 37 | namespace kiss_icp { 38 | struct VoxelHashMap { 39 | explicit VoxelHashMap(double voxel_size, double max_distance, unsigned int max_points_per_voxel) 40 | : voxel_size_(voxel_size), 41 | max_distance_(max_distance), 42 | max_points_per_voxel_(max_points_per_voxel) {} 43 | 44 | inline void Clear() { map_.clear(); } 45 | inline bool Empty() const { return map_.empty(); } 46 | void Update(const std::vector &points, const Eigen::Vector3d &origin); 47 | void Update(const std::vector &points, const Sophus::SE3d &pose); 48 | void AddPoints(const std::vector &points); 49 | void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); 50 | std::vector Pointcloud() const; 51 | std::vector GetPoints(const std::vector &query_voxels) const; 52 | 53 | double voxel_size_; 54 | double max_distance_; 55 | unsigned int max_points_per_voxel_; 56 | tsl::robin_map> map_; 57 | }; 58 | } // namespace kiss_icp 59 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/VoxelUtils.cpp: -------------------------------------------------------------------------------- 1 | #include "VoxelUtils.hpp" 2 | 3 | #include 4 | 5 | namespace kiss_icp { 6 | 7 | std::vector VoxelDownsample(const std::vector &frame, 8 | const double voxel_size) { 9 | tsl::robin_map grid; 10 | grid.reserve(frame.size()); 11 | std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { 12 | const auto voxel = PointToVoxel(point, voxel_size); 13 | if (!grid.contains(voxel)) grid.insert({voxel, point}); 14 | }); 15 | std::vector frame_dowsampled; 16 | frame_dowsampled.reserve(grid.size()); 17 | std::for_each(grid.cbegin(), grid.cend(), [&](const auto &voxel_and_point) { 18 | frame_dowsampled.emplace_back(voxel_and_point.second); 19 | }); 20 | return frame_dowsampled; 21 | } 22 | 23 | } // namespace kiss_icp 24 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/core/VoxelUtils.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 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 | #pragma once 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace kiss_icp { 31 | 32 | using Voxel = Eigen::Vector3i; 33 | inline Voxel PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) { 34 | return Voxel(static_cast(std::floor(point.x() / voxel_size)), 35 | static_cast(std::floor(point.y() / voxel_size)), 36 | static_cast(std::floor(point.z() / voxel_size))); 37 | } 38 | 39 | /// Voxelize a point cloud keeping the original coordinates 40 | std::vector VoxelDownsample(const std::vector &frame, 41 | const double voxel_size); 42 | 43 | } // namespace kiss_icp 44 | 45 | template <> 46 | struct std::hash { 47 | std::size_t operator()(const kiss_icp::Voxel &voxel) const { 48 | const uint32_t *vec = reinterpret_cast(voxel.data()); 49 | return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); 50 | } 51 | }; 52 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/metrics/CMakeLists.txt: -------------------------------------------------------------------------------- 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 | add_library(kiss_icp_metrics STATIC) 24 | target_sources(kiss_icp_metrics PRIVATE Metrics.cpp) 25 | target_link_libraries(kiss_icp_metrics PUBLIC Eigen3::Eigen) 26 | set_global_target_properties(kiss_icp_metrics) 27 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/metrics/Metrics.hpp: -------------------------------------------------------------------------------- 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 | #ifndef KITTI_UTILS_H_ 25 | #define KITTI_UTILS_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace kiss_icp::metrics { 32 | 33 | std::tuple SeqError(const std::vector &poses_gt, 34 | const std::vector &poses_result); 35 | 36 | std::tuple AbsoluteTrajectoryError(const std::vector &poses_gt, 37 | const std::vector &poses_result); 38 | } // namespace kiss_icp::metrics 39 | 40 | #endif // KITTI_UTILS_H_ 41 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/pipeline/CMakeLists.txt: -------------------------------------------------------------------------------- 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 | add_library(kiss_icp_pipeline STATIC) 25 | target_sources(kiss_icp_pipeline PRIVATE KissICP.cpp) 26 | target_link_libraries(kiss_icp_pipeline PUBLIC kiss_icp_core) 27 | set_global_target_properties(kiss_icp_pipeline) 28 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/cpp/kiss_icp/pipeline/KissICP.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 3 | // Stachniss. 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 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "kiss_icp/core/Registration.hpp" 30 | #include "kiss_icp/core/Threshold.hpp" 31 | #include "kiss_icp/core/VoxelHashMap.hpp" 32 | 33 | namespace kiss_icp::pipeline { 34 | 35 | struct KISSConfig { 36 | // map params 37 | double voxel_size = 1.0; 38 | double max_range = 100.0; 39 | double min_range = 5.0; 40 | int max_points_per_voxel = 20; 41 | 42 | // th parms 43 | double min_motion_th = 0.1; 44 | double initial_threshold = 2.0; 45 | 46 | // registration params 47 | int max_num_iterations = 500; 48 | double convergence_criterion = 0.0001; 49 | int max_num_threads = 0; 50 | 51 | // Motion compensation 52 | bool deskew = false; 53 | }; 54 | 55 | class KissICP { 56 | public: 57 | using Vector3dVector = std::vector; 58 | using Vector3dVectorTuple = std::tuple; 59 | 60 | public: 61 | explicit KissICP(const KISSConfig &config) 62 | : config_(config), 63 | registration_( 64 | config.max_num_iterations, config.convergence_criterion, config.max_num_threads), 65 | local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel), 66 | adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} 67 | 68 | public: 69 | Vector3dVectorTuple RegisterFrame(const std::vector &frame); 70 | Vector3dVectorTuple RegisterFrame(const std::vector &frame, 71 | const std::vector ×tamps); 72 | Vector3dVectorTuple Voxelize(const std::vector &frame) const; 73 | 74 | std::vector LocalMap() const { return local_map_.Pointcloud(); }; 75 | 76 | const VoxelHashMap &VoxelMap() const { return local_map_; }; 77 | VoxelHashMap &VoxelMap() { return local_map_; }; 78 | 79 | const Sophus::SE3d &pose() const { return last_pose_; } 80 | Sophus::SE3d &pose() { return last_pose_; } 81 | 82 | const Sophus::SE3d &delta() const { return last_delta_; } 83 | Sophus::SE3d &delta() { return last_delta_; } 84 | 85 | private: 86 | Sophus::SE3d last_pose_; 87 | Sophus::SE3d last_delta_; 88 | 89 | // KISS-ICP pipeline modules 90 | KISSConfig config_; 91 | Registration registration_; 92 | VoxelHashMap local_map_; 93 | AdaptiveThreshold adaptive_threshold_; 94 | }; 95 | 96 | } // namespace kiss_icp::pipeline 97 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/CMakeLists.txt: -------------------------------------------------------------------------------- 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 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(kiss_icp_pybind VERSION 1.0.0 LANGUAGES CXX) 25 | 26 | # Set build type 27 | set(CMAKE_BUILD_TYPE Release) 28 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 29 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 30 | 31 | set(PYBIND11_NEWPYTHON ON) 32 | find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) 33 | find_package(pybind11 CONFIG REQUIRED) 34 | 35 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp/) 36 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp ${CMAKE_CURRENT_BINARY_DIR}/kiss_icp) 37 | else() 38 | cmake_minimum_required(VERSION 3.18) 39 | message(STATUS "Performing out-of-tree build, fetching KISS-ICP v${CMAKE_PROJECT_VERSION} Release from Github") 40 | include(FetchContent) 41 | FetchContent_Declare( 42 | ext_kiss_icp_core PREFIX kiss_icp_core 43 | URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v${CMAKE_PROJECT_VERSION}.tar.gz SOURCE_SUBDIR 44 | cpp/kiss_icp) 45 | FetchContent_MakeAvailable(ext_kiss_icp_core) 46 | endif() 47 | 48 | add_subdirectory(kiss_icp/pybind) 49 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenDriveLab/MTGS/68b4c3f7d2486bbfa872c2d0f0423185dd82c199/thirdparty/kiss-icp/python/COLCON_IGNORE -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/LICENSE: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/MANIFEST.in: -------------------------------------------------------------------------------- 1 | recursive-include kiss_icp *.py 2 | recursive-include kiss_icp *.cpp 3 | recursive-include kiss_icp *.h 4 | global-include CMakeLists.txt 5 | global-include LICENSE 6 | global-include *.cmake 7 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/__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 | __version__ = "1.0.0" 24 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/config/__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 | 24 | from .parser import KISSConfig, load_config, write_config 25 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/config/config.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 typing import Optional 24 | 25 | from pydantic import BaseModel 26 | 27 | 28 | class DataConfig(BaseModel): 29 | max_range: float = 100.0 30 | min_range: float = 5.0 31 | deskew: bool = False 32 | 33 | 34 | class MappingConfig(BaseModel): 35 | voxel_size: Optional[float] = None # default: take it from data 36 | max_points_per_voxel: int = 20 37 | 38 | 39 | class RegistrationConfig(BaseModel): 40 | max_num_iterations: Optional[int] = 500 41 | convergence_criterion: Optional[float] = 0.0001 42 | max_num_threads: Optional[int] = 0 # 0 means automatic 43 | 44 | 45 | class AdaptiveThresholdConfig(BaseModel): 46 | fixed_threshold: Optional[float] = None 47 | initial_threshold: float = 2.0 48 | min_motion_th: float = 0.1 49 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/datasets/__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"] 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 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/datasets/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 | from pyquaternion import Quaternion 32 | 33 | 34 | class ApolloDataset: 35 | def __init__(self, data_dir: Path, *_, **__): 36 | try: 37 | self.o3d = importlib.import_module("open3d") 38 | except ModuleNotFoundError: 39 | print( 40 | 'pcd files requires open3d and is not installed on your system run "pip install open3d"' 41 | ) 42 | sys.exit(1) 43 | 44 | self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/pcds/*.pcd")) 45 | self.gt_poses = self.read_poses(f"{data_dir}/poses/gt_poses.txt") 46 | self.sequence_id = os.path.basename(data_dir) 47 | 48 | def __len__(self): 49 | return len(self.scan_files) 50 | 51 | def __getitem__(self, idx): 52 | return self.get_scan(self.scan_files[idx]) 53 | 54 | def get_scan(self, scan_file: str): 55 | points = np.asarray(self.o3d.io.read_point_cloud(scan_file).points, dtype=np.float64) 56 | return points.astype(np.float64) 57 | 58 | @staticmethod 59 | def read_poses(file): 60 | data = np.loadtxt(file) 61 | _, _, translations, qxyzw = np.split(data, [1, 2, 5], axis=1) 62 | rotations = np.array( 63 | [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in qxyzw] 64 | ) 65 | poses = np.zeros([rotations.shape[0], 4, 4]) 66 | poses[:, :3, -1] = translations 67 | poses[:, :3, :3] = rotations 68 | poses[:, -1, -1] = 1 69 | # Convert from global coordinate poses to local poses 70 | first_pose = poses[0, :, :] 71 | return np.linalg.inv(first_pose) @ poses 72 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/datasets/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 = poses.copy() 67 | new_poses[:, :3, :3] = np.eye(3) 68 | return new_poses 69 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/datasets/tum.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 | from pathlib import Path 26 | 27 | import numpy as np 28 | from pyquaternion import Quaternion 29 | 30 | 31 | class TUMDataset: 32 | def __init__(self, data_dir: Path, *_, **__): 33 | try: 34 | self.o3d = importlib.import_module("open3d") 35 | except ModuleNotFoundError as err: 36 | print(f'open3d is not installed on your system, run "pip install open3d"') 37 | exit(1) 38 | 39 | self.data_dir = Path(data_dir) 40 | self.sequence_id = os.path.basename(data_dir) 41 | 42 | # Load depth frames 43 | self.depth_frames = np.loadtxt(fname=self.data_dir / "depth.txt", dtype=str) 44 | 45 | # rgb single frame 46 | rgb_path = os.path.join(self.data_dir, "rgb", os.listdir(self.data_dir / "rgb")[0]) 47 | self.rgb_default_frame = self.o3d.io.read_image(rgb_path) 48 | 49 | # Load GT poses 50 | gt_list = np.loadtxt(fname=self.data_dir / "groundtruth.txt", dtype=str) 51 | self.gt_poses = self.load_poses(gt_list) 52 | 53 | def __len__(self): 54 | return len(self.depth_frames) 55 | 56 | def load_poses(self, gt_list): 57 | indices = np.unique( 58 | np.abs( 59 | ( 60 | np.subtract.outer( 61 | gt_list[:, 0].astype(np.float64), 62 | self.depth_frames[:, 0].astype(np.float64), 63 | ) 64 | ) 65 | ).argmin(0) 66 | ) 67 | xyz = gt_list[indices][:, 1:4] 68 | 69 | rotations = np.array( 70 | [ 71 | Quaternion(x=x, y=y, z=z, w=w).rotation_matrix 72 | for x, y, z, w in gt_list[indices][:, 4:] 73 | ] 74 | ) 75 | num_poses = rotations.shape[0] 76 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) 77 | poses[:, :3, :3] = rotations 78 | poses[:, :3, 3] = xyz 79 | return poses 80 | 81 | def get_frames_timestamps(self): 82 | return self.depth_frames[:, 0] 83 | 84 | def __getitem__(self, idx): 85 | depth_id = self.depth_frames[idx][-1] 86 | depth_raw = self.o3d.io.read_image(str(self.data_dir / depth_id)) 87 | rgbd_image = self.o3d.geometry.RGBDImage.create_from_tum_format( 88 | self.rgb_default_frame, depth_raw 89 | ) 90 | pcd = self.o3d.geometry.PointCloud.create_from_rgbd_image( 91 | rgbd_image, 92 | self.o3d.camera.PinholeCameraIntrinsic( 93 | self.o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault 94 | ), 95 | ) 96 | return np.array(pcd.points, dtype=np.float64) 97 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/deskew.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 numpy as np 24 | 25 | from kiss_icp.config import KISSConfig 26 | from kiss_icp.pybind import kiss_icp_pybind 27 | 28 | 29 | def get_motion_compensator(config: KISSConfig): 30 | return MotionCompensator() if config.data.deskew else StubCompensator() 31 | 32 | 33 | class StubCompensator: 34 | def deskew_scan(self, frame, timestamps, delta): 35 | return frame 36 | 37 | 38 | class MotionCompensator: 39 | def deskew_scan(self, frame, timestamps, delta): 40 | deskew_frame = kiss_icp_pybind._deskew_scan( 41 | frame=kiss_icp_pybind._Vector3dVector(frame), 42 | timestamps=timestamps, 43 | delta=delta, 44 | ) 45 | return np.asarray(deskew_frame) 46 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/mapping.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 numpy as np 24 | 25 | from kiss_icp.config import KISSConfig 26 | from kiss_icp.pybind import kiss_icp_pybind 27 | 28 | 29 | def get_voxel_hash_map(config: KISSConfig): 30 | return VoxelHashMap( 31 | voxel_size=config.mapping.voxel_size, 32 | max_distance=config.data.max_range * 2, 33 | max_points_per_voxel=config.mapping.max_points_per_voxel, 34 | ) 35 | 36 | 37 | class VoxelHashMap: 38 | def __init__(self, voxel_size: float, max_distance: float, max_points_per_voxel: int): 39 | self._internal_map = kiss_icp_pybind._VoxelHashMap( 40 | voxel_size=voxel_size, 41 | max_distance=max_distance, 42 | max_points_per_voxel=max_points_per_voxel, 43 | ) 44 | 45 | def clear(self): 46 | return self._internal_map._clear() 47 | 48 | def empty(self): 49 | return self._internal_map._empty() 50 | 51 | def update(self, points: np.ndarray, pose: np.ndarray = np.eye(4)): 52 | """Add points to the inernal map representaion. 53 | 54 | The origin is needed to remove the far away poitns 55 | 56 | TODO(NACHO): Use similar overload API as we did for VDBFusion 57 | """ 58 | self._internal_map._update(kiss_icp_pybind._Vector3dVector(points), pose) 59 | 60 | def add_points(self, points): 61 | self._internal_map._add_points(kiss_icp_pybind._Vector3dVector(points)) 62 | 63 | def remove_far_away_points(self, origin): 64 | self._internal_map._remove_far_away_points(origin) 65 | 66 | def point_cloud(self) -> np.ndarray: 67 | """Return the internal representaion as a np.array (pointcloud).""" 68 | return np.asarray(self._internal_map._point_cloud()) 69 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/metrics.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 typing import Tuple 24 | 25 | import numpy as np 26 | 27 | from kiss_icp.pybind import kiss_icp_pybind 28 | 29 | 30 | def sequence_error(gt_poses: np.ndarray, results_poses: np.ndarray) -> Tuple[float, float]: 31 | """Sptis the sequence error for a given trajectory in camera coordinate frames.""" 32 | return kiss_icp_pybind._kitti_seq_error(gt_poses, results_poses) 33 | 34 | 35 | def absolute_trajectory_error( 36 | gt_poses: np.ndarray, results_poses: np.ndarray 37 | ) -> Tuple[float, float]: 38 | """Sptis the sequence error for a given trajectory in camera coordinate frames.""" 39 | return kiss_icp_pybind._absolute_trajectory_error(gt_poses, results_poses) 40 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/preprocess.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 numpy as np 24 | 25 | from kiss_icp.config import KISSConfig 26 | from kiss_icp.pybind import kiss_icp_pybind 27 | 28 | 29 | def get_preprocessor(config: KISSConfig): 30 | return Preprocessor(config) 31 | 32 | 33 | class Preprocessor: 34 | def __init__(self, config: KISSConfig): 35 | self.config = config 36 | 37 | def __call__(self, frame: np.ndarray): 38 | return np.asarray( 39 | kiss_icp_pybind._preprocess( 40 | kiss_icp_pybind._Vector3dVector(frame), 41 | self.config.data.max_range, 42 | self.config.data.min_range, 43 | ) 44 | ) 45 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/pybind/.gitignore: -------------------------------------------------------------------------------- 1 | include/ 2 | share/ 3 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/pybind/CMakeLists.txt: -------------------------------------------------------------------------------- 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 | pybind11_add_module(kiss_icp_pybind MODULE kiss_icp_pybind.cpp) 24 | target_link_libraries(kiss_icp_pybind PRIVATE kiss_icp_core kiss_icp_metrics) 25 | install(TARGETS kiss_icp_pybind DESTINATION .) 26 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/registration.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 numpy as np 24 | 25 | from kiss_icp.config.parser import KISSConfig 26 | from kiss_icp.mapping import VoxelHashMap 27 | from kiss_icp.pybind import kiss_icp_pybind 28 | 29 | 30 | def get_registration(config: KISSConfig): 31 | return Registration( 32 | max_num_iterations=config.registration.max_num_iterations, 33 | convergence_criterion=config.registration.convergence_criterion, 34 | max_num_threads=config.registration.max_num_threads, 35 | ) 36 | 37 | 38 | class Registration: 39 | def __init__( 40 | self, 41 | max_num_iterations: int, 42 | convergence_criterion: float, 43 | max_num_threads: int = 0, 44 | ): 45 | self._registration = kiss_icp_pybind._Registration( 46 | max_num_iterations=max_num_iterations, 47 | convergence_criterion=convergence_criterion, 48 | max_num_threads=max_num_threads, 49 | ) 50 | 51 | def align_points_to_map( 52 | self, 53 | points: np.ndarray, 54 | voxel_map: VoxelHashMap, 55 | initial_guess: np.ndarray, 56 | max_correspondance_distance: float, 57 | kernel: float, 58 | ) -> np.ndarray: 59 | return self._registration._align_points_to_map( 60 | points=kiss_icp_pybind._Vector3dVector(points), 61 | voxel_map=voxel_map._internal_map, 62 | initial_guess=initial_guess, 63 | max_correspondance_distance=max_correspondance_distance, 64 | kernel=kernel, 65 | ) 66 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/threshold.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 numpy as np 24 | 25 | from kiss_icp.config import KISSConfig 26 | from kiss_icp.pybind import kiss_icp_pybind 27 | 28 | 29 | def get_threshold_estimator(config: KISSConfig): 30 | if config.adaptive_threshold.fixed_threshold is not None: 31 | return FixedThreshold(config.adaptive_threshold.fixed_threshold) 32 | return AdaptiveThreshold(config) 33 | 34 | 35 | class FixedThreshold: 36 | def __init__(self, fixed_threshold: float): 37 | self.fixed_threshold = fixed_threshold 38 | 39 | def get_threshold(self): 40 | return self.fixed_threshold 41 | 42 | def update_model_deviation(self, model_deviation): 43 | pass 44 | 45 | 46 | class AdaptiveThreshold: 47 | def __init__(self, config: KISSConfig): 48 | self._estimator = kiss_icp_pybind._AdaptiveThreshold( 49 | initial_threshold=config.adaptive_threshold.initial_threshold, 50 | min_motion_th=config.adaptive_threshold.min_motion_th, 51 | max_range=config.data.max_range, 52 | ) 53 | 54 | def get_threshold(self): 55 | return self._estimator._compute_threshold() 56 | 57 | def update_model_deviation(self, model_deviation: np.ndarray): 58 | self._estimator._update_model_deviation(model_deviation=model_deviation) 59 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/tools/__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 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/tools/pipeline_results.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 dataclasses import dataclass 24 | from typing import Optional 25 | 26 | from rich import box 27 | from rich.console import Console 28 | from rich.table import Table 29 | 30 | 31 | class PipelineResults: 32 | def __init__(self) -> None: 33 | self._results = [] 34 | 35 | def empty(self) -> bool: 36 | return len(self._results) == 0 37 | 38 | def print(self) -> None: 39 | # TODO(Nacho): Add mechanism to print ASCII if rich is not available 40 | self.log_to_console() 41 | 42 | def append(self, desc: str, units: str, value: float, trunc: bool = False): 43 | @dataclass 44 | class Metric: 45 | desc: str 46 | units: str 47 | value: float 48 | 49 | self._results.append(Metric(desc, units, int(value) if trunc else value)) 50 | 51 | def log_to_file(self, filename: str, title: Optional[str]) -> None: 52 | with open(filename, "wt") as logfile: 53 | console = Console(file=logfile, width=100, force_jupyter=False) 54 | if title: 55 | console.rule(title) 56 | console.print(self._rich_table(table_format=box.ASCII_DOUBLE_HEAD)) 57 | 58 | def log_to_console(self) -> None: 59 | if self.empty(): 60 | return 61 | console = Console() 62 | console.print(self._rich_table()) 63 | 64 | def _rich_table(self, table_format: box.Box = box.HORIZONTALS) -> Table: 65 | """Takes a metrics dictionary and sptis a Rich Table with the experiment results""" 66 | table = Table(box=table_format) 67 | table.add_column("Metric", justify="right", style="cyan") 68 | table.add_column("Value", justify="center", style="magenta") 69 | table.add_column("Units", justify="left", style="green") 70 | for result in self._results: 71 | table.add_row( 72 | result.desc, 73 | f"{result.value:{'.3f' if isinstance(result.value, float) else ''}}", 74 | result.units, 75 | ) 76 | return table 77 | 78 | def __iter__(self): 79 | return self._results.__iter__() 80 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/tools/progress_bar.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 tqdm.auto import trange 24 | 25 | 26 | def get_progress_bar(first_scan, last_scan): 27 | return trange(first_scan, last_scan, unit=" frames", ncols=120) 28 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/kiss_icp/voxelization.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 numpy as np 24 | 25 | from kiss_icp.pybind import kiss_icp_pybind 26 | 27 | 28 | def voxel_down_sample(points: np.ndarray, voxel_size: float): 29 | _points = kiss_icp_pybind._Vector3dVector(points) 30 | return np.asarray(kiss_icp_pybind._voxel_down_sample(_points, voxel_size)) 31 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["scikit_build_core", "pybind11"] 3 | build-backend = "scikit_build_core.build" 4 | 5 | [project] 6 | name = "kiss-icp" 7 | version = "1.0.0" 8 | description = "Simple yet effective 3D LiDAR-Odometry registration pipeline" 9 | readme = "README.md" 10 | authors = [ 11 | { name = "Ignacio Vizzo", email = "ignaciovizzo@gmail.com" }, 12 | ] 13 | requires-python = ">=3.7" 14 | keywords = [ 15 | "LiDAR", 16 | "Localization", 17 | "Odometry", 18 | "SLAM", 19 | ] 20 | classifiers = [ 21 | "Intended Audience :: Developers", 22 | "Intended Audience :: Education", 23 | "Intended Audience :: Other Audience", 24 | "Intended Audience :: Science/Research", 25 | "License :: OSI Approved :: MIT License", 26 | "Operating System :: MacOS", 27 | "Operating System :: Microsoft :: Windows", 28 | "Operating System :: Unix", 29 | "Programming Language :: C++", 30 | "Programming Language :: Python :: 3", 31 | "Programming Language :: Python :: 3.7", 32 | "Programming Language :: Python :: 3.8", 33 | "Programming Language :: Python :: 3.9", 34 | "Programming Language :: Python :: 3.10", 35 | "Programming Language :: Python :: 3.11", 36 | ] 37 | dependencies = [ 38 | "natsort", 39 | "numpy", 40 | "plyfile", 41 | "pydantic>=2", 42 | "pydantic-settings", 43 | "pyquaternion", 44 | "rich", 45 | "tqdm", 46 | "typer[all]>=0.6.0", 47 | ] 48 | 49 | [project.optional-dependencies] 50 | all = [ 51 | "open3d>=0.13", 52 | "polyscope>=2.2.1", 53 | "ouster-sdk>=0.11", 54 | "pyntcloud", 55 | "PyYAML", 56 | "trimesh", 57 | ] 58 | test = [ 59 | "pytest", 60 | ] 61 | visualizer = [ 62 | "open3d>=0.13", 63 | ] 64 | 65 | [project.scripts] 66 | kiss_icp_pipeline = "kiss_icp.tools.cmd:run" 67 | kiss_icp_dump_config = "kiss_icp.config.parser:write_config" 68 | 69 | [project.urls] 70 | Homepage = "https://github.com/PRBonn/kiss-icp" 71 | 72 | [tool.scikit-build] 73 | build-dir = "build/{wheel_tag}" 74 | cmake.verbose = false 75 | cmake.minimum-version = "3.16" 76 | editable.mode = "redirect" 77 | editable.rebuild = true 78 | editable.verbose = true 79 | sdist.exclude = ["pybind/"] 80 | wheel.install-dir = "kiss_icp/pybind/" 81 | 82 | [tool.black] 83 | line-length = 100 84 | 85 | [tool.isort] 86 | profile = "black" 87 | 88 | [tool.pylint.format] 89 | max-line-length = "100" 90 | 91 | [tool.cibuildwheel] 92 | archs = ["auto64"] 93 | skip = ["*-musllinux*", "pp*", "cp36-*"] 94 | 95 | [tool.cibuildwheel.macos] 96 | environment = "MACOSX_DEPLOYMENT_TARGET=10.14" 97 | archs = ["auto64", "arm64"] 98 | 99 | [tool.pytest.ini_options] 100 | testpaths = ['tests'] 101 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/python/tests/test_kiss_icp.py: -------------------------------------------------------------------------------- 1 | def test_import(): 2 | from kiss_icp.kiss_icp import KissICP 3 | 4 | assert KissICP is not None 5 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/ros/CMakeLists.txt: -------------------------------------------------------------------------------- 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 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(kiss_icp VERSION 1.0.0 LANGUAGES CXX) 25 | 26 | set(CMAKE_BUILD_TYPE Release) 27 | 28 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp/ AND NOT IS_SYMLINK ${CMAKE_CURRENT_SOURCE_DIR}) 29 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp ${CMAKE_CURRENT_BINARY_DIR}/kiss_icp) 30 | else() 31 | cmake_minimum_required(VERSION 3.18) 32 | message(STATUS "Performing out-of-tree build, fetching KISS-ICP v${CMAKE_PROJECT_VERSION} Release from Github") 33 | include(FetchContent) 34 | FetchContent_Declare( 35 | ext_kiss_icp_core PREFIX kiss_icp_core 36 | URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v${CMAKE_PROJECT_VERSION}.tar.gz SOURCE_SUBDIR 37 | cpp/kiss_icp) 38 | FetchContent_MakeAvailable(ext_kiss_icp_core) 39 | endif() 40 | 41 | find_package(ament_cmake REQUIRED) 42 | find_package(geometry_msgs REQUIRED) 43 | find_package(nav_msgs REQUIRED) 44 | find_package(rclcpp REQUIRED) 45 | find_package(rclcpp_components REQUIRED) 46 | find_package(rcutils REQUIRED) 47 | find_package(sensor_msgs REQUIRED) 48 | find_package(std_msgs REQUIRED) 49 | find_package(tf2_ros REQUIRED) 50 | 51 | # ROS 2 node 52 | add_library(odometry_component SHARED src/OdometryServer.cpp) 53 | target_compile_features(odometry_component PUBLIC cxx_std_20) 54 | target_include_directories(odometry_component PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) 55 | target_link_libraries(odometry_component kiss_icp_pipeline) 56 | ament_target_dependencies( 57 | odometry_component 58 | geometry_msgs 59 | nav_msgs 60 | rclcpp 61 | rclcpp_components 62 | rcutils 63 | sensor_msgs 64 | std_msgs 65 | tf2_ros) 66 | 67 | rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE kiss_icp_node) 68 | 69 | install(TARGETS odometry_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) 70 | install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/) 71 | 72 | ament_package() 73 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/ros/LICENSE: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/ros/README.md: -------------------------------------------------------------------------------- 1 | # KISS-ICP ROS 2 Wrapper 2 | 3 | ### How to build 4 | 5 | You should not need any extra dependency, just clone and build: 6 | 7 | ```sh 8 | git clone https://github.com/PRBonn/kiss-icp 9 | colcon build 10 | source ./install/setup.bash 11 | ``` 12 | 13 | ### How to run 14 | 15 | The only required argument to provide is the **topic name** so KISS-ICP knows which PointCloud2 to process: 16 | 17 | ```sh 18 | ros2 launch kiss_icp odometry.launch.py bagfile:= topic:= 19 | ``` 20 | 21 | You can optionally launch the node with any bagfile, and play the bagfiles on a different shell: 22 | 23 | ```sh 24 | ros2 launch kiss_icp odometry.launch.py topic:= 25 | ``` 26 | 27 | and then, 28 | 29 | ```sh 30 | ros2 bag play *.bag 31 | ``` 32 | 33 | ## Out of source builds 34 | 35 | Good news! If you don't have git or you don't need to change the core KISS-ICP library, you can just 36 | copy paste this folder into your ROS2 workspace and build as usual. The build system will fetch 37 | the latest stable release for you. 38 | 39 | ## Looking how to run KITTI on ROS along with KISS-ICP? 40 | 41 | I believe you could use the python API instead, to avoid converting all the data to a format that is 42 | not the original one. If you still insist in doing it, I've created a separate repository for this, 43 | just clone and build [this package](https://github.com/nachovizzo/kiss_icp_kitti) 44 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | kiss_icp 28 | 1.0.0 29 | KISS-ICP ROS 2 Wrapper 30 | ivizzo 31 | MIT 32 | 33 | ament_cmake 34 | 35 | geometry_msgs 36 | nav_msgs 37 | rclcpp 38 | rclcpp_components 39 | rcutils 40 | sensor_msgs 41 | std_msgs 42 | tf2_ros 43 | 44 | eigen 45 | sophus 46 | tbb 47 | robin-map-dev 48 | 49 | ros2launch 50 | 51 | 52 | ament_cmake 53 | 54 | 55 | -------------------------------------------------------------------------------- /thirdparty/kiss-icp/ros/src/OdometryServer.hpp: -------------------------------------------------------------------------------- 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 | #pragma once 24 | 25 | // KISS-ICP 26 | #include "kiss_icp/pipeline/KissICP.hpp" 27 | 28 | // ROS 2 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace kiss_icp_ros { 40 | 41 | class OdometryServer : public rclcpp::Node { 42 | public: 43 | /// OdometryServer constructor 44 | OdometryServer() = delete; 45 | explicit OdometryServer(const rclcpp::NodeOptions &options); 46 | 47 | private: 48 | /// Register new frame 49 | void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 50 | 51 | /// Stream the estimated pose to ROS 52 | void PublishOdometry(const Sophus::SE3d &kiss_pose, const std_msgs::msg::Header &header); 53 | 54 | /// Stream the debugging point clouds for visualization (if required) 55 | void PublishClouds(const std::vector frame, 56 | const std::vector keypoints, 57 | const std_msgs::msg::Header &header); 58 | 59 | private: 60 | /// Tools for broadcasting TFs. 61 | std::unique_ptr tf_broadcaster_; 62 | std::unique_ptr tf2_buffer_; 63 | std::unique_ptr tf2_listener_; 64 | bool invert_odom_tf_; 65 | bool publish_odom_tf_; 66 | bool publish_debug_clouds_; 67 | 68 | /// Data subscribers. 69 | rclcpp::Subscription::SharedPtr pointcloud_sub_; 70 | 71 | /// Data publishers. 72 | rclcpp::Publisher::SharedPtr odom_publisher_; 73 | rclcpp::Publisher::SharedPtr frame_publisher_; 74 | rclcpp::Publisher::SharedPtr kpoints_publisher_; 75 | rclcpp::Publisher::SharedPtr map_publisher_; 76 | 77 | /// KISS-ICP 78 | std::unique_ptr kiss_icp_; 79 | 80 | /// Global/map coordinate frame. 81 | std::string lidar_odom_frame_{"odom_lidar"}; 82 | std::string base_frame_{}; 83 | 84 | /// Covariance diagonal 85 | double position_covariance_; 86 | double orientation_covariance_; 87 | }; 88 | 89 | } // namespace kiss_icp_ros 90 | --------------------------------------------------------------------------------