diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..e65e8c4
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,175 @@
+.idea/
+
+# logs
+logs*
+test.py
+
+# data
+data/
+results/
+
+
+### Python ###
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+# Usually these files are written by a python script from a template
+# before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+# For a library or package, you might want to ignore these files since the code is
+# intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+# However, in case of collaboration, if having platform-specific dependencies or dependencies
+# having no cross-platform support, pipenv may install dependencies that don't work, or not
+# install all needed dependencies.
+#Pipfile.lock
+
+# poetry
+# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+# This is especially recommended for binary packages to ensure reproducibility, and is more
+# commonly ignored for libraries.
+# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# pdm
+# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
+#pdm.lock
+# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
+# in version control.
+# https://pdm.fming.dev/#use-with-ide
+.pdm.toml
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+# JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+# and can be added to the global gitignore or merged into this file. For a more nuclear
+# option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/
+
+# End of https://www.toptal.com/developers/gitignore/api/python
+nn
\ No newline at end of file
diff --git a/PREPARE_DATA.md b/PREPARE_DATA.md
new file mode 100644
index 0000000..c53ba9c
--- /dev/null
+++ b/PREPARE_DATA.md
@@ -0,0 +1,63 @@
+## IPBLab dataset
+Downloading IPBLab dataset from our server:
+```shell
+cd ir-mcl && mkdir data && cd data
+wget https://www.ipb.uni-bonn.de/html/projects/kuang2023ral/ipblab.zip
+unzip ipblab.zip
+```
+
+For each sequence, we provide :
+- seq_{id}.bag: the ROS bag format, include raw odometer reading and raw lidar reading.
+- seq_{id}.json: include raw odometer reading, ground-truth poses, and raw lidar reading.
+- seq_{id}_gt_pose: the ground-truth poses in TUM format (for evaluation with evo).
+
+Besides, there are also some configuration files are provided:
+- lidar_info.json: the parameters of the 2D LiDAR sensor.
+- occmap.npy: the pre-built occupancy grid map.
+- b2l.txt: the transformation from the lidar link to robot's base link
+
+The final data structure should look like
+```
+data/
+├── ipblab/
+│ ├── loc_test/
+│ │ ├── test1/
+│ │ │ ├──seq_1.bag
+│ │ │ ├──seq_1.json
+│ │ │ ├──seq_1_gt_pose.txt
+│ │ ├──b2l.txt
+│ ├──lidar_info.json
+│ ├──occmap.npy
+```
+
+There is one sequence available for the localization experiments now, the full dataset will be released after our dataset paper is published!
+
+## Intel Lab datatse, Freiburg Building 079 dataset, and MIT CSAIL dataset
+Downloading these three classical indoor 2D SLAM datasets from our server:
+```shell
+cd ir-mcl && mkdir data && cd data
+wget https://www.ipb.uni-bonn.de/html/projects/kuang2023ral/2dslam.zip
+unzip 2dslam.zip
+```
+
+For each sequence, we provide :
+- train.json: the training set which is used for mapping or train the NOF model.
+- val.json: the validation set for evaluating the model during training.
+- test.json: the test set for evaluating the final model.
+- occmap.npy: the pre-built occupancy grid map by using training set.
+
+The final data structure should look like
+```
+data/
+├── fr079/
+│ ├──occmap.npy
+│ ├──train.json
+│ ├──val.json
+│ ├──test.json
+├── intel/
+│ ├──...
+├── mit/
+│ ├──...
+```
+
+Here, we provide the converted format of these dataset for ease of use. The raw data could be found on our website: [2D Laser Dataset](https://www.ipb.uni-bonn.de/datasets/).
\ No newline at end of file
diff --git a/README.md b/README.md
index 4a975b7..9f639ce 100644
--- a/README.md
+++ b/README.md
@@ -1,13 +1,24 @@
-# IR-MCL: Implicit Representation-Based Online Global Localization
-This repo contains the code of the paper:
-*IR-MCL: Implicit Representation-Based Online Global Localization*,
-
-by [Haofei Kuang](https://www.ipb.uni-bonn.de/people/haofei-kuang/), [Xieyuanli Chen](https://www.ipb.uni-bonn.de/people/xieyuanli-chen/), [Tiziano Guadagnino](https://phd.uniroma1.it/web/TIZIANO-GUADAGNINO_nP1536210_IT.aspx), [Nicky Zimmerman](https://www.ipb.uni-bonn.de/people/nicky-zimmerman/), [Jens Behley](https://www.ipb.uni-bonn.de/people/jens-behley/) and [Cyrill Stachniss](https://www.ipb.uni-bonn.de/people/cyrill-stachniss/)
+
+
IR-MCL: Implicit Representation-Based Online Global Localization
+
+ Haofei Kuang
+ ·
+ Xieyuanli Chen
+ ·
+ Tiziano Guadagnino
+ ·
+ Nicky Zimmerman
+ ·
+ Jens Behley
+ ·
+ Cyrill Stachniss
+
+ University of Bonn
+
+
+
-[Arxiv Preprint](https://arxiv.org/abs/2210.03113)
-----
-### IR-MCL pipeline
@@ -21,9 +32,122 @@ by [Haofei Kuang](https://www.ipb.uni-bonn.de/people/haofei-kuang/), [Xieyuanli
## Abstract
Determining the state of a mobile robot is an essential building block of robot navigation systems. In this paper, we address the problem of estimating the robot’s pose in an indoor environment using 2D LiDAR data and investigate how modern environment models can improve gold standard Monte-Carlo localization (MCL) systems. We propose a neural occupancy field (NOF) to implicitly represent the scene using a neural network. With the pretrained network, we can synthesize 2D LiDAR scans for an arbitrary robot pose through volume rendering. Based on the implicit representation, we can obtain the similarity between a synthesized and actual scan as an observation model and integrate it into an MCL system to perform accurate localization. We evaluate our approach on five sequences of a self-recorded dataset and three publicly available datasets. We show that we can accurately and efficiently localize a robot using our approach surpassing the localization performance of state-of-the-art methods. The experiments suggest that the presented implicit representation is able to predict more accurate 2D LiDAR scans leading to an improved observation model for our particle filter-based localization.
-## Code
-Coming soon!
+
+## Dependencies
+The code was tested with Ubuntu 20.04 with:
+- python version **3.9**.
+- pytorch version **1.13.1** with **CUDA 11.6**
+- pytorch-lighting with **1.9.0**
+
+### Installation
+- Clone the repo:
+ ```shell
+ git clone https://github.com/PRBonn/ir-mcl.git
+ cd ir-mcl
+ ```
+
+- Prepare the python environment (Anaconda is recommended here):
+ ```shell
+ conda env create -f environment.yml
+ ```
+ or
+ ```shell
+ conda create --name irmcl python=3.9.13
+ conda activate irmcl
+
+ conda install -c conda-forge pybind11
+ pip install torch torchvision --extra-index-url https://download.pytorch.org/whl/cu116
+ pip install pytorch-lightning tensorboardX
+ pip install matplotlib scipy open3d
+ pip install evo --upgrade --no-binary evo
+ ```
+
+- Compile the motion model and resampling module
+ ```shell
+ cd ir-mcl/mcl & conda activate ir-mcl
+ make -j4
+ ```
+
+## Preparation
+
+### Datasets
+Please refer to [PREPARE_DATA](PREPARE_DATA.md) to prepare the datasets
+
+### Pre-trained Weights
+
+The pre-trained weights are stored at `config` folder, includes:
+- IPBLab dataset: `config/ipblab_nof_weights.ckpt`
+- Freiburg Building 079 dataset: `config/fr079_nof_weights.ckpt`
+- Intel Lab dataset: `config/intel_nof_weights.ckpt`
+- MIT CSAIL dataset: `config/mit_nof_weights.ckpt`
+
+## Run Experiments
+
+### Global Localization Experiments on IPBLab dataset
+- Pre-training NOF on IPBLab dataset (**The train/eval/test set of IPBLab dataset are not available now, they will be released after our dataset paper is published!**)
+ ```shell
+ cd ~/ir-mcl
+ bash ./shells/pretraining/ipblab.sh
+ ```
+- Global localization experiments
+ ```shell
+ cd ir-mcl
+ python main.py --config_file ./config/global_localization/loc_config_{sequence_id}.yml
+ # for example: python main.py --config_file ./config/global_localization/loc_config_test1.yml
+ ```
+
+- Pose-tracking experiments
+ ```shell
+ cd ir-mcl
+ python main.py --config_file ./config/pose_tracking/loc_config_{sequence_id}.yml
+ # for example: python main.py --config_file ./config/pose_tracking/loc_config_test1.yml
+ ```
+
+### Observation Model Experiments
+- Train/Test (replace "dataset" in "fr079", "intel", or "mit")
+ ```shell
+ cd ir-mcl
+ bash ./shells/pretraining/{dataset}.sh
+ # for example: bash ./shells/pretraining/intel.sh
+ ```
+
+## Supplements for the Experimental Results
+Due to the space limitation of the paper, we provide some experimental results as supplements here.
+
+### Memery cost
+We provide an ablation study on the memory cost between the occupancy grid map (OGM), Hilbert map, and our neural occupancy field (NOF).
+
+| Maps type | Approximate memory | Loc. method | RMSE: location (cm) / yaw (degree) |
+|:----------------------|:--------------------:|:--------------------------:|:--------------------------------------------:|
+| OGM (5cm grid size) | 4.00MB | AMCL
NMCL
SRRG-Loc | 11.11 / 4.15
19.57 / 3.62
8.74 / 1.68 |
+| OGM (10cm grid size) | 2.00MB | AMCL
NMCL
SRRG-Loc | 15.01 / 4.18
36.27 / 4.04
12.15 / 1.53 |
+| Hilbert Map | 0.01MB | HMCL | 20.04 / 4.50 |
+| NOF | 1.96NB | IR-MCL | **6.62** / **1.11** |
+
+
+### Ablation study on fixed particle numbers
+We also provide the experiment to study the performance of global localization under the same particle numbers for all methods. We fixed the number of particles to 100,000. In the below table, all baselines and IR-MCL∗ always use 100,000 particles. IR-MCL is shown for reference.
+
+| Method | RMSE: location (cm) / yaw (degree) |
+|:-------------------------------------------------------:|:------------------------------------------------------------------------------:|
+| AMCL
NMCL
HMCL
SRRG-Loc
IR-MCL∗ | 11.56 / 4.12
19.57 / 3.62
20.54 / 4.70
8.74 / 1.68
6.71 / **1.11** |
+| IR-MCL | **6.62** / **1.11** |
+
+## Citation
+
+If you use this library for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kuang2023ral.pdf).
+
+```bibtex
+@article{kuang2023ral,
+ author = {Kuang, Haofei and Chen, Xieyuanli and Guadagnino, Tiziano and Zimmerman, Nicky and Behley, Jens and Stachniss, Cyrill},
+ title = {{IR-MCL: Implicit Representation-Based Online Global Localization}},
+ journal = {IEEE Robotics and Automation Letters (RA-L)},
+ doi = {10.1109/LRA.2023.3239318},
+ year = {2023},
+ codeurl = {https://github.com/PRBonn/ir-mcl},
+}
+```
## Acknowledgment
This work has partially been funded by the European Union’s Horizon 2020 research and innovation programme under grant agreement
-No 101017008 (Harmony).
+No 101017008 (Harmony).
\ No newline at end of file
diff --git a/config/fr079_nof_weights.ckpt b/config/fr079_nof_weights.ckpt
new file mode 100644
index 0000000..dcf0503
Binary files /dev/null and b/config/fr079_nof_weights.ckpt differ
diff --git a/config/global_localization/loc_config_test1.yml b/config/global_localization/loc_config_test1.yml
new file mode 100644
index 0000000..591e401
--- /dev/null
+++ b/config/global_localization/loc_config_test1.yml
@@ -0,0 +1,65 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test1/seq_1.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test1/seq_1.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test1/seq_1_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/loc_test/test1/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# odometry noisy coefficient
+r1_noisy: 0.01
+d_noisy: 0.01
+r2_noisy: 0.01
+# motion model noisy coefficient
+alpha: [0.01, 0.01, 0.05, 0.05]
+# pose tracking or global localization
+pose_tracking: false
+init_noise: [0.25, 0.25, 0.0685]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 100000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/global_localization/loc_config_test2.yml b/config/global_localization/loc_config_test2.yml
new file mode 100644
index 0000000..e649228
--- /dev/null
+++ b/config/global_localization/loc_config_test2.yml
@@ -0,0 +1,67 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test2/seq_2.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test2/seq_2.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test2/seq_2_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/loc_test/test2/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# odometry noisy coefficient
+r1_noisy: 0.01
+d_noisy: 0.01
+r2_noisy: 0.01
+# motion model noisy coefficient
+#alpha: [0.314, 0.314, 0.5, 0.5]
+alpha: [0.01, 0.01, 0.05, 0.05]
+# pose tracking or global localization
+pose_tracking: false
+init_noise: [0.25, 0.25, 0.0685]
+#init_noise: [0, 0, 0]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 100000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/global_localization/loc_config_test3.yml b/config/global_localization/loc_config_test3.yml
new file mode 100644
index 0000000..5d32717
--- /dev/null
+++ b/config/global_localization/loc_config_test3.yml
@@ -0,0 +1,67 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test3/seq_3.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test3/seq_3.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test3/seq_3_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/loc_test/test3/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# odometry noisy coefficient
+r1_noisy: 0.01
+d_noisy: 0.01
+r2_noisy: 0.01
+# motion model noisy coefficient
+#alpha: [0.314, 0.314, 0.5, 0.5]
+alpha: [0.01, 0.01, 0.05, 0.05]
+# pose tracking or global localization
+pose_tracking: false
+init_noise: [0.25, 0.25, 0.0685]
+#init_noise: [0, 0, 0]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 100000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/global_localization/loc_config_test4.yml b/config/global_localization/loc_config_test4.yml
new file mode 100644
index 0000000..9a1382b
--- /dev/null
+++ b/config/global_localization/loc_config_test4.yml
@@ -0,0 +1,67 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test4/seq_4.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test4/seq_4.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test4/seq_4_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/loc_test/test4/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# odometry noisy coefficient
+r1_noisy: 0.01
+d_noisy: 0.01
+r2_noisy: 0.01
+# motion model noisy coefficient
+#alpha: [0.314, 0.314, 0.5, 0.5]
+alpha: [0.01, 0.01, 0.05, 0.05]
+# pose tracking or global localization
+pose_tracking: false
+init_noise: [0.25, 0.25, 0.0685]
+#init_noise: [0, 0, 0]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 100000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/global_localization/loc_config_test5.yml b/config/global_localization/loc_config_test5.yml
new file mode 100644
index 0000000..effda1a
--- /dev/null
+++ b/config/global_localization/loc_config_test5.yml
@@ -0,0 +1,67 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test5/seq_5.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test5/seq_5.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test5/seq_5_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/loc_test/test5/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# odometry noisy coefficient
+r1_noisy: 0.01
+d_noisy: 0.01
+r2_noisy: 0.01
+# motion model noisy coefficient
+#alpha: [0.314, 0.314, 0.5, 0.5]
+alpha: [0.01, 0.01, 0.05, 0.05]
+# pose tracking or global localization
+pose_tracking: false
+init_noise: [0.25, 0.25, 0.0685]
+#init_noise: [0, 0, 0]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 100000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/intel_nof_weights.ckpt b/config/intel_nof_weights.ckpt
new file mode 100644
index 0000000..b446561
Binary files /dev/null and b/config/intel_nof_weights.ckpt differ
diff --git a/config/ipblab_nof_weights.ckpt b/config/ipblab_nof_weights.ckpt
new file mode 100644
index 0000000..509d94d
Binary files /dev/null and b/config/ipblab_nof_weights.ckpt differ
diff --git a/config/mit_nof_weights.ckpt b/config/mit_nof_weights.ckpt
new file mode 100644
index 0000000..ab6b5a6
Binary files /dev/null and b/config/mit_nof_weights.ckpt differ
diff --git a/config/pose_tracking/loc_config_test1.yml b/config/pose_tracking/loc_config_test1.yml
new file mode 100644
index 0000000..a182025
--- /dev/null
+++ b/config/pose_tracking/loc_config_test1.yml
@@ -0,0 +1,59 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test1/seq_1.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test1/seq_1.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test1/seq_1_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/pose_tracking/test1/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# pose tracking or global localization
+pose_tracking: true
+init_noise: [0.5, 0.5, 0.0685]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 5000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/pose_tracking/loc_config_test2.yml b/config/pose_tracking/loc_config_test2.yml
new file mode 100644
index 0000000..6190c68
--- /dev/null
+++ b/config/pose_tracking/loc_config_test2.yml
@@ -0,0 +1,59 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test2/seq_2.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test2/seq_2.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test2/seq_2_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/pose_tracking/test2/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# pose tracking or global localization
+pose_tracking: true
+init_noise: [0.5, 0.5, 0.0685]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 5000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/pose_tracking/loc_config_test3.yml b/config/pose_tracking/loc_config_test3.yml
new file mode 100644
index 0000000..900cb3b
--- /dev/null
+++ b/config/pose_tracking/loc_config_test3.yml
@@ -0,0 +1,59 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test3/seq_3.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test3/seq_3.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test3/seq_3_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/pose_tracking/test3/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# pose tracking or global localization
+pose_tracking: true
+init_noise: [0.5, 0.5, 0.0685]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 5000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/pose_tracking/loc_config_test4.yml b/config/pose_tracking/loc_config_test4.yml
new file mode 100644
index 0000000..ddde38b
--- /dev/null
+++ b/config/pose_tracking/loc_config_test4.yml
@@ -0,0 +1,59 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test4/seq_4.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test4/seq_4.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test4/seq_4_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/pose_tracking/test4/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# pose tracking or global localization
+pose_tracking: true
+init_noise: [0.5, 0.5, 0.0685]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 5000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/config/pose_tracking/loc_config_test5.yml b/config/pose_tracking/loc_config_test5.yml
new file mode 100644
index 0000000..8fd2b08
--- /dev/null
+++ b/config/pose_tracking/loc_config_test5.yml
@@ -0,0 +1,59 @@
+# Configuration for Range-image-based Monte Carlo Localization
+
+# Inputs
+# Mesh map file
+map_file: 'data/ipblab/occmap.npy'
+map_res: 0.05
+# poses used to build the map
+map_pose_file: 'data/ipblab/loc_test/test5/seq_5.json'
+# ground truth poses file
+data_file: 'data/ipblab/loc_test/test5/seq_5.json'
+# downsample beams:
+max_beams: 32
+# Transformation from LiDAR to Robot
+T_b2l: 'data/ipblab/loc_test/b2l.txt'
+# the size of map: [min_x, max_x, min_y, max_y]
+map_size: [-15, 17.5, -12.5, 5]
+
+# Evaluation
+gt_file: 'data/ipblab/loc_test/test5/seq_5_gt_pose.txt'
+
+# Output
+# path to save localization results
+result_path: 'results/ipblab/pose_tracking/test5/loc_results.npz'
+# plot the final localization results
+plot_loc_results: True
+
+
+sensor_model: 'nog'
+# NOF parameters
+L_pos: 10
+feature_size: 256
+use_skip: True
+N_samples: 256
+chunk: 266496 # 1041*256
+use_disp: False
+ckpt_path: 'config/ipblab_nof_weights.ckpt'
+# NOG
+nog_size: [-50, 50, -50, 50]
+nog_res: 0.05
+
+# MCL parameters
+# pose tracking or global localization
+pose_tracking: true
+init_noise: [0.5, 0.5, 0.0685]
+# start frame index
+start_idx: 0
+# resolution of grid still in use for initialization
+grid_res: 1
+# number of particles
+numParticles: 5000
+# when the number of occupied tile map is smaller than the threshold,
+# we say the localization converged.
+converge_thres: 2
+# after convergence, we reduce the number of particles
+# and using only num_reduced particles.
+num_reduced: 5000
+# visualize the localization results online
+visualize: false
+
diff --git a/environment.yml b/environment.yml
new file mode 100644
index 0000000..99f4457
--- /dev/null
+++ b/environment.yml
@@ -0,0 +1,134 @@
+name: irmcl
+channels:
+ - conda-forge
+ - defaults
+dependencies:
+ - _libgcc_mutex=0.1=main
+ - _openmp_mutex=5.1=1_gnu
+ - ca-certificates=2022.12.7=ha878542_0
+ - certifi=2022.12.7=pyhd8ed1ab_0
+ - ld_impl_linux-64=2.38=h1181459_1
+ - libffi=3.3=he6710b0_2
+ - libgcc-ng=11.2.0=h1234567_1
+ - libgomp=11.2.0=h1234567_1
+ - libstdcxx-ng=11.2.0=h1234567_1
+ - ncurses=6.4=h6a678d5_0
+ - openssl=1.1.1s=h7f8727e_0
+ - pip=22.3.1=py39h06a4308_0
+ - pybind11=2.6.1=py39h081fc7a_0
+ - python=3.9.13=haa1d7c7_2
+ - python_abi=3.9=2_cp39
+ - readline=8.2=h5eee18b_0
+ - setuptools=65.6.3=py39h06a4308_0
+ - sqlite=3.40.1=h5082296_0
+ - tk=8.6.12=h1ccaba5_0
+ - tzdata=2022g=h04d1e81_0
+ - wheel=0.37.1=pyhd3eb1b0_0
+ - xz=5.2.10=h5eee18b_1
+ - zlib=1.2.13=h5eee18b_0
+ - pip:
+ - --extra-index-url https://download.pytorch.org/whl/cu116
+ - addict==2.4.0
+ - aiohttp==3.8.3
+ - aiosignal==1.3.1
+ - argcomplete==2.0.0
+ - asttokens==2.2.1
+ - async-timeout==4.0.2
+ - attrs==22.2.0
+ - backcall==0.2.0
+ - charset-normalizer==2.1.1
+ - click==8.1.3
+ - colorama==0.4.6
+ - comm==0.1.2
+ - configargparse==1.5.3
+ - contourpy==1.0.7
+ - cycler==0.11.0
+ - dash==2.8.1
+ - dash-core-components==2.0.0
+ - dash-html-components==2.0.0
+ - dash-table==5.0.0
+ - debugpy==1.6.6
+ - decorator==5.1.1
+ - evo==1.21.0
+ - executing==1.2.0
+ - fastjsonschema==2.16.2
+ - flask==2.2.2
+ - fonttools==4.38.0
+ - frozenlist==1.3.3
+ - fsspec==2023.1.0
+ - idna==3.4
+ - importlib-metadata==6.0.0
+ - ipykernel==6.21.1
+ - ipython==8.9.0
+ - ipywidgets==8.0.4
+ - itsdangerous==2.1.2
+ - jedi==0.18.2
+ - jinja2==3.1.2
+ - joblib==1.2.0
+ - jsonschema==4.17.3
+ - jupyter-client==8.0.2
+ - jupyter-core==5.2.0
+ - jupyterlab-widgets==3.0.5
+ - kiwisolver==1.4.4
+ - lightning-utilities==0.6.0.post0
+ - lz4==4.3.2
+ - markupsafe==2.1.2
+ - matplotlib==3.6.3
+ - matplotlib-inline==0.1.6
+ - multidict==6.0.4
+ - natsort==8.2.0
+ - nbformat==5.5.0
+ - nest-asyncio==1.5.6
+ - numexpr==2.8.4
+ - numpy==1.24.1
+ - open3d==0.16.0
+ - packaging==23.0
+ - pandas==1.5.3
+ - parso==0.8.3
+ - pexpect==4.8.0
+ - pickleshare==0.7.5
+ - pillow==9.4.0
+ - platformdirs==2.6.2
+ - plotly==5.13.0
+ - prompt-toolkit==3.0.36
+ - protobuf==3.20.1
+ - psutil==5.9.4
+ - ptyprocess==0.7.0
+ - pure-eval==0.2.2
+ - pybind11-global==2.6.1
+ - pygments==2.14.0
+ - pyparsing==3.0.9
+ - pyquaternion==0.9.9
+ - pyrsistent==0.19.3
+ - python-dateutil==2.8.2
+ - pytorch-lightning==1.9.0
+ - pytz==2022.7.1
+ - pyyaml==6.0
+ - pyzmq==25.0.0
+ - requests==2.28.2
+ - rosbags==0.9.14
+ - ruamel-yaml==0.17.21
+ - ruamel-yaml-clib==0.2.7
+ - scikit-learn==1.2.1
+ - scipy==1.10.0
+ - seaborn==0.12.2
+ - six==1.16.0
+ - stack-data==0.6.2
+ - tenacity==8.1.0
+ - tensorboardx==2.5.1
+ - threadpoolctl==3.1.0
+ - torch==1.13.1+cu116
+ - torchmetrics==0.11.1
+ - torchvision==0.14.1+cu116
+ - tornado==6.2
+ - tqdm==4.64.1
+ - traitlets==5.9.0
+ - typing-extensions==4.4.0
+ - urllib3==1.26.14
+ - wcwidth==0.2.6
+ - werkzeug==2.2.2
+ - widgetsnbextension==4.0.5
+ - yarl==1.8.2
+ - zipp==3.12.0
+ - zstandard==0.19.0
+prefix: /home/ipb-hk/anaconda3/envs/irmcl
diff --git a/eval.py b/eval.py
new file mode 100644
index 0000000..38306e7
--- /dev/null
+++ b/eval.py
@@ -0,0 +1,131 @@
+import argparse
+from torch.utils.data import DataLoader
+
+from tqdm import tqdm
+
+from nof.dataset import IPB2DMapping
+from nof.networks import Embedding, NOF
+from nof.render import render_rays
+from nof.nof_utils import load_ckpt, decode_batch
+from nof.criteria.metrics import *
+
+
+def get_opts():
+ parser = argparse.ArgumentParser()
+
+ # data
+ parser.add_argument('--root_dir', type=str,
+ default='~/ir-mcl/data/ipblab',
+ help='root directory of dataset')
+
+ # data
+ parser.add_argument('--ckpt_path', type=str, default=None,
+ help='pretrained checkpoint path to load')
+ parser.add_argument('--chunk', type=int, default=32 * 1024,
+ help='chunk size to split the input to avoid OOM')
+ parser.add_argument('--N_samples', type=int, default=64,
+ help='number of coarse samples')
+ parser.add_argument('--use_disp', default=False, action="store_true",
+ help='use disparity depth sampling')
+ parser.add_argument('--perturb', type=float, default=0.0,
+ help='factor to perturb depth sampling points')
+ parser.add_argument('--noise_std', type=float, default=0.0,
+ help='std dev of noise added to regularize sigma')
+ parser.add_argument('--L_pos', type=int, default=10,
+ help='the frequency of the positional encoding.')
+
+ # model
+ parser.add_argument('--feature_size', type=int, default=256,
+ help='the dimension of the feature maps.')
+ parser.add_argument('--use_skip', default=False, action="store_true",
+ help='use skip architecture')
+
+ return parser.parse_args()
+
+
+def error_metrics(pred, gt, rays, valid_mask_gt):
+ # ranges to pointclouds
+ rays_o, rays_d = rays[:, :2], rays[:, 2:4]
+ pred_pts = torch.column_stack((rays_o + rays_d * pred.unsqueeze(-1),
+ torch.zeros(pred.shape[0])))
+ gt_pts = torch.column_stack((rays_o + rays_d * gt.unsqueeze(-1),
+ torch.zeros(gt.shape[0])))
+
+ abs_error_ = abs_error(pred, gt, valid_mask_gt)
+ acc_thres_ = acc_thres(pred, gt, valid_mask_gt)
+ cd, fscore = eval_points(pred_pts, gt_pts, valid_mask_gt)
+
+ return abs_error_, acc_thres_, cd, fscore
+
+
+def summary_errors(errors):
+ print("\nError Evaluation")
+ errors = torch.Tensor(errors)
+
+ # Mean Errors
+ mean_errors = errors.mean(0)
+
+ print(("\t{:>8}" * 4).format("Avg. Error", "Acc", "CD", "F"))
+ print(("\t{: 8.2f}" * 4).format(*mean_errors.tolist()))
+
+ print("\n-> Done!")
+
+
+if __name__ == '__main__':
+ ######################## Step 0: Define ########################
+ hparams = get_opts()
+
+ use_cuda: bool = torch.cuda.is_available()
+ device = torch.device('cuda' if use_cuda else 'cpu')
+ print('Using: ', device)
+
+ ######################## Step 1: Create Dataset and Model ########################
+ print("\nLoading Model and Data")
+ # define models
+ embedding_position = Embedding(in_channels=2, N_freq=hparams.L_pos)
+
+ # Define NOF model
+ nof_model = NOF(feature_size=hparams.feature_size,
+ in_channels_xy=2 + 2 * hparams.L_pos * 2,
+ use_skip=hparams.use_skip)
+
+ # loading pretrained weights
+ nof_ckpt = hparams.ckpt_path
+ load_ckpt(nof_model, nof_ckpt, model_name='nof')
+ nof_model.to(device).eval()
+
+ dataset = IPB2DMapping(hparams.root_dir, split='test')
+ print("-> Done!")
+
+ ######################## Step 2: Evaluate Synthesis Scan ########################
+ errors = []
+
+ dataloader = DataLoader(dataset, shuffle=False, num_workers=4,
+ batch_size=1, pin_memory=True)
+
+ print("\nSynthesis scans with NOF")
+ for data in tqdm(dataloader):
+ rays, ranges = decode_batch(data)
+ rays = rays.squeeze() # shape: (N_beams, 6)
+ ranges = ranges.squeeze() # shape: (N_beams,)
+ valid_mask_gt = data['valid_mask_gt'].squeeze() # shape: (N_beams,)
+
+ rays = rays.to(device)
+ ranges = ranges.to(device)
+
+ with torch.no_grad():
+ rendered_rays = render_rays(
+ model=nof_model, embedding_xy=embedding_position, rays=rays,
+ N_samples=hparams.N_samples, use_disp=hparams.use_disp, perturb=hparams.perturb,
+ noise_std=hparams.noise_std, chunk=hparams.chunk
+ )
+
+ pred = rendered_rays['depth'].cpu()
+ gt = ranges.cpu()
+ errors.append(error_metrics(pred, gt, rays.cpu(), valid_mask_gt))
+
+ print("-> Done!")
+
+ # calculate errors
+ summary_errors(errors)
+
diff --git a/loc_demo.py b/loc_demo.py
new file mode 100644
index 0000000..a848361
--- /dev/null
+++ b/loc_demo.py
@@ -0,0 +1,56 @@
+import argparse
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from utils import load_data
+from vis_loc_result import plot_traj_result, vis_offline
+
+
+def get_args():
+ parser = argparse.ArgumentParser()
+
+ # results
+ parser.add_argument('--loc_results', type=str,
+ default='results/fr079/loc_results.npz',
+ help='the file path of localization results')
+
+ # map
+ parser.add_argument('--occ_map', type=str, default=None,
+ help='the file path of the occupancy grid map for visualization.')
+ parser.add_argument('--map_size', nargs='+', type=float, default=[-25, 20, -10, 10],
+ help='the size of the map.')
+
+ # output GIF
+ parser.add_argument('--output_gif', type=str,
+ default=None,
+ help='the GIF path for saving the localization process.')
+
+ return parser.parse_args()
+
+
+if __name__ == '__main__':
+ args = get_args()
+
+ # loading localization results
+ loc_results = np.load(args.loc_results)
+
+ poses_gt = loc_results['poses_gt']
+ particles = loc_results['particles']
+ start_idx = loc_results['start_idx']
+ numParticles = loc_results['numParticles']
+
+ if args.occ_map:
+ occ_map = np.load(args.occ_map)
+ else:
+ occ_map = None
+
+ grid_res = 1
+ offset = 284
+
+ if args.output_gif:
+ mapsize = args.map_size
+ anim = vis_offline(particles, poses_gt, poses_gt, mapsize, occ_map=occ_map,
+ numParticles=numParticles, grid_res=grid_res, start_idx=start_idx)
+ anim.save(args.output_gif, fps=10)
+ plt.show()
diff --git a/main.py b/main.py
new file mode 100644
index 0000000..da4b735
--- /dev/null
+++ b/main.py
@@ -0,0 +1,171 @@
+"""The main function for global localization experiments
+Code partially borrowed from
+https://github.com/PRBonn/range-mcl/blob/main/src/main_range_mcl.py.
+MIT License
+Copyright (c) 2021 Xieyuanli Chen, Ignacio Vizzo, Thomas Läbe, Jens Behley,
+Cyrill Stachniss, Photogrammetry and Robotics Lab, University of Bonn.
+"""
+
+import argparse
+import os
+import yaml
+import time
+import numpy as np
+import matplotlib.pyplot as plt
+
+from utils import load_data, summary_loc
+from mcl.initialization import init_particles_pose_tracking, init_particles_uniform
+from mcl.motion_model import gen_commands_srrg
+from mcl.sensor_model import SensorModel
+from mcl.srrg_utils.pf_library.pf_utils import PfUtils
+from mcl.vis_loc_result import plot_traj_result
+from mcl.visualizer import Visualizer
+
+
+def get_args():
+ parser = argparse.ArgumentParser()
+
+ parser.add_argument('--config_file', type=str,
+ default='~/ir-mcl/config/loc_config_test1.yml',
+ help='the path for the configuration file.')
+
+ return parser.parse_args()
+
+
+if __name__ == '__main__':
+ args = get_args()
+ # load config file
+ config_filename = args.config_file
+ config = yaml.safe_load(open(config_filename))
+
+ # load parameters
+ start_idx = config['start_idx']
+ grid_res = config['grid_res']
+ numParticles = config['numParticles']
+ reduced_num = config['num_reduced']
+ visualize = config['visualize']
+ result_path = config['result_path']
+
+ # load input data
+ map_pose_file = config['map_pose_file']
+ data_file = config['data_file']
+ mapsize = config['map_size']
+
+ print('\nLoading data......')
+ t = time.time()
+ # load poses
+ timestamps_mapping, map_poses, _, _, _ = load_data(map_pose_file)
+ timestamps_gt, poses_gt, odoms, scans, params = \
+ load_data(data_file, max_beams=config['max_beams'])
+
+ if config['T_b2l']:
+ T_b2l = np.loadtxt(config['T_b2l'])
+ else:
+ T_b2l = None
+
+ # loading the occupancy grid map for visualization
+ occmap = np.load(config['map_file'])
+
+ print('All data are loaded! Time consume: {:.2f}s'.format(time.time() - t))
+
+ # load parameters of sensor model
+ params.update({'map_file': config['map_file']})
+ params.update({'map_res': config['map_res']})
+
+ params.update({'sensor_model': config['sensor_model']})
+ params.update({'L_pos': config['L_pos']})
+ params.update({'feature_size': config['feature_size']})
+ params.update({'use_skip': config['use_skip']})
+ params.update({'N_samples': config['N_samples']})
+ params.update({'chunk': config['chunk']})
+ params.update({'use_disp': config['use_disp']})
+ params.update({'ckpt_path': config['ckpt_path']})
+ params.update({'nog_size': config['nog_size']})
+ params.update({'nog_res': config['nog_res']})
+
+ # initialize particles
+ print('\nMonte Carlo localization initializing...')
+ # map_size, road_coords = gen_coords_given_poses(map_poses)
+ if config['pose_tracking']:
+ init_noise = config['init_noise']
+ particles = init_particles_pose_tracking(
+ numParticles, poses_gt[start_idx], noises=init_noise)
+ else:
+ particles = init_particles_uniform(mapsize, numParticles)
+
+ # initialize sensor model
+ # load parameters of sensor model
+ print("\nSetting up the Sensor Model......")
+ sensor_model = SensorModel(scans, params, mapsize)
+ update_weights = sensor_model.update_weights
+ print("Sensor Model Setup Successfully!\n")
+
+ # generate odom commands
+ commands = gen_commands_srrg(odoms)
+ srrg_utils = PfUtils()
+
+ # initialize a visualizer
+ if visualize:
+ plt.ion()
+ visualizer = Visualizer(mapsize, poses=poses_gt, map_poses=map_poses, occ_map=occmap,
+ odoms=odoms, grid_res=grid_res, start_idx=start_idx)
+
+ # Starts irmcl
+ results = np.full((len(poses_gt), numParticles, 4), 0, np.float32)
+ # add a small offset to avoid showing the meaning less estimations before convergence
+ offset = 0
+ is_converged = False
+
+ for frame_idx in range(start_idx, len(poses_gt)):
+ curr_timestamp = timestamps_gt[frame_idx]
+ start = time.time()
+
+ # only update while the robot moves
+ if np.linalg.norm(commands[frame_idx]) > 1e-8:
+ # motion model
+ particles = srrg_utils.motion_model(particles, commands[frame_idx])
+
+ # IR-based sensor model
+ particles = update_weights(particles, frame_idx, T_b2l=T_b2l)
+
+ if curr_timestamp > 20 and not is_converged:
+ is_converged = True
+ offset = frame_idx
+ print('Initialization is finished!')
+ # cutoff redundant particles and leave only num of particles
+ idxes = np.argsort(particles[:, 3])[::-1]
+ particles = particles[idxes[:reduced_num]]
+
+ # resampling
+ particles = srrg_utils.resample(particles)
+
+ cost_time = np.round(time.time() - start, 10)
+ print('finished frame {} at timestamp {:.2f}s with time cost: {}s'.format(
+ frame_idx, timestamps_gt[frame_idx], cost_time))
+
+ curr_numParticles = particles.shape[0]
+ results[frame_idx, :curr_numParticles] = particles
+
+ if visualize:
+ visualizer.update(frame_idx, particles)
+ visualizer.fig.canvas.draw()
+ visualizer.fig.canvas.flush_events()
+
+
+ # evaluate localization results (through evo)
+ if not os.path.exists(os.path.dirname(result_path)):
+ os.makedirs(os.path.dirname(result_path))
+
+ result_dir = os.path.dirname(result_path)
+ summary_loc(results, start_idx, numParticles,
+ timestamps_gt, result_dir, config['gt_file'])
+
+ np.savez_compressed(result_path, timestamps=timestamps_gt, odoms=odoms, poses_gt=poses_gt,
+ particles=results, start_idx=start_idx, numParticles=numParticles)
+
+ print('save the localization results at:', result_path)
+
+ if config['plot_loc_results']:
+ plot_traj_result(results, poses_gt, grid_res=grid_res, occ_map=occmap,
+ numParticles=numParticles, start_idx=start_idx + offset)
+
diff --git a/mcl/CMakeLists.txt b/mcl/CMakeLists.txt
new file mode 100644
index 0000000..e74437c
--- /dev/null
+++ b/mcl/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.16)
+project(pf_library)
+
+# Set build type
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+set(CMAKE_CXX_STANDARD 17)
+
+find_package(Eigen3 REQUIRED)
+find_package(pybind11 REQUIRED)
+
+add_subdirectory(srrg_utils/pf_library/pybind)
diff --git a/mcl/Makefile b/mcl/Makefile
new file mode 100644
index 0000000..99ab7c5
--- /dev/null
+++ b/mcl/Makefile
@@ -0,0 +1,2 @@
+install:
+ pip3 -v install -e . && cp build/*/compile_commands.json build/
diff --git a/mcl/initialization.py b/mcl/initialization.py
new file mode 100644
index 0000000..b85a828
--- /dev/null
+++ b/mcl/initialization.py
@@ -0,0 +1,53 @@
+"""The functions for particles initialization
+Code partially borrowed from
+https://github.com/PRBonn/range-mcl/blob/main/src/initialization.py.
+MIT License
+Copyright (c) 2021 Xieyuanli Chen, Ignacio Vizzo, Thomas Läbe, Jens Behley,
+Cyrill Stachniss, Photogrammetry and Robotics Lab, University of Bonn.
+"""
+
+import numpy as np
+
+np.random.seed(0)
+
+
+def init_particles_uniform(map_size, numParticles):
+ """ Initialize particles uniformly.
+ Args:
+ map_size: size of the map.
+ numParticles: number of particles.
+ Return:
+ particles.
+ """
+ [x_min, x_max, y_min, y_max] = map_size
+ particles = []
+ rand = np.random.rand
+ for i in range(numParticles):
+ x = (x_max - x_min) * rand(1).item() + x_min
+ y = (y_max - y_min) * rand(1).item() + y_min
+ theta = -np.pi + 2 * np.pi * rand(1).item()
+ weight = 1
+ particles.append([x, y, theta, weight])
+
+ return np.array(particles)
+
+
+def init_particles_pose_tracking(numParticles, init_pose, noises=[2, 2, np.pi / 6.0], init_weight=1.0):
+ """ Initialize particles with a noisy initial pose.
+ Here, we use ground truth pose with noises defaulted as [±5 meters, ±5 meters, ±π/6 rad]
+ to mimic a non-accurate GPS information as a coarse initial guess of the global pose.
+ Args:
+ numParticles: number of particles.
+ init_pose: initial pose.
+ noises: range of noises.
+ init_weight: initialization weight.
+ Return:
+ particles.
+ """
+ mu = np.array(init_pose)
+ cov = np.diag(noises)
+ particles = np.random.multivariate_normal(mean=mu, cov=cov, size=numParticles)
+ init_weights = np.ones((numParticles, 1)) * init_weight
+ particles = np.hstack((particles, init_weights))
+
+ return np.array(particles, dtype=float)
diff --git a/mcl/motion_model.py b/mcl/motion_model.py
new file mode 100644
index 0000000..1d8360c
--- /dev/null
+++ b/mcl/motion_model.py
@@ -0,0 +1,183 @@
+"""The functions for the motion model of mobile robot
+Code partially borrowed from
+https://github.com/PRBonn/range-mcl/blob/main/src/motion_model.py.
+MIT License
+Copyright (c) 2021 Xieyuanli Chen, Ignacio Vizzo, Thomas Läbe, Jens Behley,
+Cyrill Stachniss, Photogrammetry and Robotics Lab, University of Bonn.
+"""
+
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+
+def sample(b):
+ tot = 0
+ for i in range(12):
+ tot += np.random.uniform(-b, b)
+
+ tot = tot / 2
+ return tot
+
+
+def motion_model(particles, u, alpha=[0.0, 0.0, 0.0, 0.0], real_command=False, duration=0.1):
+ """ MOTION performs the sampling from the proposal.
+ distribution, here the rotation-translation-rotation motion model
+
+ input:
+ particles: the particles as in the main script
+ u: the command in the form [rot1 trasl rot2] or real odometry [v, w]
+ noise: the variances for producing the Gaussian noise for
+ perturbating the motion, noise = [noiseR1 noiseTrasl noiseR2]
+
+ output:
+ the same particles, with updated poses.
+
+ The position of the i-th particle is given by the 3D vector
+ particles(i).pose which represents (x, y, theta).
+
+ Assume Gaussian noise in each of the three parameters of the motion model.
+ These three parameters may be used as standard deviations for sampling.
+ """
+ num_particles = len(particles)
+ if not real_command:
+ # Sample-based Odometry Model
+ alpha_1, alpha_2, alpha_3, alpha_4 = alpha
+ rot1, trans, rot2 = u
+
+ rot1_hat = rot1 + sample(np.repeat(alpha_1 * np.abs(rot1) + alpha_2 * trans, num_particles))
+ trans_hat = trans + sample(np.repeat(alpha_3 * trans + alpha_4 * (np.abs(rot1) + np.abs(rot2)), num_particles))
+ rot2_hat = rot2 + sample(np.repeat(alpha_1 * np.abs(rot2) + alpha_2 * trans, num_particles))
+
+ # update pose using motion model
+ particles[:, 0] += trans_hat * np.cos(particles[:, 2] + rot1_hat)
+ particles[:, 1] += trans_hat * np.sin(particles[:, 2] + rot1_hat)
+ particles[:, 2] += rot1_hat + rot2_hat
+
+ else: # use real commands with duration
+ # noise in the [v, w] commands when moving the particles
+ MOTION_NOISE = [0.05, 0.05]
+ vNoise = MOTION_NOISE[0]
+ wNoise = MOTION_NOISE[1]
+
+ # use the Gaussian noise to simulate the noise in the motion model
+ v = u[0] + vNoise * np.random.randn(num_particles)
+ w = u[1] + wNoise * np.random.randn(num_particles)
+ gamma = wNoise * np.random.randn(num_particles)
+
+ # update pose using motion models
+ particles[:, 0] += - v / w * np.sin(particles[:, 2]) + v / w * np.sin(particles[:, 2] + w * duration)
+ particles[:, 1] += v / w * np.cos(particles[:, 2]) - v / w * np.cos(particles[:, 2] + w * duration)
+ particles[:, 2] += w * duration + gamma * duration
+
+ return particles
+
+
+def gen_commands(poses, r1_noisy, d_noisy, r2_noisy):
+ """ Create commands out of the ground truth with noise.
+ input:
+ ground truth poses and noisy coefficients
+
+ output:
+ commands for each frame.
+ """
+ # compute noisy-free commands
+ # set the default command = [0,0,0]'
+ commands = np.zeros((len(poses), 3))
+
+ # compute relative poses
+ headings = poses[:, 2]
+
+ dx = (poses[1:, 0] - poses[:-1, 0])
+ dy = (poses[1:, 1] - poses[:-1, 1])
+
+ direct = np.arctan2(dy, dx) # atan2(dy, dx), 1X(S-1) direction of the movement
+
+ r1 = []
+ r2 = []
+ distance = []
+
+ for idx in range(len(poses) - 1):
+ r1.append(direct[idx] - headings[idx])
+ r2.append(headings[idx + 1] - direct[idx])
+ distance.append(np.sqrt(dx[idx] * dx[idx] + dy[idx] * dy[idx]))
+
+ r1 = np.array(r1)
+ r2 = np.array(r2)
+ distance = np.array(distance)
+
+ # add noise to commands
+ commands_ = np.c_[r1, distance, r2]
+ commands[1:] = commands_ + np.array([r1_noisy * np.random.randn(len(commands_)),
+ d_noisy * np.random.randn(len(commands_)),
+ r2_noisy * np.random.randn(len(commands_))]).T
+
+ return commands
+
+
+def odom2matrix(pose):
+ """
+ Generate 2D transformation from pose, the rotation counterclockwise about the origin
+ :param pose: shape: (3,)
+ :return:
+ """
+ odom_x, odom_y, odom_theta = pose
+ matrix = [[np.cos(odom_theta), -np.sin(odom_theta), 0.0, odom_x],
+ [np.sin(odom_theta), np.cos(odom_theta), 0.0, odom_y],
+ [0.0, 0.0, 1.0, 0.0],
+ [0.0, 0.0, 0.0, 1.0]]
+
+ return np.array(matrix)
+
+
+def matrix2odom(T):
+ x, y = T[:2, -1]
+ _, _, yaw = R.from_matrix(T[:3, :3]).as_euler('xyz', degrees=False)
+
+ return np.array([x, y, yaw])
+
+
+def gen_commands_srrg(poses):
+ """ Create commands from the Odometer.
+ input:
+ odometer reading
+
+ output:
+ commands for each frame.
+ """
+ # compute noisy-free commands
+ # set the default command = [0,0,0]'
+ commands = np.zeros((len(poses), 3))
+
+ for idx in range(1, len(poses)):
+ T_last = odom2matrix(poses[idx - 1])
+ T_current = odom2matrix(poses[idx])
+ T_rel = np.linalg.inv(T_last) @ T_current
+ commands[idx] = matrix2odom(T_rel)
+
+ return commands
+
+
+def gen_motion_reckon(commands):
+ """ Generate motion reckon only for comparison.
+ """
+
+ particle = [0, 0, 0, 1]
+ motion_reckon = []
+ for cmmand in commands:
+ # use the Gaussian noise to simulate the noise in the motion model
+ rot1 = cmmand[0]
+ tras1 = cmmand[1]
+ rot2 = cmmand[2]
+
+ # update pose using motion model
+ particle[0] = particle[0] + tras1 * np.cos(particle[2] + rot1)
+ particle[1] = particle[1] + tras1 * np.sin(particle[2] + rot1)
+ particle[2] = particle[2] + rot1 + rot2
+
+ motion_reckon.append([particle[0], particle[1]])
+
+ return np.array(motion_reckon)
+
+
+if __name__ == '__main__':
+ pass
diff --git a/mcl/rendering.py b/mcl/rendering.py
new file mode 100644
index 0000000..1e89055
--- /dev/null
+++ b/mcl/rendering.py
@@ -0,0 +1,176 @@
+"""The classes for rendering 2D LiDAR scan of particles
+"""
+
+import time
+
+import numpy as np
+
+import torch
+
+from nof.dataset.ray_utils import get_rays
+from nof.render import render_rays, render_rays_grid
+from nof.networks.models import NOF, Embedding
+from nof.nof_utils import load_ckpt
+
+
+class NOFRendering:
+ """
+ Rendering novel views with Neural Occupancy Field
+ """
+
+ def __init__(self, directions, near, far,
+ ckpt_path, L_pos=10, feature_size=256, use_skip=True,
+ N_samples=256, chunk=32 * 1024, use_disp=False,
+ device=torch.device('cpu')):
+ # lidar params
+ self.directions = directions
+ self.near = near
+ self.far = far
+
+ # models
+ self.device = device
+ self.embedding_position = Embedding(in_channels=2, N_freq=L_pos)
+ self.nof_model = NOF(feature_size=feature_size,
+ in_channels_xy=2 + 2 * L_pos * 2,
+ use_skip=use_skip)
+ # loading pretrained weights
+ load_ckpt(self.nof_model, ckpt_path, model_name='nof')
+ self.nof_model.to(self.device).eval()
+
+ # rendering params
+ self.N_samples = N_samples
+ self.chunk = chunk
+ self.use_disp = use_disp
+
+ def get_laser_rays(self, pose):
+ # load range reading
+ rays_o, rays_d = get_rays(self.directions, pose)
+ rays_o = torch.FloatTensor(rays_o.copy())
+ rays_d = torch.FloatTensor(rays_d.copy())
+
+ near, far = self.near * torch.ones_like(rays_d[..., :1]), \
+ self.far * torch.ones_like(rays_d[..., :1])
+ rays = torch.cat([rays_o, rays_d, near, far], -1)
+
+ return rays
+
+ def render(self, poses):
+ preds = []
+ for pose in poses:
+ # data processing
+ rays = self.get_laser_rays(pose).to(self.device)
+
+ with torch.no_grad():
+ rendered_rays = render_rays(
+ model=self.nof_model, embedding_xy=self.embedding_position, rays=rays,
+ N_samples=self.N_samples, use_disp=self.use_disp, chunk=self.chunk
+ )
+
+ preds.append(rendered_rays['depth'].cpu().numpy())
+
+ preds = np.array(preds)
+
+ return preds
+
+
+class NOGRendering:
+ """
+ Rendering novel views with Neural Occupancy Grid
+ """
+
+ def __init__(self, directions, near, far, ckpt_path,
+ L_pos=10, feature_size=256, use_skip=True, N_samples=256,
+ map_size=[-50, 50, -50, 50], grid_res=0.05, device=torch.device('cpu')):
+ # lidar params
+ self.directions = directions
+ self.near = near
+ self.far = far
+
+ # models
+ print("\nInitialize the Neural Occupancy Field model......")
+ t = time.time()
+ self.device = device
+ self.embedding_position = Embedding(in_channels=2, N_freq=L_pos)
+ self.nof_model = NOF(feature_size=feature_size,
+ in_channels_xy=2 + 2 * L_pos * 2, use_skip=use_skip)
+ # loading pretrained weights
+ load_ckpt(self.nof_model, ckpt_path, model_name='nof')
+ self.embedding_position.to(self.device).eval()
+ self.nof_model.to(self.device).eval()
+ print("Models are ready! Time consume: {:.2f}s".format(time.time() - t))
+
+ # generate grid
+ self.grid = None
+ self.map_size = map_size
+ self.grid_res = grid_res
+ self.get_grid()
+
+ # rendering params
+ self.N_samples = N_samples
+
+ def get_grid(self):
+ map_size = np.array(self.map_size)
+ x_steps = int((map_size[1] - map_size[0]) / self.grid_res)
+ y_steps = int((map_size[3] - map_size[2]) / self.grid_res)
+
+ # generate grids of coordinates
+ x = torch.linspace(map_size[0], map_size[1] - self.grid_res, steps=x_steps)
+ y = torch.linspace(map_size[2], map_size[3] - self.grid_res, steps=y_steps)
+
+ grid_x, grid_y = torch.meshgrid(x, y)
+ grid = torch.stack([grid_x, grid_y], dim=-1).reshape(-1, 2).to(self.device)
+
+ print('\nPredicting the occupancy probability for each cells of grid......')
+ t = time.time()
+
+ # inference
+ with torch.no_grad():
+ N_cells = grid.shape[0]
+ chunk = 2500000
+ out_grid = []
+ for i in range(0, N_cells, chunk):
+ out_grid.append(self.nof_model(self.embedding_position(grid[i:i + chunk])))
+ self.grid = torch.cat(out_grid, dim=0).reshape(x_steps, y_steps)
+ # # create grid will use amount of GPU memory, it should be released from the cache.
+ torch.cuda.empty_cache()
+ print("Inference Finished! Time consume: {:.2f}s".format(time.time() - t))
+
+ def _get_rays(self, Ts_w2l):
+ # rays_ds: shape (N_rays, 2, N_particles)
+ rays_ds = np.einsum('ij, jkn->ikn',
+ self.directions, Ts_w2l[:, :2, :].transpose(1, 0, 2))
+
+ # normalize direction vector
+ rays_ds = rays_ds / np.linalg.norm(rays_ds, axis=1, keepdims=True)
+
+ # The origin of all rays is the camera origin in world coordinate
+ rays_os = np.broadcast_to(Ts_w2l[:, 2, :], rays_ds.shape)
+
+ rays_ds = rays_ds.transpose((2, 0, 1))
+ rays_os = rays_os.transpose((2, 0, 1))
+
+ return rays_os, rays_ds
+
+ def get_laser_rays(self, Ts_w2l):
+ # load range reading
+ rays_os, rays_ds = self._get_rays(Ts_w2l) # shape (N_particles, N_rays, 2)
+
+ rays_os = torch.FloatTensor(rays_os.copy())
+ rays_ds = torch.FloatTensor(rays_ds.copy())
+
+ near, far = self.near * torch.ones_like(rays_ds[..., :1]), \
+ self.far * torch.ones_like(rays_ds[..., :1])
+
+ rays = torch.cat([rays_os, rays_ds, near, far], -1)
+
+ return rays
+
+ def render(self, Ts_w2l):
+ # data processing
+ rays = self.get_laser_rays(Ts_w2l).to(self.device)
+
+ preds = render_rays_grid(self.grid, self.map_size, self.grid_res,
+ rays=rays, N_samples=self.N_samples, chunk=500)
+
+ preds = preds.cpu().numpy()
+ return preds
diff --git a/mcl/sensor_model.py b/mcl/sensor_model.py
new file mode 100644
index 0000000..731c6b1
--- /dev/null
+++ b/mcl/sensor_model.py
@@ -0,0 +1,78 @@
+"""The class for the implicit representation-based observation model
+Code partially borrowed from
+https://github.com/PRBonn/range-mcl/blob/main/src/sensor_model.py.
+MIT License
+Copyright (c) 2021 Xieyuanli Chen, Ignacio Vizzo, Thomas Läbe, Jens Behley,
+Cyrill Stachniss, Photogrammetry and Robotics Lab, University of Bonn.
+"""
+
+import numpy as np
+import torch
+
+from .rendering import NOFRendering, NOGRendering
+
+
+class SensorModel:
+ def __init__(self, scans, params, map_size):
+ # load the map module.
+ self.scans = scans
+
+ use_cuda: bool = torch.cuda.is_available()
+ device = torch.device('cuda' if use_cuda else 'cpu')
+ print('Using: ', device)
+
+ self.sensor_model_type = params['sensor_model']
+
+ if self.sensor_model_type == 'nof':
+ self.model = NOFRendering(
+ directions=params['directions'], near=params['near'], far=params['far'],
+ ckpt_path=params['ckpt_path'], L_pos=params['L_pos'], feature_size=params['feature_size'],
+ use_skip=params['use_skip'], N_samples=params['N_samples'], chunk=params['chunk'],
+ use_disp=params['use_disp'], device=device
+ )
+
+ elif self.sensor_model_type == 'nog':
+ self.model = NOGRendering(
+ directions=params['directions'], near=params['near'], far=params['far'],
+ ckpt_path=params['ckpt_path'], L_pos=params['L_pos'], feature_size=params['feature_size'],
+ use_skip=params['use_skip'], N_samples=params['N_samples'], map_size=params['nog_size'],
+ grid_res=params['nog_res'], device=device)
+
+ self.map_min_x = map_size[0]
+ self.map_max_x = map_size[1]
+ self.map_min_y = map_size[2]
+ self.map_max_y = map_size[3]
+
+ def update_weights(self, particles, frame_idx, T_b2l=None):
+ current_scan = self.scans[frame_idx]
+
+ # TODO: skip the particles outside the map
+
+ particle_poses = particles[..., :-1]
+ # 1. particles_poses (N, 3) to particles_mat (N, 4, 4)
+ xs, ys, yaws = particle_poses[:, 0], particle_poses[:, 1], particle_poses[:, 2]
+ particles_mat = [[np.cos(yaws), -np.sin(yaws), np.zeros_like(xs), xs],
+ [np.sin(yaws), np.cos(yaws), np.zeros_like(xs), ys],
+ [np.zeros_like(xs), np.zeros_like(xs), np.ones_like(xs), np.zeros_like(xs)],
+ [np.zeros_like(xs), np.zeros_like(xs), np.zeros_like(xs), np.ones_like(xs)]]
+ particles_mat = np.array(particles_mat)
+
+ # 2. transfer robot poses to lidar poses: T_w2b @ T_b2l -> T_w2l
+ if T_b2l is not None:
+ particles_mat = np.einsum('ijn, jk->ikn', particles_mat, T_b2l)
+ particles_mat = particles_mat[:2, [0, 1, 3], :]
+
+ # generate synthetic laser scan
+ particle_scans = self.model.render(particles_mat)
+
+ # calculate similarity
+ # L1 Distance
+ diffs = np.abs(particle_scans - current_scan)
+ # Gaussian Kernel
+ scores = np.exp(-0.5 * np.mean(diffs[:, current_scan > 0], axis=1) ** 2 / (0.5 ** 2))
+
+ # normalize the particles' weight
+ particles[:, 3] = particles[:, 3] * scores
+ particles[:, 3] /= np.sum(particles[:, 3])
+
+ return particles
diff --git a/mcl/setup.py b/mcl/setup.py
new file mode 100644
index 0000000..9fe91cf
--- /dev/null
+++ b/mcl/setup.py
@@ -0,0 +1,85 @@
+# -*- coding: utf-8 -*-
+import multiprocessing
+import os
+import subprocess
+import sys
+
+from setuptools import Extension, find_packages, setup
+from setuptools.command.build_ext import build_ext
+
+
+class CMakeExtension(Extension):
+ def __init__(self, name, sourcedir=""):
+ Extension.__init__(self, name, sources=[])
+ self.sourcedir = os.path.abspath(sourcedir)
+
+
+class CMakeBuild(build_ext):
+ def build_extension(self, ext):
+ extdir = os.path.abspath(os.path.dirname(
+ self.get_ext_fullpath(ext.name)))
+
+ # required for auto-detection of auxiliary "native" libs
+ if not extdir.endswith(os.path.sep):
+ extdir += os.path.sep
+
+ debug = int(os.environ.get("DEBUG", 0)
+ ) if self.debug is None else self.debug
+ cfg = "Debug" if debug else "Release"
+
+ # CMake lets you override the generator - we need to check this.
+ # Can be set with Conda-Build, for example.
+ cmake_generator = os.environ.get("CMAKE_GENERATOR", "")
+
+ # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON
+ # EXAMPLE_VERSION_INFO shows you how to pass a value into the C++ code
+ # from Python.
+ cmake_args = [
+ f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={extdir}",
+ f"-DPYTHON_EXECUTABLE={sys.executable}",
+ f"-DCMAKE_BUILD_TYPE={cfg}", # not used on MSVC, but no harm
+ ]
+ build_args = []
+
+ # Adding CMake arguments set as environment variable
+ # (needed e.g. to build for ARM OSx on conda-forge)
+ if "CMAKE_ARGS" in os.environ:
+ cmake_args += [
+ item for item in os.environ["CMAKE_ARGS"].split(" ") if item]
+
+ # Single config generators are handled "normally"
+ single_config = any(x in cmake_generator for x in {"NMake", "Ninja"})
+
+ # Multi-config generators have a different way to specify configs
+ if not single_config:
+ cmake_args += [
+ f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}"]
+ build_args += ["--config", cfg]
+
+ # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level across all generators.
+ if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ:
+ # passing --global-option="build_ext" --global-option="-j8" to pip seems not to work,
+ # and launches 2 time the entire build process. Therefore if nothing has specified the
+ # parallel jobs so far, we are going to hack it here. Not the best design, but pip just
+ # doesn't seem to care about this flag, CMake 3.12+ only.
+ self.parallel = multiprocessing.cpu_count() if not self.parallel else self.parallel
+ build_args += [f"-j{self.parallel}"]
+
+ if not os.path.exists(self.build_temp):
+ os.makedirs(self.build_temp)
+
+ subprocess.check_call(["cmake", ext.sourcedir] +
+ cmake_args, cwd=self.build_temp)
+ subprocess.check_call(["cmake", "--build", "."] +
+ build_args, cwd=self.build_temp)
+
+
+setup(
+ name="pf_library",
+ version="0.0.1",
+ packages=find_packages("srrg_utils"),
+ package_dir={"": "srrg_utils"},
+ ext_modules=[CMakeExtension(
+ "pf_library.pybind.pf_library_pybind")],
+ cmdclass={"build_ext": CMakeBuild},
+)
diff --git a/mcl/srrg_utils/pf_library/pf_utils.py b/mcl/srrg_utils/pf_library/pf_utils.py
new file mode 100644
index 0000000..adcdb7e
--- /dev/null
+++ b/mcl/srrg_utils/pf_library/pf_utils.py
@@ -0,0 +1,18 @@
+import numpy as np
+from pf_library.pybind import pf_library_pybind
+
+
+class PfUtils:
+ def __init__(self):
+ self.utils_provider = pf_library_pybind._MotionModelAndResampling()
+
+ def motion_model(self, particles: np.ndarray, control: np.ndarray):
+ _positions = pf_library_pybind._Vector4dVector(particles)
+ return np.asarray(self.utils_provider._predict(_positions, control))
+
+ def resample(self, particles):
+ weights = particles[:, 3]
+ indices = np.asarray(self.utils_provider._resample_uniform(weights))
+ new_particles = particles[indices]
+ new_particles[:, 3] = 1.0
+ return new_particles
diff --git a/mcl/srrg_utils/pf_library/pybind/CMakeLists.txt b/mcl/srrg_utils/pf_library/pybind/CMakeLists.txt
new file mode 100644
index 0000000..7b8426c
--- /dev/null
+++ b/mcl/srrg_utils/pf_library/pybind/CMakeLists.txt
@@ -0,0 +1,4 @@
+add_library(pf_library pf_library.cpp)
+target_link_libraries(pf_library Eigen3::Eigen)
+pybind11_add_module(pf_library_pybind pf_library_pybind.cpp)
+target_link_libraries(pf_library_pybind PRIVATE pf_library)
diff --git a/mcl/srrg_utils/pf_library/pybind/pf_library.cpp b/mcl/srrg_utils/pf_library/pybind/pf_library.cpp
new file mode 100644
index 0000000..78301d5
--- /dev/null
+++ b/mcl/srrg_utils/pf_library/pybind/pf_library.cpp
@@ -0,0 +1,74 @@
+#include "pf_library.h"
+#include
+
+namespace pf_library {
+MotionModelAndResample::MotionModelAndResample() : rd(), gen(rd()) {
+ _noise_coeffs << 0.01, 0.0005, 0.0002, 0.0005, 0.0001, 0.0001, 0.001, 0.00001,
+ 0.05;
+}
+
+Vector4dVector
+MotionModelAndResample::predict(const Vector4dVector &particles_positions,
+ const Vector3d &control) {
+ Vector4dVector new_particles_positions(particles_positions);
+ if (control.squaredNorm() < 1e-8) {
+ return new_particles_positions;
+ }
+ Vector3d scales(fabs(control[0]), fabs(control[1]), fabs(control[2]));
+ Vector3d std_deviations = (_noise_coeffs * scales).cwiseSqrt();
+ int num_particles = new_particles_positions.size();
+ for (int p = 0; p < num_particles; ++p) {
+ Vector3d noise;
+ for (int i = 0; i < 3; ++i) {
+ noise[i] = std_deviations[i] * normal_distribution_(gen);
+ }
+ Vector3d noisy_control = control + noise;
+ const Vector3d &position = particles_positions[p].head<3>();
+ new_particles_positions[p].head<3>() =
+ t2v(v2t(position) * v2t(noisy_control));
+ }
+ return new_particles_positions;
+}
+
+std::vector
+MotionModelAndResample::resample_uniform(const std::vector &weights) {
+ double acc = 0;
+ for (const double &w : weights) {
+ acc += w;
+ }
+ double inverse_acc = 1. / acc;
+ double cumulative_value = 0;
+ int n = weights.size();
+ double step = 1. / n;
+ double threshold = step * drand48();
+ std::vector indices(n);
+ std::iota(indices.begin(), indices.end(), 0);
+ auto idx = indices.begin();
+ int k = 0;
+ for (int i = 0; i < n; i++) {
+ cumulative_value += weights[i] * inverse_acc;
+ while (cumulative_value > threshold) {
+ *idx = i;
+ idx++;
+ k++;
+ threshold += step;
+ }
+ }
+ return indices;
+}
+Vector3d MotionModelAndResample::t2v(const Isometry2d &T) {
+ Vector3d v = Vector3d::Zero();
+ v.head<2>() = T.translation();
+ const auto &R = T.linear();
+ v(2) = std::atan2(R(1, 0), R(0, 0));
+ return v;
+}
+Isometry2d MotionModelAndResample::v2t(const Vector3d &v) {
+ Isometry2d T = Isometry2d::Identity();
+ T.translation() = v.head<2>();
+ Rotation2Dd R(v(2));
+ T.linear() = R.matrix();
+ return T;
+}
+
+} // namespace pf_library
diff --git a/mcl/srrg_utils/pf_library/pybind/pf_library.h b/mcl/srrg_utils/pf_library/pybind/pf_library.h
new file mode 100644
index 0000000..443bf65
--- /dev/null
+++ b/mcl/srrg_utils/pf_library/pybind/pf_library.h
@@ -0,0 +1,26 @@
+#include
+#include
+#include
+#include
+using namespace Eigen;
+
+using Vector4dVector = std::vector;
+namespace pf_library {
+class MotionModelAndResample {
+public:
+ MotionModelAndResample();
+
+ Vector4dVector predict(const Vector4dVector &particles_positions,
+ const Vector3d &control);
+
+ std::vector resample_uniform(const std::vector &weights);
+
+protected:
+ Matrix3d _noise_coeffs;
+ Vector3d t2v(const Isometry2d &T);
+ Isometry2d v2t(const Vector3d &v);
+ std::random_device rd;
+ std::mt19937 gen;
+ std::normal_distribution normal_distribution_;
+};
+} // namespace pf_library
diff --git a/mcl/srrg_utils/pf_library/pybind/pf_library_pybind.cpp b/mcl/srrg_utils/pf_library/pybind/pf_library_pybind.cpp
new file mode 100644
index 0000000..3e7c17f
--- /dev/null
+++ b/mcl/srrg_utils/pf_library/pybind/pf_library_pybind.cpp
@@ -0,0 +1,29 @@
+#include
+#include
+#include
+#include
+#include
+
+#include "pf_library.h"
+#include "stl_vector_eigen.h"
+#include
+#include
+namespace py = pybind11;
+using namespace py::literals;
+
+PYBIND11_MAKE_OPAQUE(std::vector);
+namespace pf_library {
+PYBIND11_MODULE(pf_library_pybind, m) {
+ auto vector3dvector = pybind_eigen_vector_of_vector(
+ m, "_Vector4dVector", "std::vector",
+ py::py_array_to_vectors_double);
+
+ py::class_ pf(m, "_MotionModelAndResampling",
+ "Don't use this");
+ pf.def(py::init<>())
+ .def("_predict", &MotionModelAndResample::predict, "particles"_a,
+ "control"_a)
+ .def("_resample_uniform", &MotionModelAndResample::resample_uniform,
+ "weights"_a);
+}
+} // namespace pf_library
diff --git a/mcl/srrg_utils/pf_library/pybind/stl_vector_eigen.h b/mcl/srrg_utils/pf_library/pybind/stl_vector_eigen.h
new file mode 100644
index 0000000..9769f75
--- /dev/null
+++ b/mcl/srrg_utils/pf_library/pybind/stl_vector_eigen.h
@@ -0,0 +1,105 @@
+#pragma once
+#include
+#include
+
+// pollute namespace with py
+#include
+namespace py = pybind11;
+#include
+#include
+#include
+#include
+#include
+
+namespace pybind11 {
+
+template , typename... Args>
+py::class_ bind_vector_without_repr(py::module &m,
+ std::string const &name,
+ Args &&...args) {
+ // hack function to disable __repr__ for the convenient function
+ // bind_vector()
+ using Class_ = py::class_;
+ Class_ cl(m, name.c_str(), std::forward(args)...);
+ cl.def(py::init<>());
+ cl.def(
+ "__bool__", [](const Vector &v) -> bool { return !v.empty(); },
+ "Check whether the list is nonempty");
+ cl.def("__len__", &Vector::size);
+ return cl;
+}
+
+// - This function is used by Pybind for std::vector constructor.
+// This optional constructor is added to avoid too many Python <-> C++ API
+// calls when the vector size is large using the default biding method.
+// Pybind matches np.float64 array to py::array_t buffer.
+// - Directly using templates for the py::array_t and py::array_t
+// and etc. doesn't work. The current solution is to explicitly implement
+// bindings for each py array types.
+template
+std::vector py_array_to_vectors_double(
+ py::array_t array) {
+ int64_t eigen_vector_size = EigenVector::SizeAtCompileTime;
+ if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) {
+ throw py::cast_error();
+ }
+ std::vector eigen_vectors(array.shape(0));
+ auto array_unchecked = array.mutable_unchecked<2>();
+ for (auto i = 0; i < array_unchecked.shape(0); ++i) {
+ eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0));
+ }
+ return eigen_vectors;
+}
+
+template
+std::vector py_array_to_vectors_int(
+ py::array_t array) {
+ int64_t eigen_vector_size = EigenVector::SizeAtCompileTime;
+ if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) {
+ throw py::cast_error();
+ }
+ std::vector eigen_vectors(array.shape(0));
+ auto array_unchecked = array.mutable_unchecked<2>();
+ for (auto i = 0; i < array_unchecked.shape(0); ++i) {
+ eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0));
+ }
+ return eigen_vectors;
+}
+
+} // namespace pybind11
+
+template ,
+ typename holder_type = std::unique_ptr,
+ typename InitFunc>
+py::class_ pybind_eigen_vector_of_vector(py::module &m,
+ const std::string &bind_name,
+ const std::string &repr_name,
+ InitFunc init_func) {
+ using Scalar = typename EigenVector::Scalar;
+ auto vec = py::bind_vector_without_repr>(
+ m, bind_name, py::buffer_protocol(), py::module_local());
+ vec.def(py::init(init_func));
+ vec.def_buffer([](std::vector &v) -> py::buffer_info {
+ size_t rows = EigenVector::RowsAtCompileTime;
+ return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor::format(), 2,
+ {v.size(), rows}, {sizeof(EigenVector), sizeof(Scalar)});
+ });
+ vec.def("__repr__", [repr_name](const std::vector &v) {
+ return repr_name + std::string(" with ") + std::to_string(v.size()) +
+ std::string(" elements.\n") + std::string("Use numpy.asarray() to access data.");
+ });
+ vec.def("__copy__", [](std::vector &v) { return std::vector(v); });
+ vec.def("__deepcopy__",
+ [](std::vector &v) { return std::vector(v); });
+
+ // py::detail must be after custom constructor
+ using Class_ = py::class_>;
+ py::detail::vector_if_copy_constructible(vec);
+ py::detail::vector_if_equal_operator(vec);
+ py::detail::vector_modifiers(vec);
+ py::detail::vector_accessor(vec);
+
+ return vec;
+}
+
diff --git a/mcl/srrg_utils/test_srrg.py b/mcl/srrg_utils/test_srrg.py
new file mode 100644
index 0000000..b6ebe46
--- /dev/null
+++ b/mcl/srrg_utils/test_srrg.py
@@ -0,0 +1,13 @@
+from pf_library.pf_utils import PfUtils
+import numpy as np
+
+P_prev = np.random.randn(10000, 4)**2
+P_prev[:, 3] = P_prev[:, 3]/np.sum(P_prev[:, 3])
+control = np.array([1, 0, 0])
+utils = PfUtils()
+
+P = utils.motion_model(P_prev, control)
+
+P[0, 3] = 1
+P[1:, 3] = 0.0
+P_new = utils.resample(P)
diff --git a/mcl/vis_loc_result.py b/mcl/vis_loc_result.py
new file mode 100644
index 0000000..09f30fd
--- /dev/null
+++ b/mcl/vis_loc_result.py
@@ -0,0 +1,178 @@
+"""The functions for visualizing the localization result
+Code partially borrowed from
+https://github.com/PRBonn/range-mcl/blob/main/src/vis_loc_result.py.
+MIT License
+Copyright (c) 2021 Xieyuanli Chen, Ignacio Vizzo, Thomas Läbe, Jens Behley,
+Cyrill Stachniss, Photogrammetry and Robotics Lab, University of Bonn.
+"""
+
+import os
+import matplotlib.animation as animation
+
+import utils
+import numpy as np
+import matplotlib.pyplot as plt
+
+from .visualizer import Visualizer
+
+
+def plot_traj_result(results, poses, odoms=None, occ_map=None, numParticles=1000, grid_res=0.2, start_idx=0,
+ ratio=0.8, converge_thres=5, eva_thres=100):
+ """ Plot the final localization trajectory.
+ Args:
+ results: localization results including particles in every timestamp.
+ poses: ground truth poses.
+ odoms: odometry
+ occ_map: occupancy grid map background
+ numParticles: number of particles.
+ grid_res: the resolution of the grids.
+ start_idx: the start index.
+ ratio: the ratio of particles used to estimate the poes.
+ converge_thres: a threshold used to tell whether the localization converged or not.
+ eva_thres: a threshold to check the estimation results.
+ """
+ # get ground truth xy and yaw separately
+ gt_location = poses[start_idx:, :-1]
+ gt_heading = poses[start_idx:, -1]
+
+ if occ_map is not None:
+ map_res = 0.05
+ ox, oy = occ_map.shape[0] // 2, occ_map.shape[1] // 2
+
+ occ_set = []
+ unknow_set = []
+ X, Y = occ_map.shape
+ for x in range(X):
+ for y in range(Y):
+ if occ_map[x, y] > 0.9:
+ occ_set.append([(x - ox) * map_res, (y - oy) * map_res])
+ elif occ_map[x, y] == 0.5:
+ unknow_set.append([(x - ox) * map_res, (y - oy) * map_res])
+
+ occ_set = np.array(occ_set)
+ unknow_set = np.array(unknow_set)
+
+ estimated_traj = []
+
+ for frame_idx in range(start_idx, len(poses)):
+ particles = results[frame_idx]
+ # collect top 80% of particles to estimate pose
+ idxes = np.argsort(particles[:, 3])[::-1]
+ idxes = idxes[:int(ratio * numParticles)]
+
+ partial_particles = particles[idxes]
+ if np.sum(partial_particles[:, 3]) == 0:
+ continue
+
+ estimated_pose = utils.particles2pose(partial_particles)
+ estimated_traj.append(estimated_pose)
+
+ estimated_traj = np.array(estimated_traj)
+
+ # generate statistics for location (x, y)
+ diffs_xy = np.array(estimated_traj[:, :2] * grid_res - gt_location)
+ diffs_dist = np.linalg.norm(diffs_xy, axis=1) # diff in euclidean
+
+ # generate statistics for yaw
+ diffs_heading = np.minimum(abs(estimated_traj[:, 2] - gt_heading),
+ 2. * np.pi - abs(estimated_traj[:, 2] - gt_heading)) * 180. / np.pi
+
+ # check if every eva_thres success converged
+ if len(diffs_dist) > eva_thres and np.all(diffs_dist[eva_thres::eva_thres] < converge_thres):
+ # calculate location error
+ diffs_location = diffs_dist[eva_thres:]
+ mean_location = np.mean(diffs_location)
+ mean_square_error = np.mean(diffs_location * diffs_location)
+ rmse_location = np.sqrt(mean_square_error)
+
+ mean_heading = np.mean(diffs_heading)
+ mean_square_error_heading = np.mean(diffs_heading * diffs_heading)
+ rmse_heading = np.sqrt(mean_square_error_heading)
+
+ # print('rmse_location: ', rmse_location)
+ # print('rmse_heading: ', rmse_heading)
+
+ # plot results
+ plt.close('all')
+ fig = plt.figure(figsize=(16, 16))
+ ax = fig.add_subplot(111)
+
+ if occ_map is not None:
+ ax.plot(occ_set[:, 0], occ_set[:, 1], '.', alpha=0.5, c='black')
+ # ax.plot(unknow_set[:, 0], unknow_set[:, 1], '.', alpha=0.5, c='gray')
+
+ ax.plot(poses[:, 0], poses[:, 1], c='r', label='ground_truth')
+
+ if odoms is not None:
+ ax.plot(odoms[:, 0], odoms[:, 1], c='c', label='odometry')
+
+ ax.plot(estimated_traj[:, 0] * grid_res, estimated_traj[:, 1] * grid_res, label='estimated trajectory')
+ plt.legend()
+ plt.show()
+
+
+def save_loc_result(frame_idx, map_size, poses, particles, est_poses, results_folder):
+ """ Save the intermediate plots of localization results.
+ Args:
+ frame_idx: index of the current frame.
+ map_size: size of the map.
+ poses: ground truth poses.
+ particles: current particles.
+ est_poses: pose estimates.
+ results_folder: folder to store the plots
+ """
+ particles[:, -1] /= np.max(particles[:, -1])
+ # collect top 80% of particles to estimate pose
+ idxes = np.argsort(particles[:, 3])[::-1]
+ idxes = idxes[:int(0.8 * len(particles))]
+
+ partial_particles = particles[idxes]
+
+ normalized_weight = partial_particles[:, 3] / np.sum(partial_particles[:, 3])
+ estimated_xy = partial_particles[:, :2].T.dot(normalized_weight.T)
+ est_poses.append(estimated_xy)
+ est_traj = np.array(est_poses)
+ # plot results
+ fig = plt.figure(figsize=(16, 10))
+ ax = fig.add_subplot(111)
+
+ ax.scatter(particles[:, 0], particles[:, 1], c=particles[:, 3], cmap='Blues', s=10)
+ ax.plot(poses[:frame_idx + 1, 0], poses[:frame_idx + 1, 1], c='r', label='ground_truth')
+ ax.plot(est_traj[:, 0], est_traj[:, 1], label='weighted_mean_80%')
+
+ ax.axis('square')
+ ax.set(xlim=map_size[:2], ylim=map_size[2:])
+
+ plt.xlabel('x [m]')
+ plt.ylabel('y [m]')
+ plt.legend(loc='upper right')
+ plt.savefig(os.path.join(results_folder, str(frame_idx).zfill(6)))
+ plt.close()
+
+
+def vis_offline(results, poses, map_poses, mapsize, odoms=None, occ_map=None,
+ numParticles=1000, grid_res=0.2, start_idx=0):
+ """ Visualize localization results offline.
+ Args:
+ results: localization results including particles in every timestamp.
+ poses: ground truth poses.
+ map_poses: poses used to generate the map.
+ odoms: odometry.
+ occ_map: occupancy grid map background
+ mapsize: size of the map.
+ numParticles: number of particles.
+ grid_res: the resolution of the grids.
+ start_idx: the start index.
+ """
+ visualizer = Visualizer(mapsize, poses, map_poses, odoms=odoms, occ_map=occ_map,
+ numParticles=numParticles, grid_res=grid_res, start_idx=start_idx)
+
+ # for animation
+ anim = animation.FuncAnimation(visualizer.fig, visualizer.update_offline,
+ frames=len(poses), fargs=[results])
+
+ return anim
+
+
+if __name__ == '__main__':
+ pass
diff --git a/mcl/visualizer.py b/mcl/visualizer.py
new file mode 100644
index 0000000..d95a1a1
--- /dev/null
+++ b/mcl/visualizer.py
@@ -0,0 +1,282 @@
+"""The class for visualizing the localization result
+Code partially borrowed from
+https://github.com/PRBonn/range-mcl/blob/main/src/visualizer.py.
+MIT License
+Copyright (c) 2021 Xieyuanli Chen, Ignacio Vizzo, Thomas Läbe, Jens Behley,
+Cyrill Stachniss, Photogrammetry and Robotics Lab, University of Bonn.
+"""
+import copy
+
+import utils
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+class Visualizer(object):
+ """ This class is a visualizer for localization results.
+ """
+
+ def __init__(self, map_size, poses, map_poses, odoms=None, occ_map=None,
+ numParticles=1000, grid_res=0.2, start_idx=0, converge_thres=5):
+ """ Initialization:
+ mapsize: the size of the given map
+ poses: ground truth poses.
+ map_poses: poses used to generate the map.
+ odoms: odometry.
+ occ_map: occupancy grid map background.
+ numParticles: number of particles.
+ grid_res: the resolution of the grids.
+ start_idx: the start index.
+ converge_thres: a threshold used to tell whether the localization converged or not.
+ """
+ self.numpoints = numParticles
+ self.grid_res = grid_res
+
+ # Setup the figure and axes...
+ self.fig, self.ax = plt.subplots(3, 1, figsize=(16, 9), gridspec_kw={'height_ratios': [5, 1, 1]})
+ self.ax0 = self.ax[0]
+ self.ax1 = self.ax[1]
+ self.ax2 = self.ax[2]
+
+ # set size
+ self.point_size = 1
+ self.map_size = map_size
+ self.plot_size = np.array(map_size) * grid_res
+ self.start_idx = start_idx
+ self.converge_idx = copy.copy(start_idx)
+
+ # set ground truth
+ self.location_gt = poses[:, :2]
+ gt_heading = []
+ for pose in poses:
+ gt_heading.append(pose[-1])
+ self.heading_gt = np.array(gt_heading)
+
+ # set odometry
+ self.plot_odom = False
+ if odoms is not None:
+ self.plot_odom = True
+ self.location_odom = odoms[:, :2]
+ odom_heading = []
+ for odom in odoms:
+ odom_heading.append(odom[-1])
+ self.heading_odom = np.array(odom_heading)
+
+ # init estimates and errors
+ self.location_estimates = np.zeros((len(poses), 2))
+ self.location_err = np.zeros(len(poses))
+ self.heading_err = np.zeros(len(poses))
+
+ # Then setup FuncAnimation.
+ self.err_thres = converge_thres
+ self.setup_plot()
+
+ # set the map
+ self.ax0.plot(map_poses[:, 0], map_poses[:, 1], '--', alpha=0.5, c='black', label='map')
+ if occ_map is not None:
+ map_res = 0.05
+ ox, oy = occ_map.shape[0] // 2, occ_map.shape[1] // 2
+
+ occ_set = []
+ unknow_set = []
+ # X, Y = occ_map.shape
+ X_min, X_max = int(self.map_size[0] / map_res + ox), int(self.map_size[1] / map_res + ox)
+ Y_min, Y_max = int(self.map_size[2] / map_res + oy), int(self.map_size[3] / map_res + oy)
+ for x in range(X_min, X_max):
+ for y in range(Y_min, Y_max):
+ if occ_map[x, y] > 0.9:
+ occ_set.append([(x - ox) * map_res, (y - oy) * map_res])
+ elif occ_map[x, y] == 0.5:
+ unknow_set.append([(x - ox) * map_res, (y - oy) * map_res])
+
+ occ_set = np.array(occ_set)
+ unknow_set = np.array(unknow_set)
+ self.ax0.plot(occ_set[:, 0], occ_set[:, 1], '.', alpha=0.5, c='black')
+ # self.ax0.plot(unknow_set[:, 0], unknow_set[:, 1], '.', alpha=0.5, c='gray')
+
+ # for zoom animation
+ self.x_min_offset = 0
+ self.x_max_offset = 0
+ self.y_min_offset = 0
+ self.y_max_offset = 0
+
+ def setup_plot(self):
+ """ Initial drawing of the scatter plot.
+ """
+ # setup ax0
+ self.ax_gt, = self.ax0.plot([], [], c='r', label='reference pose')
+ self.ax_est, = self.ax0.plot([], [], c='b', label='estimated pose')
+
+ if self.plot_odom:
+ self.ax_odom, = self.ax0.plot([], [], c='c', label='odometry')
+
+ self.scat = self.ax0.scatter([], [], c=[], s=self.point_size, cmap="Blues", vmin=0, vmax=1)
+
+ self.ax0.axis('square')
+ self.ax0.set(xlim=self.plot_size[:2], ylim=self.plot_size[2:])
+ self.ax0.set_xlabel('X [m]')
+ self.ax0.set_ylabel('Y [m]')
+ self.ax0.legend()
+
+ # setup ax1
+ self.ax_location_err, = self.ax1.plot([], [], c='g')
+
+ self.ax1.set(xlim=[0, len(self.location_gt)], ylim=[0, self.err_thres])
+ self.ax1.set_xlabel('Frame id')
+ self.ax1.set_ylabel('Location err [m]')
+
+ # setup ax2
+ self.ax_heading_err, = self.ax2.plot([], [], c='g')
+
+ self.ax2.set(xlim=[0, len(self.location_gt)], ylim=[0, self.err_thres])
+ self.ax2.set_xlabel('Frame id')
+ self.ax2.set_ylabel('Heading err [deg]')
+
+ # combine all artists
+ self.patches = [self.ax_gt, self.ax_est, self.scat, self.ax_location_err, self.ax_heading_err]
+
+ # For FuncAnimation's sake, we need to return the artist we'll be using
+ # Note that it expects a sequence of artists, thus the trailing comma.
+ return self.patches
+
+ def get_estimates(self, sorted_data, selection_rate=0.8):
+ """ calculate the estimated poses.
+ """
+ # only use the top selection_rate particles to estimate the position
+ selected_particles = sorted_data[-int(selection_rate * self.numpoints):]
+ estimated_pose = utils.particles2pose(selected_particles)
+ estimated_location = estimated_pose[:2]
+ estimated_heading = estimated_pose[2]
+
+ return estimated_location, estimated_heading
+
+ def compute_errs(self, frame_idx, particles):
+ """ Calculate the errors.
+ """
+ sorted_data = particles[particles[:, 3].argsort()]
+ new_location_estimate, new_heading_estimate = self.get_estimates(sorted_data)
+ self.location_estimates[frame_idx] = new_location_estimate
+ self.location_err[frame_idx] = np.linalg.norm(new_location_estimate - self.location_gt[frame_idx])
+ self.heading_err[frame_idx] = np.minimum(
+ abs(new_heading_estimate - self.heading_gt[frame_idx]),
+ 2. * np.pi - abs(new_heading_estimate - self.heading_gt[frame_idx])) * 180. / np.pi
+
+ return sorted_data
+
+ def update(self, frame_idx, particles):
+ """ Update the scatter plot.
+ """
+ # particles[:, -1] /= np.max(particles[:, -1])
+ particle_xyc = self.compute_errs(frame_idx, particles)[-10000:]
+
+ # Only show the estimated trajectory when localization successfully converges
+ if self.location_err[frame_idx] < self.err_thres:
+ # set ground truth
+ self.ax_gt.set_data(self.location_gt[self.start_idx:frame_idx + 1, 0],
+ self.location_gt[self.start_idx:frame_idx + 1, 1])
+
+ # set odometry
+ if self.plot_odom:
+ self.ax_odom.set_data(self.location_odom[self.start_idx:frame_idx + 1, 0],
+ self.location_odom[self.start_idx:frame_idx + 1, 1])
+
+ # set estimated pose
+ self.ax_est.set_data(self.location_estimates[self.converge_idx:frame_idx + 1, 0],
+ self.location_estimates[self.converge_idx:frame_idx + 1, 1])
+
+ # Set x and y data
+ self.scat.set_offsets(particle_xyc[:, :2] * self.grid_res)
+ # Set colors
+ self.scat.set_array(particle_xyc[:, 3])
+
+ # set err
+ self.ax_location_err.set_data(np.arange(self.start_idx, frame_idx),
+ self.location_err[self.start_idx:frame_idx])
+ self.ax_heading_err.set_data(np.arange(self.start_idx, frame_idx),
+ self.heading_err[self.start_idx:frame_idx])
+
+ else:
+ # set ground truth
+ self.ax_gt.set_data(self.location_gt[self.start_idx:frame_idx + 1, 0],
+ self.location_gt[self.start_idx:frame_idx + 1, 1])
+
+ # set odometry
+ if self.plot_odom:
+ self.ax_odom.set_data(self.location_odom[self.start_idx:frame_idx + 1, 0],
+ self.location_odom[self.start_idx:frame_idx + 1, 1])
+
+ # Set x and y data
+ self.scat.set_offsets(particle_xyc[:, :2] * self.grid_res)
+ # Set colors according to weights
+ self.scat.set_array(particle_xyc[:, 3])
+
+ # set err
+ self.ax_location_err.set_data(np.arange(self.start_idx, frame_idx),
+ self.location_err[self.start_idx:frame_idx])
+ self.ax_heading_err.set_data(np.arange(self.start_idx, frame_idx),
+ self.heading_err[self.start_idx:frame_idx])
+
+ self.converge_idx += 1
+
+ # We need to return the updated artist for FuncAnimation to draw..
+ # Note that it expects a sequence of artists, thus the trailing comma.
+ return self.patches
+
+ def update_offline(self, frame_idx, results):
+ """ Update the scatter plot.
+ """
+ particles = results[frame_idx]
+ # particles[:, -1] /= np.max(particles[:, -1])
+ particle_xyc = self.compute_errs(frame_idx, particles)[-10000:]
+
+ # Only show the estimated trajectory when localization successfully converges
+ if self.location_err[frame_idx] < self.err_thres:
+ # set ground truth
+ self.ax_gt.set_data(self.location_gt[self.start_idx:frame_idx + 1, 0],
+ self.location_gt[self.start_idx:frame_idx + 1, 1])
+
+ # set odometry
+ if self.plot_odom:
+ self.ax_odom.set_data(self.location_odom[self.start_idx:frame_idx + 1, 0],
+ self.location_odom[self.start_idx:frame_idx + 1, 1])
+
+ # set estimated pose
+ self.ax_est.set_data(self.location_estimates[self.converge_idx:frame_idx + 1, 0],
+ self.location_estimates[self.converge_idx:frame_idx + 1, 1])
+
+ # Set x and y data
+ self.scat.set_offsets(particle_xyc[:, :2] * self.grid_res)
+ # Set colors
+ self.scat.set_array(particle_xyc[:, 3])
+
+ # set err
+ self.ax_location_err.set_data(np.arange(self.start_idx, frame_idx),
+ self.location_err[self.start_idx:frame_idx])
+ self.ax_heading_err.set_data(np.arange(self.start_idx, frame_idx),
+ self.heading_err[self.start_idx:frame_idx])
+
+ else:
+ # set ground truth
+ self.ax_gt.set_data(self.location_gt[self.start_idx:frame_idx + 1, 0],
+ self.location_gt[self.start_idx:frame_idx + 1, 1])
+
+ # Set x and y data
+ self.scat.set_offsets(particle_xyc[:, :2] * self.grid_res)
+ # Set colors according to weights
+ self.scat.set_array(particle_xyc[:, 3])
+
+ # set err
+ self.ax_location_err.set_data(np.arange(self.start_idx, frame_idx),
+ self.location_err[self.start_idx:frame_idx])
+ self.ax_heading_err.set_data(np.arange(self.start_idx, frame_idx),
+ self.heading_err[self.start_idx:frame_idx])
+
+ self.converge_idx += 1
+
+ # We need to return the updated artist for FuncAnimation to draw..
+ # Note that it expects a sequence of artists, thus the trailing comma.
+ return self.patches
+
+
+if __name__ == '__main__':
+ pass
diff --git a/nof/__init__.py b/nof/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/nof/criteria/__init__.py b/nof/criteria/__init__.py
new file mode 100644
index 0000000..9609339
--- /dev/null
+++ b/nof/criteria/__init__.py
@@ -0,0 +1,8 @@
+from .loss import *
+
+
+nof_loss = {
+ 'mse': NOFMSELoss,
+ 'l1': NOFL1Loss,
+ 'smoothl1': NOFSmoothL1Loss
+}
\ No newline at end of file
diff --git a/nof/criteria/loss.py b/nof/criteria/loss.py
new file mode 100644
index 0000000..dcf3eba
--- /dev/null
+++ b/nof/criteria/loss.py
@@ -0,0 +1,47 @@
+"""The functions for loss of NOF training
+"""
+
+from torch import nn
+
+
+class NOFLoss(nn.Module):
+ def __init__(self):
+ super(NOFLoss, self).__init__()
+ self.loss = None
+
+ def forward(self, pred, target, valid_mask=None):
+ if valid_mask is not None:
+ pred = pred[valid_mask]
+ target = target[valid_mask]
+ loss = self.loss(pred, target)
+ return loss
+
+
+class NOFMSELoss(NOFLoss):
+ """
+ MSELoss for predicted scan with real scan
+ """
+
+ def __init__(self):
+ super(NOFMSELoss, self).__init__()
+ self.loss = nn.MSELoss(reduction='mean')
+
+
+class NOFL1Loss(NOFLoss):
+ """
+ L1Loss for predicted scan with real scan
+ """
+
+ def __init__(self):
+ super(NOFL1Loss, self).__init__()
+ self.loss = nn.L1Loss(reduction='mean')
+
+
+class NOFSmoothL1Loss(NOFLoss):
+ """
+ SmoothL1Loss for predicted scan with real scan
+ """
+
+ def __init__(self):
+ super(NOFSmoothL1Loss, self).__init__()
+ self.loss = nn.SmoothL1Loss(reduction='mean')
diff --git a/nof/criteria/metrics.py b/nof/criteria/metrics.py
new file mode 100644
index 0000000..149c24b
--- /dev/null
+++ b/nof/criteria/metrics.py
@@ -0,0 +1,30 @@
+import torch
+from .pointcloud_metrics import eval_pts
+
+
+def abs_error(pred, gt, valid_mask=None):
+ value = torch.abs(pred - gt)
+ if valid_mask is not None:
+ value = value[valid_mask]
+
+ return torch.mean(value)
+
+
+def acc_thres(pred, gt, valid_mask=None):
+ error = torch.abs(pred - gt)
+ if valid_mask is not None:
+ error = error[valid_mask]
+ acc_thres = error < 0.5
+ acc_thres = torch.sum(acc_thres) / acc_thres.shape[0] * 100
+
+ return acc_thres
+
+
+def eval_points(pred_pts, gt_pts, valid_mask=None):
+ if valid_mask is not None:
+ pred_pts = pred_pts[valid_mask].cpu().numpy()
+ gt_pts = gt_pts[valid_mask].cpu().numpy()
+
+ cd, fscore = eval_pts(pred_pts, gt_pts)
+
+ return cd, fscore
diff --git a/nof/criteria/pointcloud_metrics.py b/nof/criteria/pointcloud_metrics.py
new file mode 100644
index 0000000..b560025
--- /dev/null
+++ b/nof/criteria/pointcloud_metrics.py
@@ -0,0 +1,44 @@
+import open3d as o3d
+import numpy as np
+
+
+def nn_correspondance(verts1, verts2):
+ """ for each vertex in verts2 find the nearest vertex in verts1
+
+ Args:
+ nx3 np.array's
+ Returns:
+ ([indices], [distances])
+
+ """
+
+ indices = []
+ distances = []
+ if len(verts1) == 0 or len(verts2) == 0:
+ return indices, distances
+
+ pcd = o3d.geometry.PointCloud()
+ pcd.points = o3d.utility.Vector3dVector(verts1)
+ kdtree = o3d.geometry.KDTreeFlann(pcd)
+
+ for vert in verts2:
+ _, inds, dist = kdtree.search_knn_vector_3d(vert, 1)
+ indices.append(inds[0])
+ distances.append(np.sqrt(dist[0]))
+
+ return indices, distances
+
+
+def eval_pts(pts1, pts2, threshold=0.5):
+ _, dist1 = nn_correspondance(pts1, pts2)
+ _, dist2 = nn_correspondance(pts2, pts1)
+ dist1 = np.array(dist1)
+ dist2 = np.array(dist2)
+
+
+ precision = np.mean((dist1 0, valid_mask_gt < self.max_range)] = 1
+ valid_mask_gt[valid_mask_gt >= self.max_range] = 0
+
+ # set invalid value (no return) to 0
+ range_readings[range_readings >= self.max_range] = 0
+
+ range_readings = torch.Tensor(range_readings)
+ valid_mask_gt = torch.BoolTensor(valid_mask_gt)
+
+ return range_readings, valid_mask_gt
+
+ def __getitem__(self, index):
+ if self.split == 'train':
+ sample = {'rays': self.all_rays[index],
+ 'ranges': self.all_ranges[index]}
+
+ else:
+ scan = self.meta['scans'][index]
+ odom = torch.FloatTensor(scan['odom'])
+ T_w2l = torch.FloatTensor(scan['transform_matrix'])[:2, :3]
+
+ # load range reading
+ range_readings, valid_mask_gt = self.load_scan(scan['range_readings'])
+
+ rays_o, rays_d = get_rays(self.directions, T_w2l)
+
+ near, far = self.near * torch.ones_like(rays_d[..., :1]), self.far * torch.ones_like(rays_d[..., :1])
+ rays = torch.cat([rays_o, rays_d, near, far], -1)
+
+ sample = {'rays': rays,
+ 'ranges': range_readings,
+ 'odom': odom,
+ 'valid_mask_gt': valid_mask_gt}
+
+ return sample
+
+ def __len__(self):
+ if self.split == 'train':
+ return len(self.all_rays)
+ else:
+ return len(self.meta['scans'])
diff --git a/nof/dataset/ray_utils.py b/nof/dataset/ray_utils.py
new file mode 100644
index 0000000..a4ed813
--- /dev/null
+++ b/nof/dataset/ray_utils.py
@@ -0,0 +1,58 @@
+"""The functions for supporting the IPB2DMapping dataset class
+Code partially borrowed from
+https://github.com/kwea123/nerf_pl/blob/master/datasets/ray_utils.py.
+MIT License
+Copyright (c) 2020 Quei-An Chen
+"""
+
+import torch
+
+
+def get_ray_directions(angle_min, angle_max, angle_res):
+ """
+ Get the direction of laser beam in lidar coordinate.
+
+ :param angle_min: the start angle of one scan (scalar)
+ :param angle_max: the end angle of one scan (scalar)
+ :param angle_res: the interval angle between two consecutive beams (scalar)
+
+ :return: the direction of each beams (shape: (N, 2))
+ """
+ r = 1
+ beams = torch.arange(angle_min, angle_max, angle_res)
+
+ x = r * torch.cos(beams)
+ y = r * torch.sin(beams)
+
+ directions = torch.stack([x, y], dim=-1)
+ return directions
+
+
+def get_rays(directions, T_w2l):
+ """
+ Get ray origin and normalized directions in world coordinate for all beams in one scan.
+
+ :param directions: ray directions in the lidar coordinate. (shape: (N, 2))
+ :param T_w2l: 2*3 transformation matrix from lidar coordinate
+ to world coordinate. (shape: (2, 3))
+
+ :return rays_0: the origin of the rays in world coordinate. (shape: (N, 2))
+ :return rays_d: the normalized direction of the rays in world coordinate. (shape: (N, 2))
+
+ """
+ rays_d = directions @ T_w2l[:, :2].T # (N, 2)
+ # same as below:
+ # d = directions.t()
+ # rays_d = T_w2l[:, :2] @ d
+ # rays_d = rays.t().view(180, 2)
+
+ # normalize direction vector
+ rays_d = rays_d / torch.norm(rays_d, dim=-1, keepdim=True)
+
+ # The origin of all rays is the camera origin in world coordinate
+ rays_o = T_w2l[:, 2].expand(rays_d.shape)
+
+ rays_d = rays_d.view(-1, 2)
+ rays_o = rays_o.view(-1, 2)
+
+ return rays_o, rays_d
diff --git a/nof/networks/__init__.py b/nof/networks/__init__.py
new file mode 100644
index 0000000..d60e6f0
--- /dev/null
+++ b/nof/networks/__init__.py
@@ -0,0 +1 @@
+from .models import NOF, Embedding
\ No newline at end of file
diff --git a/nof/networks/models.py b/nof/networks/models.py
new file mode 100644
index 0000000..ca5618c
--- /dev/null
+++ b/nof/networks/models.py
@@ -0,0 +1,123 @@
+"""The functions for Neural Occupancy Field (NOF) models
+Code partially borrowed from
+https://github.com/kwea123/nerf_pl/blob/master/models/nerf.py.
+MIT License
+Copyright (c) 2020 Quei-An Chen
+"""
+import torch
+import torch.nn as nn
+
+
+class Embedding(nn.Module):
+ """
+ Defines a function that embeds x to (x, sin(2^k x), cos(2^k x), ...)
+
+ :param in_channels: number of input channels (2 for both xyz and direction)
+ :param N_freq: number of scale of embeddings
+ :param logscale: whether to use log scale (default: True)
+
+ """
+
+ def __init__(self, in_channels, N_freq, logscale=True):
+ super(Embedding, self).__init__()
+ self.N_freq = N_freq
+ self.in_channels = in_channels
+
+ self.funcs = [torch.sin, torch.cos]
+
+ if logscale:
+ self.freq_bands = 2 ** torch.linspace(0, N_freq - 1, N_freq)
+ else:
+ self.freq_bands = torch.linspace(1, 2 ** (N_freq - 1), N_freq)
+
+ def forward(self, x):
+ """
+ Embeds x to (x, sin(2^k x), cos(2^k x), ...)
+
+ :param x: (B, self.in_channels)
+ :return out: (B, self.N_freq * self.in_channels * len(self.funcs))
+ """
+ out = [x]
+ for freq in self.freq_bands:
+ for func in self.funcs:
+ out.append(func(freq * x))
+
+ out = torch.cat(out, -1)
+ return out
+
+
+class NOF(nn.Module):
+ def __init__(self, feature_size=256, in_channels_xy=42, use_skip=True):
+ """
+ The model of NOF.
+
+ :param feature_size: number of hidden units in each layer
+ :param in_channels_xy: number of input channels for xy (default: 2+2*10*2=42)
+ :param use_skip: whether to use skip architecture (default: True)
+ """
+ super(NOF, self).__init__()
+ self.feature_size = feature_size
+ self.in_channels_xy = in_channels_xy
+ self.use_skip = use_skip
+
+ # define nerf model
+ # layer 1: first 4-layers MLP
+ self.layer1 = []
+ for i in range(4):
+ if i == 0:
+ self.layer1.append(
+ nn.Linear(in_features=self.in_channels_xy, out_features=self.feature_size))
+ else:
+ self.layer1.append(
+ nn.Linear(in_features=self.feature_size, out_features=self.feature_size))
+ self.layer1.append(nn.BatchNorm1d(num_features=self.feature_size))
+ self.layer1.append(nn.ReLU(True))
+
+ self.layer1 = nn.Sequential(*self.layer1)
+
+ # layer 2: second 4-layers MLP
+ self.layer2 = []
+ for i in range(4):
+ if i == 0:
+ if self.use_skip:
+ self.layer2.append(
+ nn.Linear(in_features=self.in_channels_xy + self.feature_size,
+ out_features=self.feature_size))
+ else:
+ self.layer2.append(
+ nn.Linear(in_features=self.feature_size, out_features=self.feature_size))
+ else:
+ self.layer2.append(
+ nn.Linear(in_features=self.feature_size, out_features=self.feature_size))
+ self.layer2.append(nn.BatchNorm1d(num_features=self.feature_size))
+ self.layer2.append(nn.ReLU(True))
+
+ self.layer2 = nn.Sequential(*self.layer2)
+
+ # occupancy probability
+ self.occ_out = nn.Sequential(
+ nn.Linear(in_features=self.feature_size, out_features=1),
+ nn.Sigmoid()
+ )
+
+
+ def forward(self, x):
+ """
+ Encodes input position (xy) to occupancy probability (p_occ)
+ :param x: the embedded vector of a 2D position
+ (shape: (B, self.in_channels_xy))
+
+ :return p_occ: the occupancy probability of the input position (shape: (B, 1))
+ """
+ input_xy = x
+
+ feature1 = self.layer1(input_xy)
+
+ if self.use_skip:
+ feature1 = torch.cat([input_xy, feature1], dim=1)
+
+ feature2 = self.layer2(feature1)
+
+ p_occ = self.occ_out(feature2)
+
+ return p_occ
diff --git a/nof/nof_utils.py b/nof/nof_utils.py
new file mode 100644
index 0000000..5759e5b
--- /dev/null
+++ b/nof/nof_utils.py
@@ -0,0 +1,134 @@
+"""The functions for some tools
+"""
+
+import torch
+from torch.optim import SGD, Adam
+import argparse
+
+
+def get_opts():
+ parser = argparse.ArgumentParser()
+
+ # data
+ parser.add_argument('--root_dir', type=str, default='~/ir-mcl/data/ipblab',
+ help='root directory of dataset')
+ parser.add_argument('--N_samples', type=int, default=64,
+ help='number of coarse samples')
+ parser.add_argument('--perturb', type=float, default=0.0,
+ help='factor to perturb depth sampling points')
+ parser.add_argument('--noise_std', type=float, default=0.0,
+ help='std dev of noise added to regularize sigma')
+ parser.add_argument('--use_disp', default=False, action="store_true",
+ help='use disparity depth sampling')
+ parser.add_argument('--L_pos', type=int, default=10,
+ help='the frequency of the positional encoding.')
+
+ # model
+ parser.add_argument('--feature_size', type=int, default=256,
+ help='the dimension of the feature maps.')
+ parser.add_argument('--use_skip', default=False, action="store_true",
+ help='use skip architecture')
+
+ # maps
+ parser.add_argument('--map_size', type=float, default=50,
+ help='the size of the occupancy grid map.')
+ parser.add_argument('--map_res', type=float, default=0.05,
+ help='the resolution of the occupancy grid map.')
+ parser.add_argument('--lower_origin', default=False, action="store_true",
+ help='use [0, 0] as the origin of the map.')
+
+ # optimization
+ parser.add_argument('--seed', type=int, default=None,
+ help='set a seed for fairly comparison during training')
+
+ parser.add_argument('--loss_type', type=str, default='smoothl1',
+ choices=['mse', 'l1', 'smoothl1'],
+ help='loss to use')
+
+ parser.add_argument('--batch_size', type=int, default=1024,
+ help='batch size')
+ parser.add_argument('--chunk', type=int, default=32 * 1024,
+ help='chunk size to split the input to avoid OOM')
+ parser.add_argument('--num_epochs', type=int, default=16,
+ help='number of training epochs')
+ parser.add_argument('--num_gpus', type=int, default=1,
+ help='number of gpus')
+
+ parser.add_argument('--ckpt_path', type=str, default=None,
+ help='pretrained checkpoint path to load')
+ parser.add_argument('--prefixes_to_ignore', nargs='+', type=str, default=['loss'],
+ help='the prefixes to ignore in the checkpoint state dict')
+
+ parser.add_argument('--optimizer', type=str, default='adam',
+ help='optimizer type',
+ choices=['sgd', 'adam'])
+ parser.add_argument('--lr', type=float, default=5e-4,
+ help='learning rate')
+ parser.add_argument('--momentum', type=float, default=0.9,
+ help='learning rate momentum')
+ parser.add_argument('--weight_decay', type=float, default=0,
+ help='weight decay')
+ parser.add_argument('--lambda_opacity', type=float, default=1e-5,
+ help='the weights of opacity regularization')
+
+ ###########################
+ #### params for steplr ####
+ parser.add_argument('--decay_step', nargs='+', type=int, default=[200],
+ help='scheduler decay step')
+ parser.add_argument('--decay_gamma', type=float, default=0.1,
+ help='learning rate decay amount')
+
+ parser.add_argument('--exp_name', type=str, default='exp',
+ help='experiment name')
+
+ return parser.parse_args()
+
+
+def get_learning_rate(optimizer):
+ for param_group in optimizer.param_groups:
+ return param_group['lr']
+
+
+def get_optimizer(hparams, parameters):
+ eps = 1e-8
+ if hparams.optimizer == 'sgd':
+ optimizer = SGD(parameters, lr=hparams.lr,
+ momentum=hparams.momentum, weight_decay=hparams.weight_decay)
+ elif hparams.optimizer == 'adam':
+ optimizer = Adam(parameters, lr=hparams.lr, eps=eps,
+ weight_decay=hparams.weight_decay)
+ else:
+ raise ValueError('optimizer not recognized!')
+
+ return optimizer
+
+
+def extract_model_state_dict(ckpt_path, model_name='model', prefixes_to_ignore=[]):
+ checkpoint = torch.load(ckpt_path, map_location=torch.device('cpu'))
+ checkpoint_ = {}
+ if 'state_dict' in checkpoint: # if it's a pytorch-lightning checkpoint
+ checkpoint = checkpoint['state_dict']
+ for k, v in checkpoint.items():
+ if not k.startswith(model_name):
+ continue
+ k = k[len(model_name) + 1:]
+ for prefix in prefixes_to_ignore:
+ if k.startswith(prefix):
+ print('ignore', k)
+ break
+ else:
+ checkpoint_[k] = v
+ return checkpoint_
+
+
+def load_ckpt(model, ckpt_path, model_name='model', prefixes_to_ignore=[]):
+ model_dict = model.state_dict()
+ checkpoint_ = extract_model_state_dict(ckpt_path, model_name, prefixes_to_ignore)
+ model_dict.update(checkpoint_)
+ model.load_state_dict(model_dict)
+
+
+def decode_batch(batch):
+ rays = batch['rays'] # shape: (B, 6)
+ rangs = batch['ranges'] # shape: (B, 1)
+ return rays, rangs
diff --git a/nof/render.py b/nof/render.py
new file mode 100644
index 0000000..590aa3b
--- /dev/null
+++ b/nof/render.py
@@ -0,0 +1,194 @@
+"""The functions for rendering a 2D laser scan from NOF's output
+Code partially borrowed from https://github.com/kwea123/nerf_pl/blob/master/models/rendering.py.
+MIT License
+Copyright (c) 2020 Quei-An Chen
+"""
+
+import torch
+from .networks import NOF, Embedding
+
+__all__ = ['render_rays']
+
+
+def inference(model: NOF, embedding_xy: Embedding, samples_xy: torch.Tensor,
+ z_vals: torch.Tensor, chunk=1024 * 32, noise_std=1, epsilon=1e-10):
+ """
+ Helper function that performs model inference.
+
+ :param model: NOF model
+ :param embedding_xy: position embedding module
+ :param samples_xy: sampled position (shape: (N_rays, N_samples, 2))
+ :param z_vals: depths of the sampled positions (shape: N_rays, N_samples)
+ :param chunk: the chunk size in batched inference (default: 1024*32)
+ :param noise_std: factor to perturb the model's prediction of sigma (default: 1)
+ :param epsilon: a small number to avoid the 0 of weights_sum (default: 1e-10)
+
+ :return:
+ depth_final: rendered range value for each Lidar beams (shape: (N_rays,))
+ weights: weights of each sample (shape: (N_rays, N_samples))
+ opacity: the cross entropy of the predicted occupancy values
+ """
+ N_rays = samples_xy.shape[0]
+ N_samples = samples_xy.shape[1]
+
+ # Embed directions
+ samples_xy = samples_xy.view(-1, 2) # shape: (N_rays * N_samples, 2)
+
+ # prediction, to get rangepred and raw sigma
+ B = samples_xy.shape[0] # N_rays * N_samples
+ out_chunks = []
+ for i in range(0, B, chunk):
+ # embed position by chunk
+ embed_xy = embedding_xy(samples_xy[i:i + chunk])
+ # embed_xy = samples_xy[i:i + chunk]
+ out_chunks += [model(embed_xy)]
+
+ out = torch.cat(out_chunks, dim=0)
+ prob_occ = out.view(N_rays, N_samples) # shape: (N_rays, N_samples)
+
+ # Volume Rendering: synthesis the 2d lidar scan
+ prob_free = 1 - prob_occ # (1-p)
+ prob_free_shift = torch.cat(
+ [torch.ones_like(prob_free[:, :1]), prob_free], dim=-1
+ )
+ prob_free_cum = torch.cumprod(prob_free_shift, dim=-1)[:, :-1]
+ weights = prob_free_cum * prob_occ
+
+ # add noise
+ noise = torch.randn(weights.shape, device=weights.device) * noise_std
+ weights = weights + noise
+
+ # normalize
+ weights = weights / (torch.sum(weights, dim=-1).reshape(-1, 1) + epsilon)
+
+ depth_final = torch.sum(weights * z_vals, dim=-1) # shape: (N_rays,)
+
+ # opacity regularization
+ opacity = torch.mean(torch.log(0.1 + prob_occ) + torch.log(0.1 + prob_free) + 2.20727)
+
+ return depth_final, weights, opacity
+
+
+def render_rays(model: NOF, embedding_xy: Embedding, rays: torch.Tensor,
+ N_samples=64, use_disp=False, perturb=0, noise_std=1, chunk=1024 * 3):
+ """
+ Render rays by computing the output of @model applied on @rays
+
+ :param model: NOF model, defined by models.NOF()
+ :param embedding_xy: embedding model for position, defined by models.Embedding()
+
+ :param rays: the input data, include: ray original, directions, near and far depth bounds
+ (shape: (N_rays, 2+2+2))
+ :param N_samples: number of samples pre ray (default: 64)
+ :param use_disp: whether to sample in disparity space (inverse depth) (default: False)
+ :param perturb: factor to perturb the sampling position on the ray
+ (0 for default, for coarse model only)
+ :param noise_std: factor to perturb the model's prediction of sigma (1 for default)
+ :param chunk: the chunk size in batched inference
+
+ :return result: dictionary containing final range value, weights and opacity
+ """
+ # Decompose the inputs
+ N_rays = rays.shape[0]
+ rays_o, rays_d = rays[:, :2], rays[:, 2:4] # shape: (N_rays, 2)
+ near, far = rays[:, 4].view(-1, 1), rays[:, 5].view(-1, 1) # shape: (N_rays, 1)
+
+ z_steps = torch.linspace(0, 1, N_samples, device=rays.device) # shape: (N_samples,)
+ z_steps = z_steps.expand(N_rays, N_samples)
+
+ if use_disp:
+ # linear sampling in disparity space
+ z_vals = 1 / (1 / near * (1 - z_steps) + 1 / far * z_steps)
+ else:
+ # linear sampling in depth space
+ z_vals = near * (1 - z_steps) + far * z_steps
+
+ # perturb sampling depths (z_vals)
+ if perturb > 0:
+ z_vals_mid = 0.5 * (z_vals[:, :-1] + z_vals[:, 1:]) # shape: (N_rays, N_samples-1)
+ upper = torch.cat([z_vals_mid, z_vals[:, -1:]], dim=-1)
+ lower = torch.cat([z_vals[:, :1], z_vals_mid], dim=-1)
+
+ perturb_rand = perturb * torch.rand(z_vals.shape, device=rays.device)
+ z_vals = lower + (upper - lower) * perturb_rand
+
+ samples_xy = rays_o.unsqueeze(1) + \
+ rays_d.unsqueeze(1) * z_vals.unsqueeze(2) # shape: (N_rays, N_samples, 2)
+
+ depth, weights, opacity = \
+ inference(model, embedding_xy, samples_xy, z_vals, chunk, noise_std)
+
+ results = {'depth': depth,
+ 'weights': weights.sum(1),
+ 'opacity': opacity
+ }
+
+ return results
+
+
+def render_rays_grid(grid: torch.Tensor, map_size, grid_res: float,
+ rays: torch.Tensor, N_samples=64, chunk=1000, epsilon=1e-10):
+ """
+ Render rays from neural occupancy grid, only used for accelerating MCL
+
+ :param grid: occupancy grid from NOF
+ :param map_size: the size of the grid
+ :param grid_res: the cell's resolutions of grid
+ :param rays: the input data, include: ray original, directions, near and far depth bounds
+ (shape: (N_rays, 2+2+2))
+ :param N_samples: number of samples pre ray (default: 64)
+ :param chunk: the chunk size in batched inference
+ :param epsilon: a small number to avoid the 0 of weights_sum (default: 1e-10)
+
+ :return result: dictionary containing final range value, weights and opacity
+ """
+ # Decompose the inputs
+ N_particles, N_rays = rays.shape[:2]
+
+ rays_os, rays_ds = rays[..., :2], rays[..., 2:4] # shape: (N_particles, N_rays, 2)
+
+ # shape: (N_particles, N_rays, 1)
+ near, far = rays[..., 4].view(-1, N_rays, 1), rays[..., 5].view(-1, N_rays, 1)
+
+ # Sampling depth points
+ z_steps = torch.linspace(0, 1, N_samples, device=rays.device) # shape: (N_samples,)
+
+ render_scans = []
+ for i in range(0, N_particles, chunk):
+ # linear sampling in depth space
+ z_vals = near[i:i + chunk] * (1 - z_steps) + far[i:i + chunk] * z_steps
+
+ # shape: (chunk, N_rays, N_samples, 2)
+ samples_xy = rays_os[i:i + chunk].unsqueeze(2) + \
+ rays_ds[i:i + chunk].unsqueeze(2) * z_vals.unsqueeze(3)
+
+ N_chunk = samples_xy.shape[0]
+
+ # Embed directions
+ samples_xy = samples_xy.view(-1, 2) # shape: (N_chunk * N_rays * N_samples, 2)
+
+ # prediction, to get rangepred and raw sigma
+ samples_xy[:, 0] -= map_size[0]
+ samples_xy[:, 1] -= map_size[2]
+ samples_grid = torch.round(samples_xy / grid_res).type(torch.long)
+
+ out = grid[samples_grid[:, 0], samples_grid[:, 1]]
+ prob_occ = out.view(N_chunk, N_rays, N_samples) # shape: (N_chunk, N_rays, N_samples)
+
+ # Volume Rendering: synthesis the 2d lidar scan
+ prob_free = 1 - prob_occ # (1-p)
+ prob_free_shift = torch.cat(
+ [torch.ones_like(prob_free[..., :1]), prob_free], dim=-1
+ )
+ prob_free_cum = torch.cumprod(prob_free_shift, dim=-1)[..., :-1]
+ weights = prob_free_cum * prob_occ
+
+ # normalize
+ weights = weights / (torch.sum(weights, dim=-1).reshape(N_chunk, N_rays, 1) + epsilon)
+ depth = torch.sum(weights * z_vals, dim=-1) # shape: (N_chunk, N_rays)
+
+ render_scans.append(depth)
+
+ render_scans = torch.cat(render_scans, dim=0)
+
+ return render_scans
diff --git a/shells/loc_demo.sh b/shells/loc_demo.sh
new file mode 100644
index 0000000..fd59e80
--- /dev/null
+++ b/shells/loc_demo.sh
@@ -0,0 +1,6 @@
+cd ~/ir-mcl
+python loc_demo.py \
+ --loc_results ./results/ipblab/loc_test/test1/loc_results.npz \
+ --occ_map ./data/ipblab/occmap.npy \
+ --output_gif ./results/ipblab/loc_test/test1/loc_demo.gif \
+ --map_size -15 17.5 -12.5 5
diff --git a/shells/pretraining/fr079.sh b/shells/pretraining/fr079.sh
new file mode 100644
index 0000000..550bb0b
--- /dev/null
+++ b/shells/pretraining/fr079.sh
@@ -0,0 +1,12 @@
+python train.py \
+ --root_dir ./data/fr079 --N_samples 256 --perturb 1 --noise_std 0 --L_pos 10 \
+ --feature_size 256 --use_skip --seed 42 --batch_size 1024 --chunk 262144 \
+ --num_epochs 32 --loss_type smoothl1 --optimizer adam --weight_decay 1e-3 \
+ --lr 1e-4 --decay_step 4 8 --decay_gamma 0.5 --lambda_opacity 1e-5 \
+ --exp_name nof_fr079
+
+python eval.py \
+ --root_dir ./data/fr079 \
+ --ckpt_path ./logs/nof_fr079/version_0/checkpoints/best.ckpt \
+ --N_samples 256 --chunk 92160 --perturb 0 --noise_std 0 --L_pos 10 \
+ --feature_size 256 --use_skip
diff --git a/shells/pretraining/intel.sh b/shells/pretraining/intel.sh
new file mode 100755
index 0000000..9116ebb
--- /dev/null
+++ b/shells/pretraining/intel.sh
@@ -0,0 +1,13 @@
+python train.py \
+ --root_dir ./data/intel --N_samples 1024 --perturb 1 \
+ --noise_std 0 --L_pos 10 --feature_size 256 --use_skip --seed 42 \
+ --batch_size 512 --chunk 262144 --num_epochs 128 --loss_type smoothl1 \
+ --optimizer adam --weight_decay 1e-3 --lr 1e-4 --decay_step 64 --decay_gamma 0.1 \
+ --lambda_opacity 1e-5 --exp_name nof_intel
+
+
+python eval.py \
+ --root_dir ./data/intel \
+ --ckpt_path ./logs/nof_intel/version_0/checkpoints/best.ckpt \
+ --N_samples 1024 --chunk 184320 --perturb 0 --noise_std 0 --L_pos 10 \
+ --feature_size 256 --use_skip
diff --git a/shells/pretraining/ipblab.sh b/shells/pretraining/ipblab.sh
new file mode 100644
index 0000000..40250d0
--- /dev/null
+++ b/shells/pretraining/ipblab.sh
@@ -0,0 +1,12 @@
+python train.py \
+ --root_dir ./data/ipblab --N_samples 256 --perturb 1 --noise_std 0 --L_pos 10 \
+ --feature_size 256 --use_skip --seed 42 --batch_size 1024 --chunk 262144 \
+ --num_epochs 32 --loss_type smoothl1 --optimizer adam --weight_decay 1e-3 \
+ --lr 1e-4 --decay_step 4 8 --decay_gamma 0.5 --lambda_opacity 1e-5 \
+ --exp_name nof_ipblab
+
+python eval.py \
+ --root_dir ./data/ipblab \
+ --ckpt_path ./logs/nof_ipblab/version_0/checkpoints/best.ckpt \
+ --N_samples 256 --chunk 92160 --perturb 0 --noise_std 0 --L_pos 10 \
+ --feature_size 256 --use_skip
diff --git a/shells/pretraining/mit.sh b/shells/pretraining/mit.sh
new file mode 100644
index 0000000..0b28182
--- /dev/null
+++ b/shells/pretraining/mit.sh
@@ -0,0 +1,12 @@
+python train.py \
+ --root_dir ./data/mit --N_samples 1024 --perturb 1 --noise_std 0 --L_pos 10 \
+ --feature_size 256 --use_skip --seed 42 --batch_size 512 --chunk 262144 \
+ --num_epochs 32 --loss_type smoothl1 --optimizer adam --weight_decay 1e-3 \
+ --lr 1e-4 --decay_step 4 8 --decay_gamma 0.5 --lambda_opacity 1e-5 \
+ --exp_name nof_mit
+
+python eval.py \
+ --root_dir ./data/mit \
+ --ckpt_path ./logs/nof_mit/version_2/checkpoints/best.ckpt \
+ --N_samples 1024 --chunk 92160 --perturb 0 --noise_std 0 --L_pos 10 \
+ --feature_size 256 --use_skip
diff --git a/train.py b/train.py
new file mode 100644
index 0000000..ba5c181
--- /dev/null
+++ b/train.py
@@ -0,0 +1,159 @@
+"""The functions for NOF pretraining
+Code partially borrowed from
+https://github.com/kwea123/nerf_pl/blob/master/train.py.
+MIT License
+Copyright (c) 2020 Quei-An Chen
+"""
+
+# pytorch
+import torch
+from torch.utils.data import DataLoader
+from torch.optim.lr_scheduler import MultiStepLR
+
+# pytorch-lightning
+from pytorch_lightning import LightningModule, Trainer, seed_everything
+from pytorch_lightning.callbacks import ModelCheckpoint
+from pytorch_lightning.loggers import TensorBoardLogger
+
+from nof.dataset import nof_dataset
+from nof.networks import Embedding, NOF
+from nof.render import render_rays
+from nof.criteria import nof_loss
+from nof.criteria.metrics import abs_error, acc_thres, eval_points
+from nof.nof_utils import get_opts, get_learning_rate, get_optimizer, decode_batch
+
+
+class NOFSystem(LightningModule):
+ def __init__(self, hparams):
+ super(NOFSystem, self).__init__()
+ self.save_hyperparameters(hparams)
+
+ # define model
+ self.embedding_position = Embedding(in_channels=2, N_freq=self.hparams.L_pos)
+
+ self.nof = NOF(feature_size=self.hparams.feature_size,
+ in_channels_xy=2 + 2 * self.hparams.L_pos * 2,
+ use_skip=self.hparams.use_skip)
+
+ self.loss = nof_loss[self.hparams.loss_type]()
+
+ # datasets
+ def prepare_data(self):
+ dataset = nof_dataset['ipb2d']
+ kwargs = {'root_dir': self.hparams.root_dir}
+
+ self.train_dataset = dataset(split='train', **kwargs)
+ self.val_dataset = dataset(split='val', **kwargs)
+
+ def train_dataloader(self):
+ return DataLoader(self.train_dataset, shuffle=True, num_workers=4,
+ batch_size=self.hparams.batch_size, pin_memory=True)
+
+ def val_dataloader(self):
+ return DataLoader(self.val_dataset, shuffle=False, num_workers=4,
+ batch_size=1, pin_memory=True)
+
+ def forward(self, rays):
+ rendered_rays = render_rays(
+ model=self.nof, embedding_xy=self.embedding_position, rays=rays,
+ N_samples=self.hparams.N_samples, use_disp=self.hparams.use_disp,
+ perturb=self.hparams.perturb, noise_std=self.hparams.noise_std,
+ chunk=self.hparams.chunk,
+ )
+
+ return rendered_rays
+
+ def configure_optimizers(self):
+ parameters = []
+ parameters += list(self.nof.parameters())
+
+ self.optimizer = get_optimizer(self.hparams, parameters)
+ self.scheduler = MultiStepLR(self.optimizer, milestones=self.hparams.decay_step,
+ gamma=self.hparams.decay_gamma)
+
+ return [self.optimizer], [self.scheduler]
+
+ def training_step(self, batch, batch_idx):
+ self.log('lr', get_learning_rate(self.optimizer))
+
+ # load data
+ rays, gt_ranges = decode_batch(batch)
+
+ # inference
+ results = self.forward(rays)
+
+ # compute loss and record
+ pred_ranges = results['depth']
+ loss = self.loss(pred_ranges, gt_ranges) + \
+ self.hparams.lambda_opacity * results['opacity']
+ self.log('train/loss', loss)
+
+ with torch.no_grad():
+ abs_error_ = abs_error(pred_ranges, gt_ranges)
+ acc_thres_ = acc_thres(pred_ranges, gt_ranges)
+ self.log('train/avg_error', abs_error_)
+ self.log('train/acc_thres', acc_thres_)
+
+ return loss
+
+ def validation_step(self, batch, batch_idx):
+ # load data
+ rays, gt_ranges = decode_batch(batch)
+ rays = rays.squeeze() # shape: (N_beams, 6)
+ gt_ranges = gt_ranges.squeeze() # shape: (N_beams,)
+ valid_mask_gt = batch['valid_mask_gt'].squeeze() # shape: (N_beams,)
+
+ # inference
+ results = self.forward(rays)
+ # record
+ pred_ranges = results['depth']
+
+ loss = self.loss(pred_ranges, gt_ranges, valid_mask_gt)
+ abs_error_ = abs_error(pred_ranges, gt_ranges, valid_mask_gt)
+ acc_thres_ = acc_thres(pred_ranges, gt_ranges, valid_mask_gt)
+
+ rays_o, rays_d = rays[:, :2], rays[:, 2:4]
+ pred_pts = torch.column_stack((rays_o + rays_d * pred_ranges.unsqueeze(-1),
+ torch.zeros(pred_ranges.shape[0]).cuda()))
+ gt_pts = torch.column_stack((rays_o + rays_d * gt_ranges.unsqueeze(-1),
+ torch.zeros(gt_ranges.shape[0]).cuda()))
+ cd, fscore = eval_points(pred_pts, gt_pts, valid_mask_gt)
+ self.log('val/loss', loss, prog_bar=True)
+ self.log('val/avg_error', abs_error_, prog_bar=True)
+ self.log('val/acc_thres', acc_thres_, prog_bar=True)
+ self.log('val/cd', cd, prog_bar=True)
+ self.log('val/fscore', fscore, prog_bar=True)
+
+
+if __name__ == '__main__':
+ ######################## Step 0: Define Arguments ########################
+ hparams = get_opts()
+
+ if hparams.seed:
+ seed_everything(hparams.seed, workers=True)
+
+ ######################## Step 1: Model and Trainer ########################
+ nof_system = NOFSystem(hparams=hparams)
+
+ checkpoint_callback = ModelCheckpoint(
+ monitor='val/avg_error', mode='min', save_top_k=1, filename='best', save_last=True)
+
+ logger = TensorBoardLogger(
+ save_dir="logs",
+ name=hparams.exp_name,
+ )
+
+ # arguments
+ print("CUDA is available:", torch.cuda.is_available())
+ trainer = Trainer(max_epochs=hparams.num_epochs,
+ callbacks=[checkpoint_callback],
+ logger=logger,
+ enable_model_summary=False,
+ accelerator='gpu',
+ devices=1,
+ num_sanity_val_steps=-1,
+ benchmark=True)
+
+ trainer.fit(nof_system)
+
+ print(checkpoint_callback.best_model_path)
diff --git a/utils.py b/utils.py
new file mode 100644
index 0000000..62f14be
--- /dev/null
+++ b/utils.py
@@ -0,0 +1,236 @@
+"""The functions for supporting the MCL experiments
+"""
+
+import json
+import os.path
+
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+from evo.core import metrics, sync
+from evo.tools import file_interface
+
+from nof.dataset.ray_utils import get_ray_directions
+
+
+def load_data(pose_path, max_beams=None):
+ # Read and parse the poses
+ timestamps = []
+ poses_gt = []
+ odoms = []
+ scans = []
+ params = {}
+
+ try:
+ with open(pose_path, 'r') as f:
+ all_data = json.load(f)
+
+ # downsample beams number according to the max_beams
+ if max_beams is not None:
+ downsample_factor = all_data['num_beams'] // max_beams
+ all_data['angle_res'] *= downsample_factor
+ all_data['num_beams'] = max_beams
+ all_data['angle_max'] = all_data['angle_min'] + all_data['angle_res'] * max_beams
+
+ params.update({'num_beams': all_data['num_beams']})
+ params.update({'angle_min': all_data['angle_min']})
+ params.update({'angle_max': all_data['angle_max']})
+ params.update({'angle_res': all_data['angle_res']})
+ params.update({'max_range': all_data['max_range']})
+
+ near = 0.02
+ far = np.floor(all_data['max_range'])
+ bound = np.array([near, far])
+ # ray directions for all beams in the lidar coordinate, shape: (N, 2)
+ directions = get_ray_directions(all_data['angle_min'], all_data['angle_max'],
+ all_data['angle_res'])
+
+ params.update({'near': near})
+ params.update({'far': far})
+ params.update({'bound': bound})
+ params.update({'directions': directions})
+
+ for data in all_data['scans']:
+ timestamps.append(data['timestamp'])
+
+ pose = data['pose_gt']
+ poses_gt.append(pose)
+
+ odom = data['odom_reading']
+ odoms.append(odom)
+
+ scan = np.array(data['range_reading'])
+ if max_beams is not None:
+ scan = scan[::downsample_factor][:max_beams]
+ scan[scan >= all_data['max_range']] = 0
+ scans.append(scan)
+
+ except FileNotFoundError:
+ print('Ground truth poses are not available.')
+
+ return np.array(timestamps), np.array(poses_gt), np.array(odoms), np.array(scans), params
+
+
+def particles2pose(particles):
+ """
+ Convert particles to the estimated pose accodring to the particles' distribution
+ :param particles: 2-D array, (N, 4) shape
+ :return: a estimated 2D pose, 1-D array, (3,) shape
+ """
+ normalized_weight = particles[:, 3] / np.sum(particles[:, 3])
+
+ # average angle (https://vicrucann.github.io/tutorials/phase-average/)
+ particles_mat = np.zeros_like(particles)
+ particles_mat[:, :2] = particles[:, :2]
+ particles_mat[:, 2] = np.cos(particles[:, 2])
+ particles_mat[:, 3] = np.sin(particles[:, 2])
+ estimated_pose_temp = particles_mat.T.dot(normalized_weight.T)
+
+ estimated_pose = np.zeros(shape=(3,))
+ estimated_pose[:2] = estimated_pose_temp[:2]
+ estimated_pose[2] = np.arctan2(estimated_pose_temp[-1], estimated_pose_temp[-2])
+
+ return estimated_pose
+
+
+def get_est_poses(all_particles, start_idx, numParticles):
+ estimated_traj = []
+ ratio = 0.8
+
+ for frame_idx in range(start_idx, all_particles.shape[0]):
+ particles = all_particles[frame_idx]
+ # collect top 80% of particles to estimate pose
+ idxes = np.argsort(particles[:, 3])[::-1]
+ idxes = idxes[:int(ratio * numParticles)]
+
+ partial_particles = particles[idxes]
+ if np.sum(partial_particles[:, 3]) == 0:
+ continue
+
+ estimated_pose = particles2pose(partial_particles)
+ estimated_traj.append(estimated_pose)
+
+ estimated_traj = np.array(estimated_traj)
+
+ return estimated_traj
+
+
+def convert2tum(timestamps, poses):
+ tum_poses = []
+
+ for t, pose in zip(timestamps, poses):
+ x, y, yaw = pose
+ q = R.from_euler('z', yaw).as_quat()
+ curr_data = [t,
+ x, y, 0,
+ q[0], q[1], q[2], q[3]]
+
+ tum_poses.append(curr_data)
+
+ tum_poses = np.array(tum_poses)
+
+ return tum_poses
+
+
+def evaluate_APE(est_poses, gt_poses, use_converge=False):
+ # align est and gt
+ max_diff = 0.01
+ traj_ref, traj_est = sync.associate_trajectories(gt_poses, est_poses, max_diff)
+ data = (traj_ref, traj_est)
+
+ # location error
+ ape_location = metrics.APE(metrics.PoseRelation.translation_part)
+ ape_location.process_data(data)
+ location_errors = ape_location.error
+
+ location_ptc5 = location_errors < 0.05
+ location_ptc5 = np.sum(location_ptc5) / location_ptc5.shape[0] * 100
+
+ location_ptc10 = location_errors < 0.1
+ location_ptc10 = np.sum(location_ptc10) / location_ptc10.shape[0] * 100
+
+ location_ptc20 = location_errors < 0.2
+ location_ptc20 = np.sum(location_ptc20) / location_ptc20.shape[0] * 100
+
+ location_rmse = ape_location.get_statistic(metrics.StatisticsType.rmse) * 100
+
+ # yaw error
+ ape_yaw = metrics.APE(metrics.PoseRelation.rotation_angle_deg)
+ ape_yaw.process_data(data)
+
+ yaw_errors = ape_yaw.error
+ yaw_ptc5 = yaw_errors < 0.5
+ yaw_ptc5 = np.sum(yaw_ptc5) / yaw_ptc5.shape[0] * 100
+
+ yaw_ptc10 = yaw_errors < 1.0
+ yaw_ptc10 = np.sum(yaw_ptc10) / yaw_ptc10.shape[0] * 100
+
+ yaw_ptc20 = yaw_errors < 2.0
+ yaw_ptc20 = np.sum(yaw_ptc20) / yaw_ptc20.shape[0] * 100
+
+ yaw_rmse = ape_yaw.get_statistic(metrics.StatisticsType.rmse)
+
+ if use_converge:
+ converge_idx = 0
+ for idx in range(location_errors.shape[0]):
+ if location_errors[idx] < 0.5 and yaw_errors[idx] < 10:
+ converge_idx = idx
+ break
+ location_rmse = np.sqrt(np.mean(location_errors[converge_idx:] ** 2)) * 100
+ yaw_rmse = np.sqrt(np.mean(yaw_errors[converge_idx:] ** 2))
+
+ return location_rmse, location_ptc5, location_ptc10, location_ptc20, \
+ yaw_rmse, yaw_ptc5, yaw_ptc10, yaw_ptc20
+
+
+def summary_loc(loc_results, start_idx, numParticles, timestamps,
+ result_dir, gt_file, init_time_thres=20, use_converge=False):
+ # convert loc_results to tum format
+ timestamps = timestamps[start_idx:]
+
+ # get estimated poses
+ est_poses = get_est_poses(loc_results, start_idx, numParticles)
+ est_tum = convert2tum(timestamps, est_poses)
+
+ # save est_traj in tum format
+ est_tum_file = os.path.join(result_dir, 'IRMCL.txt')
+ np.savetxt(est_tum_file, est_tum)
+
+ # evo evaluation
+ print('\nEvaluation')
+
+ # Estimated poses
+ est_poses = file_interface.read_tum_trajectory_file(est_tum_file)
+ est_poses.reduce_to_time_range(init_time_thres)
+
+ # GT
+ gt_poses = file_interface.read_tum_trajectory_file(gt_file)
+ gt_poses.reduce_to_time_range(init_time_thres)
+
+ print("Sequence information: ", gt_poses)
+ print(("{:>15}\t" * 8).format(
+ "location_rmse", "location_ptc5", "location_ptc10", "location_ptc20",
+ "yaw_rmse", "yaw_ptc5", "yaw_ptc10", "yaw_ptc20"))
+
+ location_rmse, location_ptc5, location_ptc10, location_ptc20, \
+ yaw_rmse, yaw_ptc5, yaw_ptc10, yaw_ptc20 = \
+ evaluate_APE(est_poses, gt_poses, use_converge=use_converge)
+
+ # print error info
+ print(("{:15.2f}\t" * 8).format(
+ location_rmse, location_ptc5, location_ptc10, location_ptc20,
+ yaw_rmse, yaw_ptc5, yaw_ptc10, yaw_ptc20))
+
+
+if __name__ == '__main__':
+ demo_results = np.load('./results/ipblab/loc_test/test1/loc_results.npz')
+ # loading localization results
+ timestamps = demo_results['timestamps']
+ particles = demo_results['particles']
+ start_idx = demo_results['start_idx']
+ numParticles = demo_results['numParticles']
+
+ gt_file ='./data/ipblab/loc_test/test1/seq_1_gt_pose.txt'
+ result_dir = './results/ipblab/loc_test/test1/'
+
+ summary_loc(particles, start_idx, numParticles, timestamps, result_dir, gt_file)