├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── build_benchmark_md.py ├── config ├── dataset │ ├── ford_campus.yaml │ ├── kitti.yaml │ ├── kitti_360.yaml │ ├── nclt.yaml │ ├── nhcd.yaml │ └── urban_loco.yaml ├── hydra │ └── output │ │ ├── slam.yaml │ │ └── training.yaml ├── slam.yaml ├── slam │ ├── backend │ │ ├── graph_slam.yaml │ │ └── none.yaml │ ├── initialization │ │ ├── CV.yaml │ │ ├── EI.yaml │ │ └── PoseNet.yaml │ ├── loop_closure │ │ └── none.yaml │ ├── odometry │ │ ├── alignment │ │ │ └── point_to_plane_GN.yaml │ │ ├── deep_odometry.yaml │ │ ├── icp_odometry.yaml │ │ └── local_map │ │ │ ├── kdtree.yaml │ │ │ └── projective.yaml │ └── preprocessing │ │ ├── grid_sample.yaml │ │ ├── none.yaml │ │ └── voxelization.yaml ├── train_posenet.yaml └── training │ ├── loss │ ├── supervised.yaml │ └── unsupervised.yaml │ └── prediction │ └── poseresnet18.yaml ├── docs ├── benchmark.md ├── data │ ├── ei.png │ ├── example_pointclouds.png │ ├── minimal_example.png │ ├── overview.png │ ├── point_to_plane.png │ ├── project_structure.png │ ├── pylidar_slam_1.png │ └── pylidar_slam_3.png ├── datasets.md ├── results │ ├── KITTI │ │ └── kitti_benchmark.md │ └── NHCD │ │ └── nhcd_benchmark.md └── toolbox.md ├── replay.py ├── requirements.txt ├── run.py ├── scripts └── generate_urban_loco_gt.py ├── slam ├── __init__.py ├── backend.py ├── common │ ├── __init__.py │ ├── geometry.py │ ├── io.py │ ├── modules.py │ ├── optimization.py │ ├── pointcloud.py │ ├── pose.py │ ├── projection.py │ ├── registration.py │ ├── rotation.py │ ├── timer.py │ ├── torch_utils.py │ └── utils.py ├── dataset │ ├── __init__.py │ ├── configuration.py │ ├── ct_icp_dataset.py │ ├── dataset.py │ ├── ford_dataset.py │ ├── kitti_360_dataset.py │ ├── kitti_dataset.py │ ├── nclt_dataset.py │ ├── nhcd_dataset.py │ ├── rosbag_dataset.py │ ├── sequence_dataset.py │ └── urban_loco_dataset.py ├── eval │ ├── __init__.py │ └── eval_odometry.py ├── initialization.py ├── loop_closure.py ├── models │ ├── __init__.py │ ├── _resnet.py │ ├── layers.py │ └── posenet.py ├── odometry │ ├── __init__.py │ ├── alignment.py │ ├── ct_icp_odometry.py │ ├── icp_odometry.py │ ├── local_map.py │ ├── odometry.py │ ├── odometry_runner.py │ └── posenet_odometry.py ├── preprocessing.py ├── slam.py ├── training │ ├── __init__.py │ ├── loss_modules.py │ ├── prediction_modules.py │ └── trainer.py └── viz │ ├── __init__.py │ ├── color_map.py │ └── visualizer.py ├── tests ├── test_backend.py ├── test_geometry.py ├── test_optimization.py └── test_pointcloud.py └── train.py /.gitignore: -------------------------------------------------------------------------------- 1 | # untrack .* hidden folders 2 | .* 3 | 4 | # ommit git files from this rule 5 | !.gitignore 6 | !.gitmodules 7 | !.hydra 8 | 9 | # Define a rule to hide specific folder and files 10 | __* 11 | # Ommit __init__.py (s) from this rule 12 | !__init__.py 13 | modules/* 14 | 15 | # ignore cached compiled python 16 | *__pycache__* 17 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "modules/external/pybind11"] 2 | path = modules/external/pybind11 3 | url = https://github.com/pybind/pybind11 4 | branch = stable 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Kitware (Pierre Dellenbach) 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. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pyLiDAR-SLAM 2 | 3 | This codebase proposes modular light python and pytorch implementations of several LiDAR Odometry methods, 4 | which can easily be evaluated and compared on a set of public Datasets. 5 | 6 | It heavily relies on [omegaconf](https://omegaconf.readthedocs.io/en/2.0_branch/) and [hydra](https://hydra.cc/), 7 | which allows us to easily test the different modules and parameters with few but structured configuration files. 8 | 9 | 10 | This is a research project provided "as-is" without garanties, 11 | use at your own risk. It is actively used for **[Kitware Vision team](https://www.kitware.fr/equipe-vision-par-odinateur/)** internal research thus is likely to be heavily extended, 12 | rewritten (and hopefully improved) in a near future. 13 | 14 | 15 | 16 | ## Overview 17 | 18 | ![KITTI Sequence 00 with pyLiDAR-SLAM](docs/data/example_pointclouds.png) 19 | 20 | *pyLIDAR-SLAM* is designed to be modular, multiple components are implemented at each stage of the pipeline. 21 | Its modularity can make it a bit complicated to use. We provide this [wiki](https://github.com/Kitware/pyLiDAR-SLAM/wiki) to help you navigate it. 22 | If you have any questions, do not hesitate raising issues. 23 | 24 | The documentation is organised as follows: 25 | 26 | - [`INSTALLATION:`](https://github.com/Kitware/pyLiDAR-SLAM/wiki/INSTALLATION) Describes how to install pyLiDAR-SLAM and its different components 27 | - [`DATASETS:`](https://github.com/Kitware/pyLiDAR-SLAM/wiki/DATASETS) Describes the different datasets integrated in pyLiDAR-SLAM, and how to install them 28 | - [`TOOLBOX:`](https://github.com/Kitware/pyLiDAR-SLAM/wiki/SLAM-LiDAR-Toolbox) Describes the contents of the toolbox and the different modules proposed 29 | - [`BENCHMARK:`](https://github.com/Kitware/pyLiDAR-SLAM/wiki/Benchmark) Describes the benchmarks supported in the Dataset **/!\ Note:** This section is still in construction 30 | 31 | The goal for the future is to gradually add functionalities to pyLIDAR-SLAM (Loop Closure, Motion Segmentation, Multi-Sensors, etc...). 32 | 33 | ## News 34 | 35 | > **[08/10/2021]:** We also introduce support for individual rosbags (Introducing naturally an overhead compared to using ROS directly, but provides the flexibility of pyLiDAR-SLAM) 36 | > 37 | > **[08/10/2021]:** We release code for Loop Closure with **pyLiDAR-SLAM** accompanied with a simple **PoseGraph** Optimization. 38 | > 39 | > **[08/10/2021]:** We release our [new work](https://arxiv.org/abs/2109.12979) on arXiv. It proposes a new state-of-the-art pure LiDAR odometry implemented in C++ (check the [project](https://github.com/jedeschaud/ct_icp) main page). python wrappings are available, and it can be used with **pyLiDAR-SLAM**. 40 | 41 | ## Installation 42 | 43 | See the wiki page [INSTALLATION](https://github.com/Kitware/pyLiDAR-SLAM/wiki/INSTALLATION) for instruction to install the code base and the modules you are interested in. 44 | 45 | ### DATASETS 46 | 47 | *pyLIDAR-SLAM* incorporates different datasets, see [DATASETS](https://github.com/Kitware/pyLiDAR-SLAM/wiki/DATASETS) for installation and setup instructions for each of these datasets. 48 | Only the datasets implemented in *pyLIDAR-SLAM* are compatible with hydra's mode and the scripts `run.py` and `train.py`. 49 | 50 | But you can define your own datasets by extending the class [`DatasetLoader`](slam/dataset/dataset.py). 51 | 52 | **New:** We support individual rosbags (without requiring a complete ROS installation). See the minimal example for more details. 53 | 54 | 55 | ## A Minimal Example 56 | 57 | > Download a rosbag (e.g. From Rosbag Cartographer): 58 | [example_rosbag](https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-15-51-36.bag) 59 | > 60 | > **Note:** You need the `rosbag` python module installed to run this example (see [INSTALLATION](https://github.com/Kitware/pyLiDAR-SLAM/wiki/INSTALLATION) for instructions) 61 | 62 | > Launch the SLAM: 63 | 64 | ``` 65 | python3 run.py num_workers=1 / # The number of process workers to load the dataset (should be at most 1 for a rosbag) 66 | slam/initialization=NI / # The initialization considered (NI=No Initialization / CV=Constant Velocity, etc...) 67 | slam/preprocessing=grid_sample / # Preprocessing on the point clouds 68 | slam/odometry=icp_odometry / # The Odometry algorithm 69 | slam.odometry.viz_debug=True / # Whether to launch the visualization of the odometry 70 | slam/loop_closure=none / # The loop closure algorithm selected (none by default) 71 | slam/backend=none / # The backend algorithm (none by default) 72 | dataset=rosbag / # The dataset selected (a simple rosbag here) 73 | dataset.main_topic=horizontal_laser_3d / # The pointcloud topic of the rosbag 74 | dataset.accumulate_scans=True / # Whether to accumulate multiple messages (a sensor can return multiple scans lines or an accumulation of scans) 75 | dataset.file_path=/b3-2016-04-05-15-51-36.bag / # The path to the rosbag file 76 | hydra.run.dir=.outputs/TEST_DOC # The log directory where the trajectory will be saved 77 | ``` 78 | 79 | > This will output the trajectory, log files (including the full config) on disk at location `.outputs/TEST_DOC`. 80 | > 81 | > Our minimal LiDAR Odometry, is actually a naïve baseline implementation, which is mostly designed and tested on driving datasets (see the KITTI benchmark). 82 | > Thus in many cases it will fail, be imprecise or too slow. 83 | > 84 | > We recommend you install the module `pyct_icp` from [our recent work](https://github.com/jedeschaud/ct_icp), which provides a much more versatile and precise LiDAR-Odometry. 85 | > 86 | > See [the wiki page INSTALLATION](https://github.com/Kitware/pyLiDAR-SLAM/wiki/INSTALLATION) for more details on how to install the different modules. 87 | > If you want to visualize in real time the quality of the SLAM, consider also installing the module `pyviz3d`. 88 | 89 | > Once `pyct_icp` is installed, you can modify the command line above: 90 | ``` 91 | python3 run.py num_workers=1 / 92 | slam/initialization=NI / 93 | slam/preprocessing=none / 94 | slam/odometry=ct_icp_robust_shaky / # The CT-ICP algorithm for shaky robot sensor (here it is for a backpack) 95 | slam.odometry.viz_debug=True / 96 | slam/loop_closure=none / 97 | slam/backend=none / 98 | dataset=rosbag / 99 | dataset.main_topic=horizontal_laser_3d / 100 | dataset.accumulate_scans=True / 101 | dataset.file_path=/b3-2016-04-05-15-51-36.bag / 102 | hydra.run.dir=.outputs/TEST_DOC 103 | ``` 104 | 105 | > It will launch pyct_icp on the same rosbag (running much faster than our python based odometry) 106 | > 107 | > With `pyviz3d` you should see the following reconstruction (obtained by a backpack mounting the stairs of a museum): 108 | 109 | ![Minimal Example](docs/data/minimal_example.png) 110 | 111 | 112 | ## More advanced examples / Motivation 113 | 114 | > **pyLiDAR-SLAM** will progressively include more and more modules, to build more powerful and more accessible LiDAR odometries. 115 | > 116 | > For a more detailed / advanced usage of the toolbox please refer to our documentation in the wiki [HOME](https://github.com/Kitware/pyLiDAR-SLAM/wiki/HOME). 117 | 118 | > The motivation behind the toolbox, is really to compare different modules, **hydra** is very useful for this purpose. 119 | > 120 | > For example the script below launches consecutively the `pyct_icp` and `icp_odometry` odometries on the same datasets. 121 | 122 | ``` 123 | python3 run.py -m / # We specify the -m option to tell hydra to perform a sweep (or grid search on the given arguments) 124 | num_workers=1 / 125 | slam/initialization=NI / 126 | slam/preprocessing=none / 127 | slam/odometry=ct_icp_robust_shaky, icp_odometry / # The two parameters of the grid search: two different odometries 128 | slam.odometry.viz_debug=True / 129 | slam/loop_closure=none / 130 | slam/backend=none / 131 | dataset=rosbag / 132 | dataset.main_topic=horizontal_laser_3d / 133 | dataset.accumulate_scans=True / 134 | dataset.file_path=/b3-2016-04-05-15-51-36.bag / 135 | hydra.run.dir=.outputs/TEST_DOC 136 | ``` 137 | 138 | #### Benchmarks 139 | 140 | We use this functionality of *pyLIDAR-SLAM* to compare the performances of its different modules on different datasets. 141 | In [Benchmark](https://github.com/Kitware/pyLiDAR-SLAM/wiki/Benchmark) we present the results of *pyLIDAR-SLAM* on the most popular open-source datasets. 142 | 143 | Note this work in still in construction, and we aim to improve it and make it more extensive in the future. 144 | 145 | 146 | #### Research results 147 | 148 | > Small improvements will be regularly made to `pyLiDAR-SLAM`, However *major changes / new modules* will more likely be introduced along research articles (which we aim to integrate with this project in the future) 149 | > 150 | > Please check [RESEARCH](https://github.com/Kitware/pyLiDAR-SLAM/wiki/RESEARCH) to see the research papers associated to this work. 151 | 152 | 153 | 154 | 155 | 156 | ### System Tested 157 | 158 | | OS | CUDA | pytorch | python | hydra | 159 | | --- | --- | --- | --- | --- | 160 | | Ubuntu 18.04 | 10.2 | 1.7.1 | 3.8.8 | 1.0 | 161 | 162 | ### Author 163 | This is a work realised in the context of Pierre Dellenbach PhD thesis under supervision of [Bastien Jacquet](https://www.linkedin.com/in/bastienjacquet/?originalSubdomain=fr) ([Kitware](https://www.kitware.com/computer-vision/)), 164 | Jean-Emmanuel Deschaud & François Goulette (Mines ParisTech). 165 | 166 | ### Cite 167 | 168 | If you use this work for your research, consider citing: 169 | 170 | ``` 171 | @misc{dellenbach2021s, 172 | title={What's in My LiDAR Odometry Toolbox?}, 173 | author={Pierre Dellenbach, 174 | Jean-Emmanuel Deschaud, 175 | Bastien Jacquet, 176 | François Goulette}, 177 | year={2021}, 178 | } 179 | ``` 180 | -------------------------------------------------------------------------------- /build_benchmark_md.py: -------------------------------------------------------------------------------- 1 | """ 2 | This script builds a `_benchmark.md` file which aggregates the results saved on disk. 3 | 4 | It searches for recursively for all results in a root directory, computes the trajectory error, ranks the results, 5 | And display writes the `nhcd_benchmark.md` files which contains the table aggregating all the results. 6 | 7 | If many trajectories need to be evaluated, this script can take a long time. 8 | """ 9 | 10 | import sys 11 | import os 12 | from dataclasses import dataclass 13 | 14 | import hydra 15 | from hydra.core.config_store import ConfigStore 16 | 17 | from slam.common.io import * 18 | from slam.eval.eval_odometry import * 19 | 20 | 21 | @dataclass 22 | class BenchmarkBuilderConfig: 23 | root_dir: str = "." 24 | dataset: str = "kitti" 25 | output_dir: str = ".benchmark" 26 | 27 | 28 | cs = ConfigStore.instance() 29 | cs.store(name="benchmark", node=BenchmarkBuilderConfig) 30 | 31 | 32 | def load_dataset(dataset: str) -> tuple: 33 | _datasets = ["kitti", "nhcd", "ford_campus", "nclt", "kitti_360", "ct_icp_kitti", "ct_icp_kitti_carla"] 34 | assert_debug(dataset in _datasets, 35 | f"The dataset {dataset} is not supported") 36 | if dataset == "kitti": 37 | return "KITTI", [f"{i:02}" for i in range(11)] 38 | if dataset == "kitti_360": 39 | return "KITTI_360", ["0", "2", "3", "4", "5", "6", "7", "9", "10"] 40 | if dataset == "nhcd": 41 | return "Newer Handheld College Dataset", ["01_short_experiment", "02_long_experiment"] 42 | if dataset == "nclt": 43 | return "NCLT Long Pose Dataset", ["2012-01-08", "2012-01-15", "2012-01-22", "2013-01-10"] 44 | if dataset == "ford_campus": 45 | return "Ford Campus Dataset", ["dataset-1", "dataset-2"] 46 | if dataset == "ct_icp_kitti": 47 | return "KITTI_raw", [f"{i:02}" for i in range(11) if i != 3] 48 | if dataset == "ct_icp_kitti_carla": 49 | return "KITTI_CARLA", [f"Town{i + 1:02}" for i in range(7)] 50 | 51 | 52 | @hydra.main(config_path=None, config_name="benchmark") 53 | def build_benchmark(cfg: BenchmarkBuilderConfig) -> None: 54 | """Builds the benchmark""" 55 | 56 | root_dir = cfg.root_dir 57 | dataset = cfg.dataset 58 | 59 | dataset_name, folder_names = load_dataset(dataset) 60 | 61 | metrics = {} # map root_path -> computed metrics 62 | # Recursively search all child directories for folder with the appropriate name 63 | directory_list = [x[0] for x in os.walk(root_dir)] # Absolute paths 64 | 65 | output_root = Path(cfg.output_dir) 66 | if not output_root.exists(): 67 | output_root.mkdir() 68 | 69 | for new_dir in directory_list: 70 | 71 | is_metrics_dir = False 72 | new_dir_path = Path(new_dir) 73 | for folder in folder_names: 74 | if (new_dir_path / folder).exists(): 75 | is_metrics_dir = True 76 | 77 | if is_metrics_dir: 78 | # New entry found compute and add the metrics for each sequence 79 | new_metrics = {} 80 | has_all_sequences = True 81 | for sequence_name in folder_names: 82 | sequence_path = new_dir_path / sequence_name 83 | 84 | poses_file = sequence_path / f"{sequence_name}.poses.txt" 85 | gt_poses_file = sequence_path / f"{sequence_name}_gt.poses.txt" 86 | if poses_file.exists() and gt_poses_file.exists(): 87 | if not new_metrics: 88 | print(f"[INFO]Found a results directory at {new_dir}") 89 | 90 | print(f"[INFO]Computing trajectory error for sequence {sequence_name}") 91 | # Can compute metrics on both files 92 | gt_poses = read_poses_from_disk(gt_poses_file) 93 | poses = read_poses_from_disk(poses_file) 94 | 95 | # Try to read the configuration files and metrics 96 | metrics_yaml = sequence_path / "metrics.yaml" 97 | time_ms = -1.0 98 | if metrics_yaml.exists(): 99 | with open(str(metrics_yaml), "r") as stream: 100 | metrics_dict = yaml.safe_load(stream) 101 | if sequence_name in metrics_dict and "nsecs_per_frame" in metrics_dict[sequence_name]: 102 | time_ms = float(metrics_dict[sequence_name]["nsecs_per_frame"]) * 1000.0 103 | 104 | tr_err, rot_err, errors = compute_kitti_metrics(poses, gt_poses) 105 | 106 | new_metrics[sequence_name] = { 107 | "tr_err": tr_err, 108 | "rot_err": rot_err, 109 | "errors": errors, 110 | "average_time": time_ms 111 | } 112 | else: 113 | has_all_sequences = False 114 | 115 | if new_metrics: 116 | if has_all_sequences: 117 | # Compute the average errors 118 | _errors = [] 119 | for seq_metrics in new_metrics.values(): 120 | _errors += seq_metrics["errors"] 121 | seq_metrics.pop("errors") 122 | avg_tr_err = sum([error["tr_err"][0] for error in _errors]) / len(_errors) 123 | new_metrics["AVG_tr_err"] = avg_tr_err * 100 124 | new_metrics["AVG_time"] = sum([new_metrics[seq]["average_time"] for seq in folder_names]) / len( 125 | folder_names) 126 | else: 127 | new_metrics["AVG_tr_err"] = -1.0 128 | new_metrics["AVG_time"] = -1.0 129 | 130 | # Try to read the config to find a git_hash 131 | config_path = new_dir_path / "config.yaml" 132 | if config_path.exists(): 133 | with open(str(config_path), "r") as stream: 134 | config_dict = yaml.safe_load(stream) 135 | if "git_hash" in config_dict: 136 | new_metrics["git_hash"] = config_dict["git_hash"] 137 | 138 | new_metrics["has_all_sequences"] = has_all_sequences 139 | metrics[new_dir] = new_metrics 140 | 141 | # Try and load the overrides 142 | overrides_file = new_dir_path / ".hydra" / "overrides.yaml" 143 | if overrides_file.exists(): 144 | with open(str(overrides_file), "r") as stream: 145 | overrides_list = yaml.safe_load(stream) 146 | command_line = "`python run.py " + " ".join(overrides_list) + "`" 147 | new_metrics["command"] = command_line 148 | 149 | # Build the nhcd_benchmark.md table 150 | db_metrics = [(path, 151 | entry_metrics["AVG_tr_err"], 152 | entry_metrics["has_all_sequences"]) for path, entry_metrics in metrics.items()] 153 | db_metrics.sort(key=lambda x: x[1] if x[2] else float("inf")) 154 | 155 | # Build the list 156 | header = [f"## {dataset_name} Benchmark:\n\n\n"] 157 | main_table_lines = ["#### Sorted trajectory error on all sequences:\n", 158 | f"| **Sequence Folder**|{' | '.join(folder_names)} | AVG | AVG Time (ms) |\n", 159 | "| ---: " * (len(folder_names) + 3) + "|\n"] 160 | 161 | command_lines = ["#### Command Lines for each entry\n", 162 | f"| **Sequence Folder** | Command Line | git hash |\n", 163 | "| ---: | ---: | ---: |\n"] 164 | 165 | for entry in db_metrics: 166 | path, avg, add_avg = entry 167 | path_id = os.path.split(path)[1] 168 | _metrics = metrics[path] 169 | avg_time = _metrics["AVG_time"] 170 | columns = ' | '.join( 171 | [f"{float(_metrics[seq]['tr_err']) * 100:.4f}" if seq in _metrics else '' for seq in folder_names]) 172 | 173 | path_link = f"[{path_id}]({str(Path(path).resolve())})" 174 | line = f"| {path_link} | {columns} | {f'{avg:.4f}' if add_avg else ''} | {f'{avg_time:.3f}'} |\n" 175 | main_table_lines.append(line) 176 | 177 | command_lines.append( 178 | f"| {path_link} | {_metrics['command'] if 'command' in _metrics else ''} | {_metrics['git_hash'] if 'git_hash' in _metrics else ''}|\n") 179 | 180 | output_file = str(output_root / f"{dataset_name}_benchmark.md") 181 | with open(output_file, "w") as stream: 182 | stream.writelines(header) 183 | stream.write("\n\n") 184 | stream.writelines(main_table_lines) 185 | stream.write("\n\n") 186 | stream.writelines(command_lines) 187 | 188 | 189 | if __name__ == "__main__": 190 | # Set the working directory to current directory 191 | sys.argv.append(f'hydra.run.dir={os.getcwd()}') 192 | # Disable logging 193 | sys.argv.append("hydra/hydra_logging=disabled") 194 | sys.argv.append("hydra/job_logging=disabled") 195 | sys.argv.append("hydra.output_subdir=null") 196 | build_benchmark() 197 | -------------------------------------------------------------------------------- /config/dataset/ford_campus.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | dataset: ford_campus 3 | root_dir: ${env:FORD_ROOT} 4 | 5 | train_sequences: ["dataset-1", "dataset-2"] 6 | test_sequences: [] 7 | eval_sequences: [] 8 | 9 | lidar_height: 64 10 | lidar_width: 720 11 | up_fov: 3 12 | down_fov: -24 13 | 14 | -------------------------------------------------------------------------------- /config/dataset/kitti.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | dataset: kitti 3 | kitti_sequence_dir: ${env:KITTI_ODOM_ROOT} 4 | lidar_height: 64 5 | lidar_width: 720 6 | up_fov: 3 7 | down_fov: -24 8 | train_sequences: [ "00", "01", "02", "03", "04", "05", "06", "07", "08"] 9 | test_sequences: ["09", "10"] 10 | eval_sequences: ["09", "10"] 11 | -------------------------------------------------------------------------------- /config/dataset/kitti_360.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | dataset: kitti_360 3 | root_dir: ${env:KITTI_360_ROOT} 4 | lidar_height: 64 5 | lidar_width: 720 6 | up_fov: 3 7 | down_fov: -24 8 | train_sequences: [ 0, 2, 3, 4, 5, 6, 7, 9, 10 ] 9 | test_sequences: [ 9, 10 ] 10 | eval_sequences: [ 9, 10 ] 11 | -------------------------------------------------------------------------------- /config/dataset/nclt.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | dataset: nclt 3 | root_dir: ${env:NCLT_ROOT} 4 | 5 | train_sequences: ["2012-01-08"] 6 | test_sequences: [] 7 | eval_sequences: [] 8 | 9 | # LiDAR Spherical projection configuration 10 | lidar_height: 40 11 | lidar_width: 720 12 | up_fov: 30.0 13 | down_fov: -5.0 14 | 15 | -------------------------------------------------------------------------------- /config/dataset/nhcd.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | dataset: nhcd 3 | root_dir: ${env:NHCD_ROOT} 4 | lidar_height: 64 5 | lidar_width: 720 6 | up_fov: 25 7 | down_fov: -25 8 | train_sequences: [ "01_short_experiment", "02_long_experiment" ] 9 | test_sequences: [ ] 10 | eval_sequences: [ ] 11 | -------------------------------------------------------------------------------- /config/dataset/urban_loco.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | dataset: urban_loco 3 | root_dir: ${env:URBAN_LOCO_ROOT} 4 | lidar_height: 64 5 | lidar_width: 720 6 | up_fov: 3 7 | down_fov: -24 8 | train_sequences: [ "CALombardStreet", "CAGoldenBridge", 9 | "CABayBridge", "CAMarketStreet", "CARussianHill", 10 | "CAChinaTown", "CAColiTower", "HK-Data20190426-2", 11 | "HK-Data20190426-2", "HK-Data20190426-1", 12 | "HK-Data20190316-2", "HK-Data20190316-1" ] 13 | 14 | test_sequences: [ ] 15 | eval_sequences: [ ] 16 | -------------------------------------------------------------------------------- /config/hydra/output/slam.yaml: -------------------------------------------------------------------------------- 1 | # @package hydra 2 | 3 | run: 4 | dir: .outputs/slam/${now:%Y-%m-%d}_${now:%H-%M-%S} 5 | sweep: 6 | dir: .outputs/slam_sweep/${now:%Y-%m-%d}_${now:%H-%M-%S} 7 | subdir: ${hydra.job.num} -------------------------------------------------------------------------------- /config/hydra/output/training.yaml: -------------------------------------------------------------------------------- 1 | # @package hydra 2 | 3 | run: 4 | dir: .outputs/.training/${now:%Y-%m-%d}_${now:%H-%M-%S} 5 | sweep: 6 | dir: .outputs/.training_sweep/${now:%Y-%m-%d}_${now:%H-%M-%S} 7 | subdir: ${hydra.job.num} -------------------------------------------------------------------------------- /config/slam.yaml: -------------------------------------------------------------------------------- 1 | num_workers: 4 # The number of workers to load the data 2 | device: cpu # The device for the main pytorch computations 3 | fail_dir: "" 4 | move_if_fail: false 5 | 6 | dataset: 7 | with_numpy_pc: true # Whether to add numpy_pc in the data_dict 8 | 9 | slam: 10 | 11 | # Preprocessing config 12 | preprocessing: ??? 13 | 14 | # Odometry config 15 | odometry: ??? 16 | 17 | # Backend Config 18 | backend: ??? 19 | 20 | # Loop Closure Config 21 | loop_closure: ??? 22 | 23 | defaults: 24 | - slam/odometry: icp_odometry 25 | - dataset: kitti 26 | - slam/preprocessing: none 27 | - slam/initialization: CV 28 | - slam/loop_closure: none 29 | - slam/backend: none 30 | - hydra/output: slam 31 | 32 | -------------------------------------------------------------------------------- /config/slam/backend/graph_slam.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | type: graph_slam -------------------------------------------------------------------------------- /config/slam/backend/none.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | type: none -------------------------------------------------------------------------------- /config/slam/initialization/CV.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | type: cv -------------------------------------------------------------------------------- /config/slam/initialization/EI.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | type: ei 3 | debug: false 4 | ni_if_failure: false 5 | registration_config: 6 | z_min: 0 7 | z_max: 10 8 | -------------------------------------------------------------------------------- /config/slam/initialization/PoseNet.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | type: posenet 3 | train_dir: ${env:TRAIN_DIR} 4 | 5 | prediction: ??? 6 | 7 | -------------------------------------------------------------------------------- /config/slam/loop_closure/none.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | type: none -------------------------------------------------------------------------------- /config/slam/odometry/alignment/point_to_plane_GN.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | 3 | mode: point_to_plane_gauss_newton 4 | 5 | gauss_newton_config: 6 | scheme: geman_mcclure 7 | sigma: 0.3 8 | max_iters: 1 9 | -------------------------------------------------------------------------------- /config/slam/odometry/deep_odometry.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | 3 | algorithm: posenet 4 | checkpoint_file: checkpoint.ckp 5 | train_config_file: config.yaml 6 | train_dir: ${env:TRAIN_DIR} 7 | posenet_config: ??? 8 | 9 | -------------------------------------------------------------------------------- /config/slam/odometry/icp_odometry.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | 3 | algorithm: "icp_F2M" 4 | max_num_alignments: 10 5 | 6 | data_key: numpy_pc # The pointcloud on which the ICP is performed 7 | 8 | local_map: ??? # config of the local map 9 | initialization: ??? # config of the initialization 10 | alignment: ??? # config of the rigid alignment 11 | -------------------------------------------------------------------------------- /config/slam/odometry/local_map/kdtree.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | type: kdtree_local_map 3 | local_map_size: 20 4 | 5 | 6 | -------------------------------------------------------------------------------- /config/slam/odometry/local_map/projective.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | type: projective_local_map 3 | local_map_size: 20 4 | -------------------------------------------------------------------------------- /config/slam/preprocessing/grid_sample.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | 3 | filters: 4 | 5 | "1": 6 | filter_name: distortion 7 | force: false # Whether to fail if timestamps cannot be found 8 | activate: true # Distortion is only activated if this activate=true 9 | pointcloud_key: numpy_pc # The point cloud key in the dict 10 | timestamps_key: numpy_pc_timestamps # The timestamps key in the dict 11 | output_key: distorted 12 | 13 | "2": 14 | filter_name: grid_sample 15 | voxel_size: 0.3 16 | pointcloud_key: distorted 17 | 18 | "3": 19 | filter_name: to_tensor 20 | keys: 21 | sample_points: input_data 22 | -------------------------------------------------------------------------------- /config/slam/preprocessing/none.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | filters: 3 | -------------------------------------------------------------------------------- /config/slam/preprocessing/voxelization.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | 3 | filters: 4 | "1": 5 | filter_name: voxelization 6 | voxel_size: 0.2 7 | 8 | "2": 9 | filter_name: to_tensor 10 | keys: 11 | voxel_means: input_data 12 | voxel_covariances: input_covariances 13 | -------------------------------------------------------------------------------- /config/train_posenet.yaml: -------------------------------------------------------------------------------- 1 | train_dir: ${env:TRAIN_DIR} 2 | sequence_len: 2 3 | num_input_channels: 3 4 | 5 | defaults: 6 | - dataset: kitti 7 | - training/loss: unsupervised 8 | - training/prediction: poseresnet18 9 | - hydra/output: training 10 | -------------------------------------------------------------------------------- /config/training/loss/supervised.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | mode: supervised -------------------------------------------------------------------------------- /config/training/loss/unsupervised.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | mode: unsupervised 3 | -------------------------------------------------------------------------------- /config/training/prediction/poseresnet18.yaml: -------------------------------------------------------------------------------- 1 | # @package _group_ 2 | sequence_len: ??? 3 | num_input_channels: ??? 4 | 5 | posenet_config: 6 | type: poseresnet 7 | model: 18 8 | -------------------------------------------------------------------------------- /docs/benchmark.md: -------------------------------------------------------------------------------- 1 | ## Towards reproducible research... 2 | 3 | > pyLIDAR-SLAM is a research project. One of its goal is to compare and benchmark different algorithms on a variety of datasets. 4 | 5 | > In `build_benchmark_md.py` we provide a tool to aggregate the results obtained after different runs of the algorithm on a given dataset. 6 | > Hydra creates a lot of directories, and it is rather cumbersome to search for results by hand. The script thus searches recursively for all directories having both ground truth and estimated poses, starting at a given root. 7 | > For each found entry, it computes the metrics (which can take a while depending on the number of results to aggregate), and generates a `_benchmark.md` file similar to [kitti_benchmark.md](results/KITTI/kitti_benchmark.md) 8 | 9 | > For reproducibility benchmark.md will save the *git_hash* and the *command_line* used to generate the results. 10 | > Note that the command line might not be valid at any given commit, but using the *git_hash* you should be able to retrieve the same execution without (too much) complications. 11 | 12 | > You should be able to reproduce the results without too much difficulty. Note that *hydra* provides useful tool to this end, notably the *multi-run* mechanism. 13 | > To launch multiple runs (with different set of parameters simply add `-m` and the different parameters desired). For example: 14 | ```bash 15 | export JOB_NAME=kitti_F2M # The folder to log hydra output files 16 | export DATASET=KITTI # Name of the Dataset to construct the destination folder 17 | export KITTI_ODOM_ROOT= # The path to KITTI odometry benchmark files 18 | 19 | # Run the script 20 | python run.py -m dataset=kitti num_workers=4 device=cuda:0 slam/odometry/local_map=projective \ 21 | slam/odometry/local_map=projective \ 22 | slam/odometry/initialization=CV \ 23 | slam/odometry/alignment=point_to_plane_GN \ 24 | dataset=kitti \ 25 | +dataset.train_sequences=["00","01","02","03","04","05","06",'07','08','09','10'] \ 26 | device=cuda:0 \ 27 | slam.odometry.data_key=vertex_map \ 28 | slam.odometry.local_map.local_map_size=10,20,30 \ # Different local map sizes 29 | slam.odometry.max_num_alignments=10,15,20 \ # Different number of alignments 30 | slam.odometry.alignment.gauss_newton_config.scheme=geman_mcclure,neighborhood,cauchy \ # Different loss functions 31 | slam.odometry.alignment.gauss_newton_config.sigma=0.1,0.2,0.5 \ # Range of loss function arguments 32 | +slam.odometry.viz_debug=False 33 | ``` 34 | 35 | > The above command will iteratively launch the 3 * 3 * 3 * 3 = 81 different executions on the grid of parameters defined. 36 | > This will generate a lot of files and directories. So you can use `build_benchmark_md.py` to aggregate results. 37 | ### KITTI 38 | 39 | > A benchmark of the methods implemented in **pyLIDAR-SLAM** evaluated on *KITTI* can be found at [kitti_benchmark.md](results/KITTI/kitti_benchmark.md). 40 | 41 | #### Newer Handheld College Dataset 42 | 43 | > The benchmark is still in construction and can be found at [nhcd_kbenchmark.md](results/NHCD/nhcd_benchmark.md). 44 | 45 | 46 | #### TODOs 47 | - [x] KITTI 48 | - [x] New Handheld College Dataset 49 | - [ ] NCLT 50 | - [ ] FORD CAMPUS -------------------------------------------------------------------------------- /docs/data/ei.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/docs/data/ei.png -------------------------------------------------------------------------------- /docs/data/example_pointclouds.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/docs/data/example_pointclouds.png -------------------------------------------------------------------------------- /docs/data/minimal_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/docs/data/minimal_example.png -------------------------------------------------------------------------------- /docs/data/overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/docs/data/overview.png -------------------------------------------------------------------------------- /docs/data/point_to_plane.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/docs/data/point_to_plane.png -------------------------------------------------------------------------------- /docs/data/project_structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/docs/data/project_structure.png -------------------------------------------------------------------------------- /docs/data/pylidar_slam_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/docs/data/pylidar_slam_1.png -------------------------------------------------------------------------------- /docs/data/pylidar_slam_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/docs/data/pylidar_slam_3.png -------------------------------------------------------------------------------- /docs/datasets.md: -------------------------------------------------------------------------------- 1 | ## DATASETS 2 | 3 | >PyLIDAR-SLAM integrates the following LiDAR Datasets: 4 | > - [KITTI](#kitti) 5 | > - [Ford-Campus](#ford_campus) 6 | > - [NCLT](#nclt) 7 | > - [Newest Handheld College Dataset](#NHCD) 8 | > 9 | >The following describes for each dataset the installation instructions, how to adapt the configurations, and how to run 10 | the SLAM on these Datasets. 11 | 12 | #### Hydra Configurations 13 | 14 | > For each dataset a default YAML config file is given in the folder [dataset](../config/dataset). 15 | > These configs are loaded by the scripts `run.py` and `train.py` while loading the config. To select the appropriate dataset, use the command argument `dataset=` where `` is the name of the associated hydra config. 16 | > For example `python run.py dataset=kitti` will launch the default SLAM on the KITTI benchmark. 17 | 18 | > The hydra dataset config files are: 19 | - [ford_campus.yaml](../config/dataset/ford_campus.yaml) 20 | - [kitti.yaml](../config/dataset/kitti.yaml) 21 | - [nclt.yaml](../config/dataset/nclt.yaml) 22 | - [nhcd.yaml](../config/dataset/nhcd.yaml) 23 | 24 | 25 | > To modify fields of a `dataset` config in the command line, pass it as argument using hydra's mechanism, (e.g. `python run.py dataset=kitti dataset.lidar_height=64 dataset.lidar_width=840` will project the dataset pointclouds in *vertex maps* of dimension 3x64x840) 26 | 27 | > /!\ Note that the dataset config files will require some environment variables to be set which points to the root folder of each datasets. 28 | > If error arise, make sure that the environment variables (mentioned below) have been properly set. 29 | 30 | 31 | #### Programmatically 32 | > The configurations can also be defined programmatically (*ie* not using the scripts `run.py` not `train.py`). 33 | >Each dataset defines a [`DatasetConfig`](../slam/dataset/dataset.py) is the (abstract) configuration dataclass matching a hydra's config file. 34 | >And a [`DatasetLoader`](../slam/dataset/dataset.py) which can load each sequence defined in the dataset as a `torch.utils.data.Dataset`. 35 | > 36 | 37 | ## KITTI's Odometry Benchmark 38 | 39 | The odometry benchmark of KITTI's dataset (follow [link](http://www.cvlibs.net/datasets/kitti/eval_odometry.php)) consists of 40 | 21 sequences, 11 of which have the ground truth poses (for more than 20k lidar frames). 41 | The LiDAR sensor capturing the environment is a rotating 64 LiDAR Channel. 42 | 43 | > /!\ The frame are corrected using pose obtained from a Visual-SLAM, and is thus not the raw data obtained from the sensor. 44 | 45 | ### Installation Instructions 46 | 47 | - Download the lidar frames of the odometry benchmark (at this [download link](http://www.cvlibs.net/download.php?file=data_odometry_velodyne.zip) (~80GB)) 48 | - Download the ground truth poses of the odometry benchmark (at this [download link](http://www.cvlibs.net/download.php?file=data_odometry_poses.zip)) 49 | - Extract both archives in the same root directory 50 | 51 | The layout of KITTI's data on disk must be as follows: 52 | ```bash 53 | ├─ # Root of the Odometry benchmark data 54 | ├─ poses # The ground truth files 55 | ├─ 00.txt 56 | ├─ 01.txt 57 | ... 58 | └─ 10.txt 59 | └─ sequences 60 | ├─ 00 61 | ├─ calib.txt 62 | └─ velodyne 63 | ├─ 000000.bin 64 | ├─ 000001.bin 65 | ... 66 | ├─ 01 67 | ├─ calib.txt 68 | └─ velodyne 69 | ├─ 000000.bin 70 | ... 71 | ... 72 | ``` 73 | #### Configuration setup 74 | 75 | Before calling `run.py` or `train.py`, the following environment variables must be set: 76 | > `KITTI_ODOM_ROOT`: points to the root dataset as defined above. 77 | 78 | For example, to run the ICP odometry on KITTI: 79 | ```bash 80 | # Set up the following required environment variables 81 | JOB_NAME=kitti_F2M 82 | DATASET=KITTI 83 | KITTI_ODOM_ROOT= 84 | 85 | # Run the script 86 | python run.py dataset=kitti 87 | ``` 88 | 89 | #### See [kitti_dataset.py](../slam/dataset/kitti_dataset.py) for more details. 90 | 91 | 92 | ## Ford Campus 93 | 94 | The Ford Campus Dataset consists of 2 large sequences of LiDAR data acquired with a HDL-64 with GPS ground truth (see the page [Ford Campus](http://robots.engin.umich.edu/SoftwareData/Ford) for more details). 95 | 96 | ### Installation Instructions 97 | 98 | - Download the two sequences of the dataset: [dataset-1](http://robots.engin.umich.edu/uploads/SoftwareData/Ford/dataset-1.tar.gz) ~78GB, [dataset-2](http://robots.engin.umich.edu/uploads/SoftwareData/Ford/dataset-2.tar.gz) ~119GB 99 | - Alternatively download the sampled data of the [dataset-1](http://robots.engin.umich.edu/uploads/SoftwareData/Ford/dataset-1-subset.tgz) ~6GB 100 | - Extract both archives in the same root directory 101 | 102 | The expected layout of Ford Campus data on disk is as follows: 103 | ```bash 104 | ├─ # Root of the data 105 | ├─ IJRR-Dataset-1 106 | └─ SCANS # The pointclouds are read from the Scan files 107 | ├─ Scan0075.mat 108 | ├─ Scan0076.mat 109 | ... 110 | └─ IJRR-Dataset-2 111 | ... 112 | ``` 113 | 114 | #### Configuration setup 115 | Before calling `run.py` or `train.py`, the following environment variables must be set: 116 | - `FORD_ROOT`: points to the root dataset as defined above. 117 | 118 | Example running the ICP odometry on Ford Campus: 119 | 120 | ```bash 121 | # Set up the following required environment variables 122 | JOB_NAME=ford_campus_F2M # required by run.py to define the output path 123 | DATASET=FORD_CAMPUS # required by run.py to define the output path 124 | FORD_ROOT= 125 | 126 | # Run the script 127 | python run.py dataset=ford_campus 128 | ``` 129 | #### See [ford_dataset.py](../slam/dataset/ford_dataset.py) for more details. 130 | 131 | 132 | ## NCLT 133 | 134 | NCLT contains multiple sequences of the same environment captured with multiple sensors including a HDL-32 mounted on a segway and accurate GPS groundtruth. 135 | 136 | #### Installation instructions 137 | The data for each sequences can be downloaded on the dataset main page [NCLT](http://robots.engin.umich.edu/nclt/). 138 | 139 | - Download the sequences you are interested in (column `Velodyne`) 140 | - For each sequence downloaded, download the associated ground truth (column `Ground Truth Pose`) 141 | - Extract from the archives of each sequence the files with the following layout: 142 | 143 | ```bash 144 | ├─ # Root of the data 145 | ├─ 2012-04-29 # Date of acquisition of the sequence 146 | └─ velodyne_sync # The folder containing the velodyne aggregated pointclouds 147 | ├─ 1335704127712909.bin 148 | ├─ 1335704127712912.bin 149 | ... 150 | └─ groundtruth_2012-04-29.csv # Corresponding ground truth file 151 | ... # Idem for other sequences 152 | ``` 153 | 154 | #### Configuration setup 155 | 156 | Before calling `run.py` or `train.py`, the following environment variables must be set: 157 | - `NCLT_ROOT`: points to the root dataset as defined above. 158 | 159 | Example running the ICP odometry on NCLT: 160 | ```bash 161 | # Set up the following required environment variables 162 | JOB_NAME=nclt_F2M 163 | DATASET=NCLT 164 | NCLT_ROOT= 165 | 166 | # Run the script 167 | python run.py dataset=nclt 168 | ``` 169 | 170 | #### See [nclt_dataset.py](../slam/dataset/nclt_dataset.py) for more details. 171 | 172 | ## Newest Handheld College Dataset 173 | 174 | Contains long sequences of a college campus acquired with a Ouster LiDAR sensor with 64 channels, mounted on a handheld acquisition platform. 175 | The dataset has accurate ground truth acquired by registration of ouster scan on a precise 3D map acquired with a survey-grade laser scanner. 176 | 177 | #### Installation instruction 178 | 179 | - Go to the dataset [webpage](https://ori-drs.github.io/newer-college-dataset/) and register to access the google drive. 180 | - Download from `01_short_experiment` and `02_long_experiment` folders on the drive the poses (relative path: `ground_truth/registered_poses.csv`) and the ouster scans as `.pcd` files (in the relative path `raw_format/ouster_scan/.`) 181 | - Extract the zip files in order to have the following layout on disk: 182 | 183 | ```bash 184 | ├─ # Root of the data 185 | ├─ 01_short_experiment # Date of acquisition of the sequence 186 | └─ ouster_scan # The folder containing the ouster scans 187 | ├─ cloud_1583836591_182590976.pcd 188 | ... 189 | └─ ground_truth # The sequence ground truth files (in the reference frame of the left camera) 190 | ├─ registered_poses.csv 191 | 192 | ... # Idem for other sequence 193 | ``` 194 | 195 | #### Configuration Setup 196 | 197 | Before calling `run.py` or `train.py`, the following environment variables must be set: 198 | - `NHCD_ROOT`: points to the root dataset as defined above. 199 | 200 | Example running the ICP odometry on the Newer Handheld College Dataset: 201 | ```bash 202 | # Set up the following required environment variables 203 | export JOB_NAME=nhcd_F2M 204 | export DATASET=NHCD 205 | export NHCD_ROOT= 206 | 207 | # Run the script 208 | python run.py dataset=nhcd 209 | ``` 210 | 211 | 212 | #### See [nhcd_dataset.py](../slam/dataset/nhcd_dataset.py) for more details. 213 | -------------------------------------------------------------------------------- /docs/results/KITTI/kitti_benchmark.md: -------------------------------------------------------------------------------- 1 | ## KITTI Benchmark: 2 | 3 | 4 | 5 | 6 | #### Sorted trajectory error on all sequences: 7 | | **Sequence Folder**|00 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | AVG | AVG Time (ms) | 8 | | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | 9 | | [EI+KdF2M](/home/pdell/dev/pylidar-slam/docs/results/KITTI/.results/EI+KdF2M) | 0.5113 | 0.7913 | 0.5048 | 0.6405 | 0.3650 | 0.2880 | 0.2870 | 0.3212 | 0.7786 | 0.4559 | 0.5699 | 0.5332 | 187.256 | 10 | | [CV+KdF2M](/home/pdell/dev/pylidar-slam/docs/results/KITTI/.results/CV+KdF2M) | 0.5111 | 0.7924 | 0.5052 | 0.6422 | 0.3634 | 0.2874 | 0.2873 | 0.3214 | 0.7782 | 0.4569 | 0.5712 | 0.5333 | 174.792 | 11 | | [EI+PF2M](/home/pdell/dev/pylidar-slam/docs/results/KITTI/.results/EI+PF2M) | 0.5731 | 0.6793 | 0.6297 | 0.8308 | 0.5058 | 0.3726 | 0.3637 | 0.3243 | 0.9304 | 0.6049 | 1.0158 | 0.6412 | 136.496 | 12 | | [CV+PF2M](/home/pdell/dev/pylidar-slam/docs/results/KITTI/.results/CV+PF2M) | 0.5722 | 0.7104 | 0.6292 | 0.8203 | 1.1266 | 0.3675 | 0.3638 | 0.3254 | 0.9275 | 0.6055 | 1.0132 | 0.6428 | 116.620 | 13 | 14 | 15 | #### Command Lines for each entry 16 | | **Sequence Folder** | Command Line | git hash | 17 | | ---: | ---: | ---: | 18 | | [EI+KdF2M](/home/pdell/dev/pylidar-slam/docs/results/KITTI/.results/EI+KdF2M) | `python run.py slam/odometry/local_map=kdtree slam/odometry/initialization=EI slam/odometry/alignment=point_to_plane_GN dataset=kitti +dataset.train_sequences=[00,01,02,03,04,05,06,'07','08','09','10'] device=cpu slam.odometry.local_map.local_map_size=30 slam.odometry.max_num_alignments=20 slam.odometry.alignment.gauss_newton_config.scheme=neighborhood slam.odometry.alignment.gauss_newton_config.sigma=0.2 num_workers=1 slam/odometry/preprocessing=grid_sample slam.odometry.preprocessing.filters.1.voxel_size=0.4 slam.odometry.data_key=input_data +slam.odometry.viz_debug=False` | 2e38b67| 19 | | [CV+KdF2M](/home/pdell/dev/pylidar-slam/docs/results/KITTI/.results/CV+KdF2M) | `python run.py slam/odometry/local_map=kdtree slam/odometry/initialization=CV slam/odometry/alignment=point_to_plane_GN dataset=kitti +dataset.train_sequences=[00,01,02,03,04,05,06,'07','08','09','10'] device=cpu slam.odometry.local_map.local_map_size=30 slam.odometry.max_num_alignments=20 slam.odometry.alignment.gauss_newton_config.scheme=neighborhood slam.odometry.alignment.gauss_newton_config.sigma=0.2 num_workers=1 slam/odometry/preprocessing=grid_sample slam.odometry.preprocessing.filters.1.voxel_size=0.4 slam.odometry.data_key=input_data +slam.odometry.viz_debug=False` | d02c7b3| 20 | | [EI+PF2M](/home/pdell/dev/pylidar-slam/docs/results/KITTI/.results/EI+PF2M) | `python run.py slam/odometry/local_map=projective slam/odometry/initialization=EI slam/odometry/alignment=point_to_plane_GN dataset=kitti +dataset.train_sequences=[04,00,01,02,03,05,06,'07','08','09','10'] device=cuda:0 num_workers=4 slam.odometry.data_key=vertex_map slam.odometry.local_map.local_map_size=20 slam.odometry.max_num_alignments=15 slam.odometry.alignment.gauss_newton_config.scheme=neighborhood slam.odometry.alignment.gauss_newton_config.sigma=0.2 +slam.odometry.viz_debug=False` | 64181ae| 21 | | [CV+PF2M](/home/pdell/dev/pylidar-slam/docs/results/KITTI/.results/CV+PF2M) | `python run.py slam/odometry/local_map=projective slam/odometry/initialization=CV slam/odometry/alignment=point_to_plane_GN dataset=kitti +dataset.train_sequences=[00,01,02,03,04,05,06,'07','08','09','10'] device=cuda:0 slam.odometry.data_key=vertex_map slam.odometry.local_map.local_map_size=20 slam.odometry.max_num_alignments=15 slam.odometry.alignment.gauss_newton_config.scheme=neighborhood slam.odometry.alignment.gauss_newton_config.sigma=0.2 +slam.odometry.viz_debug=False` | d02c7b3| 22 | -------------------------------------------------------------------------------- /docs/results/NHCD/nhcd_benchmark.md: -------------------------------------------------------------------------------- 1 | ## Newer Handheld College Dataset Benchmark: 2 | 3 | 4 | #### Sorted trajectory error on all sequences: 5 | | **Sequence Folder**|01_short_experiment | 02_long_experiment | AVG | AVG Time (ms) | 6 | | ---: | ---: | ---: | ---: | ---: | 7 | | [EI-KdF2M](/home/pdell/dev/pylidar-slam/docs/results/NHCD/.results/EI-KdF2M) | 1.5249 | | | -1.000 | 8 | 9 | 10 | #### Command Lines for each entry 11 | | **Sequence Folder** | Command Line | git hash | 12 | | ---: | ---: | ---: | 13 | | [EI-KdF2M](/home/pdell/dev/pylidar-slam/docs/results/NHCD/.results/EI-KdF2M) | `python run.py slam/odometry/local_map=kdtree slam/odometry/initialization=EI slam/odometry/alignment=point_to_plane_GN dataset=nhcd +dataset.train_sequences=[01_short_experiment] device=cpu slam.odometry.local_map.local_map_size=30 slam.odometry.max_num_alignments=20 slam.odometry.alignment.gauss_newton_config.scheme=neighborhood slam.odometry.alignment.gauss_newton_config.sigma=0.2 num_workers=1 slam/odometry/preprocessing=grid_sample slam.odometry.preprocessing.filters.1.voxel_size=0.4 slam.odometry.data_key=input_data +slam.odometry.viz_debug=False` | da0e571| 14 | -------------------------------------------------------------------------------- /docs/toolbox.md: -------------------------------------------------------------------------------- 1 | # SLAM LiDAR Toolbox 2 | 3 | ![Main Architecture](data/project_structure.png) 4 | 5 | >This page describes in depth the content of the project. **pyLIDAR-SLAM** is designed as a modular toolbox of interchangeable pieces. 6 | The basic structure of a **SLAM** algorithm is presented in the figure above. 7 | For each component, multiple algorithms are implemented, (more will be added in the future). 8 | 9 | >Using Hydra, custom SLAM pipelines can be defined simply by modifying a command line argument. 10 | >Note that it leads to lengthy command lines (see the [benchmark](benchmark.md) to have an idea), but it is always possible to use pyLIDAR-SLAM as a library in your own scripts. 11 | 12 | 13 | > A **SLAM** in this project processes data *frame-by-frame*. At each time step, the data is passed to the **SLAM** in a python dictionary. 14 | >The key mappings are typically resolved using the configuration. See [slam.py](../slam/slam.py) for reference. 15 | 16 | > The script [`run.py`](../run.py) launches the **SLAM** specified by the configuration on a set of sequences of a given dataset see [dataset.md](datasets.md) for a list of the available datasets and how to install them. 17 | 18 | > Below, we review component by component all the modules implemented in **pyLIDAR-SLAM**. 19 | 20 | # Motion Initialization 21 | 22 | > The initialization of the motion is very important for LiDAR Odometries. Indeed, most lidar odometries rely on a variant of the ICP which has a very small convergence region. 23 | > The strategies proposed are the following: 24 | > - **CV** (for constant velocity): the motion estimated is the same as the previous motion between two frames. 25 | > - **PoseNet**: the motion is predicted using a CNN based pose regression. 26 | > - **EI** (for Elevation Images): the motion is predicted using a 2D image feature based registration. 27 | > - **NI** (for No Initialization) 28 | 29 | > To select the initialization module, pass in argument `slam/initialization=` where `init_name` is one of `CV,EI,PoseNet,NI`. 30 | 31 | > All initialization models can be found in [initialization.py](../slam/initialization.py). 32 | #### Constant Velocity 33 | > This model is typically the default model for a LiDAR mounted on a car (which has enough inertia for this model to hold). 34 | > For other acquisition methods (hand-held or mounted on a segway or a drone which lead to abrupt rotations, this might not be a good enough model) 35 | 36 | - **Command arguments**: `slam/initialization=CV` 37 | 38 | #### Elevation Images 39 | > The relative motion of a sensor is often strictly planar. Our **Elevation Images** based initializes the motion by estimating this 2D motion using Image Registration. 40 | > Pointclouds are projected into elevation images. A 2D rotation and translation is robustly fitted, and is used to initialize the 3D motion. 41 | 42 | ![Elevation Images](data/ei.png) 43 | 44 | > Very useful for a dataset such as **NCLT** (which has abrupt *yaw* rotations) 45 | 46 | > **Limits**: 47 | > for this method to work, it requires that the sensor is mounted parallel to the ground on which the robot moves. It is also not robust to strong *pitch/roll* orientation changes. 48 | 49 | > **Note:** This method requires the OpenCV package `cv2` to be installed (e.g. via `pip install opencv-python`). 50 | > 51 | - **Command arguments**: `slam/initialization=EI` (to visualize the 2D alignments with opencv :`slam.odometry.initialization.debug=true`) 52 | 53 | > See [`EIConfig`](../slam/initialization.py) and [`ElevationImageRegistration`](../slam/common/registration.py) for more details on the configuration options. 54 | 55 | #### PoseNet 56 | 57 | > **PoseNet** (for **RPR** (Relative Pose Regression) is a convolutional regressor predicting the 6 parameters of relative pose pose between two frames. 58 | 59 | > The implementation can be found in [prediction_modules.py](../slam/training/prediction_modules.py). **PoseNet** needs to be trained on a Dataset (KITTI for instance). 60 | > The saved model will be loaded by the initialization algorithm which will regress at each step a relative pose between two consecutive frames. 61 | 62 | > Follow the instruction below to launch the training of PoseNet: 63 | 64 | - Launch the train command (for example): 65 | ```bash 66 | export DATASET=kitti 67 | export JOB_NAME=train_posenet 68 | export KITTI_ODOM_ROOT= # The path to KITTI odometry benchmark files 69 | export TRAIN_DIR= # Path to the output models 70 | 71 | # Launches the Training of PoseNet 72 | python train.py +device=cuda:0 +num_workers=4 +num_epochs=100 dataset=kitti 73 | ``` 74 | > Note: This will typically take a night for training on KITTI. 75 | - Define the environment variable `TRAIN_DIR` pointing to the directory containing the `checkpoint.ckp` file containing the trained model parameters. 76 | ```bash 77 | export DATASET=kitti 78 | export JOB_NAME=slam_with_posenet 79 | export KITTI_ODOM_ROOT= # The path to KITTI odometry benchmark files 80 | export TRAIN_DIR= # Path to the output models 81 | 82 | # Launches the Training of PoseNet 83 | python run.py +device=cuda:0 +num_workers=4 dataset=kitti slam/initialization=PoseNet 84 | ``` 85 | 86 | > See our paper [What's In My LiDAR Odometry Toolbox](https://arxiv.org/abs/2103.09708) for an in depth discussion of the relevance/interest of PoseNet. To summarize: beware of its lack of generalization capacities. 87 | 88 | # Preprocessing 89 | > Preprocessing in pyLiDAR-SLAM is implemented as a set of filters. Each filter is applied consecutively to the input frame dictionary. 90 | 91 | > The preprocessing modules currently implemented are: 92 | > - **cv_distortion**: applies an initial distortion of the frame following the constant velocity assumption (uses timestamps and the initial pose predicted) 93 | > - **voxelization**: voxelize a frame computes the voxel statistics mean/covariance 94 | > - **grid_sample**: select a point by voxel for a selected voxel size 95 | > - **none** by default 96 | 97 | 98 | > **Command argument**: `slam/preprocessing=`. See [preprocessing.py](../slam/preprocessing.py) for more details. 99 | 100 | # Odometry 101 | 102 | > The different type of odometries accessible in **pyLIDAR-SLAM** are: 103 | > - Classical ICP based **Frame-to-Model** odometry: (see [icp_odometry.py](../slam/odometry/icp_odometry.py)) 104 | > - Deep PoseNet based odometry: (see [posenet_odometry.py](../slam/odometry/posenet_odometry.py)) 105 | 106 | ## Classical ICP based **Frame-to-Model** odometry 107 | 108 | > The frame-to-model odometry estimates the relative pose of a new frame related to the previous registered pose. 109 | 110 | > This is performed by registering the new frame against a **Model** which is typically a **Local Map** built from the previous registered frames. 111 | 112 | > It will at each time step: 113 | - Estimate the motion of the new frame as seen above 114 | - Preprocess the new frame as seen above 115 | - Register the new frame against the map (*ie* estimate the relative pose) 116 | - Search for neighbor points of the current frame in the map and their normals 117 | - Minimize an robust energy (typically a point) 118 | - Update the map 119 | - Log the pose and other values to the frame dictionary 120 | 121 | #### Local Maps 122 | 123 | >**pyLIDAR-SLAM** proposes the following implementations of a Local Map (all are implemented in [local_map.py](../slam/odometry/local_map.py)): 124 | 125 | **1.** **kdtree** (cpu): 126 | > At each time step the previous *n* pointclouds are transformed in the last inserted pose coordinate frame, 127 | > and a kdtree is constructed on the aggregated pointcloud. Points of the new frame are queried against the newly formed kdtree. 128 | > Note that the creation and querying of the **KdTree** is expensive, so consider sampling points to improve the speed. 129 | > Use `slam/odometry/local_map=kdtree` to select the kdtree. 130 | 131 | **2.** **projective** (gpu): 132 | 133 | > The previous *n* pointclouds are also transformed in the last inserted pose coordinate frame, but they are projected in 2D space using a spherical projection. 134 | > This allows for projective data association which can be performed on the **GPU** using pytorch's cuda backend. To select this option use `slam/odometry/local_map=projective`. 135 | > The projective local map uses the `Projector` defined by the dataset (which can be tweaked in the configuration). This method is probably not the most desirable for very sparse LiDARs as neighborhood are defined by shared pixel values. However for a Dataset such as KITTI they allow to build a SLAM in real-time (sacrificing a bit of precision). 136 | 137 | >To select a **NVIDIA gpu** to perform projective alignment, select the appropriate device using the argument `device=cuda:0`. 138 | 139 | 140 | #### Robust Alignment 141 | 142 | > The ICP-based odometry proposes a robust **Point-to-Plane** alignment scheme. Each neighbor search leads to a set of point-to-plane residuals. 143 | > The minimization is performed by a gauss-newton. To improve the robustness to outliers the following robust loss function schemes can be selected in the configuration: 144 | > - **least_square**: the default untruncated squared loss 145 | > - **huber** 146 | > - **geman_mcclure** 147 | > - **cauchy** 148 | > - **neighborhood**: weights residuals by the distance between neighbors (or the confidence that a match is good) 149 | 150 | > To select a loss function select the following arguments `slam.odometry.alignment.gauss_newton_config.scheme=neighborhood,cauchy,etc..` 151 | 152 | > More details can be found at [alignment.py](../slam/odometry/alignment.py). 153 | 154 | 155 | # Loop Closure (*Under Development) 156 | 157 | 158 | -------------------------------------------------------------------------------- /replay.py: -------------------------------------------------------------------------------- 1 | # Hydra and OmegaConf 2 | from dataclasses import MISSING, dataclass, field 3 | import numpy as np 4 | from pathlib import Path 5 | from typing import Optional 6 | from slam.common.modules import _with_viz3d 7 | 8 | if _with_viz3d: 9 | from viz3d.window import OpenGLWindow 10 | 11 | from omegaconf import DictConfig, OmegaConf 12 | 13 | # Project Imports 14 | from tqdm import tqdm 15 | 16 | from slam.dataset.dataset import WindowDataset 17 | from slam.odometry.odometry_runner import SLAMRunner 18 | from argparse import ArgumentParser 19 | 20 | 21 | @dataclass 22 | class ReplayArguments: 23 | config_path: str = "" # The path to the SLAMRunner Config 24 | sequence: str = "" # The name of sequence to replay 25 | root_dir: Path = field(default_factory=lambda: Path()) 26 | sequence_dir: Path = field(default_factory=lambda: Path()) 27 | start_index: int = 0 28 | num_frames: int = -1 29 | show_information: bool = True # Whether to print information about the to experiment be replayed 30 | overrides_path: Optional[str] = None # The path to the yaml containing the overrides 31 | 32 | 33 | def parse_arguments() -> ReplayArguments: 34 | parser = ArgumentParser() 35 | parser.add_argument("--root_dir", type=str, help="Path to the root of the execution", required=True) 36 | parser.add_argument("--start_index", type=int, help="The index at which the SLAM should start", required=False) 37 | parser.add_argument("--seq", type=str, help="The name of the sequence to replay", required=True) 38 | parser.add_argument("--info", action="store_true", 39 | help="Whether to display information of the sequence prior to the replay") 40 | parser.add_argument("--overrides", type=str, 41 | help="The path (optional) to the overrides") 42 | 43 | args, _ = parser.parse_known_args() 44 | options = ReplayArguments() 45 | root_dir = Path(args.root_dir) 46 | assert root_dir.exists(), f"The root dir {root_dir} for the execution does not exist" 47 | options.root_dir = root_dir 48 | options.sequence_dir = root_dir / args.seq 49 | assert options.sequence_dir.exists(), f"The sequence dir {options.sequence_dir} does not exist" 50 | config_path = root_dir / "config.yaml" 51 | assert config_path.exists(), f"The config path {config_path} does not exist" 52 | options.config_path = str(config_path) 53 | options.start_index = args.start_index 54 | options.sequence = args.seq 55 | options.show_information = args.info 56 | options.overrides_path = args.overrides 57 | 58 | return options 59 | 60 | 61 | def replay_slam(options: ReplayArguments) -> None: 62 | """The main entry point to the script running the SLAM""" 63 | # Load the config 64 | from slam.common.io import read_poses_from_disk 65 | import time 66 | 67 | # Display information about the previous execution 68 | poses: Optional[np.ndarray] = None 69 | poses_file_path = options.sequence_dir / f"{options.sequence}.poses.txt" 70 | gt_file_path = options.sequence_dir / f"{options.sequence}_gt.poses.txt" 71 | if poses_file_path.exists(): 72 | poses = read_poses_from_disk(str(poses_file_path)) 73 | 74 | if options.show_information: 75 | print("*" * 80) 76 | if poses is not None: 77 | print(f"[INFO]Found Pose Estimate file {poses_file_path}.") 78 | if gt_file_path.exists(): 79 | print(f"[INFO]Found GT Pose Estimate file {poses_file_path}. The algorithm run to completion.") 80 | else: 81 | if poses is not None: 82 | print(f"[INFO]The execution stopped after {poses.shape[0]} frames.") 83 | print("*" * 80) 84 | 85 | # Run the algorithm again on the same data 86 | config: DictConfig = OmegaConf.load(options.config_path) 87 | 88 | if options.overrides_path is not None: 89 | overrides_conf = OmegaConf.load(options.overrides_path) 90 | # Merge the two config 91 | config.merge_with(overrides_conf) 92 | 93 | config.dataset.train_sequences = [options.sequence] 94 | config.debug = True 95 | config.log_dir = f"/tmp/{time.time()}" 96 | Path(config.log_dir).mkdir() 97 | 98 | runner = SLAMRunner(config) 99 | 100 | # Load the Datasets 101 | datasets: list = runner.load_datasets() 102 | for sequence_name, dataset in datasets: 103 | 104 | window = None 105 | try: 106 | # Build dataloader 107 | num_frames = options.num_frames if options.num_frames > 0 else len(dataset) - options.start_index 108 | dataset = WindowDataset(dataset, options.start_index, num_frames) 109 | slam = runner.load_slam_algorithm() 110 | slam.init() 111 | 112 | elapsed = 0.0 113 | relative_ground_truth = runner.ground_truth(sequence_name) 114 | if _with_viz3d: 115 | window = OpenGLWindow() 116 | if poses is not None: 117 | window.init() 118 | saved_poses = poses[options.start_index:] 119 | if saved_poses.shape[0] > 0: 120 | saved_poses = np.einsum("ij,njk->nik", np.linalg.inv(saved_poses[0]), saved_poses) 121 | window.set_poses(0, saved_poses.astype(np.float32)) 122 | 123 | for data_dict in tqdm(dataset, desc=f"Sequence {sequence_name}", ncols=100, total=num_frames): 124 | start = time.time() 125 | 126 | # Process next frame 127 | slam.process_next_frame(data_dict) 128 | 129 | # Measure the time spent on the processing of the next frame 130 | elapsed_sec = time.time() - start 131 | elapsed += elapsed_sec 132 | 133 | if window is not None: 134 | window.close(True) 135 | except KeyboardInterrupt: 136 | if _with_viz3d and window is not None: 137 | window.close(True) 138 | 139 | del slam 140 | 141 | 142 | if __name__ == "__main__": 143 | options: ReplayArguments = parse_arguments() 144 | replay_slam(options) 145 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | # torch==1.7.1 => Only tested on pytorch 1.7.1 2 | # hydra-core => should be installed separately 3 | 4 | omegaconf 5 | pykdtree 6 | matplotlib 7 | seaborn 8 | tqdm 9 | pandas 10 | scipy 11 | tensorboard 12 | numba 13 | open3d # For IO + Visualization purposes 14 | torchvision 15 | typeguard 16 | 17 | # cv2 -- Optional : not having OpenCV installed will deactivate some functionalities -------------------------------------------------------------------------------- /run.py: -------------------------------------------------------------------------------- 1 | # Hydra and OmegaConf 2 | import hydra 3 | from omegaconf import DictConfig 4 | 5 | # Project Imports 6 | from slam.odometry.odometry_runner import SLAMRunner, SLAMRunnerConfig 7 | 8 | 9 | @hydra.main(config_path="config", config_name="slam") 10 | def run_slam(cfg: DictConfig) -> None: 11 | """The main entry point to the script running the SLAM""" 12 | _odometry_runner = SLAMRunner(SLAMRunnerConfig(**cfg)) 13 | _odometry_runner.run_odometry() 14 | 15 | 16 | if __name__ == "__main__": 17 | run_slam() 18 | -------------------------------------------------------------------------------- /scripts/generate_urban_loco_gt.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import logging 3 | from argparse import ArgumentParser 4 | 5 | from pathlib import Path 6 | 7 | from slam.dataset import DATASET 8 | from slam.dataset.urban_loco_dataset import UrbanLocoConfig, UrbanLocoDatasetLoader 9 | 10 | logging.basicConfig(stream=sys.stdout, level=logging.INFO) 11 | 12 | 13 | def parse_args(): 14 | """Parses arguments from the command line""" 15 | parser = ArgumentParser() 16 | parser.add_argument("--root_dir", type=str, 17 | help="The path to the directory containing UrbanLoco's RosBags", required=True) 18 | args, _ = parser.parse_known_args() 19 | 20 | return args 21 | 22 | 23 | def main(): 24 | """Main function: builds iteratively the UrbanLoco ground truth files""" 25 | args = parse_args() 26 | 27 | root_dir = Path(args.root_dir) 28 | assert root_dir.exists(), f"The UrbanLoco root dir {root_dir} does not exist on disk" 29 | 30 | config = UrbanLocoConfig() 31 | 32 | config.root_dir = args.root_dir 33 | dataloader: UrbanLocoDatasetLoader = DATASET.load(config) 34 | dataloader.generate_ground_truth(config.train_sequences) 35 | 36 | 37 | if __name__ == "__main__": 38 | main() 39 | -------------------------------------------------------------------------------- /slam/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/slam/__init__.py -------------------------------------------------------------------------------- /slam/common/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/slam/common/__init__.py -------------------------------------------------------------------------------- /slam/common/io.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import numpy as np 4 | import pandas as pd 5 | 6 | # Project Imports 7 | from slam.common.utils import assert_debug, check_tensor 8 | 9 | 10 | def delimiter(): 11 | """ 12 | The column delimiter in pandas csv 13 | """ 14 | return "," 15 | 16 | 17 | def write_poses_to_disk(file_path: str, poses: np.ndarray): 18 | """ 19 | Writes an array of poses to disk 20 | 21 | Parameters 22 | ---------- 23 | file_path : str 24 | poses : np.ndarray [N, 4, 4] 25 | """ 26 | check_tensor(poses, [-1, 4, 4]) 27 | path = Path(file_path) 28 | assert_debug(path.parent.exists()) 29 | poses_to_df(poses).to_csv(file_path, sep=delimiter(), index=False) 30 | 31 | 32 | def read_poses_from_disk(file_path: str, _delimiter: str = delimiter()) -> np.ndarray: 33 | """ 34 | Reads an array of poses from disk 35 | 36 | Returns 37 | ---------- 38 | poses : np.ndarray [N, 4, 4] 39 | """ 40 | path = Path(file_path) 41 | assert_debug(path.exists() and path.is_file()) 42 | return df_to_poses(pd.read_csv(path, sep=_delimiter, index_col=None)) 43 | 44 | 45 | def df_to_poses(df: pd.DataFrame) -> np.ndarray: 46 | """ 47 | Converts a pd.DataFrame to a [N, 4, 4] array of poses read from a dataframe 48 | 49 | Parameters 50 | ---------- 51 | df : pd.DataFrame 52 | A DataFrame of size [N, 12] with the 12 values of the first 3 rows of the pose matrix 53 | """ 54 | array = df.to_numpy(dtype=np.float32) 55 | assert_debug(array.shape[1] == 12) 56 | num_rows = array.shape[0] 57 | poses = array.reshape([num_rows, 3, 4]) 58 | last_row = np.concatenate((np.zeros((num_rows, 3)), np.ones((num_rows, 1))), axis=1) 59 | last_row = np.expand_dims(last_row, axis=1) 60 | poses = np.concatenate((poses, last_row), axis=1) 61 | 62 | return poses 63 | 64 | 65 | def poses_to_df(poses_array: np.ndarray): 66 | """ 67 | Converts an array of pose matrices [N, 4, 4] to a DataFrame [N, 12] 68 | """ 69 | shape = poses_array.shape 70 | assert_debug(len(shape) == 3) 71 | assert_debug(shape[1] == 4 and shape[2] == 4) 72 | nrows = shape[0] 73 | reshaped = poses_array[:, :3, :].reshape([nrows, 12]) 74 | df = pd.DataFrame(reshaped) 75 | 76 | return df 77 | -------------------------------------------------------------------------------- /slam/common/modules.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | # ---------------------------------------------------------------------------------------------------------------------- 4 | # pyct_icp 5 | try: 6 | import pyct_icp 7 | 8 | _with_ct_icp = True 9 | except ImportError: 10 | logging.warning("pyct_icp (wrapping for CT_ICP) not found. Corresponding module disabled.") 11 | _with_ct_icp = False 12 | 13 | # ---------------------------------------------------------------------------------------------------------------------- 14 | # OpenCV 15 | try: 16 | import cv2 17 | 18 | _with_cv2 = True 19 | except ModuleNotFoundError: 20 | logging.warning("OpenCV (cv2 python module) not found, visualization disabled") 21 | _with_cv2 = False 22 | 23 | # ---------------------------------------------------------------------------------------------------------------------- 24 | # viz3d 25 | try: 26 | import viz3d 27 | 28 | _with_viz3d = True 29 | except ImportError: 30 | _with_viz3d = False 31 | 32 | # ---------------------------------------------------------------------------------------------------------------------- 33 | # open3d 34 | try: 35 | import open3d 36 | 37 | _with_o3d = True 38 | except ImportError: 39 | logging.warning("Open3D (open3d python module) not found, some features will be disabled") 40 | _with_o3d = False 41 | 42 | # ---------------------------------------------------------------------------------------------------------------------- 43 | # g2o 44 | try: 45 | import g2o 46 | 47 | _with_g2o = True 48 | except ImportError: 49 | logging.warning("G2O (g2o python module) not found, some features will be disabled") 50 | _with_g2o = False 51 | -------------------------------------------------------------------------------- /slam/common/pointcloud.py: -------------------------------------------------------------------------------- 1 | """ 2 | Convenient Pointcloud processing functions 3 | 4 | @note: Most functions are accelerated by numba and thus compiled to machine code with LLVM, 5 | Thus the first call of functions with decorator @nb.njit is typically very long (up to a few seconds) 6 | """ 7 | import numba as nb 8 | import numpy as np 9 | from numba import prange 10 | 11 | 12 | # ---------------------------------------------------------------------------------------------------------------------- 13 | @nb.njit(fastmath=True) 14 | def voxel_hash(x, y, z): 15 | """ 16 | Computes a hash given the int coordinates of a 3D voxel 17 | 18 | Args: 19 | x (np.int64): the x coordinate of a voxel 20 | y (np.int64): the y coordinate of a voxel 21 | z (np.int64): the z coordinate of a voxel 22 | """ 23 | return 73856093 * x + 19349669 * y + 83492791 * z 24 | 25 | 26 | @nb.njit(fastmath=True, parallel=True) 27 | def planar_hashing(voxel, hashvector): 28 | """ 29 | Computes a pillar hash for each voxel in an array of voxel indices 30 | 31 | A pillar hash is a hash for the first two coordinate of a voxel (x, y) 32 | 33 | Args: 34 | voxel (np.ndarray): An array of voxels `(n, 3)` [np.int64] 35 | """ 36 | for i in prange(voxel.shape[0]): 37 | hashvector[i] = 73856093 * voxel[i, 0] + 19349669 * voxel[i, 1] 38 | 39 | 40 | @nb.njit(parallel=True) 41 | def voxel_hashing(voxel, hashvector): 42 | """ 43 | Computes voxel hashes for an array of voxel indices 44 | 45 | Args: 46 | voxel (np.ndarray): An array of voxels `(n, 3)` [np.int64] 47 | 48 | see https://niessnerlab.org/papers/2013/4hashing/niessner2013hashing.pdf 49 | """ 50 | for i in prange(voxel.shape[0]): 51 | hashvector[i] = voxel_hash(voxel[i, 0], voxel[i, 1], voxel[i, 2]) 52 | 53 | 54 | @nb.njit(fastmath=True, parallel=True) 55 | def voxelise(pointcloud, voxel_x: float = 0.2, voxel_y: float = -1.0, voxel_z: float = -1.0): 56 | """ 57 | Computes voxel coordinates for a given pointcloud 58 | 59 | Args: 60 | pointcloud (np.ndarray): The input pointcloud `(n, 3)` [np.float32] 61 | voxel_x (float): The length of the voxel in the first coordinate 62 | voxel_y (float): The length of the voxel in the second coordinate (same as `voxel_x` by default) 63 | voxel_z (float): The length of the voxel in the third coordinate (same as `voxel_x` by default) 64 | """ 65 | 66 | if voxel_y == -1.0: 67 | voxel_y = voxel_x 68 | if voxel_z == -1.0: 69 | voxel_z = voxel_x 70 | """Return voxel coordinates of a pointcloud""" 71 | out = np.zeros((pointcloud.shape[0], 3), dtype=np.int64) 72 | for i in prange(pointcloud.shape[0]): 73 | x = int(np.round_(pointcloud[i, 0] / voxel_x)) 74 | y = int(np.round_(pointcloud[i, 1] / voxel_y)) 75 | z = int(np.round_(pointcloud[i, 2] / voxel_z)) 76 | out[i, 0] = x 77 | out[i, 1] = y 78 | out[i, 2] = z 79 | return out 80 | 81 | 82 | # ---------------------------------------------------------------------------------------------------------------------- 83 | @nb.njit() 84 | def __voxel_normal_distribution(pointcloud, voxel_hashes, is_sorted: bool = False): 85 | """ 86 | Computes the normal distribution of points in each voxel 87 | 88 | Args: 89 | pointcloud (np.ndarray): The input pointcloud `(n, 3)` [np.float32] 90 | voxel_hashes (np.ndarray): The voxel hashes `(n,)` [np.int64] 91 | is_sorted (bool): Whether the pointcloud is sorted by hash value 92 | (avoids performing this step) 93 | """ 94 | if is_sorted: 95 | sorted_pointcloud = pointcloud 96 | sorted_hashes = voxel_hashes 97 | else: 98 | sort_indices = np.argsort(voxel_hashes) 99 | sorted_pointcloud = pointcloud[sort_indices] 100 | sorted_hashes = voxel_hashes[sort_indices] 101 | 102 | n = pointcloud.shape[0] 103 | 104 | _previous_hash = sorted_hashes[0] 105 | _voxel_start_idx = 0 106 | 107 | _idx = 0 108 | covs = [] 109 | means = [] 110 | voxel_sizes = [] 111 | hashes = [] 112 | voxel_ids = [] 113 | voxel_id = 0 114 | 115 | while _idx < n + 1: 116 | if _idx == n: 117 | # Signal to aggregate the last voxel 118 | _new_hash = _previous_hash + 1 119 | else: 120 | _new_hash = sorted_hashes[_idx] 121 | 122 | if _new_hash == _previous_hash: 123 | _idx += 1 124 | voxel_ids.append(voxel_id) 125 | continue 126 | 127 | # Build the covariance matrices and mean for the voxel 128 | voxel_points = sorted_pointcloud[_voxel_start_idx:_idx] 129 | num_points = _idx - _voxel_start_idx 130 | mean = voxel_points.sum(axis=0).reshape(1, 3) / num_points 131 | centered = voxel_points - mean 132 | cov = (centered.reshape(-1, 3, 1) * centered.reshape(-1, 1, 3)).sum(axis=0) 133 | 134 | voxel_sizes.append(num_points) 135 | covs.append(cov) 136 | means.append(mean[0]) 137 | hashes.append(_previous_hash) 138 | 139 | if _idx < n: 140 | _previous_hash = _new_hash 141 | _voxel_start_idx = _idx 142 | voxel_id += 1 143 | voxel_ids.append(voxel_id) 144 | 145 | _idx += 1 146 | 147 | voxel_ids = np.array(voxel_ids) 148 | 149 | if not is_sorted: 150 | indices = np.argsort(sort_indices) 151 | voxel_ids = voxel_ids[indices] 152 | 153 | return voxel_sizes, means, covs, voxel_ids 154 | 155 | 156 | def voxel_normal_distribution(pointcloud, voxel_hashes, is_sorted: bool = False): 157 | """ 158 | Computes the normal distribution of points in each voxel 159 | 160 | Args: 161 | pointcloud (np.ndarray): The input pointcloud `(n, 3)` [np.float32] 162 | voxel_hashes (np.ndarray): The voxel hashes `(n,)` [np.int64] 163 | is_sorted (bool): Whether the pointcloud is sorted by hash value 164 | (avoids performing this step) 165 | """ 166 | voxel_sizes, means, covs, voxel_ids = __voxel_normal_distribution(pointcloud, voxel_hashes, is_sorted) 167 | return np.array(voxel_sizes), np.array(means), np.array(covs), voxel_ids 168 | 169 | 170 | def sample_from_hashes(pointcloud, hashes): 171 | """Sample one point per hash value, returns the sample points and the indices of the sampled points 172 | 173 | Args: 174 | pointcloud (np.ndarray): The input pointcloud `(n, 3)` [np.float32, np.float64] 175 | hashes (np.ndarray): The hashes `(n,)` [np.int64] 176 | """ 177 | _, unique_indices = np.unique(hashes, return_index=True) 178 | 179 | return pointcloud[unique_indices], unique_indices 180 | 181 | 182 | def grid_sample(pointcloud, voxel_size: float): 183 | """Sample one point per voxel hash, returns the sample points and the indices of the sampled points 184 | 185 | Args: 186 | pointcloud (np.ndarray): The input pointcloud `(n, 3)` [np.float32, np.float64] 187 | voxel_size (np.ndarray): The size of a voxel 188 | """ 189 | voxels = voxelise(pointcloud, voxel_size) 190 | voxelhashes = np.zeros((pointcloud.shape[0],), dtype=np.int64) 191 | voxel_hashing(voxels, voxelhashes) 192 | 193 | _, unique_indices = np.unique(voxelhashes, return_index=True) 194 | 195 | return pointcloud[unique_indices], unique_indices 196 | -------------------------------------------------------------------------------- /slam/common/pose.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import numpy as np 3 | 4 | import numpy as np 5 | from scipy.spatial.transform.rotation import Rotation as R, Slerp 6 | from scipy.interpolate.interpolate import interp1d 7 | from slam.common.utils import assert_debug, check_tensor 8 | from slam.common.rotation import torch_euler_to_mat, torch_mat_to_euler, torch_pose_matrix_jacobian_euler 9 | 10 | 11 | class PosesInterpolator: 12 | """Object which performs interpolation of poses using timestamps 13 | 14 | Poses and corresponding key timestamps are passed to the constructor. 15 | The PosesInterpolator returns a linear interpolation on these poses 16 | When called with new timestamps. 17 | """ 18 | 19 | def __init__(self, poses: np.ndarray, timestamps: np.ndarray): 20 | check_tensor(poses, [-1, 4, 4], np.ndarray) 21 | check_tensor(timestamps, [-1], np.ndarray) 22 | self.min_timestamp = timestamps.min() 23 | self.max_timestamp = timestamps.max() 24 | 25 | self.slerp = Slerp(timestamps, R.from_matrix(poses[:, :3, :3])) 26 | self.interp_tr = interp1d(timestamps, poses[:, :3, 3], axis=0) 27 | 28 | def __call__(self, timestamps: np.ndarray): 29 | if timestamps.min() < self.min_timestamp or timestamps.max() > self.max_timestamp: 30 | timestamps = np.clip(timestamps, self.min_timestamp, self.max_timestamp) 31 | tr = self.interp_tr(timestamps) 32 | rots = self.slerp(timestamps) 33 | 34 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(timestamps.shape[0], axis=0) 35 | poses[:, :3, :3] = rots.as_matrix() 36 | poses[:, :3, 3] = tr 37 | return poses 38 | 39 | 40 | def transform_pointcloud(pointcloud: np.ndarray, tr: np.ndarray): 41 | """ 42 | Applies the transform `tr` to the pointcloud 43 | 44 | Parameters 45 | ---------- 46 | pointcloud : np.ndarray (N, 3) 47 | tr : np.ndarray (4, 4) 48 | """ 49 | return np.einsum("ij,nj->ni", tr[:3, :3], pointcloud) + tr[:3, 3].reshape(1, 3) 50 | 51 | 52 | class Pose(object): 53 | """ 54 | A Pose is a tool to interpret tensors of float as SE3 poses 55 | 56 | Parameters 57 | ---------- 58 | config : dict 59 | A dictionary with the configuration of the pose 60 | """ 61 | 62 | def __init__(self, pose_type: str): 63 | self.pose_type = pose_type 64 | assert_debug(self.pose_type in self.__supported_poses()) 65 | 66 | @staticmethod 67 | def __supported_poses(): 68 | return ["euler"] # TODO , "quaternions" 69 | 70 | def euler_convention(self): 71 | """ 72 | Returns the euler convention used for the parametrisation of the rotation 73 | 74 | Fails if self.pose_type is not equal to "euler" 75 | """ 76 | assert_debug(self.pose_type == "euler") 77 | return "xyz" 78 | 79 | def num_rot_params(self) -> int: 80 | """ 81 | Returns 82 | ------- 83 | int : 84 | The number of parameters of rotation for this representation 85 | """ 86 | if self.pose_type == "quaternions": 87 | return 4 88 | else: 89 | return 3 90 | 91 | def num_params(self) -> int: 92 | """ 93 | 94 | Returns 95 | ------- 96 | int : 97 | The number of parameters (rotation + translation) for this representation 98 | """ 99 | return self.num_rot_params() + 3 100 | 101 | def inverse_pose_matrix(self, params_tensor: torch.Tensor) -> torch.Tensor: 102 | """ 103 | Returns the inverse of the pose matrix 104 | 105 | Parameters 106 | ---------- 107 | params_tensor : [B, 6/7] or [B, 4, 4] 108 | """ 109 | if len(params_tensor.shape) == 2: 110 | params_tensor = self.build_pose_matrix(params_tensor) 111 | check_tensor(params_tensor, [-1, 4, 4]) 112 | 113 | inverse = torch.zeros_like(params_tensor) 114 | rt = params_tensor[:, :3, :3].permute(0, 2, 1) 115 | inverse[:, :3, :3] = rt 116 | inverse[:, :3, 3] = - torch.einsum("bij,bj->bi", rt, params_tensor[:, :3, 3]) 117 | inverse[:, 3, 3] = 1.0 118 | return inverse 119 | 120 | def build_pose_matrix(self, params_tensor: torch.Tensor) -> torch.Tensor: 121 | """ 122 | Returns a pose matrix tensor from a pose parameters tensor 123 | 124 | Parameters 125 | ---------- 126 | params_tensor : torch.Tensor 127 | The tensor of the 6 or 7 parameters of the pose 128 | 129 | Returns 130 | ------- 131 | torch.Tensor 132 | The tensor of matrix 133 | """ 134 | check_tensor(params_tensor, [-1, self.num_rot_params() + 3]) 135 | b = params_tensor.size(0) 136 | rotation_tensor = self.rot_matrix_from_params(params_tensor[:, 3:]) 137 | pose = torch.cat([rotation_tensor, torch.zeros(b, 1, 3, 138 | device=params_tensor.device, 139 | dtype=params_tensor.dtype)], dim=1) # [B, 4, 3] 140 | trans = torch.cat([params_tensor[:, :3], 141 | torch.ones(b, 1, device=params_tensor.device, dtype=params_tensor.dtype)], dim=1) \ 142 | .unsqueeze(2) # [B, 4, 1] 143 | pose = torch.cat([pose, trans], dim=2) # [B, 4, 4] 144 | return pose 145 | 146 | def __to_pose_matrix(self, pose: torch.Tensor): 147 | if len(pose.shape) == 3 and pose.size(1) == 4 and pose.size(2) == 4: 148 | t_pose_matrix = pose 149 | else: 150 | check_tensor(pose, [-1, self.num_rot_params() + 3]) 151 | t_pose_matrix = self.build_pose_matrix(pose) 152 | return t_pose_matrix 153 | 154 | def apply_rotation(self, tensor: torch.Tensor, pose: torch.Tensor) -> torch.Tensor: 155 | """ 156 | Applies the rotation part of the pose on the point cloud or normal cloud 157 | 158 | Parameters 159 | ---------- 160 | tensor : [B, N, 3] 161 | A point or normal cloud tensor 162 | pose : [B, 4, 4] or [B, P] 163 | A pose matrix or pose params tensor 164 | """ 165 | t_pose_matrix = self.__to_pose_matrix(pose) 166 | transformed = torch.einsum("bij,bnj->bni", t_pose_matrix[:, :3, :3], tensor) 167 | return transformed 168 | 169 | def apply_transformation(self, points_3d: torch.Tensor, pose: torch.Tensor) -> torch.Tensor: 170 | """ 171 | Applies a transformation to a point cloud 172 | 173 | Parameters 174 | ---------- 175 | points_3d : [B, N, 3] 176 | A X, Y, Z point cloud tensor 177 | pose : [B, 4, 4] or [B, P] 178 | A pose matrix tensor or a pose params tensor 179 | """ 180 | t_pose_matrix = self.__to_pose_matrix(pose) 181 | 182 | rot_matrix_t = t_pose_matrix[:, :3, :3].permute(0, 2, 1) 183 | points_3d = torch.matmul(points_3d, rot_matrix_t) 184 | tr = t_pose_matrix[:, :3, 3].unsqueeze(1) 185 | points_3d = points_3d + tr 186 | return points_3d 187 | 188 | def from_pose_matrix(self, pose_matrix_tensor: torch.Tensor) -> torch.Tensor: 189 | """ 190 | Returns the tensor of the parameters of the pose 191 | 192 | Parameters 193 | ---------- 194 | pose_matrix_tensor : torch.Tensor 195 | The matrix tensor [B, 4, 4] 196 | 197 | Returns 198 | ------- 199 | torch.Tensor : [B, P] 200 | The pose parameters tensor. 201 | P is the degrees of freedom 6, (or 7 for 'quaternions') 202 | 203 | """ 204 | rotation_matrix = pose_matrix_tensor[:, :3, :3] 205 | rot_params = self.rot_params_from_matrix(rotation_matrix) 206 | trans_params = pose_matrix_tensor[:, :3, 3] 207 | return torch.cat([trans_params, rot_params], dim=1) 208 | 209 | def rot_matrix_from_params(self, rot_params: torch.Tensor) -> torch.Tensor: 210 | """ 211 | Builds a pose matrix tensor from its rotation parameters 212 | 213 | Parameters 214 | ---------- 215 | rot_params : [B, ROT_P] 216 | The rotation parameters 217 | """ 218 | if self.pose_type == "euler": 219 | return torch_euler_to_mat(rot_params, convention=self.euler_convention()) 220 | # return TF3d.euler_angles_to_matrix(rot_params, convention=self.euler_convention()) 221 | elif self.pose_type in ["quaternions", "quaternions_vec"]: 222 | quaternions = rot_params 223 | if self.pose_type == "quaternions_vec": 224 | # Transform the vector part of the quaternion (qx, qy, qz) into a unit quaternion 225 | quaternions = torch.cat([quaternions[:, :1].detach() * 0 + 1, quaternions], dim=1) 226 | 227 | # transform to unit quaternions 228 | norm_quat = quaternions / quaternions.norm(p=2, dim=1, keepdim=True) 229 | w, x, y, z = norm_quat[:, 0], norm_quat[:, 1], norm_quat[:, 2], norm_quat[:, 3] 230 | 231 | B = norm_quat.size(0) 232 | 233 | w2, x2, y2, z2 = w.pow(2), x.pow(2), y.pow(2), z.pow(2) 234 | wx, wy, wz = w * x, w * y, w * z 235 | xy, xz, yz = x * y, x * z, y * z 236 | 237 | rotation_matrix = torch.stack([w2 + x2 - y2 - z2, 2 * xy - 2 * wz, 2 * wy + 2 * xz, 238 | 2 * wz + 2 * xy, w2 - x2 + y2 - z2, 2 * yz - 2 * wx, 239 | 2 * xz - 2 * wy, 2 * wx + 2 * yz, w2 - x2 - y2 + z2], dim=1).reshape(B, 3, 3) 240 | return rotation_matrix 241 | else: 242 | raise ValueError("Unrecognised pose type") 243 | 244 | def rot_params_from_matrix(self, rot_matrix: torch.Tensor) -> torch.Tensor: 245 | """ 246 | Returns 247 | ------- 248 | torch.Tensor 249 | A [B, P] tensor with the parameters of the representation of the rotation matrices 250 | """ 251 | if self.pose_type == "euler": 252 | return torch_mat_to_euler(rot_matrix, convention=self.euler_convention()) 253 | # return TF3d.matrix_to_euler_angles(rot_matrix, convention=self.euler_convention()) 254 | elif self.pose_type in ["quaternions", "quaternions_vec"]: 255 | # TODO quaternions = self.matrix_to_quaternion(rot_matrix) 256 | raise NotImplementedError("") 257 | 258 | # # Deal with the sign ambiguity of the quaternions : force the first parameter qw to 1 259 | # quaternions = quaternions / quaternions[:, 0:1] 260 | # if self.pose_type == "quaternions": 261 | # unit_quaternions = quaternions / quaternions.norm(p=2, dim=1, keepdim=True) 262 | # return unit_quaternions 263 | # else: 264 | # # returns unscaled rotation parameters (supposing that qw = 1) 265 | # # Useful for pose prediction 266 | # return quaternions[:, 1:4] 267 | else: 268 | raise ValueError(f"Unexpected pose_type {self.pose_type}") 269 | 270 | def pose_matrix_jacobian(self, pose_params: torch.Tensor): 271 | assert_debug(self.pose_type == "euler", 'Only euler angles are supported for now') 272 | return torch_pose_matrix_jacobian_euler(pose_params) 273 | -------------------------------------------------------------------------------- /slam/common/rotation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | 4 | from slam.common.utils import assert_debug, check_tensor 5 | 6 | 7 | def Rx(phi): 8 | return np.array([ 9 | [1.0, 0.0, 0.0], 10 | [0.0, np.cos(phi), -np.sin(phi)], 11 | [0.0, np.sin(phi), np.cos(phi)]]) 12 | 13 | 14 | def torch_rx(cos_phi, sin_phi): 15 | """ 16 | >>> check_tensor(torch_rx(torch.randn(6), torch.randn(6)), [6, 3, 3]) 17 | """ 18 | check_tensor(cos_phi, [-1]) 19 | b = cos_phi.size(0) 20 | check_tensor(sin_phi, [b]) 21 | 22 | rot = torch.zeros(b, 3, 3, device=cos_phi.device, dtype=cos_phi.dtype) 23 | rot[:, 0, 0] = 1.0 24 | rot[:, 1, 1] = cos_phi 25 | rot[:, 2, 2] = cos_phi 26 | rot[:, 1, 2] = - sin_phi 27 | rot[:, 2, 1] = sin_phi 28 | return rot 29 | 30 | 31 | def JRx(phi): 32 | return np.array([ 33 | [0.0, 0.0, 0.0], 34 | [0.0, -np.sin(phi), -np.cos(phi)], 35 | [0.0, np.cos(phi), -np.sin(phi)] 36 | ]) 37 | 38 | 39 | def torch_jac_rx(cos_phi, sin_phi): 40 | check_tensor(cos_phi, [-1]) 41 | b = cos_phi.size(0) 42 | check_tensor(sin_phi, [b]) 43 | 44 | rot = torch.zeros(b, 3, 3, device=cos_phi.device, dtype=cos_phi.dtype) 45 | rot[:, 0, 0] = 0.0 46 | rot[:, 1, 1] = -sin_phi 47 | rot[:, 2, 2] = -sin_phi 48 | rot[:, 1, 2] = -cos_phi 49 | rot[:, 2, 1] = cos_phi 50 | return rot 51 | 52 | 53 | def Ry(theta): 54 | return np.array([ 55 | [np.cos(theta), 0.0, np.sin(theta)], 56 | [0.0, 1.0, 0.0], 57 | [-np.sin(theta), 0.0, np.cos(theta)]]) 58 | 59 | 60 | def torch_ry(cos_phi, sin_phi): 61 | """ 62 | >>> check_tensor(torch_ry(torch.randn(6), torch.randn(6)), [6, 3, 3]) 63 | """ 64 | check_tensor(cos_phi, [-1]) 65 | b = cos_phi.size(0) 66 | check_tensor(sin_phi, [b]) 67 | 68 | rot = torch.zeros(b, 3, 3, device=cos_phi.device, dtype=cos_phi.dtype) 69 | rot[:, 0, 0] = cos_phi 70 | rot[:, 1, 1] = 1.0 71 | rot[:, 2, 2] = cos_phi 72 | rot[:, 0, 2] = sin_phi 73 | rot[:, 2, 0] = -sin_phi 74 | return rot 75 | 76 | 77 | def JRy(theta): 78 | return np.array([ 79 | [-np.sin(theta), 0.0, np.cos(theta)], 80 | [0.0, 0.0, 0.0], 81 | [-np.cos(theta), 0.0, -np.sin(theta)] 82 | ]) 83 | 84 | 85 | def torch_jac_ry(cos_theta, sin_theta): 86 | check_tensor(cos_theta, [-1]) 87 | b = cos_theta.size(0) 88 | check_tensor(sin_theta, [b]) 89 | 90 | rot = torch.zeros(b, 3, 3, device=cos_theta.device, dtype=cos_theta.dtype) 91 | rot[:, 0, 0] = - sin_theta 92 | rot[:, 2, 2] = - sin_theta 93 | rot[:, 0, 2] = cos_theta 94 | rot[:, 2, 0] = -cos_theta 95 | return rot 96 | 97 | 98 | def Rz(psi): 99 | return np.array([ 100 | [np.cos(psi), - np.sin(psi), 0.0], 101 | [np.sin(psi), np.cos(psi), 0.0], 102 | [0.0, 0.0, 1.0]]) 103 | 104 | 105 | def torch_rz(cos_psi, sin_psi): 106 | check_tensor(cos_psi, [-1]) 107 | b = cos_psi.size(0) 108 | check_tensor(sin_psi, [b]) 109 | 110 | rot = torch.zeros(b, 3, 3, device=cos_psi.device, dtype=cos_psi.dtype) 111 | rot[:, 0, 0] = cos_psi 112 | rot[:, 1, 1] = cos_psi 113 | rot[:, 2, 2] = 1.0 114 | rot[:, 0, 1] = - sin_psi 115 | rot[:, 1, 0] = sin_psi 116 | return rot 117 | 118 | 119 | def JRz(psi): 120 | return np.array([[-np.sin(psi), -np.cos(psi), 0.0], 121 | [np.cos(psi), -np.sin(psi), 0.0], 122 | [0.0, 0.0, 0.0]]) 123 | 124 | 125 | def torch_jac_rz(cos_psi, sin_psi): 126 | check_tensor(cos_psi, [-1]) 127 | b = cos_psi.size(0) 128 | check_tensor(sin_psi, [b]) 129 | 130 | rot = torch.zeros(b, 3, 3, device=cos_psi.device, dtype=cos_psi.dtype) 131 | rot[:, 0, 0] = -sin_psi 132 | rot[:, 1, 1] = -sin_psi 133 | rot[:, 0, 1] = - cos_psi 134 | rot[:, 1, 0] = cos_psi 135 | return rot 136 | 137 | 138 | def euler_to_mat(angles, convention="xyz"): 139 | assert_debug(convention == "xyz") 140 | ex, ey, ez = angles 141 | return Rz(ez).dot(Ry(ey).dot(Rx(ex))) 142 | 143 | 144 | def torch_euler_to_mat(angles, convention="xyz"): 145 | assert_debug(convention == "xyz") 146 | torch_cos = torch.cos(angles) 147 | torch_sin = torch.sin(angles) 148 | return torch_rz(torch_cos[:, 2], torch_sin[:, 2]) @ \ 149 | torch_ry(torch_cos[:, 1], torch_sin[:, 1]) @ \ 150 | torch_rx(torch_cos[:, 0], torch_sin[:, 0]) 151 | 152 | 153 | def euler_jacobian(angles, convention="xyz"): 154 | assert_debug(convention == "xyz") 155 | ex, ey, ez = angles 156 | rot_z = Rz(ez) 157 | rot_y = Ry(ey) 158 | rot_x = Rx(ex) 159 | return np.vstack([ 160 | np.expand_dims(rot_z.dot(rot_y)._dot(JRx(ex)), 0), 161 | np.expand_dims(rot_z.dot(JRy(ey))._dot(rot_x), 0), 162 | np.expand_dims(JRz(ez).dot(rot_y)._dot(rot_x), 0) 163 | ]) 164 | 165 | 166 | def torch_euler_jacobian(angles, convention="xyz"): 167 | """ 168 | Returns 169 | ------- 170 | [B, 3, 3, 3] 171 | 172 | """ 173 | assert_debug(convention == "xyz") 174 | check_tensor(angles, [-1, 3]) 175 | torch_cos = torch.cos(angles) 176 | torch_sin = torch.sin(angles) 177 | 178 | rot_z = torch_rz(torch_cos[:, 2], torch_sin[:, 2]) 179 | rot_y = torch_ry(torch_cos[:, 1], torch_sin[:, 1]) 180 | rot_x = torch_rx(torch_cos[:, 0], torch_sin[:, 0]) 181 | return torch.cat([ 182 | (rot_z @ rot_y @ torch_jac_rx(torch_cos[:, 0], torch_sin[:, 0])).unsqueeze(1), 183 | (rot_z @ torch_jac_ry(torch_cos[:, 1], torch_sin[:, 1]) @ rot_x).unsqueeze(1), 184 | (torch_jac_rz(torch_cos[:, 2], torch_sin[:, 2]) @ rot_y @ rot_x).unsqueeze(1)], dim=1) 185 | 186 | 187 | def torch_pose_matrix_jacobian_euler(pose_params, convention="xyz"): 188 | """ 189 | 190 | Parameters 191 | ---------- 192 | pose_params : 193 | The pose parameters with euler convention [B, 6] 194 | [B, :3] the translation parameters 195 | [B, 3:] the euler angle parameters 196 | convention : 197 | The euler convention of the params 198 | 199 | Returns 200 | ------- 201 | torch.Tensor [B, 6, 4, 4] 202 | 203 | """ 204 | assert_debug(convention == "xyz") 205 | check_tensor(pose_params, [-1, 6]) 206 | n = pose_params.size(0) 207 | euler_jac = torch_euler_jacobian(pose_params[:, 3:], convention) 208 | pose_matrix_jac = torch.zeros(n, 6, 4, 4, 209 | dtype=pose_params.dtype, 210 | device=pose_params.device) 211 | pose_matrix_jac[:, 0, 0, 3] = 1.0 212 | pose_matrix_jac[:, 1, 1, 3] = 1.0 213 | pose_matrix_jac[:, 2, 2, 3] = 1.0 214 | 215 | pose_matrix_jac[:, 3:, :3, :3] = euler_jac 216 | return pose_matrix_jac 217 | 218 | 219 | def is_rotation_matrix(rot, eps=1.e-5): 220 | if isinstance(rot, np.ndarray): 221 | rot_t = np.transpose(rot) 222 | rot_t_rot = rot.dot(rot_t) 223 | id3 = np.eye(3, dtype=rot.dtype) 224 | n = np.linalg.norm(id3 - rot_t_rot) 225 | return n < eps 226 | elif isinstance(rot, torch.Tensor): 227 | check_tensor(rot, [-1, 3, 3]) 228 | rot_t = rot.transpose(1, 2) 229 | rot_t_rot = rot @ rot_t 230 | id3 = torch.eye(3, dtype=rot.dtype, device=rot.device) 231 | n = (id3 - rot_t_rot).abs().max() 232 | return n < eps 233 | else: 234 | raise NotImplementedError("") 235 | 236 | 237 | def mat_to_euler(rot, convention="xyz", eps=1.e-6): 238 | assert_debug(convention == "xyz") 239 | # assert_debug(is_rotation_matrix(rot)) 240 | sy = np.sqrt(rot[0, 0] * rot[0, 0] + rot[1, 0] * rot[1, 0]) 241 | singular = sy < eps 242 | if not singular: 243 | x = np.arctan2(rot[2, 1], rot[2, 2]) 244 | y = np.arctan2(-rot[2, 0], sy) 245 | z = np.arctan2(rot[1, 0], rot[0, 0]) 246 | else: 247 | x = np.arctan2(-rot[1, 2], rot[1, 1]) 248 | y = np.arctan2(-rot[2, 0], sy) 249 | z = 0 250 | return np.array([x, y, z]) 251 | 252 | 253 | def torch_mat_to_euler(rot, convention="xyz", eps=1.e-6): 254 | assert_debug(convention == "xyz") 255 | # assert_debug(is_rotation_matrix(rot), f" Not a rotation matrix {rot}") 256 | sy = torch.sqrt(rot[:, 0, 0] * rot[:, 0, 0] + rot[:, 1, 0] * rot[:, 1, 0]) 257 | is_singular = sy < eps 258 | 259 | x_ns = torch.atan2(rot[:, 2, 1], rot[:, 2, 2]) 260 | y_ns = torch.atan2(-rot[:, 2, 0], sy) 261 | z_ns = torch.atan2(rot[:, 1, 0], rot[:, 0, 0]) 262 | 263 | x_s = torch.atan2(-rot[:, 1, 2], rot[:, 1, 1]) 264 | y_s = torch.atan2(-rot[:, 2, 0], sy) 265 | z_s = 0 266 | 267 | return torch.cat([ 268 | (x_ns * ~is_singular + x_s * is_singular).unsqueeze(1), 269 | (y_ns * ~is_singular + y_s * is_singular).unsqueeze(1), 270 | (z_ns * ~is_singular + z_s * is_singular).unsqueeze(1)], dim=1) 271 | 272 | 273 | def torch_mat_to_euler2(rot, convention="xyz", eps=1.e-6): 274 | assert_debug(convention == "xyz") 275 | # assert_debug(is_rotation_matrix(rot)) 276 | 277 | thetas1 = -torch.asin(rot[:, 2, 0]) 278 | thetas2 = np.pi - thetas1 279 | 280 | c_theta1 = torch.cos(thetas1) 281 | c_theta2 = torch.cos(thetas2) 282 | 283 | psi1 = torch.atan2(rot[:, 2, 1] / c_theta1, rot[:, 2, 2] / c_theta1) 284 | psi2 = torch.atan2(rot[:, 2, 1] / c_theta2, rot[:, 2, 2] / c_theta2) 285 | 286 | phi1 = torch.atan2(rot[:, 1, 0] / c_theta1, rot[:, 0, 0] / c_theta1) 287 | phi2 = torch.atan2(rot[:, 1, 0] / c_theta2, rot[:, 0, 0] / c_theta2) 288 | 289 | # Mask for edge cases 290 | mask_r20_1 = (rot[:, 2, 0] - 1).abs() < eps # R[2, 0] == 1 291 | mask_r20_m1 = (rot[:, 2, 0] + 1).abs() < eps # R[2, 0] == -1 292 | 293 | phi_edge = torch.zeros_like(phi1) 294 | thetas_1 = 0.5 * np.pi * mask_r20_m1 295 | psi_1 = phi_edge + torch.atan2(rot[:, 0, 1], rot[:, 0, 2]) 296 | thetas_m1 = - 0.5 * np.pi * mask_r20_1 297 | psi_m1 = -phi_edge + torch.atan2(-rot[:, 0, 1], -rot[:, 0, 2]) 298 | 299 | mask = ~mask_r20_m1 * ~mask_r20_1 300 | 301 | phi1 = mask * phi1 + ~mask * phi_edge 302 | psi1 = mask * psi1 + mask_r20_1 * psi_1 + mask_r20_m1 * psi_m1 303 | thetas1 = mask * thetas1 + mask_r20_1 * thetas_1 + mask_r20_m1 * thetas_m1 304 | 305 | phi2 = mask * phi2 + ~mask * phi_edge 306 | psi2 = mask * psi2 + mask_r20_1 * psi_1 + mask_r20_m1 * psi_m1 307 | thetas2 = mask * thetas2 + mask_r20_1 * thetas_1 + mask_r20_m1 * thetas_m1 308 | 309 | angles1 = torch.cat([psi1.unsqueeze(1), thetas1.unsqueeze(1), phi1.unsqueeze(1)], dim=1) 310 | angles2 = torch.cat([psi2.unsqueeze(1), thetas2.unsqueeze(1), phi2.unsqueeze(1)], dim=1) 311 | 312 | mask_1 = angles1.abs().norm(dim=1, keepdim=True) < angles2.abs().norm(dim=1, keepdim=True) 313 | angles = angles1 * mask_1 + angles2 * ~mask_1 314 | return angles 315 | -------------------------------------------------------------------------------- /slam/common/timer.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import time 3 | 4 | 5 | class Duration: 6 | def __init__(self, title: str = ""): 7 | self.duration_ms: int = 0 8 | self.title = title 9 | self.iters = 0 10 | 11 | def get_message(self) -> str: 12 | return f""" 13 | ----------------------------------------------- 14 | ELAPSED {self.title} : 15 | ----------------------------------------------- 16 | milliseconds {self.duration_ms} 17 | seconds {self.duration_ms / 1000} 18 | minutes {self.duration_ms / (1000 * 60)} 19 | hours {self.duration_ms / (1000 * 3600)} 20 | ----------------------------------------------- 21 | Num Iters : {self.iters} 22 | s/it {self.duration_ms / (1000 * max(1, self.iters))} 23 | ----------------------------------------------- 24 | """ 25 | 26 | def __del__(self): 27 | print(self.get_message()) 28 | 29 | 30 | def timer() -> object: 31 | def __decorator(func): 32 | if not hasattr(func, "duration"): 33 | duration = Duration(func.__name__) 34 | setattr(func, "duration", duration) 35 | else: 36 | duration = getattr(func, "duration") 37 | 38 | def __wrapper(*args, **kwargs): 39 | duration.iters += 1 40 | start = time.clock() 41 | 42 | result = func(*args, **kwargs) 43 | 44 | elapsed = time.clock() - start 45 | duration.duration_ms += elapsed * 1000 46 | 47 | return result 48 | 49 | return __wrapper 50 | 51 | return __decorator 52 | 53 | 54 | def torch_timer(title: str = "", cuda: bool = True): 55 | duration = Duration(title) 56 | 57 | def decorator(func): 58 | def wrapper(*args, **kwargs): 59 | duration.iters += 1 60 | if cuda: 61 | torch.cuda.synchronize() 62 | 63 | start = time.clock() 64 | 65 | result = func(*args, **kwargs) 66 | if cuda: 67 | torch.cuda.synchronize() 68 | 69 | elapsed = time.clock() - start 70 | duration.duration_ms += elapsed * 1000 71 | 72 | return result 73 | 74 | return wrapper 75 | 76 | return decorator 77 | -------------------------------------------------------------------------------- /slam/common/torch_utils.py: -------------------------------------------------------------------------------- 1 | from typing import Union, Optional, Type 2 | 3 | import torch 4 | from torchvision.transforms.functional import to_tensor 5 | import collections 6 | from torch.utils.data.dataloader import default_collate 7 | 8 | import numpy as np 9 | 10 | from slam.common.utils import assert_debug, check_tensor 11 | 12 | 13 | def custom_to_tensor(data: Union[torch.Tensor, np.ndarray, dict], 14 | device: Union[str, torch.device] = "cuda", 15 | torchviz_conversion: bool = True, 16 | batch_dim: bool = False) -> Union[torch.Tensor, dict]: 17 | """ 18 | Converts data to a Tensor for compatible data types 19 | 20 | Parameters 21 | ---------- 22 | data : An data to convert to torch 23 | The data can be a map, numpy ndarray or other. 24 | All tensor like data (numpy ndarray for instance) are converted to tensor 25 | All the values of containers are transformed to tensors 26 | device : The device to send the tensors to 27 | torchviz_conversion : Whether to use torchviz conversion 28 | Default torch to_tensor method simply converts a numpy tensor to a torch Tensor 29 | torchvision to_tensor also changes the layout of the image-like tensors, 30 | A H, W, D numpy image becomes a D, H, W tensor. 31 | One must therefore be careful that this is what is intended 32 | batch_dim : bool 33 | Whether to add a dimension (in first position) 34 | A [N1, N2, ..., NK] tensor will be transformed to [1, N1, N2, ..., NK] tensor 35 | """ 36 | if isinstance(data, collections.Mapping): 37 | return {key: custom_to_tensor(data[key], 38 | device=device, 39 | torchviz_conversion=torchviz_conversion, 40 | batch_dim=batch_dim) for key in data} 41 | if isinstance(data, np.ndarray): 42 | if torchviz_conversion: 43 | tensor = to_tensor(data).to(device=device) 44 | else: 45 | tensor = torch.from_numpy(data).to(device=device) 46 | if batch_dim: 47 | tensor = tensor.unsqueeze(0) 48 | return tensor 49 | 50 | if isinstance(data, torch.Tensor): 51 | tensor = data.to(device=device) 52 | if batch_dim: 53 | tensor = tensor.unsqueeze(0) 54 | return tensor 55 | 56 | return data 57 | 58 | 59 | def send_to_device(data: Union[dict, torch.Tensor, np.ndarray], 60 | device: torch.device, 61 | convert_numpy: bool = True, 62 | torchviz_conversion: bool = True) -> object: 63 | """ 64 | Sends data to the device if it can 65 | 66 | torch.Tensor are sent to the device, 67 | containers send all the torch.Tensor to the devices 68 | Other data types are left unchanged 69 | """ 70 | if isinstance(data, torch.Tensor): 71 | data = data.to(device=device) 72 | 73 | if isinstance(data, collections.Mapping): 74 | data = {key: send_to_device(data[key], device) for key in data} 75 | 76 | if isinstance(data, np.ndarray) and convert_numpy: 77 | data = custom_to_tensor(data, 78 | device=device, 79 | torchviz_conversion=torchviz_conversion) 80 | 81 | return data 82 | 83 | 84 | def convert_pose_transform(pose: Union[torch.Tensor, np.ndarray], 85 | dest: type = torch.Tensor, 86 | device: Optional[torch.device] = None, 87 | dtype: Optional[Union[torch.dtype, np.number, Type]] = None): 88 | """Converts a [4, 4] pose tensor to the desired type 89 | 90 | Returns a tensor (either a numpy.ndarray or torch.Tensor depending on dest type) 91 | >>> check_tensor(convert_pose_transform(torch.eye(4).reshape(4, 4), np.ndarray), [4, 4]) 92 | >>> check_tensor(convert_pose_transform(torch.eye(4).reshape(1, 4, 4), np.ndarray, dtype=np.float32), [4, 4]) 93 | >>> check_tensor(convert_pose_transform(torch.eye(4).reshape(1, 4, 4), torch.Tensor, dtype=torch.float32), [1, 4, 4]) 94 | >>> check_tensor(convert_pose_transform(torch.eye(4).reshape(4, 4), torch.Tensor, dtype=torch.float32), [4, 4]) 95 | >>> check_tensor(convert_pose_transform(np.eye(4).reshape(4, 4), torch.Tensor, dtype=torch.float32), [4, 4]) 96 | >>> check_tensor(convert_pose_transform(np.eye(4).reshape(4, 4), np.ndarray, dtype=np.float32), [4, 4]) 97 | >>> check_tensor(convert_pose_transform(np.eye(4).reshape(4, 4), np.ndarray), [4, 4]) 98 | """ 99 | # Check size 100 | if isinstance(pose, torch.Tensor): 101 | assert_debug(list(pose.shape) == [1, 4, 4] or list(pose.shape) == [4, 4], 102 | f"Wrong tensor shape, expected [(1), 4, 4], got {pose.shape}") 103 | if dest == torch.Tensor: 104 | assert_debug(isinstance(dtype, torch.dtype), f"The dtype {dtype} is not a torch.dtype") 105 | return pose.to(device=device if device is not None else pose.device, 106 | dtype=dtype if dtype is not None else pose.dtype) 107 | else: 108 | assert_debug(dest == np.ndarray, "Only numpy.ndarray and torch.Tensor are supported as destination tensor") 109 | np_array = pose.detach().cpu().numpy() 110 | if dtype is not None: 111 | assert_debug(issubclass(dtype, np.number), f"Expected a numpy.dtype, got {dtype}") 112 | np_array = np_array.astype(dtype) 113 | return np_array.reshape(4, 4) 114 | else: 115 | assert_debug(isinstance(pose, np.ndarray), f"Only numpy.ndarray and torch.Tensor are supported. Got {pose}.") 116 | check_tensor(pose, [4, 4]) 117 | if dest == torch.Tensor: 118 | tensor = torch.from_numpy(pose).to(dtype=dtype, device=device) 119 | return tensor 120 | if dtype is not None: 121 | assert_debug(issubclass(dtype, np.number), f"Expected numpy.dtype, got {dtype}") 122 | new_pose = pose.astype(dtype) 123 | return new_pose 124 | return pose 125 | 126 | 127 | # ---------------------------------------------------------------------------------------------------------------------- 128 | # Collate Function 129 | from slam.common.modules import _with_ct_icp 130 | 131 | if _with_ct_icp: 132 | import pyct_icp as pct 133 | 134 | 135 | def collate_fun(batch) -> object: 136 | """ 137 | Overrides pytorch default collate function, to keep numpy arrays in dictionaries 138 | 139 | If `batch` is a dictionary, every key containing the key `numpy` will not be converted to a tensor 140 | And a suffix "_" will be appended to the key, to identify arrays by their batch index 141 | 142 | The original key will map only to the first element of the batch 143 | """ 144 | elem = batch[0] 145 | if isinstance(elem, list): 146 | return batch 147 | elif isinstance(elem, collections.Mapping): 148 | 149 | result = dict() 150 | for key in elem: 151 | cumulate_key = "numpy" in key 152 | if _with_ct_icp: 153 | if isinstance(elem[key], pct.LiDARFrame): 154 | cumulate_key = True 155 | if cumulate_key: 156 | for idx, d in enumerate(batch): 157 | if idx == 0: 158 | result[f"{key}"] = d[key] 159 | 160 | result[f"{key}_{idx}"] = d[key] 161 | 162 | else: 163 | result[key] = collate_fun([d[key] for d in batch]) 164 | return result 165 | else: 166 | return default_collate(batch) 167 | -------------------------------------------------------------------------------- /slam/dataset/__init__.py: -------------------------------------------------------------------------------- 1 | from enum import Enum 2 | 3 | from slam.common.utils import ObjectLoaderEnum 4 | from slam.dataset.configuration import DatasetLoader, DatasetConfig 5 | from slam.dataset.kitti_dataset import KITTIDatasetLoader, KITTIConfig 6 | from slam.dataset.nclt_dataset import NCLTDatasetLoader, NCLTConfig 7 | from slam.dataset.ford_dataset import FordCampusDatasetLoader, FordCampusConfig 8 | from slam.dataset.nhcd_dataset import NHCDDatasetLoader, NHCDConfig 9 | from slam.dataset.kitti_360_dataset import (KITTI360Config, KITTI360DatasetLoader) 10 | 11 | from slam.dataset.rosbag_dataset import _with_rosbag 12 | from slam.dataset.ct_icp_dataset import _with_ct_icp 13 | 14 | 15 | class DATASET(ObjectLoaderEnum, Enum): 16 | """ 17 | The different datasets covered by the dataset_config configuration 18 | A configuration must have the field dataset_config pointing to one of these keys 19 | """ 20 | 21 | kitti = (KITTIDatasetLoader, KITTIConfig) 22 | kitti_360 = (KITTI360DatasetLoader, KITTI360Config) 23 | nclt = (NCLTDatasetLoader, NCLTConfig) 24 | ford_campus = (FordCampusDatasetLoader, FordCampusConfig) 25 | nhcd = (NHCDDatasetLoader, NHCDConfig) 26 | if _with_rosbag: 27 | from slam.dataset.rosbag_dataset import RosbagDatasetConfiguration, RosbagConfig 28 | from slam.dataset.urban_loco_dataset import UrbanLocoConfig, UrbanLocoDatasetLoader 29 | rosbag = (RosbagDatasetConfiguration, RosbagConfig) 30 | urban_loco = (UrbanLocoDatasetLoader, UrbanLocoConfig) 31 | 32 | if _with_ct_icp: 33 | from slam.dataset.ct_icp_dataset import CT_ICPDatasetLoader, CT_ICPDatasetConfig 34 | ct_icp = (CT_ICPDatasetLoader, CT_ICPDatasetConfig) 35 | 36 | @classmethod 37 | def type_name(cls): 38 | return "dataset" 39 | -------------------------------------------------------------------------------- /slam/dataset/configuration.py: -------------------------------------------------------------------------------- 1 | from abc import abstractmethod, ABC 2 | from typing import Tuple, Optional 3 | 4 | from torch.utils.data import Dataset, ConcatDataset 5 | 6 | # Hydra and OmegaConf 7 | from hydra.conf import dataclass 8 | from omegaconf import MISSING 9 | 10 | # Project Imports 11 | from slam.common.projection import SphericalProjector 12 | from slam.dataset.sequence_dataset import DatasetOfSequences 13 | 14 | 15 | @dataclass 16 | class DatasetConfig: 17 | """A DatasetConfig contains the configuration values used to define a DatasetConfiguration""" 18 | dataset: str = MISSING 19 | 20 | # The length of the sequence returned 21 | sequence_len: int = 2 22 | 23 | # ---------------------------------- 24 | # Default item keys in the data_dict 25 | vertex_map_key: str = "vertex_map" 26 | numpy_pc_key: str = "numpy_pc" 27 | absolute_gt_key: str = "absolute_pose_gt" 28 | with_numpy_pc: bool = True # Whether to add the numpy pc to the data_dict 29 | 30 | 31 | class DatasetLoader(ABC): 32 | """ 33 | A DatasetConfiguration is the configuration for the construction of pytorch Datasets 34 | """ 35 | 36 | @classmethod 37 | def max_num_workers(cls): 38 | """Returns the maximum number of workers allowed by this dataset 39 | 40 | Note: Not respecting this constraint can lead to undefined behaviour for 41 | Datasets which do not support Random Access 42 | """ 43 | return 20 44 | 45 | @staticmethod 46 | def absolute_gt_key(): 47 | """The key (in data_dict) for the absolute_pose_gt""" 48 | return "absolute_pose_gt" 49 | 50 | @staticmethod 51 | def numpy_pc_key(): 52 | """The key (in data_dict) for xyz pointcloud""" 53 | return "numpy_pc" 54 | 55 | def __init__(self, config: DatasetConfig): 56 | self.config = config 57 | 58 | @abstractmethod 59 | def projector(self) -> SphericalProjector: 60 | """ 61 | Returns the Default Spherical Image projector associated to the dataset_config 62 | """ 63 | raise NotImplementedError("") 64 | 65 | @abstractmethod 66 | def sequences(self): 67 | """ 68 | Returns the train, eval and test datasets and the corresponding sequence name 69 | 70 | Returns: (train, eval, test, transform) 71 | train (Optional[List[Dataset], List]): Is an Optional pair of a list of datasets 72 | and the corresponding sequences 73 | eval (Optional[List[Dataset], List]): Idem 74 | test (Optional[List[Dataset], List]): Idem 75 | transform (callable): The function applied on the data from the given datasets 76 | 77 | """ 78 | raise NotImplementedError("") 79 | 80 | def get_dataset(self) -> Tuple[Dataset, Dataset, Dataset, callable]: 81 | """ 82 | Returns: 83 | (train_dataset, eval_dataset, test_dataset) 84 | A tuple of `DatasetOfSequences` consisting of concatenated datasets 85 | """ 86 | train_dataset, eval_datasets, test_datasets, transform = self.sequences() 87 | 88 | def __swap(dataset): 89 | if dataset[0] is not None: 90 | return ConcatDataset(dataset[0]) 91 | return None 92 | 93 | train_dataset = __swap(train_dataset) 94 | eval_datasets = __swap(eval_datasets) 95 | test_datasets = __swap(test_datasets) 96 | 97 | return train_dataset, eval_datasets, test_datasets, transform 98 | 99 | def get_sequence_dataset(self) -> Tuple[Optional[DatasetOfSequences], 100 | Optional[DatasetOfSequences], 101 | Optional[DatasetOfSequences]]: 102 | """ 103 | Returns: 104 | (train_dataset, eval_dataset, test_dataset) : A tuple of `DatasetOfSequences` 105 | """ 106 | sequence_len = self.config.sequence_len 107 | train_dataset, eval_datasets, test_datasets, transform = self.sequences() 108 | 109 | def __to_sequence_dataset(dataset_pair): 110 | if dataset_pair is None or dataset_pair[0] is None: 111 | return None 112 | return DatasetOfSequences(sequence_len, dataset_pair[0], dataset_pair[1], transform=transform) 113 | 114 | return tuple(map(__to_sequence_dataset, [train_dataset, eval_datasets, test_datasets])) 115 | 116 | @abstractmethod 117 | def get_ground_truth(self, sequence_name): 118 | """Returns the ground truth for the dataset_config for a given sequence""" 119 | return None 120 | -------------------------------------------------------------------------------- /slam/dataset/dataset.py: -------------------------------------------------------------------------------- 1 | from typing import Optional 2 | 3 | from torch.utils.data import Dataset 4 | 5 | from slam.common.utils import assert_debug 6 | 7 | 8 | class WrapperDataset(Dataset): 9 | """ 10 | A Wrapper Dataset which applies a transform on the items returned by a torch.Dataset 11 | """ 12 | 13 | def __init__(self, dataset: Dataset, transform: Optional[callable] = None): 14 | self.dataset = dataset 15 | self.transform = transform 16 | 17 | def __len__(self): 18 | return len(self.dataset) 19 | 20 | def __getitem__(self, item): 21 | data = self.dataset[item] 22 | transformed = self.transform(data) 23 | return transformed 24 | 25 | 26 | class WindowDataset(Dataset): 27 | """ 28 | A Window datasets wraps a dataset_config, and limit it to a defined window 29 | """ 30 | 31 | def __init__(self, dataset: Dataset, start: int = 0, length: int = -1): 32 | self.dataset = dataset 33 | assert_debug(start < len(dataset)) 34 | assert_debug(start + length <= len(dataset)) 35 | 36 | self.length = length 37 | self.start = start 38 | 39 | def __len__(self): 40 | return self.length 41 | 42 | def __getitem__(self, item): 43 | return self.dataset[item + self.start] 44 | -------------------------------------------------------------------------------- /slam/dataset/ford_dataset.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | from typing import Optional, List 3 | import os 4 | 5 | import numpy as np 6 | import torch 7 | from hydra.core.config_store import ConfigStore 8 | from torch.utils.data import Dataset 9 | from scipy.io import loadmat 10 | 11 | # Hydra and OmegaConf 12 | from hydra.conf import dataclass, MISSING, field 13 | 14 | # Project Imports 15 | from slam.common.pose import Pose 16 | from slam.common.projection import SphericalProjector 17 | from slam.common.utils import assert_debug 18 | from slam.dataset import DatasetLoader, DatasetConfig 19 | from slam.eval.eval_odometry import compute_relative_poses 20 | 21 | 22 | # ---------------------------------------------------------------------------------------------------------------------- 23 | class FordCampusSequence(Dataset): 24 | """ 25 | Ford Campus Sequence 26 | """ 27 | 28 | def __init__(self, 29 | sequence_dir: str, 30 | projector: Optional[SphericalProjector] = None, 31 | with_gt: bool = True, 32 | vertex_map_channel: str = "vertex_map", 33 | gt_channel: str = "trajectory_gt", 34 | pc_channel: str = "numpy_pc"): 35 | self.sequence_dir = Path(sequence_dir) / "SCANS" 36 | assert_debug(self.sequence_dir.exists()) 37 | self.list_of_files = list(sorted(os.listdir(str(self.sequence_dir)))) 38 | self.projector = projector 39 | 40 | self._with_gt = with_gt 41 | self._vmap_channel = vertex_map_channel 42 | self._gt_channel = gt_channel 43 | self._pc_channel = pc_channel 44 | self._pose = Pose("euler") 45 | self.__np_sensor_to_vehicule = np.array([[0.0, 1.0, 0.0], 46 | [-1.0, 0.0, 0.0], 47 | [0.0, 0.0, 1.0]], dtype=np.float32) 48 | self._sensor_to_vehicule = torch.tensor([[0.0, 1.0, 0.0, 0.0], 49 | [-1.0, 0.0, 0.0, 0.0], 50 | [0.0, 0.0, 1.0, 0.0], 51 | [0.0, 0.0, 0.0, 1.0]], dtype=torch.float32) 52 | 53 | def __len__(self): 54 | return len(self.list_of_files) 55 | 56 | def __read_scan(self, idx): 57 | scan_file = str(self.sequence_dir / self.list_of_files[idx]) 58 | mat_content = loadmat(scan_file) 59 | return mat_content["SCAN"] 60 | 61 | def __getitem__(self, idx): 62 | assert_debug(0 <= idx < self.__len__()) 63 | mat_content = self.__read_scan(idx) 64 | 65 | pc_sensor = mat_content["XYZ"][0, 0].T 66 | pc_sensor = pc_sensor[np.linalg.norm(pc_sensor, axis=-1) > 8] 67 | pc_vehicule = np.einsum("ij,nj->ni", self.__np_sensor_to_vehicule, pc_sensor) 68 | 69 | # Put the PC into the list, so that pytorch does not try to aggregate uncompatible pointcloud dimensions 70 | data_dict = {self._pc_channel: pc_vehicule} 71 | if self.projector: 72 | torch_pc = torch.from_numpy(pc_vehicule).unsqueeze(0) 73 | vmap = self.projector.build_projection_map(torch_pc)[0] 74 | data_dict[self._vmap_channel] = vmap.to(torch.float32) 75 | 76 | if self._with_gt: 77 | gt_params = mat_content["X_wv"][0, 0].T 78 | vehicule_to_world = self._pose.build_pose_matrix(torch.from_numpy(gt_params))[0].to(torch.float32) 79 | data_dict[self._gt_channel] = vehicule_to_world 80 | 81 | return data_dict 82 | 83 | 84 | # ---------------------------------------------------------------------------------------------------------------------- 85 | @dataclass 86 | class FordCampusConfig(DatasetConfig): 87 | """A Configuration object read from a yaml conf""" 88 | 89 | # -------------------- 90 | root_dir: str = MISSING 91 | dataset: str = "ford_campus" 92 | 93 | # ------------------- 94 | up_fov: float = 3 95 | down_fov: float = -25 96 | lidar_height: int = 64 97 | lidar_width: int = 720 98 | 99 | train_sequences: List[str] = field(default_factory=lambda: ["dataset-1", "dataset-2"]) 100 | test_sequences: List[str] = field(default_factory=lambda: ["dataset-1", "dataset-2"]) 101 | eval_sequences: List[str] = field(default_factory=lambda: []) 102 | 103 | 104 | # Hydra -- stores a FordCampusConfig `ford_campus` in the `dataset` group 105 | cs = ConfigStore.instance() 106 | cs.store(group="dataset", name="ford_campus", node=FordCampusConfig) 107 | 108 | 109 | class FordCampusDatasetLoader(DatasetLoader): 110 | """ 111 | Configuration for Ford Dataset 112 | """ 113 | 114 | def __init__(self, config: FordCampusConfig, **kwargs): 115 | super().__init__(config) 116 | self.root_dir = Path(self.config.root_dir) 117 | 118 | def projector(self) -> SphericalProjector: 119 | up_fov = self.config.up_fov 120 | down_fov = self.config.down_fov 121 | lidar_height = self.config.lidar_height 122 | lidar_width = self.config.lidar_width 123 | return SphericalProjector(up_fov=up_fov, down_fov=down_fov, height=lidar_height, width=lidar_width) 124 | 125 | _sequence_name_to_prefix = {"dataset-1": "IJRR-Dataset-1", 126 | "dataset-2": "IJRR-Dataset-2"} 127 | 128 | def sequences(self): 129 | """Returns the tuples (dataset_config, sequence_name) for train, eval and test split on FordCampus""" 130 | train_sequences = self.config.train_sequences 131 | test_sequences = self.config.test_sequences 132 | gt_pose_channel = self.config.absolute_gt_key 133 | 134 | def __get_datasets(sequences): 135 | if sequences is None: 136 | return None 137 | datasets = [] 138 | for sequence in sequences: 139 | assert_debug(sequence in self._sequence_name_to_prefix) 140 | dir = self.root_dir / self._sequence_name_to_prefix[sequence] 141 | datasets.append(FordCampusSequence(str(dir), 142 | projector=self.projector(), 143 | gt_channel=gt_pose_channel, 144 | vertex_map_channel=self.config.vertex_map_key, 145 | pc_channel=self.config.numpy_pc_key)) 146 | return datasets 147 | 148 | return (__get_datasets(train_sequences), train_sequences), \ 149 | None, \ 150 | (__get_datasets(test_sequences), test_sequences), lambda x: x 151 | 152 | def get_ground_truth(self, sequence_name): 153 | poses_path = self.root_dir / self._sequence_name_to_prefix[sequence_name] / "poses_gt.npy" 154 | if poses_path.exists(): 155 | absolute_gt = np.load(str(poses_path)) 156 | relative = compute_relative_poses(absolute_gt) 157 | return relative 158 | return None 159 | -------------------------------------------------------------------------------- /slam/dataset/nhcd_dataset.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | from torch.utils.data import Dataset 4 | import open3d as o3d 5 | from pathlib import Path 6 | from typing import Optional 7 | from scipy.spatial.transform.rotation import Rotation as R, Slerp 8 | import logging 9 | 10 | # Hydra and OmegaConf 11 | from hydra.core.config_store import ConfigStore 12 | from hydra.conf import dataclass, MISSING, field 13 | 14 | # Project Imports 15 | from slam.common.projection import SphericalProjector 16 | from slam.common.utils import assert_debug 17 | from slam.dataset.configuration import DatasetLoader, DatasetConfig 18 | from slam.eval.eval_odometry import compute_relative_poses 19 | 20 | 21 | def read_ground_truth(file_path: str): 22 | assert_debug(Path(file_path).exists()) 23 | 24 | ground_truth_df = np.genfromtxt(str(file_path), delimiter=',', dtype=np.float64) 25 | seconds = ground_truth_df[:, 0] 26 | nano_seconds = ground_truth_df[:, 1] 27 | xyz = ground_truth_df[:, 2:5] 28 | qxyzw = ground_truth_df[:, 5:] 29 | 30 | num_poses = qxyzw.shape[0] 31 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) 32 | poses[:, :3, :3] = R.from_quat(qxyzw).as_matrix() 33 | poses[:, :3, 3] = xyz 34 | 35 | T_CL = np.eye(4, dtype=np.float32) 36 | T_CL[:3, :3] = R.from_quat([0.0, 0.0, 0.924, 0.383]).as_matrix() 37 | T_CL[:3, 3] = np.array([-0.084, -0.025, 0.050], dtype=np.float32) 38 | poses = np.einsum("nij,jk->nik", poses, T_CL) 39 | 40 | poses_timestamps = seconds * 10e9 + nano_seconds 41 | poses = np.einsum("ij,njk->nik", np.linalg.inv(poses[0]), poses) 42 | return poses, poses_timestamps 43 | 44 | 45 | def pointcloud_poses(poses, poses_timestamps, filenames): 46 | """Associate to a pointcloud (given by a filename) 47 | the closest pose (in terms of timestamps) to the ground truth 48 | """ 49 | timestamps = [] 50 | for filename in filenames: 51 | tokens = filename.replace('.', '_ ').split("_") 52 | secs = float(tokens[1]) 53 | nsecs = float(tokens[2]) 54 | timestamps.append(secs * 10e9 + nsecs) 55 | 56 | file_timestamps = np.array(timestamps) 57 | # Associate closest poses (in terms of timestamps) 58 | file_indices = np.searchsorted(poses_timestamps, file_timestamps) 59 | 60 | return poses[file_indices] 61 | 62 | 63 | class NHCDOdometrySequence(Dataset): 64 | """ 65 | Dataset for a Sequence of the New Handheld College Dataset 66 | see https://ori-drs.github.io/newer-college-dataset/ 67 | 68 | Attributes: 69 | sequences_root_dir (str): The path to KITTI odometry benchmark's data 70 | sequence_id (str): The name id of the sequence in ["01_short_experiment", "02_long_experiment"] 71 | 72 | lidar_projector (SphericalProjector): The Spherical Projector, which projects pointclouds in the image plane 73 | ground_truth_channel (Optional[str]): The key in the dictionary for the ground truth absolute pose 74 | """ 75 | 76 | @staticmethod 77 | def num_frames(sequence_id: str): 78 | if sequence_id == "01_short_experiment": 79 | return 15301 80 | elif sequence_id == "02_long_experiment": 81 | # Remove the last 600 frames which correspond to the arrival of the sensor to the garage 82 | # And includes very abrupt, motions 83 | return 26000 84 | 85 | def __init__(self, 86 | sequences_root_dir: str, 87 | sequence_id: str, 88 | lidar_projector: SphericalProjector = None, 89 | pointcloud_channel: str = "numpy_pc", 90 | ground_truth_channel: Optional[str] = "absolute_pose_gt", 91 | with_numpy_pc: bool = False): 92 | self.dataset_root: Path = Path(sequences_root_dir) 93 | self.sequence_id: str = sequence_id 94 | self.ground_truth_channel = ground_truth_channel 95 | self.id = self.sequence_id 96 | self.lidar_projector = lidar_projector 97 | self._with_numpy_pc = with_numpy_pc 98 | self.pcd_paths: Path = self.dataset_root / sequence_id / "raw_format" / "ouster_scan" 99 | assert_debug(self.pcd_paths.exists(), "The path to the folders of the pcd files does not exist") 100 | self.file_names = [filename for filename in sorted(os.listdir(str(self.pcd_paths))) if "(1)" not in filename] 101 | self._size = self.num_frames(self.sequence_id) 102 | self.pointcloud_channel = pointcloud_channel 103 | 104 | ground_truth_path = self.dataset_root / sequence_id / "ground_truth" / "registered_poses.csv" 105 | self.has_gt = False 106 | self.poses = None 107 | self.poses_seconds = None 108 | self.poses_nanoseconds = None 109 | 110 | if ground_truth_path.exists(): 111 | self.has_gt = True 112 | poses, poses_timestamps = read_ground_truth(str(ground_truth_path)) 113 | 114 | # poses = np.linalg.inv(poses) 115 | self.poses = pointcloud_poses(poses, poses_timestamps, self.file_names) 116 | 117 | # For each file : associate the closest pose (in terms of timestamps) 118 | self.poses = np.einsum("ij,njk->nik", np.linalg.inv(poses[0]), poses) 119 | 120 | def __len__(self): 121 | return self._size 122 | 123 | def __getitem__(self, idx) -> dict: 124 | """ 125 | Returns: 126 | A dictionary with the mapping defined in the constructor 127 | """ 128 | assert_debug(idx < self._size) 129 | 130 | file_path = self.pcd_paths / self.file_names[idx] 131 | assert_debug(file_path.exists(), "Could not open the file " + str(file_path)) 132 | 133 | data_dict = dict() 134 | pointcloud: o3d.geometry.PointCloud = o3d.io.read_point_cloud(str(file_path), "pcd") 135 | xyz = np.asarray(pointcloud.points).copy() 136 | del pointcloud 137 | 138 | # Read timestamps 139 | data_dict[self.pointcloud_channel] = xyz.astype(np.float32) 140 | N_rows = int(xyz.shape[0] / 64) 141 | timestamps = np.arange(N_rows).reshape(N_rows, 1).repeat(64, axis=1).reshape(-1, ).astype(np.float64) 142 | min_t = timestamps.min() 143 | max_t = timestamps.max() 144 | 145 | timestamps = (timestamps - min_t) / (max_t - min_t) + idx 146 | data_dict[f"{self.pointcloud_channel}_timestamps"] = timestamps 147 | 148 | if self.has_gt: 149 | data_dict[self.ground_truth_channel] = self.poses[idx] 150 | 151 | return data_dict 152 | 153 | 154 | # ---------------------------------------------------------------------------------------------------------------------- 155 | @dataclass 156 | class NHCDConfig(DatasetConfig): 157 | """A configuration object read from a yaml conf""" 158 | # ------------------- 159 | # Required Parameters 160 | root_dir: str = MISSING 161 | dataset: str = "nhcd" 162 | 163 | # ------------------------------ 164 | # Parameters with default values 165 | lidar_height: int = 64 166 | lidar_width: int = 1024 167 | up_fov: int = 25 168 | down_fov: int = -25 169 | train_sequences: list = field(default_factory=lambda: ["02_long_experiment", "01_short_experiment"]) 170 | test_sequences: list = field(default_factory=lambda: ["01_short_experiment"]) 171 | eval_sequences: list = field(default_factory=lambda: ["01_short_experiment"]) 172 | 173 | 174 | # Hydra -- stores a NHCDConfig `nhcd` in the `dataset` group 175 | cs = ConfigStore.instance() 176 | cs.store(group="dataset", name="nhcd", node=NHCDConfig) 177 | 178 | 179 | # ---------------------------------------------------------------------------------------------------------------------- 180 | class NHCDDatasetLoader(DatasetLoader): 181 | """ 182 | Dataset Loader for NHCD's dataset 183 | see https://ori-drs.github.io/newer-college-dataset/ 184 | """ 185 | 186 | def __init__(self, config: NHCDConfig): 187 | super().__init__(config) 188 | self.root_dir = Path(config.root_dir) 189 | assert_debug(self.root_dir.exists()) 190 | 191 | def projector(self) -> SphericalProjector: 192 | """Default SphericalProjetor for NHCD (projection of a pointcloud into a Vertex Map)""" 193 | lidar_height = self.config.lidar_height 194 | lidar_with = self.config.lidar_width 195 | up_fov = self.config.up_fov 196 | down_fov = self.config.down_fov 197 | # Vertex map projector 198 | projector = SphericalProjector(lidar_height, lidar_with, 3, up_fov, down_fov) 199 | return projector 200 | 201 | def get_ground_truth(self, sequence_name): 202 | """Returns the ground truth poses""" 203 | assert_debug(sequence_name in ["01_short_experiment", "02_long_experiment"]) 204 | poses_file = self.root_dir / sequence_name / "ground_truth" / "registered_poses.csv" 205 | if not poses_file.exists(): 206 | return None 207 | poses, poses_timestamps = read_ground_truth(str(poses_file)) 208 | scans_dir = self.root_dir / sequence_name / "raw_format" / "ouster_scan" 209 | if not scans_dir.exists(): 210 | logging.log(logging.ERROR, 211 | f"The folder containing the ouster scan does not exist on disk at location {scans_dir}. " 212 | "Cannot read the ground truth") 213 | return None 214 | 215 | absolute_poses = pointcloud_poses(poses, poses_timestamps, sorted(os.listdir(str(scans_dir)))) 216 | absolute_poses = absolute_poses[:NHCDOdometrySequence.num_frames(sequence_name)] 217 | return compute_relative_poses(absolute_poses) 218 | 219 | def sequences(self): 220 | """ 221 | Returns 222 | ------- 223 | (train_dataset, eval_dataset, test_dataset, transform) : tuple 224 | train_dataset : (list, list) 225 | A list of dataset_config (one for each sequence of KITTI's Dataset), 226 | And the list of sequences used to build them 227 | eval_dataset : (list, list) 228 | idem 229 | test_dataset : (list, list) 230 | idem 231 | transform : callable 232 | A transform to be applied on the dataset_config 233 | """ 234 | 235 | gt_pose_channel = self.config.absolute_gt_key 236 | 237 | # Sets the path of the kitti benchmark 238 | train_sequence_ids = self.config.train_sequences 239 | eval_sequence_ids = self.config.eval_sequences 240 | test_sequence_ids = self.config.test_sequences 241 | 242 | def __get_datasets(sequences: list): 243 | if sequences is None or len(sequences) == 0: 244 | return None 245 | 246 | datasets = [NHCDOdometrySequence( 247 | str(self.root_dir), 248 | sequence_id, 249 | self.projector(), self.config.numpy_pc_key, gt_pose_channel, 250 | with_numpy_pc=self.config.with_numpy_pc) for sequence_id in sequences] 251 | return datasets 252 | 253 | train_datasets = __get_datasets(train_sequence_ids) 254 | eval_datasets = __get_datasets(eval_sequence_ids) 255 | test_datasets = __get_datasets(test_sequence_ids) 256 | 257 | return (train_datasets, train_sequence_ids), \ 258 | (eval_datasets, eval_sequence_ids), \ 259 | (test_datasets, test_sequence_ids), lambda x: x 260 | -------------------------------------------------------------------------------- /slam/dataset/rosbag_dataset.py: -------------------------------------------------------------------------------- 1 | import logging 2 | from dataclasses import MISSING 3 | from pathlib import Path 4 | from typing import Optional, Tuple 5 | import os 6 | 7 | from torch.utils.data import IterableDataset 8 | import numpy as np 9 | 10 | from hydra.conf import field, dataclass 11 | from hydra.core.config_store import ConfigStore 12 | from omegaconf import DictConfig, OmegaConf 13 | 14 | from slam.common.projection import SphericalProjector 15 | from slam.common.utils import assert_debug, remove_nan 16 | from slam.dataset import DatasetLoader, DatasetConfig 17 | 18 | try: 19 | import rosbag 20 | import sensor_msgs.point_cloud2 as pc2 21 | from sensor_msgs.msg import PointCloud2, PointField 22 | 23 | _with_rosbag = True 24 | except ImportError: 25 | _with_rosbag = False 26 | 27 | if _with_rosbag: 28 | 29 | @dataclass 30 | class RosbagConfig(DatasetConfig): 31 | """Config for a Rosbag Dataset""" 32 | dataset: str = "rosbag" 33 | file_path: str = field( 34 | default_factory=lambda: "" if not "ROSBAG_PATH" in os.environ else os.environ["ROSBAG_PATH"]) 35 | main_topic: str = "numpy_pc" # The Key of the main topic (which determines the number of frames) 36 | xyz_fields: str = "xyz" 37 | 38 | accumulate_scans: bool = False # Whether to accumulate the pointcloud messages (in case of raw sensor data) 39 | frame_size: int = 60 # The number of accumulated message which constitute a frame 40 | 41 | topic_mapping: dict = field(default_factory=lambda: {}) 42 | 43 | lidar_height: int = 720 44 | lidar_width: int = 720 45 | up_fov: float = 45. 46 | down_fov: float = -45. 47 | 48 | 49 | class RosbagDataset(IterableDataset): 50 | """A Dataset which wraps a RosBag 51 | 52 | Note: 53 | The dataset can only read data sequentially, and will raise an error when two calls are not consecutives 54 | 55 | Args: 56 | file_path (str): The path on disk to the rosbag 57 | main_topic (str): The name of the main topic (which sets the number of frames to be extracted) 58 | frame_size (int): The number of messages to accumulate in a frame 59 | topic_mapping (dict): The mapping topic name to key in the data_dict 60 | """ 61 | 62 | def _lazy_initialization(self, prefix: str = ""): 63 | if not self.initialized: 64 | logging.info(f"[RosbagDataset]{prefix}Loading ROSBAG {self.file_path}. May take some time") 65 | self.rosbag = rosbag.Bag(self.file_path, "r") 66 | logging.info(f"Done.") 67 | 68 | topic_info = self.rosbag.get_type_and_topic_info() 69 | for topic in self.topic_mapping: 70 | assert_debug(topic in topic_info.topics, 71 | f"{topic} is not a topic of the rosbag " 72 | f"(existing topics : {list(topic_info.topics.keys())}") 73 | self._len = self.rosbag.get_message_count(self.main_topic) // self._frame_size 74 | self.initialized = True 75 | 76 | def init(self): 77 | self._lazy_initialization() 78 | 79 | def __init__(self, config: RosbagConfig, file_path: str, main_topic: str, frame_size: int, 80 | topic_mapping: Optional[dict] = None): 81 | self.config = config 82 | self.rosbag = None 83 | self.initialized = False 84 | assert_debug(Path(file_path).exists(), f"The path to {file_path} does not exist.") 85 | self.file_path = file_path 86 | self.topic_mapping = topic_mapping if topic_mapping is not None else {} 87 | if main_topic not in self.topic_mapping: 88 | self.topic_mapping[main_topic] = "numpy_pc" 89 | self.main_topic: str = main_topic 90 | self.frame_size = frame_size 91 | self._frame_size: int = frame_size if self.config.accumulate_scans else 1 92 | self._len = -1 # 93 | self._idx = 0 94 | self._topics = list(topic_mapping.keys()) 95 | self.__iter = None 96 | 97 | def __iter__(self): 98 | self._lazy_initialization("[ITER]") 99 | self.__iter = self.rosbag.read_messages(self._topics) 100 | self._idx = 0 101 | return self 102 | 103 | @staticmethod 104 | def decode_pointcloud(msg: pc2.PointCloud2, timestamp, xyz_fieldname: str = "xyz") -> Tuple[ 105 | Optional[np.ndarray], Optional[np.ndarray]]: 106 | assert_debug("PointCloud2" in msg._type) 107 | pc = np.array(list(pc2.read_points(msg, field_names=xyz_fieldname))) 108 | timestamps = np.ones((pc.shape[0],), 109 | dtype=np.float64) * (float(timestamp.secs * 10e9) + timestamp.nsecs) 110 | return pc, timestamps 111 | 112 | def aggregate_messages(self, data_dict: dict): 113 | """Aggregates the point clouds of the main topic""" 114 | main_key = self.topic_mapping[self.main_topic] 115 | pcs = data_dict[main_key] 116 | data_dict[main_key] = np.concatenate(pcs, axis=0) 117 | timestamps_topic = f"{main_key}_timestamps" 118 | if timestamps_topic in data_dict: 119 | data_dict[timestamps_topic] = np.concatenate(data_dict[timestamps_topic], axis=0) 120 | return data_dict 121 | 122 | def _save_topic(self, data_dict, key, topic, msg, t, **kwargs): 123 | if "PointCloud2" in msg._type: 124 | data, timestamps = self.decode_pointcloud(msg, t) 125 | data_dict[key].append(data) 126 | timestamps_key = f"{key}_timestamps" 127 | if timestamps_key not in data_dict: 128 | data_dict[timestamps_key] = [] 129 | data_dict[timestamps_key].append(timestamps) 130 | 131 | def __getitem__(self, index) -> dict: 132 | self._lazy_initialization("[GETITEM]") 133 | assert_debug(index == self._idx, "A RosbagDataset does not support Random access") 134 | assert isinstance(self.config, RosbagConfig) 135 | if self.__iter is None: 136 | self.__iter__() 137 | 138 | data_dict = {key: [] for key in self.topic_mapping.values()} 139 | main_topic_key = self.topic_mapping[self.main_topic] 140 | 141 | # Append Messages until the main topic has the required number of messages 142 | while len(data_dict[main_topic_key]) < self._frame_size: 143 | topic, msg, t = next(self.__iter) 144 | _key = self.topic_mapping[topic] 145 | self._save_topic(data_dict, _key, topic, msg, t, frame_index=index) 146 | 147 | self._idx += 1 148 | # Aggregate data 149 | data_dict = self.aggregate_messages(data_dict) 150 | return data_dict 151 | 152 | def __next__(self): 153 | return self[self._idx] 154 | 155 | def __len__(self): 156 | self._lazy_initialization("[LEN]") 157 | return self._len 158 | 159 | def close(self): 160 | if self.initialized: 161 | if self.rosbag is not None: 162 | self.rosbag.close() 163 | del self.rosbag 164 | self.rosbag = None 165 | self.initialized = False 166 | self._len = -1 167 | self._idx = 0 168 | self.__iter = None 169 | 170 | def __del__(self): 171 | self.close() 172 | 173 | 174 | # Hydra -- stores a RosbagConfig `rosbag` in the `dataset` group 175 | cs = ConfigStore.instance() 176 | cs.store(group="dataset", name="rosbag", node=RosbagConfig) 177 | 178 | 179 | class RosbagDatasetConfiguration(DatasetLoader): 180 | """Returns the configuration of a Dataset built for ROS""" 181 | 182 | def __init__(self, config: RosbagConfig, **kwargs): 183 | if isinstance(config, DictConfig): 184 | config = RosbagConfig(**config) 185 | super().__init__(config) 186 | 187 | @classmethod 188 | def max_num_workers(cls): 189 | return 1 190 | 191 | def projector(self) -> SphericalProjector: 192 | return SphericalProjector(height=self.config.lidar_height, width=self.config.lidar_width, 193 | up_fov=self.config.up_fov, down_fov=self.config.down_fov) 194 | 195 | def sequences(self): 196 | assert isinstance(self.config, RosbagConfig) 197 | file_path = self.config.file_path 198 | dataset = RosbagDataset(self.config, file_path, self.config.main_topic, 199 | self.config.frame_size, 200 | OmegaConf.to_container(self.config.topic_mapping) if isinstance( 201 | self.config.topic_mapping, DictConfig) else self.config.topic_mapping) 202 | 203 | return ([dataset], [Path(file_path).stem]), None, None, lambda x: x 204 | 205 | def get_ground_truth(self, sequence_name): 206 | """No ground truth can be read from the ROSBAG""" 207 | return None 208 | -------------------------------------------------------------------------------- /slam/dataset/sequence_dataset.py: -------------------------------------------------------------------------------- 1 | from typing import Optional 2 | from torch.utils.data import Dataset 3 | import collections 4 | from torch.utils.data._utils.collate import default_collate 5 | import numpy as np 6 | 7 | 8 | class DatasetOfSequences(Dataset): 9 | """ 10 | A Dataset which concatenates data into for a small window of frames 11 | 12 | Takes a list of Datasets, each corresponding to a sequence. 13 | The dataset_config created returns a Dataset of windows. 14 | Each window takes consecutive frames of a given sequence and 15 | concatenates them 16 | 17 | Parameters 18 | sequence_len (int): The length of a window of frames 19 | dataset_list (list): The list of dataset_config 20 | sequence_ids (list): The list ids of the dataset_config list 21 | sequence_transforms (callable): A Transform to be applied 22 | """ 23 | 24 | def __init__(self, 25 | sequence_len: int, 26 | dataset_list: list, 27 | sequence_ids: list = None, 28 | transform: Optional[callable] = None, 29 | sequence_transforms: Optional[callable] = None, 30 | stride: int = 1): 31 | self.datasets: list = dataset_list 32 | self.dataset_sizes: list = [0] 33 | self.sequence_len: int = sequence_len 34 | self.transform: Optional[callable] = transform 35 | self.sequence_transforms: Optional[callable] = sequence_transforms 36 | self.stride = stride 37 | 38 | for i in range(len(dataset_list)): 39 | num_sequences_in_dataset = len(dataset_list[i]) - self.sequence_len * self.stride + 1 40 | self.dataset_sizes.append(self.dataset_sizes[i] + num_sequences_in_dataset) 41 | self.size = self.dataset_sizes[-1] 42 | self.sequence_ids = sequence_ids 43 | 44 | def find_dataset_with_idx(self, idx): 45 | assert idx < self.size, "INVALID ID" 46 | dataset_idx, sizes = list(x for x in enumerate(self.dataset_sizes) if x[1] <= idx)[-1] 47 | return self.datasets[dataset_idx], idx - sizes, self.sequence_ids[dataset_idx] 48 | 49 | def load_sequence(self, dataset, indices): 50 | sequence = [] 51 | for seq_index in indices: 52 | data_dict = dataset[seq_index] 53 | if self.transform is not None: 54 | data_dict = self.transform(data_dict) 55 | sequence.append(data_dict) 56 | return sequence 57 | 58 | def transform_sequence(self, elem): 59 | if self.sequence_transforms: 60 | elem = self.sequence_transforms(elem) 61 | return elem 62 | 63 | def __getitem__(self, idx): 64 | dataset, start_idx_in_dataset, seq_id = self.find_dataset_with_idx(idx) 65 | indices = [start_idx_in_dataset + i * self.stride for i in range(self.sequence_len)] 66 | 67 | sequence = self.load_sequence(dataset, indices) 68 | sequence_item = self.__sequence_collate(sequence) 69 | 70 | sequence_item = self.transform_sequence(sequence_item) 71 | return sequence_item 72 | 73 | def __len__(self): 74 | return self.size 75 | 76 | @staticmethod 77 | def __sequence_collate(batch): 78 | """ 79 | Agglomerate window data for a sequence 80 | 81 | Args: 82 | batch (List): A list of elements which are to be aggregated into a batch of elements 83 | """ 84 | elem = batch[0] 85 | if elem is None: 86 | return batch 87 | if isinstance(elem, collections.Mapping): 88 | result = dict() 89 | for key in elem: 90 | if "numpy" in key: 91 | for idx, d in enumerate(batch): 92 | result[f"{key}_{idx}"] = d[key] 93 | else: 94 | result[key] = DatasetOfSequences.__sequence_collate([d[key] for d in batch]) 95 | return result 96 | elif isinstance(elem, np.ndarray): 97 | return np.concatenate([np.expand_dims(e, axis=0) for e in batch], axis=0) 98 | else: 99 | return default_collate(batch) 100 | 101 | @staticmethod 102 | def _params_type() -> str: 103 | return DatasetOfSequences.__name__ 104 | -------------------------------------------------------------------------------- /slam/eval/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/slam/eval/__init__.py -------------------------------------------------------------------------------- /slam/models/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /slam/models/_resnet.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | import torchvision.models.resnet as models 3 | 4 | from slam.common.utils import assert_debug, check_tensor 5 | from slam.models.layers import ACTIVATIONS 6 | 7 | 8 | class CustomBasicBlock(models.BasicBlock): 9 | """ 10 | ResNet basic block where the ReLU activation is replaced by a Custom activation 11 | """ 12 | 13 | def __init__(self, inplanes, planes, stride=1, downsample=None, activation: str = "relu"): 14 | super().__init__(inplanes, planes, stride, downsample) 15 | self.relu = ACTIVATIONS.get(activation) 16 | 17 | def forward(self, x): 18 | return super().forward(x) 19 | 20 | 21 | class CustomBottleneck(models.Bottleneck): 22 | """ 23 | ResNet Bottleneck block where the ReLU activation is replaced by a Custom activation 24 | """ 25 | 26 | def __init__(self, inplanes, planes, stride=1, downsample=None, activation: str = "relu"): 27 | super().__init__(inplanes, planes, stride, downsample) 28 | self.relu = ACTIVATIONS.get(activation) 29 | 30 | def forward(self, x): 31 | return super().forward(x) 32 | 33 | 34 | def conv1x1(in_planes, out_planes, stride=1): 35 | """1x1 convolution with padding""" 36 | return nn.Conv2d(in_planes, out_planes, kernel_size=1, stride=stride, 37 | padding=0, bias=False) 38 | 39 | 40 | class _ResNetEncoder(nn.Module): 41 | """ 42 | Constructs a resnet encoder with varying number of input images. 43 | Adapted from https://github.com/pytorch/vision/blob/master/torchvision/models/resnet.py 44 | """ 45 | 46 | def __init__(self, num_input_channels: int, block, layers, zero_init_residual=False, activation: str = "relu"): 47 | super().__init__() 48 | self.layers = layers 49 | self.planes = [num_input_channels, 64, 64, 128, 256, 512] 50 | 51 | self.inplanes = 64 52 | self.dilation = 1 53 | self.num_input_channels = num_input_channels 54 | self.conv1 = nn.Conv2d(num_input_channels, self.inplanes, kernel_size=7, stride=2, padding=3, 55 | bias=False) 56 | self.relu = ACTIVATIONS.get(activation) 57 | self.maxpool = nn.MaxPool2d(kernel_size=3, stride=2, padding=1) 58 | self.layer1 = self._make_layer(block, 64, layers[0]) 59 | self.layer2 = self._make_layer(block, 128, layers[1], stride=2) 60 | self.layer3 = self._make_layer(block, 256, layers[2], stride=2) 61 | self.layer4 = self._make_layer(block, 512, layers[3], stride=2) 62 | 63 | for m in self.modules(): 64 | if isinstance(m, nn.Conv2d): 65 | nn.init.kaiming_normal_(m.weight, mode='fan_out', nonlinearity='relu') 66 | elif isinstance(m, (nn.BatchNorm2d, nn.GroupNorm)): 67 | nn.init.constant_(m.weight, 1) 68 | nn.init.constant_(m.bias, 0) 69 | 70 | # Zero-initialize the last BN in each residual branch, 71 | # so that the residual branch starts with zeros, and each residual block behaves like an identity. 72 | # This improves the model by 0.2~0.3% according to https://arxiv.org/abs/1706.02677 73 | if zero_init_residual: 74 | for m in self.modules(): 75 | if isinstance(m, CustomBottleneck): 76 | nn.init.constant_(m.bn3.weight, 0) 77 | elif isinstance(m, CustomBasicBlock): 78 | nn.init.constant_(m.bn2.weight, 0) 79 | 80 | def _make_layer(self, block, planes, blocks, stride=1, dilate=False): 81 | downsample = None 82 | previous_dilation = self.dilation 83 | if dilate: 84 | self.dilation *= stride 85 | stride = 1 86 | if stride != 1 or self.inplanes != planes * block.expansion: 87 | downsample = conv1x1(self.inplanes, planes * block.expansion, stride) 88 | 89 | layers = [] 90 | layers.append(block(self.inplanes, planes, stride, downsample)) 91 | self.inplanes = planes * block.expansion 92 | for _ in range(1, blocks): 93 | layers.append(block(self.inplanes, planes)) 94 | 95 | return nn.Sequential(*layers) 96 | 97 | def forward_layers(self, x): 98 | # See note [TorchScript super()] 99 | check_tensor(x, [-1, self.num_input_channels, -1, -1]) 100 | x = self.conv1(x) 101 | x0 = self.relu(x) 102 | 103 | x1 = self.layer1(self.maxpool(x0)) 104 | x2 = self.layer2(x1) 105 | x3 = self.layer3(x2) 106 | x4 = self.layer4(x3) 107 | 108 | return x0, x1, x2, x3, x4 109 | 110 | def forward(self, x): 111 | x0, x1, x2, x3, x4 = self.forward_layers(x) 112 | return x4 113 | 114 | 115 | class ResNetEncoder(_ResNetEncoder): 116 | """ 117 | A ResNet Encoder consist of the 4 layers of a ResNet which can be returned and decoded 118 | """ 119 | 120 | def __init__(self, num_input_channels, model: int = 18, activation: str = "relu", pretrained: bool = False): 121 | model_to_params = { 122 | 18: ([2, 2, 2, 2], CustomBasicBlock), 123 | 34: ([3, 4, 6, 3], CustomBasicBlock), 124 | 50: ([3, 4, 6, 3], CustomBottleneck) 125 | } 126 | assert_debug(model in model_to_params) 127 | 128 | super().__init__(num_input_channels, 129 | model_to_params[model][1], 130 | model_to_params[model][0], 131 | activation=activation) 132 | 133 | # Load pretrained model 134 | -------------------------------------------------------------------------------- /slam/models/layers.py: -------------------------------------------------------------------------------- 1 | from enum import Enum 2 | 3 | import torch 4 | import torch.nn as nn 5 | 6 | from slam.common.utils import assert_debug 7 | 8 | 9 | class ACTIVATIONS(Enum): 10 | relu = nn.ReLU() 11 | gelu = nn.GELU() 12 | sinus = torch.sin 13 | sigmoid = nn.Sigmoid() 14 | softplus = nn.Softplus() 15 | 16 | @staticmethod 17 | def get(activation: str): 18 | assert_debug(activation in ACTIVATIONS.__members__, f"activation {activation} not implemented") 19 | return ACTIVATIONS.__members__[activation].value 20 | 21 | 22 | class NORM_LAYERS(Enum): 23 | group = nn.GroupNorm 24 | batch2d = nn.BatchNorm2d 25 | instance2d = nn.InstanceNorm2d 26 | none = nn.Identity 27 | 28 | @staticmethod 29 | def get(norm_layer: str, num_groups: int = None, num_channels: int = None): 30 | assert_debug(norm_layer in NORM_LAYERS.__members__) 31 | pytorch_ctor = NORM_LAYERS.__members__[norm_layer].value 32 | if norm_layer == "group": 33 | assert_debug(num_groups is not None and num_channels is not None) 34 | return pytorch_ctor(num_channels=num_channels, num_groups=num_groups) 35 | elif norm_layer == "batch2d": 36 | assert_debug(num_channels is not None) 37 | return pytorch_ctor(num_channels) 38 | elif norm_layer == "instance2d": 39 | return pytorch_ctor(num_channels) 40 | elif norm_layer == "none": 41 | return pytorch_ctor() 42 | 43 | 44 | class Linear(nn.Module): 45 | def __init__(self, in_dim, out_dim, activation: str = "relu"): 46 | super().__init__() 47 | self.fc = nn.Linear(in_dim, out_dim) 48 | self.activation = ACTIVATIONS.get(activation) 49 | 50 | def forward(self, x): 51 | return self.activation(self.fc(x)) 52 | -------------------------------------------------------------------------------- /slam/models/posenet.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | from enum import Enum 4 | 5 | # Hydra and OmegaConf 6 | from omegaconf import DictConfig 7 | from hydra.conf import ConfigStore 8 | 9 | # Project Imports 10 | from ._resnet import ResNetEncoder 11 | from .layers import ACTIVATIONS 12 | from slam.common.pose import Pose 13 | from slam.common.utils import assert_debug, check_tensor 14 | 15 | 16 | # ---------------------------------------------------------------------------------------------------------------------- 17 | # POSERESNET 18 | # noinspection PyAbstractClass 19 | class PoseResNet(nn.Module): 20 | """ 21 | PoseResNet is a network regressing the 6 parameters of rigid transformation 22 | From a pair of images or a single image 23 | """ 24 | 25 | def __init__(self, 26 | config: DictConfig, 27 | pose: Pose = Pose("euler")): 28 | nn.Module.__init__(self) 29 | self.config = config 30 | self.num_input_channels = self.config.num_input_channels 31 | self.sequence_len = self.config.sequence_len 32 | self.num_out_poses = self.config.get("num_out_poses", 1) 33 | model = self.config.get("resnet_model", 18) 34 | 35 | self.resnet_encoder = ResNetEncoder(self.num_input_channels * self.sequence_len, 36 | model=model, 37 | activation=self.config.get("activation", "relu")) 38 | self.activation_str = self.config.get("regression_activation", "relu") 39 | self.activation = ACTIVATIONS.get(self.activation_str) 40 | self.avgpool = nn.AdaptiveAvgPool2d((1, 1)) 41 | self._last_resnet_layer = 512 42 | self.pose = pose 43 | 44 | self.fc_rot = nn.Linear(self._last_resnet_layer, pose.num_rot_params() * self.num_out_poses, bias=False) 45 | self.fc_trans = nn.Linear(self._last_resnet_layer, 3 * self.num_out_poses) 46 | 47 | # Initialize Scale to allow stable training 48 | torch.nn.init.xavier_uniform_(self.fc_rot.weight, 0.01) 49 | torch.nn.init.xavier_uniform_(self.fc_trans.weight, 0.01) 50 | 51 | def forward(self, frames) -> torch.Tensor: 52 | if isinstance(frames, list): 53 | assert_debug(len(frames) == 1, "Multiple input not supported for current PoseNet version") 54 | frames = frames[0] 55 | check_tensor(frames, [-1, self.sequence_len, self.num_input_channels, -1, -1]) 56 | features = self.resnet_encoder(frames.reshape(-1, self.num_input_channels * self.sequence_len, 57 | frames.size(3), 58 | frames.size(4))) 59 | x = self.avgpool(features).flatten(1) 60 | 61 | rot_params = 0.1 * self.fc_rot(x) # scaling which allows stable training 62 | trans_params = self.fc_trans(x) 63 | 64 | pose_params = torch.cat([trans_params, rot_params], dim=-1) 65 | pose_params = pose_params.reshape(-1, self.num_out_poses, self.pose.num_params()) 66 | return pose_params 67 | 68 | 69 | # ---------------------------------------------------------------------------------------------------------------------- 70 | class POSENET(Enum): 71 | poseresnet = PoseResNet 72 | 73 | @staticmethod 74 | def load(config: DictConfig, pose: Pose = Pose("euler")): 75 | assert_debug("type" in config) 76 | assert_debug(config.type in POSENET.__members__) 77 | 78 | return POSENET.__members__[config.type].value(config, pose=pose) 79 | -------------------------------------------------------------------------------- /slam/odometry/__init__.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | from enum import Enum 3 | 4 | # Hydra and OmegaConf 5 | from omegaconf import OmegaConf, MISSING, DictConfig 6 | 7 | # Project Import 8 | from .icp_odometry import ICPFrameToModel, ICPFrameToModelConfig, assert_debug 9 | from .posenet_odometry import PoseNetOdometry, PoseNetOdometryConfig 10 | from .odometry import OdometryConfig 11 | from slam.common.utils import ObjectLoaderEnum 12 | 13 | # ---------------------------------------------------------------------------------------------------------------------- 14 | from .ct_icp_odometry import _with_ct_icp 15 | from slam.common.utils import ObjectLoaderEnum 16 | 17 | if _with_ct_icp: 18 | from .ct_icp_odometry import CT_ICPOdometryConfig, CT_ICPOdometry 19 | 20 | 21 | # ---------------------------------------------------------------------------------------------------------------------- 22 | 23 | class ODOMETRY(ObjectLoaderEnum, Enum): 24 | """A Convenient Enum which allows to load the proper Odometry Algorithm""" 25 | icp_F2M = (ICPFrameToModel, ICPFrameToModelConfig) 26 | posenet = (PoseNetOdometry, PoseNetOdometryConfig) 27 | if _with_ct_icp: 28 | ct_icp = (CT_ICPOdometry, CT_ICPOdometryConfig) 29 | 30 | @classmethod 31 | def type_name(cls): 32 | return "algorithm" 33 | -------------------------------------------------------------------------------- /slam/odometry/alignment.py: -------------------------------------------------------------------------------- 1 | # TODO: Make the alignment compatible for both pytorch and numpy backends 2 | from abc import ABC 3 | from enum import Enum 4 | from typing import Dict, Optional, Any 5 | 6 | import torch 7 | 8 | # Hydra and OmegaConf 9 | from dataclasses import field 10 | from hydra.conf import dataclass 11 | 12 | # Project Imports 13 | from hydra.core.config_store import ConfigStore 14 | 15 | from slam.common.optimization import GaussNewton, PointToPlaneCost, PointToPointCost 16 | from slam.common.pose import Pose 17 | 18 | # ---------------------------------------------------------------------------------------------------------------------- 19 | from slam.common.registration import weighted_procrustes 20 | from slam.common.utils import assert_debug, ObjectLoaderEnum 21 | 22 | 23 | @dataclass 24 | class RigidAlignmentConfig: 25 | """Configuration for the alignments used by ICP methods""" 26 | mode: str = "???" 27 | 28 | pose: str = "euler" # The pose representation 29 | scheme: str = "huber" # The weighting scheme for robust alignment 30 | 31 | 32 | class RigidAlignment(ABC): 33 | """ 34 | Abstract class for rigid 3D alignment between Point Clouds 35 | 36 | A Rigid alignment is the estimation of the rigid transformation between a set of 3D correspondences 37 | """ 38 | 39 | def __init__(self, alignment_config: RigidAlignmentConfig, **kwargs): 40 | self.config = alignment_config 41 | self.pose: Pose = Pose(self.config.pose) 42 | self.point_to_plane: PointToPlaneCost = PointToPlaneCost(pose=self.pose) 43 | 44 | def align(self, 45 | ref_points: torch.Tensor, 46 | tgt_points: torch.Tensor, *args, **kwargs) -> [torch.Tensor, 47 | torch.Tensor]: 48 | """ 49 | Aligns corresponding pair of 3D points 50 | 51 | Computes the optimal rigid transform between 52 | reference points (`ref_points`) and target points (`tgt_points`). 53 | 54 | Args: 55 | 56 | ref_points (torch.Tensor): Reference points `(B, N, 3)` 57 | tgt_points (torch.Tensor): Target points `(B, N, 3)` 58 | *args: Other arguments for child classes 59 | **kwargs: Other named arguments required by child classes 60 | 61 | Returns: 62 | A tuple consisting of the pose matrix of the estimated transform, 63 | And the residual errors between the target and the reference 64 | """ 65 | raise NotImplementedError("") 66 | 67 | 68 | # ---------------------------------------------------------------------------------------------------------------------- 69 | @dataclass 70 | class GaussNewtonPointToPlaneConfig(RigidAlignmentConfig): 71 | """Configuration for a Gauss-Newton based Point-to-Plane rigid alignment""" 72 | 73 | mode: str = "point_to_plane_gauss_newton" 74 | num_gn_iters: int = 1 75 | 76 | # The configuration for the Gauss-Newton algorithm 77 | gauss_newton_config: Dict[str, Any] = field(default_factory=lambda: dict(max_iters=1)) 78 | 79 | 80 | class GaussNewtonPointToPlaneAlignment(RigidAlignment): 81 | """ 82 | A GaussNewton Point-To-Plane rigid alignment method, 83 | Which minimizes the Point-To-Plane distance 84 | """ 85 | 86 | def __init__(self, config: GaussNewtonPointToPlaneConfig, **kwargs): 87 | super().__init__(config, **kwargs) 88 | 89 | self.gauss_newton: GaussNewton = GaussNewton(**self.config.gauss_newton_config) 90 | 91 | def align(self, 92 | ref_points: torch.Tensor, 93 | tgt_points: torch.Tensor, 94 | ref_normals: torch.Tensor = None, 95 | initial_estimate: Optional[torch.Tensor] = None, 96 | mask: Optional[torch.Tensor] = None, **kwargs) -> [torch.Tensor, 97 | torch.Tensor]: 98 | """ 99 | Aligns the target Point Cloud on the reference Point Cloud 100 | 101 | Args: 102 | ref_points (torch.Tensor): The reference points tensor `(B, N, 3)` 103 | tgt_points (torch.Tensor): The target points tensor `(B, N, 3)` 104 | ref_normals (torch.Tensor): The reference normals tensor `(B, N, 3)` 105 | initial_estimate (torch.Tensor): An initial transform `(B, D)` 106 | Where `D` is the number of parameters of the pose representation 107 | mask (torch.Tensor): An optional mask applied on the input points to remove some from the computation 108 | """ 109 | assert_debug(ref_normals is not None, "The argument 'ref_normals' is required for a point to plane alignemnt") 110 | if initial_estimate is None: 111 | initial_estimate = torch.zeros(ref_points.shape[0], self.pose.num_params(), 112 | dtype=ref_points.dtype, 113 | device=ref_points.device) 114 | elif len(initial_estimate.shape) == 3: 115 | initial_estimate = self.pose.from_pose_matrix(initial_estimate) 116 | 117 | res_func = PointToPlaneCost.get_residual_fun(tgt_points, ref_points, 118 | ref_normals, pose=self.pose, mask=mask, 119 | **kwargs) 120 | jac_func = PointToPlaneCost.get_residual_jac_fun(tgt_points, ref_points, 121 | ref_normals, pose=self.pose, mask=mask, **kwargs) 122 | new_pose_params, residuals = self.gauss_newton.compute(initial_estimate, res_func, jac_func, 123 | # Pass the pointclouds for some weighting schemes 124 | target_points=tgt_points, reference_points=ref_points, 125 | **kwargs) 126 | 127 | return self.pose.build_pose_matrix(new_pose_params), new_pose_params, residuals 128 | 129 | 130 | # ---------------------------------------------------------------------------------------------------------------------- 131 | @dataclass 132 | class GNPointToPointConfig(RigidAlignmentConfig): 133 | """Configuration for a Gauss Newton based Point-to-Point rigid alignment""" 134 | 135 | mode: str = "point_to_point_gn" 136 | num_gn_iters: int = 1 137 | initialize_with_svd: bool = False 138 | 139 | # The configuration for the Gauss-Newton algorithm 140 | gauss_newton_config: Dict[str, Any] = field(default_factory=lambda: dict(max_iters=1)) 141 | 142 | 143 | class GaussNewtonPointToPointAlignment(RigidAlignment): 144 | """ 145 | A GaussNewton Point-To-Point rigid alignment method, 146 | Which minimizes the Point-To-Point distance 147 | """ 148 | 149 | def __init__(self, config: GNPointToPointConfig, **kwargs): 150 | super().__init__(config, **kwargs) 151 | 152 | self.gauss_newton: GaussNewton = GaussNewton(**self.config.gauss_newton_config) 153 | 154 | def align(self, 155 | ref_points: torch.Tensor, 156 | tgt_points: torch.Tensor, 157 | initial_estimate: Optional[torch.Tensor] = None, 158 | mask: Optional[torch.Tensor] = None, **kwargs) -> [torch.Tensor, 159 | torch.Tensor]: 160 | """ 161 | Aligns the target Point Cloud on the reference Point Cloud 162 | 163 | Args: 164 | ref_points (torch.Tensor): The reference points tensor `(B, N, 3)` 165 | tgt_points (torch.Tensor): The target points tensor `(B, N, 3)` 166 | initial_estimate (torch.Tensor): An initial transform `(B, D)` 167 | Where `D` is the number of parameters of the pose representation 168 | mask (torch.Tensor): An optional mask applied on the input points to remove some from the computation 169 | """ 170 | if self.config.initialize_with_svd: 171 | initial_estimate = weighted_procrustes(ref_points, tgt_points).to(dtype=ref_points.dtype) 172 | 173 | if initial_estimate is None: 174 | initial_estimate = torch.zeros(ref_points.shape[0], self.pose.num_params(), 175 | dtype=ref_points.dtype, 176 | device=ref_points.device) 177 | elif len(initial_estimate.shape) == 3: 178 | initial_estimate = self.pose.from_pose_matrix(initial_estimate) 179 | 180 | res_func = PointToPointCost.get_residual_fun(tgt_points, ref_points, pose=self.pose, mask=mask, 181 | **kwargs) 182 | jac_func = PointToPointCost.get_residual_jac_fun(tgt_points, ref_points, pose=self.pose, mask=mask, **kwargs) 183 | new_pose_params, residuals = self.gauss_newton.compute(initial_estimate, res_func, jac_func, 184 | # Pass the pointclouds for some weighting schemes 185 | target_points=tgt_points, reference_points=ref_points, 186 | **kwargs) 187 | new_pose_matrix = self.pose.build_pose_matrix(new_pose_params) 188 | 189 | return new_pose_matrix, new_pose_params, residuals 190 | 191 | 192 | # ---------------------------------------------------------------------------------------------------------------------- 193 | # Hydra Group odometry/local_map definition 194 | cs = ConfigStore.instance() 195 | cs.store(group="slam/odometry/alignment", name="point_to_plane_GN", node=GaussNewtonPointToPlaneConfig) 196 | cs.store(group="slam/odometry/alignment", name="point_to_point_GN", node=GNPointToPointConfig) 197 | 198 | 199 | # ---------------------------------------------------------------------------------------------------------------------- 200 | class RIGID_ALIGNMENT(ObjectLoaderEnum, Enum): 201 | """A Convenient Enum to load the""" 202 | 203 | point_to_plane_gauss_newton = (GaussNewtonPointToPlaneAlignment, GaussNewtonPointToPlaneConfig) 204 | point_to_point_gauss_newton = (GaussNewtonPointToPointAlignment, GNPointToPointConfig) 205 | 206 | @classmethod 207 | def type_name(cls): 208 | return "mode" 209 | -------------------------------------------------------------------------------- /slam/odometry/odometry.py: -------------------------------------------------------------------------------- 1 | import time 2 | from abc import abstractmethod, ABC 3 | 4 | import numpy as np 5 | 6 | # Hydra and OmegaConf 7 | from hydra.conf import dataclass, MISSING 8 | 9 | # Project Imports 10 | from slam.odometry.local_map import LocalMapConfig 11 | 12 | 13 | # ---------------------------------------------------------------------------------------------------------------------- 14 | @dataclass 15 | class OdometryConfig: 16 | """Abstract class which should be extended as config by childs of OdometryAlgorithm""" 17 | algorithm: str = MISSING 18 | 19 | 20 | # ---------------------------------------------------------------------------------------------------------------------- 21 | class OdometryAlgorithm(ABC): 22 | """Abstract class which acts as an interface for a Slam algorithm implemented in pytorch""" 23 | 24 | def __init__(self, config: OdometryConfig, **kwargs): 25 | self.config = config 26 | 27 | # Keeps track of the elapsed time between the processing of each frame 28 | self.elapsed: list = [] 29 | 30 | @abstractmethod 31 | def init(self): 32 | """ 33 | An initialization procedure called at the start of each sequence 34 | """ 35 | self.elapsed = [] 36 | 37 | def process_next_frame(self, data_dict: dict): 38 | """ 39 | Computes the new pose and stores it in memory 40 | 41 | Args: 42 | data_dict (dict): The new frame (consisting of a dictionary of data items) returned by the Dataset 43 | """ 44 | beginning = time.time() 45 | self.do_process_next_frame(data_dict) 46 | self.elapsed.append(time.time() - beginning) 47 | 48 | @abstractmethod 49 | def do_process_next_frame(self, data_dict: dict): 50 | """ 51 | Computes the new pose and stores it in memory 52 | 53 | Args: 54 | data_dict (dict): The new frame's data as a dictionary returned by the Dataset 55 | """ 56 | raise NotImplementedError("") 57 | 58 | def get_relative_poses(self) -> np.ndarray: 59 | """ 60 | Returns the poses of the sequence of frames started after init() was called 61 | """ 62 | raise NotImplementedError("") 63 | 64 | def get_elapsed(self) -> float: 65 | """ 66 | Returns the total elapsed time in calling process_next_frame 67 | """ 68 | return sum(self.elapsed) 69 | 70 | @staticmethod 71 | def pointcloud_key() -> str: 72 | return "odometry_pc" 73 | 74 | @staticmethod 75 | def relative_pose_key() -> str: 76 | """The key of the expected relative pose filled by the odometry at each frame 77 | 78 | The SLAM algorithm expects a [4, 4] numpy pose transform array under this key 79 | In the data_dict after do_process_next_frame is called 80 | """ 81 | return "odometry_pose" 82 | -------------------------------------------------------------------------------- /slam/odometry/odometry_runner.py: -------------------------------------------------------------------------------- 1 | import dataclasses 2 | import logging 3 | from pathlib import Path 4 | from typing import Optional 5 | import time 6 | 7 | import os 8 | import torch 9 | 10 | from abc import ABC 11 | import numpy as np 12 | from omegaconf import OmegaConf 13 | from torch.utils.data import DataLoader 14 | from tqdm import tqdm 15 | import shutil 16 | 17 | # Hydra and OmegaConf imports 18 | from hydra.core.config_store import ConfigStore 19 | from hydra.conf import dataclass, MISSING, field 20 | 21 | # Project Imports 22 | from slam.common.pose import Pose 23 | from slam.common.torch_utils import collate_fun 24 | from slam.common.utils import check_tensor, assert_debug, get_git_hash 25 | from slam.dataset import DatasetLoader, DATASET 26 | from slam.eval.eval_odometry import OdometryResults 27 | from slam.dataset.configuration import DatasetConfig 28 | 29 | from slam.slam import SLAMConfig, SLAM 30 | 31 | from slam.viz import _with_cv2 32 | 33 | if _with_cv2: 34 | import cv2 35 | 36 | 37 | @dataclass 38 | class SLAMRunnerConfig: 39 | """The configuration dataclass""" 40 | 41 | # -------------------------------- 42 | # SLAMConfig 43 | slam: SLAMConfig = MISSING 44 | dataset: DatasetConfig = MISSING 45 | 46 | # ------------------ 47 | # Default parameters 48 | max_num_frames: int = -1 # The maximum number of frames to run on 49 | log_dir: str = field(default_factory=os.getcwd) 50 | num_workers: int = 2 51 | pin_memory: bool = True 52 | device: str = "cuda" if torch.cuda.is_available() else "cpu" 53 | pose: str = "euler" 54 | 55 | fail_dir: str = field(default_factory=os.getcwd) # By default the fail_dir is the same directory 56 | move_if_fail: bool = False 57 | 58 | # ---------------- 59 | # Debug parameters 60 | viz_num_pointclouds: int = 200 61 | debug: bool = True 62 | save_results: bool = True 63 | 64 | 65 | # ------------- 66 | # HYDRA Feature 67 | # Automatically casts the config as a SLAMConfig object, and raises errors if it cannot do so 68 | cs = ConfigStore.instance() 69 | cs.store(name="slam_config", node=SLAMRunnerConfig) 70 | 71 | 72 | class SLAMRunner(ABC): 73 | """ 74 | A SLAMRunner runs a LiDAR SLAM algorithm on a set of pytorch datasets, 75 | And if the ground truth is present, it evaluates the performance of the algorithm and saved the results to disk 76 | """ 77 | 78 | def __init__(self, config: SLAMRunnerConfig): 79 | super().__init__() 80 | 81 | self.config: SLAMRunnerConfig = config 82 | 83 | # Pytorch parameters extracted 84 | self.num_workers = self.config.num_workers 85 | self.batch_size = 1 86 | self.log_dir = self.config.log_dir 87 | self.device = torch.device(self.config.device) 88 | self.pin_memory = self.config.pin_memory if self.device != torch.device("cpu") else False 89 | 90 | self.pose = Pose(self.config.pose) 91 | self.viz_num_pointclouds = self.config.viz_num_pointclouds 92 | 93 | # Dataset config 94 | dataset_config: DatasetConfig = self.config.dataset 95 | self.dataset_loader: DatasetLoader = DATASET.load(dataset_config) 96 | 97 | self.slam_config: SLAMConfig = self.config.slam 98 | 99 | def save_config(self): 100 | """Saves the config to Disk""" 101 | with open(str(Path(self.log_dir) / "config.yaml"), "w") as config_file: 102 | # Add the git hash to improve tracking of modifications 103 | config_dict = self.config.__dict__ 104 | 105 | git_hash = get_git_hash() 106 | if git_hash is not None: 107 | config_dict["git_hash"] = git_hash 108 | config_dict["_working_dir"] = os.getcwd() 109 | config_file.write(OmegaConf.to_yaml(config_dict)) 110 | 111 | def handle_failure(self): 112 | """Handles Failure cases of the SLAM runner""" 113 | # In case of failure move the current working directory and its content to another directory 114 | if self.config.move_if_fail: 115 | try: 116 | fail_dir = Path(self.config.fail_dir) 117 | assert_debug(fail_dir.exists(), 118 | f"[SLAM] -- The `failure` directory {str(fail_dir)} does not exist on disk") 119 | current_dir: Path = Path(os.getcwd()) 120 | 121 | if fail_dir.absolute() == current_dir.absolute(): 122 | logging.warning( 123 | "The `fail_dir` variable points to the current working directory. It will not be moved.") 124 | return 125 | 126 | destination_dir = fail_dir 127 | if not destination_dir.exists(): 128 | destination_dir.mkdir() 129 | 130 | shutil.move(str(current_dir), str(destination_dir)) 131 | assert_debug(not current_dir.exists(), "Could not move current working directory") 132 | except (Exception, AssertionError, KeyboardInterrupt): 133 | logging.warning("[PyLIDAR-SLAM] Could not move the directory") 134 | 135 | def run_odometry(self): 136 | """Runs the LiDAR Odometry algorithm on the different datasets""" 137 | try: 138 | # Load the Datasets 139 | datasets: list = self.load_datasets() 140 | 141 | except (KeyboardInterrupt, Exception) as e: 142 | self.handle_failure() 143 | raise 144 | 145 | for sequence_name, dataset in datasets: 146 | # Build dataloader 147 | dataloader = DataLoader(dataset, 148 | collate_fn=collate_fun, 149 | pin_memory=self.pin_memory, 150 | batch_size=self.batch_size, 151 | num_workers=self.num_workers) 152 | 153 | # Load/Init the SLAM 154 | slam = self.load_slam_algorithm() 155 | if self.config.save_results: 156 | self.save_config() 157 | slam.init() 158 | 159 | elapsed = 0.0 160 | relative_ground_truth = self.ground_truth(sequence_name) 161 | 162 | def catch_exception(): 163 | _relative_poses = slam.get_relative_poses() 164 | if _relative_poses is not None and len(_relative_poses) > 0: 165 | self.save_and_evaluate(sequence_name, _relative_poses, None) 166 | print("[ERRROR] running SLAM : the estimated trajectory was dumped") 167 | self.handle_failure() 168 | 169 | try: 170 | for b_idx, data_dict in self._progress_bar(dataloader, desc=f"Sequence {sequence_name}"): 171 | data_dict = self._send_to_device(data_dict) 172 | start = time.time() 173 | 174 | # Process next frame 175 | slam.process_next_frame(data_dict) 176 | 177 | # Measure the time spent on the processing of the next frame 178 | elapsed_sec = time.time() - start 179 | elapsed += elapsed_sec 180 | 181 | if 0 < self.config.max_num_frames <= b_idx: 182 | break 183 | except KeyboardInterrupt: 184 | catch_exception() 185 | raise 186 | except (KeyboardInterrupt, Exception, RuntimeError, AssertionError) as e: 187 | catch_exception() 188 | raise e 189 | 190 | if self.config.save_results: 191 | # Dump trajectory constraints in case of loop closure 192 | slam.dump_all_constraints(str(Path(self.log_dir) / sequence_name)) 193 | 194 | # Evaluate the SLAM if it has a ground truth 195 | relative_poses = slam.get_relative_poses() 196 | check_tensor(relative_poses, [-1, 4, 4]) 197 | if relative_ground_truth is not None: 198 | check_tensor(relative_ground_truth, [relative_poses.shape[0], 4, 4]) 199 | 200 | del slam 201 | del dataloader 202 | 203 | if self.config.save_results: 204 | self.save_and_evaluate(sequence_name, relative_poses, relative_ground_truth, elapsed=elapsed) 205 | 206 | def save_and_evaluate(self, sequence_name: str, 207 | trajectory: np.ndarray, 208 | ground_truth: Optional[np.ndarray], 209 | elapsed: Optional[float] = None): 210 | """Saves metrics and trajectory in a folder on disk""" 211 | 212 | odo_results = OdometryResults(str(Path(self.log_dir) / sequence_name)) 213 | odo_results.add_sequence(sequence_name, 214 | trajectory, 215 | ground_truth, 216 | elapsed) 217 | odo_results.close() 218 | 219 | @staticmethod 220 | def _progress_bar(dataloader: DataLoader, desc: str = ""): 221 | return tqdm(enumerate(dataloader, 0), 222 | desc=desc, 223 | total=len(dataloader), 224 | ncols=120, ascii=True) 225 | 226 | def _send_to_device(self, data_dict: dict): 227 | output_dict: dict = {} 228 | for key, item in data_dict.items(): 229 | if isinstance(item, torch.Tensor): 230 | output_dict[key] = item.to(device=self.device) 231 | else: 232 | output_dict[key] = item 233 | return output_dict 234 | 235 | def load_datasets(self) -> list: 236 | """ 237 | Loads the Datasets for which the odometry is evaluated 238 | 239 | Returns 240 | ------- 241 | A list of pairs (sequence_name :str, dataset_config :Dataset) 242 | Where : 243 | sequence_name is the name of a sequence which will be constructed 244 | """ 245 | self.num_workers = min(self.dataset_loader.max_num_workers(), self.num_workers) 246 | train_dataset, _, _, _ = self.dataset_loader.sequences() 247 | assert_debug(train_dataset is not None) 248 | pairs = [(train_dataset[1][idx], train_dataset[0][idx]) for idx in range(len(train_dataset[0]))] 249 | return pairs 250 | 251 | def load_slam_algorithm(self) -> SLAM: 252 | """ 253 | Returns the SLAM algorithm which will be run 254 | """ 255 | slam = SLAM(self.config.slam, 256 | projector=self.dataset_loader.projector(), 257 | pose=self.pose, 258 | device=self.device, 259 | viz_num_pointclouds=self.viz_num_pointclouds) 260 | return slam 261 | 262 | def ground_truth(self, sequence_name: str) -> Optional[np.ndarray]: 263 | """ 264 | Returns the ground truth associated with the sequence 265 | """ 266 | return self.dataset_loader.get_ground_truth(sequence_name) 267 | -------------------------------------------------------------------------------- /slam/odometry/posenet_odometry.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | from typing import Dict, Union, Any 3 | 4 | import numpy as np 5 | 6 | # Hydra and OmegaConf 7 | from hydra.core.config_store import ConfigStore 8 | from omegaconf import MISSING, DictConfig 9 | from hydra.conf import dataclass 10 | 11 | # Project Imports 12 | from slam.common.pose import Pose 13 | from slam.common.timer import * 14 | from slam.odometry import * 15 | from slam.odometry.odometry import OdometryAlgorithm, OdometryConfig 16 | from slam.training.prediction_modules import _PoseNetPredictionModule 17 | 18 | 19 | # ---------------------------------------------------------------------------------------------------------------------- 20 | @dataclass 21 | class PoseNetOdometryConfig(OdometryConfig): 22 | """ 23 | The Configuration for the Point-To-Plane ICP based Iterative Least Square estimation of the pose 24 | """ 25 | debug: bool = False 26 | viz_mode: str = "aggregated" 27 | algorithm: str = "posenet" 28 | 29 | train_dir: str = MISSING # The directory where the posenet_config and checkpoint file should be searched in 30 | train_config_file: str = "config.yaml" # Default value set by ATrainer 31 | checkpoint_file: str = "checkpoint.ckp" # Default value set by ATrainer 32 | 33 | device: str = MISSING 34 | pose: str = MISSING 35 | posenet_config: Dict[str, Any] = MISSING 36 | 37 | 38 | # Hydra -- Add a PoseNetOdometryCfonig 39 | cs = ConfigStore.instance() 40 | cs.store(name="poseresnet18", node=PoseNetOdometryConfig(posenet_config={"type": "poseresnet", 41 | "model": 18}), 42 | package="odometry.posenet_config") 43 | 44 | 45 | # ---------------------------------------------------------------------------------------------------------------------- 46 | class PoseNetOdometry(OdometryAlgorithm): 47 | """Deep Odometry""" 48 | 49 | def __init__(self, config: Union[PoseNetOdometryConfig, DictConfig], 50 | pose: Pose = Pose("euler"), 51 | device: torch.device = torch.device("cpu"), 52 | **kwargs): 53 | OdometryAlgorithm.__init__(self, config) 54 | 55 | # Set variables needed by the module 56 | self.device = device 57 | self.pose = pose 58 | 59 | # Loads the train config from the disk 60 | train_dir = Path(config.train_dir) 61 | assert_debug(train_dir.exists()) 62 | train_config_path = train_dir / config.train_config_file 63 | checkpoint_path = train_dir / config.checkpoint_file 64 | assert_debug(train_config_path.exists() and checkpoint_path.exists()) 65 | self.checkpoint_path = str(checkpoint_path) 66 | 67 | # Reads the prediction config from the dict 68 | with open(str(train_config_path), "r") as stream: 69 | train_config = OmegaConf.load(stream) 70 | prediction_config: DictConfig = train_config["training"]["prediction"] 71 | 72 | # Construct the Prediction module from the config read from disk 73 | self.prediction_module = _PoseNetPredictionModule(prediction_config, 74 | pose=self.pose) 75 | self.prediction_module = self.prediction_module.to(self.device) 76 | 77 | # ---------------------- 78 | # Local variable 79 | self.previous_vertex_map = None 80 | self._iter = 0 81 | self.relative_poses = [] 82 | 83 | def init(self): 84 | """ 85 | Initializes the Odometry algorithm 86 | 87 | Clears the persisted relative poses, reset the _iter to 0 88 | And loads the module parameters from disk 89 | """ 90 | super().init() 91 | self.relative_poses = [] 92 | self._iter = 0 93 | 94 | # Load the parameters of the model from the config 95 | state_dict = torch.load(self.checkpoint_path) 96 | self.prediction_module.load_state_dict(state_dict["prediction_module"]) 97 | 98 | def do_process_next_frame(self, data_dict: dict): 99 | """ 100 | Registers the new frame 101 | """ 102 | vertex_map = data_dict["vertex_map"] 103 | if self._iter == 0: 104 | self.previous_vertex_map = vertex_map.unsqueeze(0) 105 | self._iter += 1 106 | self.relative_poses.append(np.eye(4, dtype=np.float32).reshape(1, 4, 4)) 107 | return 108 | 109 | pair_vmap = torch.cat([self.previous_vertex_map, vertex_map.unsqueeze(0)], dim=1) 110 | 111 | with torch.no_grad(): 112 | output_dict = self.prediction_module(dict(vertex_map=pair_vmap)) 113 | pose_params = output_dict["pose_params"] 114 | new_rpose = self.pose.build_pose_matrix(pose_params) 115 | 116 | # Update the state of the odometry 117 | self.previous_vertex_map = vertex_map.unsqueeze(0) 118 | self.relative_poses.append(new_rpose.cpu().numpy()) 119 | self._iter += 1 120 | 121 | def get_relative_poses(self) -> np.ndarray: 122 | return np.concatenate(self.relative_poses, axis=0) 123 | -------------------------------------------------------------------------------- /slam/slam.py: -------------------------------------------------------------------------------- 1 | from abc import abstractmethod 2 | import time 3 | from pathlib import Path 4 | from typing import Optional 5 | import logging 6 | 7 | from scipy.spatial.transform import Rotation 8 | import numpy as np 9 | 10 | # Hydra and omegaconf 11 | from hydra.conf import dataclass 12 | 13 | # Project Imports 14 | from slam.backend import Backend, BackendConfig, BACKEND 15 | from slam.common.utils import assert_debug 16 | from slam.eval.eval_odometry import compute_absolute_poses 17 | from slam.initialization import Initialization, InitializationConfig, INITIALIZATION 18 | from slam.loop_closure import LoopClosure, LoopClosureConfig, LOOP_CLOSURE 19 | from slam.odometry import ODOMETRY 20 | from slam.odometry.odometry import OdometryAlgorithm, OdometryConfig 21 | from slam.preprocessing import Preprocessing, PreprocessingConfig 22 | 23 | 24 | # ---------------------------------------------------------------------------------------------------------------------- 25 | 26 | 27 | @dataclass 28 | class SLAMConfig: 29 | initialization: Optional[InitializationConfig] = None 30 | preprocessing: Optional[PreprocessingConfig] = None 31 | odometry: Optional[OdometryConfig] = None 32 | loop_closure: Optional[LoopClosureConfig] = None 33 | backend: Optional[BackendConfig] = None 34 | 35 | 36 | class SLAM: 37 | """A SLAM Algorithm for Point Cloud data (typically LiDAR Data) 38 | 39 | A SLAM of pyLIDAR-SLAM consists of four modules 40 | 41 | - Motion Initialization An Optional Initialization module, which predicts an initial estimate of 42 | The motion before Registration of a new frame 43 | 44 | - Preprocessing An Optional Preprocessing module which modifies the data_dict 45 | 46 | - Odometry: The Scan Matching algorithm which iteratively estimate the trajectory 47 | And produces frame-to-frame trajectory constraints i -> (i+1) 48 | Required 49 | 50 | - Post Processing An Optional module which modifies the contents of the data_dict : dict after the 51 | Scan matching 52 | 53 | - Loop Closure: A Loop Closure module constructs constraints between 54 | Distant poses in the trajectory i -> j (such that i < j) 55 | (Optional) 56 | 57 | - Backend: The Backend estimate an optimal trajectory given the different constraints 58 | (Optional) 59 | """ 60 | 61 | def __init__(self, config: SLAMConfig, **kwargs): 62 | 63 | self.config = config 64 | 65 | # TODO -- Separate Processes for loop_closure and backend 66 | self.initialization: Optional[Initialization] = None 67 | self.preprocessing: Optional[Preprocessing] = None 68 | self.odometry: Optional[OdometryAlgorithm] = None 69 | self.loop_closure: Optional[LoopClosure] = None 70 | self.backend: Optional[Backend] = None 71 | self._frame_idx: int = 0 72 | 73 | # Keep track of time spent by each step 74 | self.elapsed_backend = [] 75 | self.elapsed_loop_closure = [] 76 | self.elapsed_odometry = [] 77 | 78 | self.__kwargs = kwargs 79 | 80 | @abstractmethod 81 | def init(self): 82 | """ 83 | An initialization procedure called at the start of each sequence 84 | """ 85 | self._frame_idx = 0 86 | if self.initialization is not None: 87 | del self.initialization 88 | self.initialization = None 89 | 90 | if self.config.initialization is not None: 91 | self.initialization = INITIALIZATION.load(self.config.initialization, **self.__kwargs) 92 | self.initialization.init() 93 | 94 | if self.preprocessing is not None: 95 | del self.preprocessing 96 | self.preprocessing = None 97 | 98 | if self.config.preprocessing is not None: 99 | self.preprocessing = Preprocessing(self.config.preprocessing, **self.__kwargs) 100 | 101 | if self.odometry is None: 102 | assert self.config.odometry is not None 103 | self.odometry = ODOMETRY.load(self.config.odometry, **self.__kwargs) 104 | 105 | assert self.odometry is not None 106 | self.odometry.init() 107 | if self.loop_closure is None and self.config.loop_closure is not None: 108 | self.loop_closure = LOOP_CLOSURE.load(self.config.loop_closure, **self.__kwargs) 109 | if self.loop_closure is not None: 110 | self.loop_closure.init() 111 | if self.config.backend is not None: 112 | self.backend = BACKEND.load(self.config.backend, **self.__kwargs) 113 | if self.backend is not None: 114 | self.backend.init() 115 | else: 116 | logging.warning("[SLAMAlgorithm]Defined a Loop Closure Algorithm Without a Backend") 117 | 118 | def process_next_frame(self, data_dict: dict): 119 | """ 120 | Args: 121 | data_dict (dict): The new frame (consisting of a dictionary of data items) returned by the Dataset 122 | """ 123 | beginning = time.time() 124 | 125 | if self.initialization is not None: 126 | self.initialization.next_frame(data_dict) 127 | 128 | if self.preprocessing is not None: 129 | self.preprocessing.forward(data_dict) 130 | 131 | self.odometry.process_next_frame(data_dict) 132 | step_odometry = time.time() 133 | self.elapsed_loop_closure.append(step_odometry - beginning) 134 | 135 | odometry_pose = None 136 | if self.odometry.relative_pose_key() in data_dict: 137 | odometry_pose = data_dict[self.odometry.relative_pose_key()] 138 | 139 | if self.initialization is not None: 140 | self.initialization.save_real_motion(data_dict[self.odometry.relative_pose_key()], data_dict) 141 | 142 | # Convert to double and reproject to the manifold of Rotation matrices to minimize error cumulation 143 | odometry_pose = odometry_pose.astype(np.float64) 144 | odometry_pose[:3, :3] = Rotation.from_matrix(odometry_pose[:3, :3]).as_matrix() 145 | 146 | if self.loop_closure is not None: 147 | # Copy the variables for the appropriate names 148 | if odometry_pose is not None: 149 | data_dict[self.loop_closure.relative_pose_key()] = odometry_pose 150 | 151 | if self.odometry.pointcloud_key() in data_dict: 152 | data_dict[self.loop_closure.pointcloud_key()] = data_dict[self.odometry.pointcloud_key()] 153 | 154 | self.loop_closure.process_next_frame(data_dict) 155 | step_loop_closure = time.time() 156 | self.elapsed_loop_closure.append(step_loop_closure - step_odometry) 157 | 158 | if self.backend is not None: 159 | if odometry_pose is not None: 160 | measurement = (odometry_pose, None) 161 | data_dict[self.backend.se3_odometry_constraint(self._frame_idx - 1)] = measurement 162 | init_step = time.time() 163 | self.backend.next_frame(data_dict) 164 | step_backend = time.time() 165 | if self.backend.need_to_update_pose: 166 | self.loop_closure.update_positions(self.backend.absolute_poses()) 167 | self.backend.need_to_update_pose = False 168 | self.elapsed_backend.append(step_backend - init_step) 169 | 170 | self._frame_idx += 1 171 | 172 | def get_relative_poses(self): 173 | """Returns the computed relative poses along the trajectory""" 174 | if self.backend is not None: 175 | return self.backend.relative_odometry_poses() 176 | return self.odometry.get_relative_poses() 177 | 178 | def get_absolute_poses(self): 179 | """Returns the computed relative poses along the trajectory""" 180 | if self.backend is not None: 181 | return self.backend.absolute_poses() 182 | return compute_absolute_poses(self.odometry.get_relative_poses()) 183 | 184 | def dump_all_constraints(self, log_dir: str): 185 | """Save the odometry, loop and absolute constraints on disk""" 186 | if self.backend is None: 187 | return 188 | 189 | dir_path = Path(log_dir) 190 | if not dir_path.exists(): 191 | dir_path.mkdir() 192 | assert_debug(dir_path.exists()) 193 | 194 | # Log Odometry Constraints 195 | self.save_constraints([(constraint[0], constraint[0] + 1, constraint[1]) for constraint in 196 | self.backend.registered_odometry_constraints()], 197 | str(dir_path / "odometry_constraints.txt")) 198 | 199 | self.save_constraints([(constraint[0], constraint[0], constraint[1]) for constraint in 200 | self.backend.registered_absolute_constraints()], 201 | str(dir_path / "absolute_constraints.txt")) 202 | 203 | self.save_constraints([(constraint[0], constraint[1], constraint[2]) for constraint in 204 | self.backend.registered_loop_constraints()], 205 | str(dir_path / "loop_constraints.txt")) 206 | 207 | @staticmethod 208 | def save_constraints(constraints, file_path: str): 209 | 210 | import pandas as pd 211 | constraints_list = [(constraint[0], constraint[1], *constraint[2].flatten().tolist()) for constraint in 212 | constraints] 213 | constraint_df = pd.DataFrame(constraints_list, columns=["src", "tgt", *[str(i) for i in range(16)]]) 214 | constraint_df.to_csv(file_path, sep=",") 215 | 216 | @staticmethod 217 | def load_constraints(file_path: str): 218 | """Loads trajectory constraints from disk""" 219 | import pandas as pd 220 | 221 | constraints_df: pd.DataFrame = pd.read_csv(file_path, sep=",") 222 | constraint_rows = constraints_df.values.tolist() 223 | return constraint_rows 224 | -------------------------------------------------------------------------------- /slam/training/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kitware/pyLiDAR-SLAM/1baa21a67bd32f144f8e17583251ac777f81345e/slam/training/__init__.py -------------------------------------------------------------------------------- /slam/training/prediction_modules.py: -------------------------------------------------------------------------------- 1 | from typing import Optional, Dict, Any 2 | 3 | from hydra.core.config_store import ConfigStore 4 | from torch import nn as nn 5 | 6 | # Hydra and OmegaConf 7 | from omegaconf import OmegaConf 8 | from hydra.conf import dataclass, MISSING 9 | 10 | # Project Imports 11 | from slam.common.pose import Pose 12 | from slam.models.posenet import POSENET 13 | 14 | 15 | # ---------------------------------------------------------------------------------------------------------------------- 16 | @dataclass 17 | class PredictionConfig: 18 | """PoseNet Prediction Config""" 19 | num_input_channels: int = MISSING 20 | sequence_len: int = MISSING 21 | posenet_config: Optional[Dict[str, Any]] = None 22 | 23 | 24 | # Hydra -- Create a group for the Prediction Config 25 | cs = ConfigStore.instance() 26 | cs.store(group="training/prediction", name="posenet", node=PredictionConfig) 27 | 28 | 29 | # ---------------------------------------------------------------------------------------------------------------------- 30 | # POSENET PREDICTION MODULE 31 | class _PoseNetPredictionModule(nn.Module): 32 | """ 33 | Posenet Module 34 | """ 35 | 36 | def __init__(self, 37 | config: PredictionConfig, 38 | pose: Pose): 39 | nn.Module.__init__(self) 40 | self.config = PredictionConfig(**config) 41 | self.pose = pose 42 | 43 | self.num_input_channels = self.config.num_input_channels 44 | self.sequence_len: int = self.config.sequence_len 45 | config.posenet_config["sequence_len"] = self.sequence_len 46 | config.posenet_config["num_input_channels"] = self.num_input_channels 47 | self.posenet: nn.Module = POSENET.load(OmegaConf.create(config.posenet_config), pose=self.pose) 48 | 49 | def forward(self, data_dict: dict): 50 | vertex_map = data_dict["vertex_map"] 51 | pose_params = self.posenet(vertex_map)[:, 0] 52 | data_dict["pose_params"] = pose_params 53 | data_dict["pose_matrix"] = self.pose.build_pose_matrix(pose_params) 54 | 55 | if "absolute_pose_gt" in data_dict: 56 | gt = data_dict["absolute_pose_gt"] 57 | relative_gt = gt[:, 0].inverse() @ gt[:, 1] 58 | data_dict["ground_truth"] = relative_gt 59 | 60 | return data_dict 61 | -------------------------------------------------------------------------------- /slam/viz/__init__.py: -------------------------------------------------------------------------------- 1 | from .visualizer import _with_cv2 2 | -------------------------------------------------------------------------------- /slam/viz/color_map.py: -------------------------------------------------------------------------------- 1 | from typing import Union, Optional 2 | 3 | import torch 4 | import numpy as np 5 | from matplotlib import cm 6 | from matplotlib.colors import ListedColormap, LinearSegmentedColormap 7 | 8 | from slam.common.utils import assert_debug 9 | 10 | 11 | def high_res_colormap(low_res_cmap, resolution=1000, max_value=1): 12 | # Construct the list colormap, with interpolated values for higer resolution 13 | # For a linear segmented colormap, you can just specify the number of point in 14 | # cm.get_cmap(name, lutsize) with the parameter lutsize 15 | x = np.linspace(0, 1, low_res_cmap.N) 16 | low_res = low_res_cmap(x) 17 | new_x = np.linspace(0, max_value, resolution) 18 | high_res = np.stack([np.interp(new_x, x, low_res[:, i]) for i in range(low_res.shape[1])], axis=1) 19 | return ListedColormap(high_res) 20 | 21 | 22 | def opencv_rainbow(resolution=1000): 23 | # Construct the opencv equivalent of Rainbow 24 | opencv_rainbow_data = ( 25 | (0.000, (1.00, 0.00, 0.00)), 26 | (0.400, (1.00, 1.00, 0.00)), 27 | (0.600, (0.00, 1.00, 0.00)), 28 | (0.800, (0.00, 0.00, 1.00)), 29 | (1.000, (0.60, 0.00, 1.00)) 30 | ) 31 | 32 | return LinearSegmentedColormap.from_list('opencv_rainbow', opencv_rainbow_data, resolution) 33 | 34 | 35 | COLORMAPS = {'rainbow': opencv_rainbow(), 36 | "jet": high_res_colormap(cm.get_cmap("jet")), 37 | "viridis": high_res_colormap(cm.get_cmap("viridis")), 38 | 'magma': high_res_colormap(cm.get_cmap('magma')), 39 | 'bone': cm.get_cmap('bone', 10000)} 40 | 41 | 42 | def gray_color_map(np_image: np.array, color_map="rainbow", 43 | z_min: Optional[float] = None, 44 | z_max: Optional[float] = None) -> np.array: 45 | """ 46 | Returns a color image from a gray image 47 | 48 | Parameters: 49 | np_image (np.ndarray): a gray image `(H,W)` 50 | color_map (str): The name of the color map (in "rainbow", "magma", "bone") 51 | z_min (float): Optionally the z_min (other wise set to the min of the scalars observed) 52 | z_max (float): Optionally the z_min (other wise set to the min of the scalars observed) 53 | 54 | Returns: 55 | np.ndarray `(4,H,W)` 56 | """ 57 | assert len(np_image.shape) == 2 58 | min_value = z_min if z_min is not None else np_image.min() 59 | max_value = z_max if z_max is not None else np_image.max() 60 | np_image = (np_image - min_value) / (max_value - min_value) 61 | output_image = COLORMAPS[color_map](np_image).astype(np.float32) 62 | output_image = output_image.transpose(2, 0, 1) 63 | return output_image 64 | 65 | 66 | def scalar_gray_cmap(values: np.ndarray, cmap="rainbow", 67 | z_min: Optional[float] = None, z_max: Optional[float] = None) -> np.ndarray: 68 | """Returns an array of colors from an array of values 69 | 70 | Parameters: 71 | values (np.ndarray): an array of scalars `(N,)` 72 | cmap (str): The name of the color map (in "rainbow", "magma", "bone") 73 | z_min (float): Optionally the z_min (other wise set to the min of the scalars observed) 74 | z_max (float): Optionally the z_min (other wise set to the min of the scalars observed) 75 | Returns: 76 | np.ndarray `(N,3)` 77 | """ 78 | colors = gray_color_map(values.reshape(-1, 1), cmap, z_min=z_min, z_max=z_max)[:3, :, 0].T 79 | return colors 80 | 81 | 82 | def rescale_image_values(t_image: Union[torch.Tensor, np.ndarray]): 83 | """ 84 | Rescales the values of an image, for more convenient visualization 85 | Gray Image will have values in [0, 1] 86 | RGB Images will have values in [0, 255] 87 | """ 88 | assert_debug(len(t_image.shape) == 3) 89 | rescale_factor = 1. 90 | if t_image.shape[0] == 3: 91 | rescale_factor = 255. 92 | min_value = t_image.min() 93 | max_value = t_image.max() 94 | 95 | if -1 <= min_value < 0 and max_value <= 1: 96 | # Values between [-1, 1], rescale to [0, 1] or [0, 255] 97 | return (t_image * 0.5 + 0.5) * rescale_factor 98 | if min_value >= 0 and max_value <= 1: 99 | return t_image * rescale_factor 100 | if 0 <= min_value and max_value <= rescale_factor: 101 | # If the image is already properly scaled, return 102 | return t_image 103 | if min_value == max_value: 104 | return t_image 105 | # CROP OR Reduce scale ? 106 | return (t_image - min_value) / (max_value - min_value) * rescale_factor 107 | 108 | 109 | def tensor_to_image(t_image: torch.Tensor, colormap="magma", **kwargs): 110 | """ 111 | Converts an image tensor to a np.array 112 | The image extracted from the tensor will obey the following rules, depending on the size of the tensor : 113 | [B, C, H, W] => Concatenates the images to an [3, H * B, W] np.array 114 | [3, H, W] => Returns the color image [3, H, W] as an np.array (dealing with the scaling of the data) 115 | [H, W] | [1, H, W] => Returns a color map converting the gray image to a colored image 116 | 117 | If C = 2, returns an image 118 | 119 | Returns an RGB image [3, H | H * B, W] 120 | """ 121 | l_t_image = [] 122 | if len(t_image.size()) == 4: 123 | b, c, h, w = t_image.size() 124 | for i in range(b): 125 | l_t_image.append(t_image[i]) 126 | elif len(t_image.size()) == 3: 127 | l_t_image.append(t_image) 128 | elif len(t_image.size()) == 2: 129 | l_t_image.append(t_image.unsqueeze(0)) 130 | else: 131 | raise RuntimeError("Bad Input Shape") 132 | 133 | l_np_imgs = [] 134 | for t_imh in l_t_image: 135 | c, h, w = t_imh.size() 136 | if c != 2: 137 | t_imh = rescale_image_values(t_imh) 138 | if c == 3: 139 | np_imh = t_imh.to(dtype=torch.uint8).cpu().detach().numpy() 140 | l_np_imgs.append(np_imh) 141 | elif c == 1: 142 | np_imh = (gray_color_map(t_imh[0].cpu().detach().numpy(), colormap) * 255.0).astype(np.uint8) 143 | l_np_imgs.append(np_imh) 144 | else: 145 | raise RuntimeError(f"Unrecognised shape {t_imh.shape}") 146 | concat_image = np.concatenate(l_np_imgs, axis=1) 147 | return concat_image 148 | -------------------------------------------------------------------------------- /slam/viz/visualizer.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | from slam.common.utils import assert_debug 4 | from slam.viz.color_map import tensor_to_image 5 | import numpy as np 6 | 7 | from slam.common.modules import _with_cv2 8 | 9 | if _with_cv2: 10 | import cv2 11 | 12 | 13 | class _ImageVisualizer(object): 14 | """ 15 | A Visualizer displays images tensors in OpenCV windows 16 | 17 | Parameters 18 | ---------- 19 | channels : list of str 20 | The keys of image tensors in each iteration data_dict 21 | update_frequency : int 22 | The frequency of update for each image 23 | wait_key : int 24 | The number of milliseconds to wait for a key to be pressed before moving on 25 | """ 26 | 27 | def __init__(self, channels: list, update_frequency: int = 10, wait_key: int = 1): 28 | self.channels: set = set(channels) 29 | 30 | self.update_frequency = update_frequency 31 | self.global_step = 0 32 | self.wait_key = wait_key 33 | self.old_wait_key = wait_key 34 | 35 | for channel in self.channels: 36 | self.prepare_channel(channel) 37 | 38 | def prepare_channel(self, channel_name: str): 39 | assert_debug(channel_name in self.channels) 40 | for channel in self.channels: 41 | cv2.namedWindow(channel, cv2.WINDOW_KEEPRATIO | cv2.WINDOW_NORMAL) 42 | 43 | def __del__(self): 44 | for channel in self.channels: 45 | cv2.destroyWindow(channel) 46 | 47 | def visualize(self, data_dict: dict, iter_: int): 48 | """ 49 | Visualize the images in data_dict in OpenCV windows 50 | 51 | The data_dict must have all keys in in self.channels 52 | Each item for these keys must either be a torch.Tensor representing a batch of images of shape [B, C, H, W] 53 | Or a np.ndarray of shape [H, W, C] 54 | In both cases C must be in {1, 3} 55 | 56 | The images will only be displayed every self.update_frequency 57 | 58 | Parameters 59 | ---------- 60 | data_dict : dict 61 | The dictionary of tensors, from which images 62 | iter_ : int 63 | The global step for the current data_dict 64 | """ 65 | if iter_ % self.update_frequency == 0: 66 | for channel in self.channels: 67 | try: 68 | assert_debug(channel in data_dict, f"chanel {channel} not in data_dict") 69 | image_data = data_dict[channel] 70 | if isinstance(image_data, np.ndarray): 71 | has_right_dimensions = len(image_data.shape) == 3 or len(image_data.shape) == 2 72 | has_right_num_channels = len(image_data.shape) == 3 and ( 73 | image_data.shape[2] != 3 and image_data[2] != 1) 74 | assert has_right_dimensions and has_right_num_channels, \ 75 | f"[NodeVisualization] Bad shape for np.array (expect [H, W, C]), got {image_data.shape}" 76 | elif isinstance(image_data, torch.Tensor): 77 | image_data = tensor_to_image(image_data).transpose((1, 2, 0)) 78 | image_data = image_data[:, :, [2, 1, 0]] 79 | cv2.imshow(channel, image_data) 80 | except (Exception, AssertionError) as e: 81 | print(f"Error trying to visualize channel {channel}") 82 | raise e 83 | cv2.waitKey(self.wait_key) 84 | 85 | 86 | def ImageVisualizer(channels: list, update_frequency: int = 10, wait_key: int = 1): 87 | """Returns an Image Visualizer based on OpenCV if the package cv2 was found""" 88 | if _with_cv2: 89 | return _ImageVisualizer(channels, update_frequency, wait_key) 90 | else: 91 | return None 92 | -------------------------------------------------------------------------------- /tests/test_backend.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import numpy as np 3 | from scipy.spatial.transform import Rotation 4 | 5 | # Project Imports 6 | from slam.eval.eval_odometry import compute_relative_poses, compute_absolute_poses 7 | from viz3d.window import OpenGLWindow 8 | 9 | 10 | class BackendTestCase(unittest.TestCase): 11 | def test_graph_slam(self): 12 | # Construct Test problem 13 | from slam.backend import _with_g2o 14 | self.assertTrue(_with_g2o) 15 | 16 | from slam.backend import GraphSLAM, GraphSLAMConfig 17 | 18 | num_poses = 101 19 | thetas = 2 * np.pi * np.arange(num_poses) / num_poses 20 | 21 | c = np.cos(thetas) 22 | s = np.sin(thetas) 23 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) 24 | poses[:, 0, 0] = c 25 | poses[:, 1, 1] = c 26 | poses[:, 0, 1] = s 27 | poses[:, 1, 0] = -s 28 | poses[:, 0, 3] = 10 * c 29 | poses[:, 1, 3] = -10 * s 30 | 31 | relative_gt = compute_relative_poses(poses) 32 | # Apply noise to the relative poses 33 | noise = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) 34 | rotation_noise = np.random.randn(num_poses, 3) * 0.1 35 | trans_noise = np.random.randn(num_poses, 3) * 0.1 36 | noise[:, :3, 3] = trans_noise 37 | noise[:, :3, :3] = Rotation.from_euler("xyz", rotation_noise).as_matrix() 38 | 39 | relative_noisy = np.einsum("nij,njk->nik", relative_gt, noise) 40 | absolute_noisy = compute_absolute_poses(relative_noisy) 41 | 42 | # Show the problem 43 | window = OpenGLWindow() 44 | window.init() 45 | 46 | window.set_cameras(0, poses.astype(np.float32), scale=0.1, line_width=1) 47 | window.set_cameras(1, absolute_noisy.astype(np.float32), scale=0.1, line_width=1, 48 | default_color=np.array([[1.0, 0.0, 0.0]])) 49 | 50 | # Solve with the Backend 51 | config = GraphSLAMConfig() 52 | graph_slam = GraphSLAM(config) 53 | 54 | # Construct i -> (i+1) relative constraints 55 | data_dict = dict() 56 | for idx in range(1, num_poses): 57 | data_dict[GraphSLAM.se3_odometry_constraint(idx - 1, idx)] = (relative_noisy[idx], None) 58 | 59 | # Loop closure constraint 60 | data_dict[GraphSLAM.se3_odometry_constraint(0, num_poses - 1)] = (np.eye(4, dtype=np.float64), 1.e6 * np.eye(6)) 61 | 62 | graph_slam.init() 63 | graph_slam.next_frame(data_dict) 64 | 65 | for i in range(100): 66 | graph_slam.optimize(100) 67 | optimized = graph_slam.absolute_poses().astype(np.float32) 68 | window.set_cameras(2, optimized, scale=0.1, line_width=1, default_color=np.array([[0.0, 1.0, 0.0]])) 69 | 70 | window.close() 71 | 72 | 73 | if __name__ == '__main__': 74 | unittest.main() 75 | -------------------------------------------------------------------------------- /tests/test_geometry.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from slam.common.geometry import * 3 | 4 | 5 | class GeometryTestCase(unittest.TestCase): 6 | def test_compute_neighbors(self): 7 | n = 10 8 | height = 10 9 | width = 10 10 | tgt = torch.randn(1, 3, height, width) 11 | ref = torch.randn(n, 3, height, width) 12 | 13 | tgt[0, :, 0, 0] = 0.0 14 | 15 | neighbors, _ = compute_neighbors(tgt, ref) 16 | for k in range(height): 17 | for l in range(width): 18 | if k == 0.0 and l == 0.0: 19 | self.assertEqual(neighbors[0, :, 0, 0].norm(), 0.0) 20 | continue 21 | norm_neighbor = (neighbors[0, :, k, l] - tgt[0, :, k, l]).norm() 22 | for i in range(n): 23 | norm_ref = (ref[i, :, k, l] - tgt[0, :, k, l]).norm() 24 | self.assertLessEqual(norm_neighbor, norm_ref) 25 | 26 | 27 | if __name__ == '__main__': 28 | unittest.main() 29 | -------------------------------------------------------------------------------- /tests/test_optimization.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import torch 3 | 4 | from slam.common.optimization import PointToPlaneCost, GaussNewton 5 | from slam.common.pose import Pose 6 | 7 | 8 | class MyTestCase(unittest.TestCase): 9 | def test_gauss_newton(self): 10 | tgt_points = torch.randn(2, 100, 3, dtype=torch.float64) 11 | ref_normals = torch.randn(2, 100, 3, dtype=torch.float64) 12 | ref_normals /= ref_normals.norm(dim=-1, keepdim=True) 13 | 14 | pose_params = torch.randn(2, 6, dtype=torch.float64) * torch.tensor([[0.01, 0.01, 0.01, 0.001, 0.001, 0.001]], 15 | dtype=torch.float64) 16 | 17 | pose = Pose("euler") 18 | ref_points = pose.apply_transformation(tgt_points, pose_params) 19 | 20 | gauss_newton = GaussNewton(max_iters=100, norm_stop_criterion=1.e-10, scheme="huber", sigma=0.0001) 21 | cost = PointToPlaneCost() 22 | residual_fun = cost.get_residual_fun(tgt_points, ref_points, ref_normals, pose) 23 | jac_fun = cost.get_residual_jac_fun(tgt_points, ref_points, ref_normals, pose) 24 | est_params, loss = gauss_newton.compute(torch.zeros_like(pose_params), residual_fun, jac_fun=jac_fun) 25 | 26 | diff_params = (est_params - pose_params).abs().max() 27 | est_points = pose.apply_transformation(tgt_points, est_params) 28 | diff_points = (ref_points - est_points).abs().max() 29 | 30 | self.assertLessEqual(diff_points, 1.e-7) 31 | self.assertLessEqual(diff_params, 1.e-7) 32 | self.assertLessEqual(loss.abs().sum().item(), 1.e-7) 33 | 34 | 35 | if __name__ == '__main__': 36 | unittest.main() 37 | -------------------------------------------------------------------------------- /tests/test_pointcloud.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from slam.common.pointcloud import * 4 | 5 | 6 | class PointCloudCase(unittest.TestCase): 7 | def test_voxelization(self): 8 | pointcloud = np.random.randn(100000, 3) 9 | voxel_size = 0.1 10 | 11 | # Voxelization 12 | voxel_coordinates = voxelise(pointcloud, voxel_size, voxel_size, voxel_size) 13 | self.assertTrue(voxel_coordinates.dtype == np.int64) 14 | 15 | # Hashing 16 | voxel_hashes = np.zeros_like(voxel_coordinates[:, 0]) 17 | voxel_hashing(voxel_coordinates, voxel_hashes) 18 | 19 | # Statistics 20 | num_voxels, means, covs, voxel_indices = voxel_normal_distribution(pointcloud, voxel_hashes) 21 | 22 | voxel_means = means[voxel_indices] 23 | 24 | diff = np.linalg.norm(pointcloud - voxel_means, axis=-1).max() 25 | self.assertLess(diff, 0.18) 26 | 27 | 28 | if __name__ == '__main__': 29 | unittest.main() 30 | -------------------------------------------------------------------------------- /train.py: -------------------------------------------------------------------------------- 1 | from typing import Optional, Dict 2 | 3 | from torch import nn as nn 4 | from torch.utils.data import Dataset 5 | 6 | # Hydra and OmegaConf 7 | import hydra 8 | from hydra.conf import dataclass, MISSING 9 | from omegaconf import DictConfig, OmegaConf 10 | 11 | # Project Imports 12 | from slam.common.pose import Pose 13 | from slam.common.projection import SphericalProjector 14 | from slam.common.utils import assert_debug 15 | from slam.dataset import DATASET, DatasetLoader, DatasetConfig 16 | from slam.training.loss_modules import _PointToPlaneLossModule, _PoseSupervisionLossModule, LossConfig 17 | from slam.training.prediction_modules import _PoseNetPredictionModule, PredictionConfig 18 | from slam.training.trainer import ATrainer, ATrainerConfig 19 | 20 | 21 | # ---------------------------------------------------------------------------------------------------------------------- 22 | # Training Config 23 | 24 | @dataclass 25 | class TrainingConfig: 26 | """A Config for training of a PoseNet module""" 27 | loss: LossConfig = MISSING 28 | prediction: PredictionConfig = MISSING 29 | 30 | 31 | @dataclass 32 | class PoseNetTrainingConfig(ATrainerConfig): 33 | """A Config for a PoseNetTrainer""" 34 | pose: str = "euler" 35 | ei_config: Optional[Dict] = None 36 | sequence_len: int = 2 37 | num_input_channels: int = 3 38 | 39 | dataset: DatasetConfig = MISSING 40 | training: TrainingConfig = MISSING 41 | 42 | 43 | # ---------------------------------------------------------------------------------------------------------------------- 44 | # Trainer for PoseNet 45 | class PoseNetTrainer(ATrainer): 46 | """Unsupervised / Supervised Trainer for the PoseNet prediction module""" 47 | 48 | def __init__(self, config: PoseNetTrainingConfig): 49 | super().__init__(config) 50 | self.pose = Pose(self.config.pose) 51 | 52 | self.dataset_config: DatasetLoader = DATASET.load(config.dataset) 53 | self.projector: SphericalProjector = self.dataset_config.projector() 54 | 55 | # Share root parameters to Prediction Node 56 | self.config.training.prediction.sequence_len = self.config.sequence_len 57 | self.config.training.prediction.num_input_channels = self.config.num_input_channels 58 | 59 | def __transform(self, data_dict: dict): 60 | return data_dict 61 | 62 | def prediction_module(self) -> nn.Module: 63 | """Returns the PoseNet Prediction Module""" 64 | return _PoseNetPredictionModule(OmegaConf.create(self.config.training.prediction), self.pose) 65 | 66 | def loss_module(self) -> nn.Module: 67 | """Return the loss module used to train the model""" 68 | loss_config = self.config.training.loss 69 | mode = loss_config.mode 70 | assert_debug(mode in ["unsupervised", "supervised"]) 71 | if mode == "supervised": 72 | return _PoseSupervisionLossModule(loss_config, self.pose) 73 | else: 74 | return _PointToPlaneLossModule(loss_config, self.projector, self.pose) 75 | 76 | def load_datasets(self) -> (Optional[Dataset], Optional[Dataset], Optional[Dataset]): 77 | """Loads the Datasets""" 78 | train_dataset, eval_dataset, test_dataset = self.dataset_config.get_sequence_dataset() 79 | train_dataset.sequence_transforms = self.__transform 80 | if test_dataset is not None: 81 | test_dataset.sequence_transforms = self.__transform 82 | if eval_dataset is not None: 83 | eval_dataset.sequence_transforms = self.__transform 84 | return train_dataset, eval_dataset, test_dataset 85 | 86 | def test(self): 87 | pass 88 | 89 | 90 | @hydra.main(config_name="train_posenet", config_path="config") 91 | def run(cfg: PoseNetTrainingConfig): 92 | trainer = PoseNetTrainer(PoseNetTrainingConfig(**cfg)) 93 | # Initialize the trainer (Optimizer, Cuda context, etc...) 94 | trainer.init() 95 | 96 | if trainer.config.do_train: 97 | trainer.train(trainer.config.num_epochs) 98 | if trainer.config.do_test: 99 | trainer.test() 100 | 101 | 102 | if __name__ == "__main__": 103 | run() 104 | --------------------------------------------------------------------------------