├── .github └── workflows │ └── cmake-single-platform.yml ├── README.md ├── README ├── 10.1177_02783649241303525-fig15.jpg ├── PALoc.svg+xml ├── image (4).png ├── image (5).png ├── image (6).png ├── image-1706213398646-3.png ├── image-20230101200651976.png ├── image-20230106135937336.png ├── image-20230106140017020.png ├── image-20240730152951528.png ├── image-20241127080805642.png ├── image-20241127083256739.png ├── image-20241127083549685.png ├── image-20241127083603307.png ├── image-20241127083707943.png ├── image-20241127083751932.png ├── image-20241127083801691.png ├── image-20241127083813797.png ├── image-20241127083906299.png ├── image-20241127083957970.png ├── image-20241127084143144.png ├── image-20241127084154114.png ├── image-20241127084220301.png ├── image-20241127084229287.png ├── image-20241127091209844.png ├── image-20241127091224755.png ├── image-20241129091541655.png ├── image-20241129091604653.png ├── image-20241129091746334.png ├── image-20241129091823199.png ├── image-20241129091845196.png ├── image-20241129091927786.png ├── image-20241129092017261.png ├── image-20241129092052634.png ├── image-20241129123446075.png ├── image-20250212202446474.png ├── image-20250212202920950.png ├── image-20250212202933255.png ├── image-20250212203009074.png ├── image-20250212203025149.png ├── image-20250214100110872.png ├── image-20250304121753995.png ├── image-20250322192302315.png ├── image-20250322192323830.png ├── image-20250322192349614.png ├── image-20250508072506829.png ├── image-20250508072544312.png ├── image-20250508072654363.png ├── image-20250508072706474.png ├── image-20250508072719975.png ├── image-20250513202215918.png ├── image-20250513202632779.png ├── image-20250513202654237.png ├── image-20250513203004722.png ├── image-20250513203017745.png ├── image-20250513204209752.png ├── image-20250513205043100.png ├── image-20250515144617477.png ├── image-20250515144842908.png ├── image-20250515144853720.png ├── image-20250515144915631.png ├── image-20250515144943191.png ├── image-20250515145431522.png ├── image-20250517201011720.png └── image-20250517201051378.png └── map_eval ├── .idea ├── .gitignore ├── easycode.ignore ├── libraries │ ├── ROS.xml │ └── workspace.xml ├── map_eval.iml ├── misc.xml ├── modules.xml ├── ros.xml └── vcs.xml ├── CMakeLists.txt ├── config ├── config.yaml └── template.yaml ├── include └── tic_toc.h ├── scripts ├── error-visualization.py ├── voxel_errors.txt └── voxel_wasserstein_cdf.txt └── src ├── map_eval.cpp ├── map_eval.h ├── map_eval_main.cpp ├── voxel_calculator.cpp └── voxel_calculator.hpp /.github/workflows/cmake-single-platform.yml: -------------------------------------------------------------------------------- 1 | name: CMake CI (Ubuntu 20.04) 2 | 3 | on: 4 | push: 5 | branches: [ "main" ] 6 | pull_request: 7 | branches: [ "main" ] 8 | 9 | env: 10 | BUILD_TYPE: Release 11 | 12 | jobs: 13 | build: 14 | runs-on: ubuntu-20.04 # 明确指定 Ubuntu 20.04 15 | 16 | steps: 17 | - uses: actions/checkout@v4 18 | 19 | - name: Install dependencies 20 | run: | 21 | # 添加 Open3D 官方 PPA 22 | sudo apt-get install -y software-properties-common 23 | sudo add-apt-repository ppa:open3d/ppa -y 24 | sudo apt-get update 25 | 26 | # 安装指定版本 Open3D (0.17+) 27 | sudo apt-get install -y \ 28 | libopen3d-dev=0.17.0+* \ 29 | cmake \ 30 | g++ \ 31 | libeigen3-dev \ 32 | libpcl-dev \ 33 | libyaml-cpp-dev 34 | 35 | - name: Configure CMake 36 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} 37 | 38 | - name: Build 39 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} -j 2 40 | 41 | # 可选测试步骤 42 | - name: Test 43 | working-directory: ${{github.workspace}}/build 44 | run: ctest -C ${{env.BUILD_TYPE}} 45 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 | 3 |

MapEval: Towards Unified, Robust and Efficient SLAM Map Evaluation Framework

4 | 5 | [**Xiangcheng Hu**](https://github.com/JokerJohn)1 · [**Jin Wu**](https://zarathustr.github.io/)1 · [**Mingkai Jia**](https://github.com/MKJia)1· [**Hongyu Yan**](https://scholar.google.com/citations?user=TeKnXhkAAAAJ&hl=zh-CN)1· [**Yi Jiang**](https://yijiang1992.github.io/)2· [**Binqian Jiang**](https://github.com/lewisjiang/)1 6 |
7 | [**Wei Zhang**](https://ece.hkust.edu.hk/eeweiz)1 · [**Wei He**](https://sites.google.com/view/drweihecv/home/)3 · [**Ping Tan**](https://facultyprofiles.hkust.edu.hk/profiles.php?profile=ping-tan-pingtan#publications)1*† 8 | 9 | 1**HKUST   2CityU   3USTB** 10 |
11 | †Project lead *Corresponding Author 12 | 13 | Paper PDF[![GitHub Stars](https://img.shields.io/github/stars/JokerJohn/Cloud_Map_Evaluation.svg)](https://github.com/JokerJohn/Cloud_Map_Evaluation/stargazers) 14 | FORK 15 | PRs-Welcome[![GitHub Issues](https://img.shields.io/github/issues/JokerJohn/Cloud_Map_Evaluation.svg)](https://github.com/JokerJohn/Cloud_Map_Evaluation/issues)[![License](https://img.shields.io/badge/license-MIT-blue.svg)](https://opensource.org/licenses/MIT) 16 | 17 |
18 | 19 | ![image-20241127080805642](./README/image-20241127080805642.png) 20 | 21 | 22 | MapEval is a comprehensive framework for evaluating point cloud maps in SLAM systems, addressing two fundamentally distinct aspects of map quality assessment: 23 | 1. **Global Geometric Accuracy**: Measures the absolute geometric fidelity of the reconstructed map compared to ground truth. This aspect is crucial as SLAM systems often accumulate drift over long trajectories, leading to global deformation. 24 | 2. **Local Structural Consistency**: Evaluates the preservation of local geometric features and structural relationships, which is essential for tasks like obstacle avoidance and local planning, even when global accuracy may be compromised. 25 | 26 | These complementary aspects require different evaluation approaches, as global drift may exist despite excellent local reconstruction, or conversely, good global alignment might mask local inconsistencies. Our framework provides a unified solution through both traditional metrics and novel evaluation methods based on optimal transport theory. 27 | 28 | ## News 29 | 30 | - **2025/05/05:** Add new test data and remove the simulation codes. 31 | - **2025/03/05**: [Formally published](https://ieeexplore.ieee.org/document/10910156)! 32 | - **2025/02/25**: Accept! 33 | - **2025/02/12**: Codes released! 34 | - **2025/02/05**: Resubmit. 35 | - **2024/12/19**: Submitted to **IEEE RAL**! 36 | 37 | ## Key Features 38 | 39 | **Traditional Metrics Implementation**: 40 | 41 | - **Accuracy** (AC): Point-level geometric error assessment 42 | - **Completeness** (COM): Map coverage evaluation. 43 | - **Chamfer Distance** (CD): Bidirectional point cloud difference 44 | - **Mean Map Entropy** (MME): Information-theoretic local consistency metric 45 | 46 | **Novel Proposed Metrics**: 47 | 48 | - **Average Wasserstein Distance** (AWD): Robust global geometric accuracy assessment 49 | - **Spatial Consistency Score** (SCS): Enhanced local consistency evaluation 50 | 51 | | ![image-20250304121753995](./README/image-20250304121753995.png) | 52 | | ------------------------------------------------------------ | 53 | 54 | ## Results 55 | 56 | ### Simulated experiments 57 | 58 | | Noise Sensitivity | Outlier Robustness | 59 | | ------------------------------------------------------------ | ------------------------------------------------------------ | 60 | | ![image-20241129123446075](./README/image-20241129123446075.png) | ![image-20241129091845196](./README/image-20241129091845196.png) | 61 | 62 | ![image-20241127083707943](./README/image-20241127083707943.png) 63 | 64 | ### Real-world experiments 65 | 66 | | Map Evaluation via Localization Accuracy | Map Evaluation in Diverse Environments | 67 | | ------------------------------------------------------------ | ------------------------------------------------------------ | 68 | | ![image-20241127083813797](./README/image-20241127083813797.png) | ![image-20241127083801691](./README/image-20241127083801691.png) | 69 | 70 | | ![image-20241129092052634](./README/image-20241129092052634.png) | 71 | | ------------------------------------------------------------ | 72 | 73 | ## Efficiency and Parameter Analysis 74 | 75 | | ![image-20250322192323830](./README/image-20250322192323830.png) | ![image-20250322192349614](./README/image-20250322192349614.png) | 76 | | ------------------------------------------------------------ | ------------------------------------------------------------ | 77 | 78 | ## Datasets 79 | 80 | | [MS-dataset](https://github.com/JokerJohn/MS-Dataset) | [FusionPortable (FP) and FusionPortableV2 dataset](https://fusionportable.github.io/dataset/fusionportable_v2/) | [Newer College (NC)](https://ori-drs.github.io/newer-college-dataset/) | [ GEODE dataset (GE)](https://github.com/PengYu-Team/GEODE_dataset) | 81 | | ----------------------------------------------------- | ------------------------------------------------------------ | ------------------------------------------------------------ | ------------------------------------------------------------ | 82 | 83 | | ![image-20250322192302315](./README/image-20250322192302315.png) | 84 | | ------------------------------------------------------------ | 85 | 86 | 87 | 88 | ## Quickly Run 89 | 90 | ### Dependencies 91 | 92 | - *[Open3d ( >= 0.11)](https://github.com/isl-org/Open3D)* 93 | - Eigen3 94 | - yaml-cpp 95 | - Ubuntu 20.04 96 | 97 | ### Test Data(password: 1) 98 | 99 | | sequence | | Test PCD | GT PCD | 100 | | -------- | ------------------------------------------------------------ | ------------------------------------------------------------ | ------------------------------------------------------------ | 101 | | MCR_slow | ![image-20250515145431522](./README/image-20250515145431522.png) | [map.pcd](https://hkustconnect-my.sharepoint.com/:u:/g/personal/xhubd_connect_ust_hk/ES9eSANEr-9NvkFqMzMFsecBo5r3hBpBnj0c6BMPgsfXnQ?e=aijdPf) | [map_gt.pcd](https://hkustconnect-my.sharepoint.com/:u:/g/personal/xhubd_connect_ust_hk/ESfn5EEsiPlCiJcydVc_HqgBDGqy65MHoyu63XE-iKbFBQ?e=dTDon4) | 102 | | PK01 | ![image-20250515144915631](./README/image-20250515144915631.png) | [map.pcd](https://hkustconnect-my.sharepoint.com/:u:/g/personal/xhubd_connect_ust_hk/ERPFVJN6CtBKtHlPWyni-jIB0dgLzgF1FGxPTatKoCp02Q?e=TEgfBp) | [gt.pcd](https://hkustconnect-my.sharepoint.com/:u:/g/personal/xhubd_connect_ust_hk/EeztnFHwKJlCoW-fmKljaMMBSvNvT5BkTXxoA1iXqeUS5A?e=37evMi) | 103 | 104 | ### Usage 105 | 106 | 1. install open3d. (maybe a higer version of CMake is needed) 107 | 108 | ```bash 109 | git clone https://github.com/isl-org/Open3D.git 110 | cd Open3D && mkdir build && cd build 111 | cmake .. 112 | make install 113 | ``` 114 | 115 | 2. set and read the instruction of some params in [config.yaml](map_eval/config/config.yaml). 116 | 117 | ```yaml 118 | # accuracy_level, vector5d, we mainly use the result of the first element 119 | # if inlier is very small, we can try to larger the value, e.g. for outdoors, [0.5, 0.3, 0.2, 0.1, 0.05] 120 | accuracy_level: [0.2, 0.1, 0.08, 0.05, 0.01] 121 | 122 | # initial_matrix, vector16d, the initial matrix of the registration 123 | # make sure the format is correct, or you will got the error log: YAML::BadSubscript' what(): operator[] call on a scalar 124 | initial_matrix: 125 | - [1.0, 0.0, 0.0, 0.0] 126 | - [0.0, 1.0, 0.0, 0.0] 127 | - [0.0, 0.0, 1.0, 0.0] 128 | - [0.0, 0.0, 0.0, 1.0] 129 | 130 | # vmd voxel size, outdoor: 2.0-4.0; indoor: 2.0-3.0 131 | vmd_voxel_size: 3.0 132 | ``` 133 | 134 | 135 | 3. complie map_eval 136 | 137 | ```bash 138 | git clone https://github.com/JokerJohn/Cloud_Map_Evaluation.git 139 | cd Cloud_Map_Evaluation/map_eval && mkdir build 140 | cmake .. 141 | make 142 | ``` 143 | 144 | 4. get the final results 145 | 146 | ```bash 147 | ./map_eval 148 | ``` 149 | 150 | we have a point cloud map generated by a pose-slam system, and we have a ground truth point cloud map. Then we caculate related metrics. 151 | 152 | ![image-20250214100110872](./README/image-20250214100110872.png) 153 | 154 | ### Visulization 155 | 156 | We can also get a rendered raw distance-error map(10cm) and inlier distance-error map(2cm) in this process, the color R->G->B represent for the distance error at a level of 0-10cm. 157 | 158 | ![image (4)](./README/image%20(4).png) 159 | 160 | **if we do not have gt ma**p, we can only evaluate the **Mean Map Entropy (MME)**, smaller means better consistency. just set `evaluate_mme: false` in **[config.yaml](map_eval/config/config.yaml)**. 161 | 162 | ![image (5)](./README/image%20(5).png) 163 | 164 | we can also get a simpe mesh reconstructed from point cloud map. 165 | 166 | ![image-20230101200651976](README/image-20230101200651976.png) 167 | 168 | 5. we got the result flies. 169 | 170 | ![image-20250212202446474](./README/image-20250212202446474.png) 171 | 172 | 6. if you want to get the visulization of voxel errors, use the [error-visualization.py](map_eval/scripts/error-visualization.py) 173 | 174 | ```python 175 | pip install numpy matplotlib scipy 176 | 177 | python3 error-visualization.py 178 | ``` 179 | 180 | | ![image-20250212202920950](./README/image-20250212202920950.png) | ![image-20250212202933255](./README/image-20250212202933255.png) | 181 | | ------------------------------------------------------------ | ------------------------------------------------------------ | 182 | | ![image-20250212203009074](./README/image-20250212203009074.png) | ![image-20250212203025149](./README/image-20250212203025149.png) | 183 | 184 | ## Issues 185 | 186 | ### How do you get your initial pose? 187 | 188 | we can use [CloudCompare](https://github.com/CloudCompare/CloudCompare) to align LIO map to Gt map . 189 | 190 | - Roughly translate and rotate the LIO point cloud map to the GT map。 191 | 192 | - Manually register the moved LIO map (aligned) to the GT map (reference), and get the output of the terminal transfrom `T2`, then the initial pose matrix is the terminal output transform `T`. 193 | 194 | | ![image-20230106135937336](README/image-20230106135937336.png) | ![image-20230106140017020](README/image-20230106140017020.png) | 195 | | ------------------------------------------------------------ | ------------------------------------------------------------ | 196 | 197 | ### What's the difference between raw rendered map and inlier rendered map? 198 | 199 | The primary function of the r**aw rendered map** (left) is to color-code the error of all points in the map estimated by the algorithm. For each point in the estimated map that does not find a corresponding point in the **ground truth (gt) map**, it is defaulted to the maximum error (**20cm**), represented as red. On the other hand, the i**nlier rendered map** (right) excludes the non-overlapping regions of the point cloud and colors only the error of the inlier points after point cloud matching. This map therefore contains only a portion of the points from the original estimated map. (remindered by [John-Henawy](https://github.com/John-Henawy) in [issue 5](https://github.com/JokerJohn/Cloud_Map_Evaluation/issues/5)) 200 | 201 | ![image (6)](./README/image%20(6).png) 202 | 203 | ### **Applicable Scenarios:** 204 | 205 | 1. **With a ground truth map:** All metrics are applicable. 206 | 207 | 2. **Without a ground truth map** (remindered by [@Silentbarber](https://github.com/Silentbarber), [ZOUYIyi](https://github.com/ZOUYIyi) in [issue 4](https://github.com/JokerJohn/Cloud_Map_Evaluation/issues/4) and [issue 7](https://github.com/JokerJohn/Cloud_Map_Evaluation/issues/7)): 208 | 209 | - Only **MME** can be used for evaluation. It is crucial to remember that the maps being evaluated must be on the same scale. 210 | 211 | > For example, **you cannot compare a LIO map with a LIO SLAM map** that has performed loop closure optimization. This is because loop closure adjusts the local point cloud structure, leading to inaccurate MME evaluation. You can compare the MME of different LIO maps. 212 | 213 | ## Publications 214 | 215 | We recommend to cite [our paper](https://arxiv.org/abs/2411.17928) if you find this library useful: 216 | 217 | ```latex 218 | @misc{hu2024mapeval, 219 | title={MapEval: Towards Unified, Robust and Efficient SLAM Map Evaluation Framework}, 220 | author={Xiangcheng Hu and Jin Wu and Mingkai Jia and Hongyu Yan and Yi Jiang and Binqian Jiang and Wei Zhang and Wei He and Ping Tan}, 221 | year={2025}, 222 | volume={10}, 223 | number={5}, 224 | pages={4228-4235}, 225 | doi={10.1109/LRA.2025.3548441} 226 | } 227 | 228 | @article{wei2024fpv2, 229 | title={Fusionportablev2: A unified multi-sensor dataset for generalized slam across diverse platforms and scalable environments}, 230 | author={Wei, Hexiang and Jiao, Jianhao and Hu, Xiangcheng and Yu, Jingwen and Xie, Xupeng and Wu, Jin and Zhu, Yilong and Liu, Yuxuan and Wang, Lujia and Liu, Ming}, 231 | journal={The International Journal of Robotics Research}, 232 | pages={02783649241303525}, 233 | year={2024}, 234 | publisher={SAGE Publications Sage UK: London, England} 235 | } 236 | ``` 237 | 238 | ## Related Package 239 | 240 | The folloing works use MapEval for map evalution. 241 | 242 | | Work | Tasks | Date | Metrics | Demo | 243 | | ------------------------------------------------------------ | ---------------------------------------- | ---------- | --------- | ------------------------------------------------------------ | 244 | | [**LEMON-Mapping**](https://arxiv.org/abs/2505.10018) | Multi-Session Point Cloud mapping | Arxiv'2025 | MME | ![image-20250517201051378](./README/image-20250517201051378.png) | 245 | | **[CompSLAM](https://arxiv.org/abs/2505.06483)** | Multi-Modal Localization
and Mapping | Arxiv'2025 | AWD/SCS | ![image-20250513202215918](./README/image-20250513202215918.png) | 246 | | [**GEODE**](https://github.com/PengYu-Team/GEODE_dataset) | SLAM Dataset | IJRR'2025 | | ![image-20250513204209752](./README/image-20250513204209752.png) | 247 | | [**ELite**](https://github.com/dongjae0107/ELite) | LiDAR-based Lifelong Mapping | ICRA'2025 | AC/CD | ![image-20250513202654237](./README/image-20250513202654237.png) | 248 | | [**PALoc**](https://github.com/JokerJohn/PALoc) | Prior-Assisted 6-DoF Localization | TMECH'2024 | AC/CD | ![image-20250513205043100](./README/image-20250513205043100.png) | 249 | | [**MS-Mapping**](https://github.com/JokerJohn/MS-Mapping) | Multi-session LiDAR mapping | Arxiv'2024 | AC/CD/MME | ![image-20240730152951528](./README/image-20240730152951528.png) | 250 | | [**FusionPortableV2**](https://journals.sagepub.com/doi/full/10.1177/02783649241303525) | SLAM Dataset | IJRR'2024 | COM/CD | ![img](./README/10.1177_02783649241303525-fig15.jpg) | 251 | 252 | ## Contributors 253 | 254 | 255 | 256 | 257 | 258 | ![Star History Chart](https://api.star-history.com/svg?repos=JokerJohn/Cloud_Map_Evaluation&type=Date) 259 | -------------------------------------------------------------------------------- /README/10.1177_02783649241303525-fig15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/10.1177_02783649241303525-fig15.jpg -------------------------------------------------------------------------------- /README/PALoc.svg+xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | \nJokerJohn 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | \nulterzlw 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /README/image (4).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image (4).png -------------------------------------------------------------------------------- /README/image (5).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image (5).png -------------------------------------------------------------------------------- /README/image (6).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image (6).png -------------------------------------------------------------------------------- /README/image-1706213398646-3.png: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | Slack
447 | 448 | 449 | 450 | -------------------------------------------------------------------------------- /README/image-20230101200651976.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20230101200651976.png -------------------------------------------------------------------------------- /README/image-20230106135937336.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20230106135937336.png -------------------------------------------------------------------------------- /README/image-20230106140017020.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20230106140017020.png -------------------------------------------------------------------------------- /README/image-20240730152951528.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20240730152951528.png -------------------------------------------------------------------------------- /README/image-20241127080805642.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127080805642.png -------------------------------------------------------------------------------- /README/image-20241127083256739.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083256739.png -------------------------------------------------------------------------------- /README/image-20241127083549685.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083549685.png -------------------------------------------------------------------------------- /README/image-20241127083603307.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083603307.png -------------------------------------------------------------------------------- /README/image-20241127083707943.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083707943.png -------------------------------------------------------------------------------- /README/image-20241127083751932.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083751932.png -------------------------------------------------------------------------------- /README/image-20241127083801691.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083801691.png -------------------------------------------------------------------------------- /README/image-20241127083813797.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083813797.png -------------------------------------------------------------------------------- /README/image-20241127083906299.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083906299.png -------------------------------------------------------------------------------- /README/image-20241127083957970.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127083957970.png -------------------------------------------------------------------------------- /README/image-20241127084143144.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127084143144.png -------------------------------------------------------------------------------- /README/image-20241127084154114.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127084154114.png -------------------------------------------------------------------------------- /README/image-20241127084220301.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127084220301.png -------------------------------------------------------------------------------- /README/image-20241127084229287.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127084229287.png -------------------------------------------------------------------------------- /README/image-20241127091209844.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127091209844.png -------------------------------------------------------------------------------- /README/image-20241127091224755.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241127091224755.png -------------------------------------------------------------------------------- /README/image-20241129091541655.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129091541655.png -------------------------------------------------------------------------------- /README/image-20241129091604653.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129091604653.png -------------------------------------------------------------------------------- /README/image-20241129091746334.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129091746334.png -------------------------------------------------------------------------------- /README/image-20241129091823199.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129091823199.png -------------------------------------------------------------------------------- /README/image-20241129091845196.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129091845196.png -------------------------------------------------------------------------------- /README/image-20241129091927786.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129091927786.png -------------------------------------------------------------------------------- /README/image-20241129092017261.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129092017261.png -------------------------------------------------------------------------------- /README/image-20241129092052634.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129092052634.png -------------------------------------------------------------------------------- /README/image-20241129123446075.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20241129123446075.png -------------------------------------------------------------------------------- /README/image-20250212202446474.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250212202446474.png -------------------------------------------------------------------------------- /README/image-20250212202920950.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250212202920950.png -------------------------------------------------------------------------------- /README/image-20250212202933255.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250212202933255.png -------------------------------------------------------------------------------- /README/image-20250212203009074.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250212203009074.png -------------------------------------------------------------------------------- /README/image-20250212203025149.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250212203025149.png -------------------------------------------------------------------------------- /README/image-20250214100110872.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250214100110872.png -------------------------------------------------------------------------------- /README/image-20250304121753995.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250304121753995.png -------------------------------------------------------------------------------- /README/image-20250322192302315.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250322192302315.png -------------------------------------------------------------------------------- /README/image-20250322192323830.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250322192323830.png -------------------------------------------------------------------------------- /README/image-20250322192349614.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250322192349614.png -------------------------------------------------------------------------------- /README/image-20250508072506829.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250508072506829.png -------------------------------------------------------------------------------- /README/image-20250508072544312.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250508072544312.png -------------------------------------------------------------------------------- /README/image-20250508072654363.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250508072654363.png -------------------------------------------------------------------------------- /README/image-20250508072706474.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250508072706474.png -------------------------------------------------------------------------------- /README/image-20250508072719975.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250508072719975.png -------------------------------------------------------------------------------- /README/image-20250513202215918.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250513202215918.png -------------------------------------------------------------------------------- /README/image-20250513202632779.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250513202632779.png -------------------------------------------------------------------------------- /README/image-20250513202654237.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250513202654237.png -------------------------------------------------------------------------------- /README/image-20250513203004722.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250513203004722.png -------------------------------------------------------------------------------- /README/image-20250513203017745.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250513203017745.png -------------------------------------------------------------------------------- /README/image-20250513204209752.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250513204209752.png -------------------------------------------------------------------------------- /README/image-20250513205043100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250513205043100.png -------------------------------------------------------------------------------- /README/image-20250515144617477.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250515144617477.png -------------------------------------------------------------------------------- /README/image-20250515144842908.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250515144842908.png -------------------------------------------------------------------------------- /README/image-20250515144853720.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250515144853720.png -------------------------------------------------------------------------------- /README/image-20250515144915631.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250515144915631.png -------------------------------------------------------------------------------- /README/image-20250515144943191.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250515144943191.png -------------------------------------------------------------------------------- /README/image-20250515145431522.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250515145431522.png -------------------------------------------------------------------------------- /README/image-20250517201011720.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250517201011720.png -------------------------------------------------------------------------------- /README/image-20250517201051378.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JokerJohn/Cloud_Map_Evaluation/85e47beec294fdfe1cd560547441f4d9e2daed80/README/image-20250517201051378.png -------------------------------------------------------------------------------- /map_eval/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /map_eval/.idea/easycode.ignore: -------------------------------------------------------------------------------- 1 | node_modules/ 2 | dist/ 3 | vendor/ 4 | cache/ 5 | .*/ 6 | *.min.* 7 | *.test.* 8 | *.spec.* 9 | *.bundle.* 10 | *.bundle-min.* 11 | *.*.js 12 | *.*.ts 13 | *.log 14 | -------------------------------------------------------------------------------- /map_eval/.idea/libraries/ROS.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /map_eval/.idea/libraries/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /map_eval/.idea/map_eval.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /map_eval/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /map_eval/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /map_eval/.idea/ros.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | -------------------------------------------------------------------------------- /map_eval/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /map_eval/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12.0) 2 | project(map_eval) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(Open3D REQUIRED) 8 | find_package(Eigen3 REQUIRED) 9 | find_package(OpenMP REQUIRED) 10 | find_package(PCL REQUIRED) 11 | find_package(yaml-cpp REQUIRED) 12 | 13 | include_directories( 14 | include 15 | ${Open3D_INCLUDE_DIRS} 16 | ${PCL_INCLUDE_DIRS} 17 | ${EIGEN3_INCLUDE_DIR} 18 | ${YAML_CPP_INCLUDE_DIR} 19 | ) 20 | 21 | add_executable(map_eval src/map_eval.cpp src/map_eval_main.cpp src/voxel_calculator.cpp) 22 | target_link_libraries(map_eval ${Open3D_LIBRARIES} ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES}) 23 | 24 | # If OpenMP is found, add the flags for OpenMP 25 | if(OpenMP_CXX_FOUND) 26 | target_link_libraries(map_eval OpenMP::OpenMP_CXX) 27 | endif() -------------------------------------------------------------------------------- /map_eval/config/config.yaml: -------------------------------------------------------------------------------- 1 | # registration methods from Open3D: 0-point-to-point icp; 1-point-to-plane icp; 2-GICP 2 | registration_methods: 2 3 | # set icp_max_distance when search the nearest point, larger value will make the search slower 4 | icp_max_distance: 1.0 5 | 6 | # accuracy_level, vector5d, we mainly use the result of the first element 7 | # if inlier is very small, we can try to larger the value, e.g. for outdoors, [0.5, 0.3, 0.2, 0.1, 0.05] 8 | accuracy_level: [0.2, 0.1, 0.08, 0.05, 0.01] 9 | 10 | # initial_matrix, vector16d, the initial matrix of the registration 11 | # make sure the format is correct, or you will got the error log: YAML::BadSubscript' what(): operator[] call on a scalar 12 | # if you evaluate the LIO mapping result, you can use the CloudCompare to get the initial matrix first 13 | initial_matrix: 14 | - [1.0, 0.0, 0.0, 0.0] 15 | - [0.0, 1.0, 0.0, 0.0] 16 | - [0.0, 0.0, 1.0, 0.0] 17 | - [0.0, 0.0, 0.0, 1.0] 18 | 19 | # estimate map folder path, end with "/", make sure your estimate point cloud and map is renamed as "map.pcd" 20 | # results path will also in the same folder: estimate_map_path + "map_results" 21 | estimate_map_path: /home/xchu/data/ltloc_result/parkinglot_raw_3_0.5/ 22 | 23 | # ground truth map file path 24 | # note that we support ".pcd" or ".ply" format for ground truth map loading 25 | gt_map_path: /media/xchu/e81eaf80-d92c-413a-a503-1c9b35b19963/home/xchu/data/evaluation/est_file/redbird_02/hkustgz_gt_filter.pcd 26 | 27 | 28 | # scene name, not important, just for batch processing for a series of scenes 29 | scene_name: redbird_02 30 | 31 | # other settings 32 | # if we save the immediate result to a text file 33 | save_immediate_result: true 34 | 35 | # if evaluate Mean Map Entropy (MME) if you do not have ground truth map 36 | # but remember, MME can be only used for map comparison with the same scale, e.g., maps are for odometry. 37 | # it costs a lot of time to calculate MME, so we set it as false by default 38 | evaluate_mme: false 39 | 40 | # if we have ground truth map, we also want to evaluate the MME of the ground truth map 41 | evaluate_gt_mme: false 42 | 43 | # NN search radius for MME calculation (m), larger value will make the search much more slower, even the memory overflow 44 | nn_radius: 0.1 45 | 46 | # if we want to evaluate the registration result using initial matrix without alignments, this operation will save time 47 | # that means your estimate map is already aligned with the ground truth map, 48 | # or your estimate map and the ground truth map is already in the same coordinate system 49 | # e.g. the estimate map is already aligned with the ground truth map 50 | evaluate_using_initial: false 51 | 52 | # we want to add noise to the ground truth map, and evaluate the registration result 53 | # note that true will cause the system do not load the estimate map, instead the noise gt_map will be used 54 | # just for algorithm robustness evaluation experiments, not for real-world application 55 | # do not recommand to set it as true, since it will cause the memory overflow 56 | evaluate_noise_gt: false 57 | 58 | # vmd voxel size 59 | # outdoor: 2.0-4.0; indoor: 2.0-3.0 60 | vmd_voxel_size: 3.0 -------------------------------------------------------------------------------- /map_eval/config/template.yaml: -------------------------------------------------------------------------------- 1 | # // M2M 2 | # // initial_matrix << 1.000000, -0.000106, 0.000474, -0.016906, 3 | # // 0.000106, 1.000000, 0.000089, -0.014802, 4 | # // -0.000474, -0.000089, 1.000000, 0.033971, 5 | # // 0.000000, 0.000000, 0.000000, 1.000000; 6 | # // 7 | # // initial_matrix << 8 | # // 0.960812, -0.277197, 0.001265, -618.066589, 9 | # // 0.277199, 0.960811, -0.001669, -530.099548, 10 | # // -0.000753, 0.001954, 0.999998, -0.262354, 11 | # // 0.000000, 0.000000, 0.000000, 1.000000; 12 | 13 | 14 | # //pk01 15 | # // initial_matrix << 0.960144, -0.279489, 0.003107, -618.654968, 16 | # // 0.279501, 0.960135, -0.004535, -531.414307, 17 | # // -0.001716, 0.005223, 0.999985, 0.797657, 18 | # // 0.000000, 0.000000, 0.000000, 1.000000; 19 | 20 | # //cp02 21 | # // initial_matrix << 0.961640, -0.273847, -0.016017, -617.292969, 22 | # // 0.273811, 0.961773, -0.004414, -531.806335, 23 | # // 0.016614, -0.000141, 0.999862, -0.683528, 24 | # // 0.000000, 0.000000, 0.000000, 1.000000; 25 | 26 | # //rb02-pk01 27 | # // initial_matrix << 0.647740, -0.761514, 0.023015, -224.383774, 28 | # // 0.761586, 0.648022, 0.007339, -533.341492, 29 | # // -0.020503, 0.012774, 0.999708, -2.961943, 30 | # // 0.000000, 0.000000, 0.000000, 1.000000; 31 | 32 | # // pk01-rb02 33 | # // initial_matrix << 0.555416, -0.831520, -0.009418, -224.244492, 34 | # // 0.831077, 0.554658, 0.040799, -533.391968, 35 | # // -0.028702, -0.030488, 0.999123, -2.854761, 36 | # // 0.000000, 0.000000, 0.000000, 1.000000; 37 | 38 | # // final_map 39 | # // initial_matrix << 0.959998, -0.279999, 0.001843, -619.712585, 40 | # // 0.280004, 0.959994, -0.002949, -532.095276, 41 | # // -0.000944, 0.003347, 0.999994, 0.671322, 42 | # // 0.000000, 0.000000, 0.000000, 1.000000; 43 | # // final_map 44 | # // initial_matrix << 0.959998, -0.279999, 0.001843, -619.712585, 45 | # // 0.280004, 0.959994, -0.002949, -532.095276, 46 | # // -0.000944, 0.003347, 0.999994, 0.671322, 47 | # // 0.000000, 0.000000, 0.000000, 1.000000; 48 | 49 | # // corridor 50 | # // initial_matrix << 0.96644286559409200649, -0.25685421425740788204, 0.003751062431636435972, 5.2711577417659201819, 51 | # // 0.2557153541051056066, 0.96334248524454570928, 0.081122654191151540328, -1.1168942975664494433, 52 | # // -0.024450250723975192019, -0.077441211946613232512, 0.99669706976879401463, 1.3392035133175538313, 53 | # // 0, 0, 0, 1; 54 | 55 | # // garden_day 56 | # // initial_matrix << 0.92392991826390828621, -0.38182828210102536392, 0.023675701859957654001, -10.666406090136118014, 57 | # // 0.3799200830223184679, 0.92305157679461396519, 0.060301267662141853306, 4.3198569024680215866, 58 | # // -0.044878628673443480657, -0.04671928766342014961, 0.99789949016990996094, 0.12595325360521592275, 59 | # // 0, 0, 0, 1; 60 | 61 | 62 | # // canteen_day 63 | # // initial_matrix << 0.940271139, 0.336312294, 0.052765001, 14.558010101, 64 | # // -0.337770432, 0.940984249, 0.021438926, 6.669913769, 65 | # // -0.042440865, -0.037980862, 0.998376787, -0.262503862, 66 | # // 0, 0, 0, 1; 67 | 68 | # // escalator_day 69 | # // initial_matrix << 0.849545730, -0.526321899, 0.035462153, 8.465351834, 70 | # // 0.526326958, 0.850224213, 0.009950397, 5.680197954, 71 | # // -0.035387884, 0.010211390, 0.999321354, -14.453351801, 72 | # // 0.000000000, 0.000000000, 0.000000000, 1.000000000; 73 | 74 | # // building_day 75 | # // initial_matrix << 0.181766, -0.982662, -0.036567, -40.590164, 76 | # // 0.982241, 0.179678, 0.054026, -47.248627, 77 | # // -0.046519, -0.045737, 0.997870, 0.440276, 78 | # // 0.000000, 0.000000, 0.000000, 1.000000; 79 | 80 | # // MCR_slow 81 | # // initial_matrix 82 | # // << -0.0056020604196165634633, -0.99925106193029763167, 0.038287900661027018002, -3.1218551280869197771, 83 | # // 0.99985558705401715644, -0.0062116612580020565699, -0.015821133550722513169, 4.5688692239484516282, 84 | # // 0.016047114506, 0.038193736225, 0.999141514301, 0.464199572802, 85 | # // 0, 0, 0, 1; 86 | 87 | # // MCR_normal 88 | # // initial_matrix << 0.017268123495677515861, -0.99476377592509216863, 0.10073144488699185579, -2.9241390841996816532, 89 | # // 0.99972825177520137434, 0.01560025219016034088, -0.017321959286189889882, 4.9911020280692108359, 90 | # // 0.015659822151, 0.101003192365, 0.994762837887, 0.172949373722, 91 | # // 0, 0, 0, 1; 92 | 93 | # // MCR_slow_00 94 | # //initial_matrix << 0.073423121685455595366,-0.99719976377883316368,-0.014203226717673212819,-3.8523073312988105304,0.99719256862396939241, 95 | # // 0.073617619309242086538,-0.013692776804172883327,3.751613106250077957, 96 | # // 0.014700041153,-0.013157985173,0.999805390835,-0.28315025568, 97 | # // 0,0,0,1; 98 | 99 | # //MCR_slow_01 100 | # //initial_matrix <<-0.10700602171464870212,-0.99422775704721769524, -0.0078029036818991450509,-3.146451078289421617, 101 | # // 0.99413613644708572293,-0.10686650631867033195,-0.016519902842021469619,3.9637429210598714955, 102 | # // 0.015590677969,-0.009524877183,0.999833106995,-0.28624022007, 103 | # // 0,0,0,1; 104 | 105 | # //MCR_normal_00 106 | # //initial_matrix << 0.017268123495677515861, -0.99476377592509216863 ,0.10073144488699185579 ,-2.9241390841996816532, 107 | # // 0.99972825177520137434, 0.01560025219016034088 ,-0.017321959286189889882, 4.9911020280692108359, 108 | # // 0.015659822151, 0.101003192365, 0.994762837887, 0.172949373722, 109 | # // 0, 0, 0 ,1; 110 | 111 | # //MCR_normal_01 112 | # // initial_matrix 113 | # // << -0.034197986233380032788, -0.99927614790418071225, -0.016665546850371321898, -3.1286035805520017087, 114 | # // 0.99926045766011680647, -0.034481260815426883827, 0.01701738626308704885, 2.9765895215817617764, 115 | # // -0.017579715699, -0.016071259975, 0.999716281891, -0.373287737369, 116 | # // 0, 0, 0, 1; 117 | 118 | # //stairs_alpha 119 | # // initial_matrix << -0.519301, 0.850557, 0.082936, -11.347226, 120 | # // -0.852164, -0.522691, 0.024698, 3.002144, 121 | # // 0.064357, -0.057849, 0.996249, -0.715776, 122 | # // 0.000000, 0.000000, 0.000000, 1.000000; 123 | 124 | # //stairs_bob 125 | # // initial_matrix << -0.519301, 0.850557, 0.082936, -11.347226, 126 | # // -0.852164, -0.522691, 0.024698, 3.002144, 127 | # // 0.064357, -0.057849, 0.996249, -0.715776, 128 | # // 0.000000, 0.000000, 0.000000, 1.000000; 129 | 130 | # // math-institute 131 | # // initial_matrix << -0.370474, -0.928788, -0.010043, -23.358046, 132 | # // 0.928795, -0.370324, -0.014133, -33.184776, 133 | # // 0.009408, -0.014564, 0.999850, 1.068895, 134 | # // 0.000000, 0.000000, 0.000000, 1.000000; 135 | 136 | # // parkland0 137 | # // initial_matrix << 0.590137, -0.806766, 0.029430, 6.874957, 138 | # // 0.807092, 0.590424, 0.001337, -55.680973, 139 | # // -0.018454, 0.022964, 0.999566, 0.990686, 140 | # // 0.000000, 0.000000, 0.000000, 1.000000; 141 | 142 | # //parkland cc 143 | # // initial_matrix << 0.700082, -0.712298, 0.050161, 5.999269, 144 | # // 0.713462, 0.700646, -0.008230, -56.969807, 145 | # // -0.029283, 0.041549, 0.998707, 1.240506, 146 | # // 0.000000, 0.000000, 0.000000, 1.000000; 147 | 148 | 149 | # //pk01 150 | # // initial_matrix << 0.556912342, -0.830538784, -0.007341485, -223.941216745, 151 | # // 0.830297811, 0.556480475, 0.030576983, -534.004804865, 152 | # // -0.021309977, -0.023124317, 0.999505452, -1.854225961, 153 | # // 0.000000000, 0.000000000, 0.000000000, 1.000000000; 154 | -------------------------------------------------------------------------------- /map_eval/include/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc { 11 | public: 12 | TicToc() { tic(); } 13 | 14 | void tic() { start = std::chrono::system_clock::now(); } 15 | 16 | double toc() { 17 | end = std::chrono::system_clock::now(); 18 | std::chrono::duration elapsed_seconds = end - start; 19 | return elapsed_seconds.count() * 1000; 20 | } 21 | 22 | private: 23 | std::chrono::time_point start, end; 24 | }; 25 | 26 | class TicTocV2 { 27 | public: 28 | TicTocV2() { tic(); } 29 | 30 | TicTocV2(bool _disp) { 31 | disp_ = _disp; 32 | tic(); 33 | } 34 | 35 | void tic() { start = std::chrono::system_clock::now(); } 36 | 37 | void toc(std::string _about_task) { 38 | end = std::chrono::system_clock::now(); 39 | std::chrono::duration elapsed_seconds = end - start; 40 | double elapsed_ms = elapsed_seconds.count() * 1000; 41 | 42 | if (disp_) { 43 | std::cout.precision(3); // 10 for sec, 3 for ms 44 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 45 | } 46 | } 47 | 48 | private: 49 | std::chrono::time_point start, end; 50 | bool disp_ = false; 51 | }; -------------------------------------------------------------------------------- /map_eval/scripts/error-visualization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib.patches import Rectangle 4 | from matplotlib.colors import LinearSegmentedColormap 5 | from mpl_toolkits.mplot3d import Axes3D 6 | import matplotlib.ticker as ticker 7 | from matplotlib.patches import Ellipse 8 | import matplotlib.colors as colors 9 | import matplotlib.cm as cmx 10 | from scipy.stats import multivariate_normal 11 | 12 | 13 | import matplotlib 14 | matplotlib.use('TkAgg') # or another interactive backend 15 | # Set up matplotlib to use LaTeX 16 | plt.rcParams.update({ 17 | "text.usetex": True, 18 | "font.family": "serif", 19 | "font.serif": ["Times"], 20 | "font.size": 20, 21 | "axes.grid": True, 22 | "grid.linestyle": "-", 23 | "grid.linewidth": 0.2, 24 | "axes.grid.axis": "both", 25 | "legend.fontsize": 22, 26 | "legend.frameon": True, 27 | "legend.framealpha": 0.8, 28 | "legend.facecolor": "white", 29 | "legend.edgecolor": "black", 30 | "xtick.labelsize": 22, 31 | "ytick.labelsize": 22, 32 | "axes.labelsize": 22, 33 | "axes.linewidth": 1.0, 34 | # "axes.labelweight": "normal", 35 | }) 36 | 37 | 38 | def read_data(filename): 39 | data = np.loadtxt(filename, delimiter=' ') 40 | return data 41 | 42 | def plot_error_histogram(errors, output_file): 43 | plt.figure(figsize=(10, 8)) 44 | plt.hist(errors, bins=50, edgecolor='black') 45 | plt.xlabel('Wasserstein Distance Error') 46 | plt.ylabel('Number of Voxels') 47 | plt.title('Histogram of Wasserstein Distance Errors', pad=10) 48 | mean_error = np.mean(errors) 49 | std_error = np.std(errors) 50 | three_sigma = 3 * std_error 51 | plt.axvline(mean_error + three_sigma, color='r', linestyle='dashed', linewidth=2) 52 | mask = errors <= (mean_error + three_sigma) 53 | avg_error_within_bound = np.mean(errors[mask]) 54 | plt.text(0.7, 0.95, f'(3$\sigma$): {mean_error + three_sigma:.2f}', 55 | transform=plt.gca().transAxes, fontsize=20, 56 | verticalalignment='top', bbox=dict(boxstyle='round', facecolor='white', alpha=0.8)) 57 | plt.grid(True) 58 | plt.tight_layout() 59 | plt.savefig(output_file, format='pdf', dpi=600, bbox_inches='tight') 60 | plt.show() 61 | plt.close() 62 | print(f"Average WD error within 3-sigma: {avg_error_within_bound:.4f}") 63 | print(f"3-sigma value: {mean_error + three_sigma:.4f}") 64 | 65 | def plot_planar_error(data, output_file, plane='xy'): 66 | # Extract data 67 | if plane == 'xy': 68 | voxel_min_x, voxel_min_y = data[:, 0], data[:, 1] 69 | voxel_max_x, voxel_max_y = data[:, 3], data[:, 4] 70 | mean_x, mean_y = data[:, 6], data[:, 7] 71 | elif plane == 'xz': 72 | voxel_min_x, voxel_min_y = data[:, 0], data[:, 2] 73 | voxel_max_x, voxel_max_y = data[:, 3], data[:, 5] 74 | mean_x, mean_y = data[:, 6], data[:, 8] 75 | errors = data[:, 9] # Wasserstein distance error 76 | est_points = data[:, 11] 77 | gt_points = data[:, 10] 78 | fig, ax = plt.subplots(figsize=(10, 8)) 79 | # Create custom colormap: blue (low error) -> red (high error) 80 | cmap = LinearSegmentedColormap.from_list("custom", ["#4575B4", "#FFFFBF", "#D73027"]) 81 | norm = plt.Normalize(vmin=errors.min(), vmax=errors.max()) 82 | # Create custom colormap for point count ratio: red -> green -> blue 83 | ratio_cmap = LinearSegmentedColormap.from_list("ratio", ["red", "green", "blue"]) 84 | ratios = est_points / gt_points 85 | ratio_norm = plt.Normalize(vmin=ratios.min(), vmax=ratios.max()) 86 | # Use a set to keep track of unique voxels 87 | unique_voxels = set() 88 | for i in range(len(mean_x)): 89 | voxel_key = (voxel_min_x[i], voxel_min_y[i]) 90 | if voxel_key not in unique_voxels: 91 | unique_voxels.add(voxel_key) 92 | rect = Rectangle((voxel_min_x[i], voxel_min_y[i]), 93 | voxel_max_x[i] - voxel_min_x[i], 94 | voxel_max_y[i] - voxel_min_y[i], 95 | facecolor=cmap(norm(errors[i])), edgecolor='black', linewidth=0) 96 | ax.add_patch(rect) 97 | # Plot mean point with color based on point count ratio 98 | # ax.plot(mean_x[i], mean_y[i], 'o', color=ratio_cmap(ratio_norm(ratios[i])), 99 | # markeredgecolor='white', markeredgewidth=0.5, markersize=1, alpha=0.8) 100 | plt.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), label='Wasserstein Distance Error') 101 | if plane == 'xy': 102 | plt.xlabel('X (m)') 103 | plt.ylabel('Y (m)') 104 | plt.title('Voxel Error Visualization (XY)', pad=10) 105 | elif plane == 'xz': 106 | plt.xlabel('X (m)') 107 | plt.ylabel('Z (m)') 108 | plt.title('Voxel Error Visualization (XZ)', pad=10) 109 | # Set the extent of the plot to match the voxel grid 110 | ax.set_xlim(voxel_min_x.min(), voxel_max_x.max()) 111 | ax.set_ylim(voxel_min_y.min(), voxel_max_y.max()) 112 | plt.grid(True) 113 | plt.tight_layout() 114 | plt.savefig(output_file, format='pdf', dpi=600, bbox_inches='tight') 115 | plt.show() 116 | plt.close() 117 | 118 | 119 | def plot_planar_error_combined(data, output_file): 120 | fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(16, 10)) # Increased figure height 121 | # Create custom colormap: blue (low error) -> red (high error) 122 | cmap = LinearSegmentedColormap.from_list("custom", ["#4575B4", "#FFFFBF", "#D73027"]) 123 | norm = plt.Normalize(vmin=data[:, 9].min(), vmax=data[:, 9].max()) 124 | # Create custom colormap for point count ratio: red -> green -> blue 125 | ratio_cmap = LinearSegmentedColormap.from_list("ratio", ["red", "green", "blue"]) 126 | ratios = data[:, 10] / data[:, 11] 127 | ratio_norm = plt.Normalize(vmin=ratios.min(), vmax=ratios.max()) 128 | for idx, (ax, plane) in enumerate([(ax1, 'xy'), (ax2, 'xz')]): 129 | if plane == 'xy': 130 | voxel_min_x, voxel_min_y = data[:, 0], data[:, 1] 131 | voxel_max_x, voxel_max_y = data[:, 3], data[:, 4] 132 | mean_x, mean_y = data[:, 6], data[:, 7] 133 | elif plane == 'xz': 134 | voxel_min_x, voxel_min_y = data[:, 0], data[:, 2] 135 | voxel_max_x, voxel_max_y = data[:, 3], data[:, 5] 136 | mean_x, mean_y = data[:, 6], data[:, 8] 137 | errors = data[:, 9] # Wasserstein distance error 138 | # Use a set to keep track of unique voxels 139 | unique_voxels = set() 140 | for i in range(len(mean_x)): 141 | voxel_key = (voxel_min_x[i], voxel_min_y[i]) 142 | if voxel_key not in unique_voxels: 143 | unique_voxels.add(voxel_key) 144 | rect = Rectangle((voxel_min_x[i], voxel_min_y[i]), 145 | voxel_max_x[i] - voxel_min_x[i], 146 | voxel_max_y[i] - voxel_min_y[i], 147 | facecolor=cmap(norm(errors[i])), edgecolor='black', linewidth=0.5) 148 | ax.add_patch(rect) 149 | # Plot mean point with color based on point count ratio 150 | ax.plot(mean_x[i], mean_y[i], 'o', color=ratio_cmap(ratio_norm(ratios[i])), 151 | markeredgecolor='white', markeredgewidth=0.5, markersize=4, alpha=0.8) 152 | ax.set_xlim(voxel_min_x.min(), voxel_max_x.max()) 153 | ax.set_ylim(voxel_min_y.min(), voxel_max_y.max()) 154 | ax.grid(True) 155 | if plane == 'xy': 156 | ax.set_xlabel('X (m)') 157 | ax.set_ylabel('Y (m)') 158 | ax.set_title('Voxel Error Visualization (XY)', pad=20) 159 | elif plane == 'xz': 160 | ax.set_xlabel('X (m)') 161 | ax.set_ylabel('Z (m)') 162 | ax.set_title('Voxel Error Visualization (XZ)', pad=20) 163 | # Adjust the layout to make room for the colorbar 164 | plt.tight_layout(rect=[0, 0.1, 1, 1]) # Adjust the bottom value to create space for colorbar 165 | # Add a colorbar below the subplots 166 | cbar_ax = fig.add_axes([0.1, 0.08, 0.8, 0.04]) # [left, bottom, width, height] 167 | cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), cax=cbar_ax, orientation='horizontal') 168 | cbar.set_label('Wasserstein Distance Error') 169 | plt.savefig(output_file, format='pdf', dpi=600, bbox_inches='tight') 170 | plt.show() 171 | plt.close() 172 | 173 | 174 | 175 | # Modified function to create a combined 2x2 plot 176 | def plot_combined_2x2(data, output_file): 177 | fig = plt.figure(figsize=(16, 12)) # Create a figure 178 | # Custom colormap 179 | cmap = LinearSegmentedColormap.from_list("custom", ["#4575B4", "#FFFFBF", "#D73027"]) 180 | norm = plt.Normalize(vmin=data[:, 9].min(), vmax=data[:, 9].max()) 181 | ratio_cmap = LinearSegmentedColormap.from_list("ratio", ["red", "green", "blue"]) 182 | ratios = data[:, 10] / data[:, 11] 183 | ratio_norm = plt.Normalize(vmin=ratios.min(), vmax=ratios.max()) 184 | 185 | # XY Plane Plot 186 | ax_xy = fig.add_subplot(2, 2, 1) 187 | voxel_min_x, voxel_min_y = data[:, 0], data[:, 1] 188 | voxel_max_x, voxel_max_y = data[:, 3], data[:, 4] 189 | mean_x, mean_y = data[:, 6], data[:, 7] 190 | errors = data[:, 9] 191 | unique_voxels = set() 192 | for i in range(len(mean_x)): 193 | voxel_key = (voxel_min_x[i], voxel_min_y[i]) 194 | if voxel_key not in unique_voxels: 195 | unique_voxels.add(voxel_key) 196 | rect = Rectangle((voxel_min_x[i], voxel_min_y[i]), 197 | voxel_max_x[i] - voxel_min_x[i], 198 | voxel_max_y[i] - voxel_min_y[i], 199 | facecolor=cmap(norm(errors[i])), edgecolor='black', linewidth=0.5) 200 | ax_xy.add_patch(rect) 201 | # Plot mean point with color based on point count ratio 202 | ax_xy.plot(mean_x[i], mean_y[i], 'o', color=ratio_cmap(ratio_norm(ratios[i])), 203 | markeredgecolor='white', markeredgewidth=0.5, markersize=1, alpha=0.8) 204 | ax_xy.set_xlim(voxel_min_x.min(), voxel_max_x.max()) 205 | ax_xy.set_ylim(voxel_min_y.min(), voxel_max_y.max()) 206 | ax_xy.grid(True) 207 | ax_xy.set_xlabel('X (m)') 208 | ax_xy.set_ylabel('Y (m)') 209 | 210 | # XZ Plane Plot 211 | ax_xz = fig.add_subplot(2, 2, 2) 212 | voxel_min_x, voxel_min_y = data[:, 0], data[:, 2] 213 | voxel_max_x, voxel_max_y = data[:, 3], data[:, 5] 214 | mean_x, mean_y = data[:, 6], data[:, 8] 215 | errors = data[:, 9] 216 | unique_voxels = set() 217 | for i in range(len(mean_x)): 218 | voxel_key = (voxel_min_x[i], voxel_min_y[i]) 219 | if voxel_key not in unique_voxels: 220 | unique_voxels.add(voxel_key) 221 | rect = Rectangle((voxel_min_x[i], voxel_min_y[i]), 222 | voxel_max_x[i] - voxel_min_x[i], 223 | voxel_max_y[i] - voxel_min_y[i], 224 | facecolor=cmap(norm(errors[i])), edgecolor='black', linewidth=0.5) 225 | ax_xz.add_patch(rect) 226 | # Plot mean point with color based on point count ratio 227 | ax_xz.plot(mean_x[i], mean_y[i], 'o', color=ratio_cmap(ratio_norm(ratios[i])), 228 | markeredgecolor='white', markeredgewidth=0.5, markersize=1, alpha=0.8) 229 | ax_xz.set_xlim(voxel_min_x.min(), voxel_max_x.max()) 230 | ax_xz.set_ylim(voxel_min_y.min(), voxel_max_y.max()) 231 | ax_xz.grid(True) 232 | ax_xz.set_xlabel('X (m)') 233 | ax_xz.set_ylabel('Z (m)') 234 | 235 | # YZ Plane Plot 236 | ax_yz = fig.add_subplot(2, 2, 3) 237 | voxel_min_x, voxel_min_y = data[:, 1], data[:, 2] 238 | voxel_max_x, voxel_max_y = data[:, 4], data[:, 5] 239 | mean_x, mean_y = data[:, 7], data[:, 8] 240 | errors = data[:, 9] 241 | unique_voxels = set() 242 | for i in range(len(mean_x)): 243 | voxel_key = (voxel_min_x[i], voxel_min_y[i]) 244 | if voxel_key not in unique_voxels: 245 | unique_voxels.add(voxel_key) 246 | rect = Rectangle((voxel_min_x[i], voxel_min_y[i]), 247 | voxel_max_x[i] - voxel_min_x[i], 248 | voxel_max_y[i] - voxel_min_y[i], 249 | facecolor=cmap(norm(errors[i])), edgecolor='black', linewidth=0.5) 250 | ax_yz.add_patch(rect) 251 | # Plot mean point with color based on point count ratio 252 | ax_yz.plot(mean_x[i], mean_y[i], 'o', color=ratio_cmap(ratio_norm(ratios[i])), 253 | markeredgecolor='white', markeredgewidth=0.5, markersize=1, alpha=0.8) 254 | ax_yz.set_xlim(voxel_min_x.min(), voxel_max_x.max()) 255 | ax_yz.set_ylim(voxel_min_y.min(), voxel_max_y.max()) 256 | ax_yz.grid(True) 257 | ax_yz.set_xlabel('Y (m)') 258 | ax_yz.set_ylabel('Z (m)') 259 | # 3D plot in the lower right corner 260 | ax3d = fig.add_subplot(2, 2, 4, projection='3d') 261 | # ax3d.set_box_aspect([1, 1, 1]) # Set equal aspect ratio for 3D plot 262 | voxel_min_x, voxel_min_y, voxel_min_z = data[:, 0], data[:, 1], data[:, 2] 263 | voxel_max_x, voxel_max_y, voxel_max_z = data[:, 3], data[:, 4], data[:, 5] 264 | mean_x, mean_y, mean_z = data[:, 6], data[:, 7], data[:, 8] 265 | errors = data[:, 9] 266 | est_points = data[:, 10] 267 | gt_points = data[:, 11] 268 | ratios = est_points / gt_points 269 | unique_voxels = set() 270 | for i in range(len(mean_x)): 271 | voxel_key = (voxel_min_x[i], voxel_min_y[i], voxel_min_z[i]) 272 | if voxel_key not in unique_voxels: 273 | unique_voxels.add(voxel_key) 274 | # Plot voxel 275 | dx = voxel_max_x[i] - voxel_min_x[i] 276 | dy = voxel_max_y[i] - voxel_min_y[i] 277 | dz = voxel_max_z[i] - voxel_min_z[i] 278 | ax3d.bar3d(voxel_min_x[i], voxel_min_y[i], voxel_min_z[i], 279 | dx, dy, dz, 280 | color=cmap(norm(errors[i])), # Color based on Wasserstein distance error 281 | alpha=0.8, shade=True) 282 | # Plot mean point 283 | ax3d.scatter(mean_x[i], mean_y[i], mean_z[i], 284 | c=ratio_cmap(ratio_norm(ratios[i])), # Color based on point count ratio 285 | s=20, edgecolors='white', linewidths=0.5) 286 | ax3d.set_xlabel('X (m)', labelpad=20) 287 | ax3d.set_ylabel('Y (m)', labelpad=20) 288 | ax3d.set_zlabel('Z (m)', labelpad=20) 289 | ax3d.set_frame_on(False) # Remove 2D frame around 3D plot 290 | # Adjust the layout to make room for the colorbar 291 | plt.tight_layout(rect=[0, 0.1, 1, 1]) 292 | # Add a colorbar below the subplots 293 | cbar_ax = fig.add_axes([0.1, 0.08, 0.8, 0.02]) # [left, bottom, width, height] 294 | cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), cax=cbar_ax, orientation='horizontal') 295 | cbar.set_label('Wasserstein Distance Error') 296 | plt.savefig(output_file, format='pdf', dpi=600, bbox_inches='tight') 297 | plt.show() 298 | plt.close() 299 | 300 | def plot_point_count_ratio(data, output_file): 301 | voxel_min_x, voxel_min_y = data[:, 0], data[:, 1] 302 | voxel_max_x, voxel_max_y = data[:, 3], data[:, 4] 303 | est_points = data[:, 11] 304 | # gt_points = data[:, 11] 305 | ratios = est_points 306 | fig, ax = plt.subplots(figsize=(10, 8)) 307 | # Create custom colormap: red -> green -> blue 308 | cmap = LinearSegmentedColormap.from_list("custom", ["red", "green", "blue"]) 309 | norm = plt.Normalize(vmin=ratios.min(), vmax=ratios.max()) 310 | # Use a set to keep track of unique voxels 311 | unique_voxels = set() 312 | for i in range(len(voxel_min_x)): 313 | voxel_key = (voxel_min_x[i], voxel_min_y[i]) 314 | if voxel_key not in unique_voxels: 315 | unique_voxels.add(voxel_key) 316 | rect = Rectangle((voxel_min_x[i], voxel_min_y[i]), 317 | voxel_max_x[i] - voxel_min_x[i], 318 | voxel_max_y[i] - voxel_min_y[i], 319 | facecolor=cmap(norm(ratios[i])), edgecolor='black', linewidth=0.5) 320 | ax.add_patch(rect) 321 | # Add colorbar with formatted tick labels 322 | cbar = plt.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), label='Est/GT Point Count Ratio') 323 | cbar.ax.yaxis.set_major_formatter(ticker.FuncFormatter(lambda x, p: f'{x:.1e}' if x >= 1000 else f'{x:.0f}')) 324 | plt.xlabel('X (m)') 325 | plt.ylabel('Y (m)') 326 | plt.title('Voxel Point Count Visualization (XY)', pad=10) 327 | # Set the extent of the plot to match the voxel grid 328 | ax.set_xlim(voxel_min_x.min(), voxel_max_x.max()) 329 | ax.set_ylim(voxel_min_y.min(), voxel_max_y.max()) 330 | plt.grid(True) 331 | plt.tight_layout() 332 | plt.savefig(output_file, format='pdf', dpi=600, bbox_inches='tight') 333 | plt.show() 334 | plt.close() 335 | 336 | def plot_3d_error(data, output_file): 337 | fig = plt.figure(figsize=(12, 10)) 338 | ax = fig.add_subplot(111, projection='3d') 339 | voxel_min_x, voxel_min_y, voxel_min_z = data[:, 0], data[:, 1], data[:, 2] 340 | voxel_max_x, voxel_max_y, voxel_max_z = data[:, 3], data[:, 4], data[:, 5] 341 | mean_x, mean_y, mean_z = data[:, 6], data[:, 7], data[:, 8] 342 | errors = data[:, 9] # Wasserstein distance error 343 | est_points = data[:, 10] 344 | gt_points = data[:, 11] 345 | # Create custom colormap: blue (low error) -> red (high error) 346 | cmap = LinearSegmentedColormap.from_list("custom", ["#4575B4", "#FFFFBF", "#D73027"]) 347 | norm = plt.Normalize(vmin=errors.min(), vmax=errors.max()) 348 | # Create custom colormap for point count ratio: red -> green -> blue 349 | ratio_cmap = LinearSegmentedColormap.from_list("ratio", ["red", "green", "blue"]) 350 | ratios = est_points / gt_points 351 | ratio_norm = plt.Normalize(vmin=ratios.min(), vmax=ratios.max()) 352 | 353 | # Use a set to keep track of unique voxels 354 | unique_voxels = set() 355 | for i in range(len(mean_x)): 356 | voxel_key = (voxel_min_x[i], voxel_min_y[i], voxel_min_z[i]) 357 | if voxel_key not in unique_voxels: 358 | unique_voxels.add(voxel_key) 359 | # Plot voxel 360 | dx = voxel_max_x[i] - voxel_min_x[i] 361 | dy = voxel_max_y[i] - voxel_min_y[i] 362 | dz = voxel_max_z[i] - voxel_min_z[i] 363 | ax.bar3d(voxel_min_x[i], voxel_min_y[i], voxel_min_z[i], 364 | dx, dy, dz, 365 | color=cmap(norm(errors[i])), # Color based on Wasserstein distance error 366 | alpha=0.8, shade=True) 367 | # Plot mean point 368 | ax.scatter(mean_x[i], mean_y[i], mean_z[i], 369 | c=ratio_cmap(ratio_norm(ratios[i])), # Color based on point count ratio 370 | s=20, edgecolors='white', linewidths=0.5) 371 | ax.set_xlabel('X (m)', labelpad=20) 372 | ax.set_ylabel('Y (m)', labelpad=20) 373 | ax.set_zlabel('Z (m)', labelpad=20) 374 | ax.set_title('3D Voxel Error Visualization', pad=20) 375 | 376 | # for axis in [ax.xaxis, ax.yaxis, ax.zaxis]: 377 | # axis.line.set_linewidth(2) 378 | # axis.line.set_color('black') 379 | # axis.set_pane_color((0.8, 0.8, 0.8, 1.0)) 380 | 381 | # Add colorbar for Wasserstein distance error 382 | # cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), 383 | # ax=ax, pad=0.1, aspect=20, label='Wasserstein Distance Error') 384 | 385 | cax = fig.add_axes([ax.get_position().x1 + 0.02, ax.get_position().y0, 0.02, ax.get_position().height * 1.0]) 386 | cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap),pad=10, 387 | cax=cax, label='Wasserstein Distance Error') 388 | 389 | plt.tight_layout() 390 | plt.savefig(output_file, format='pdf', dpi=600, bbox_inches='tight') 391 | plt.show() 392 | plt.close() 393 | 394 | 395 | def read_data_cov(filename): 396 | data = np.loadtxt(filename, delimiter=' ') 397 | means = data[:, 6:9] 398 | cov_00, cov_01, cov_02 = data[:, 12], data[:, 13], data[:, 14] 399 | cov_11, cov_12, cov_22 = data[:, 15], data[:, 16], data[:, 17] 400 | cov_matrices = np.array([[cov_00, cov_01, cov_02], 401 | [cov_01, cov_11, cov_12], 402 | [cov_02, cov_12, cov_22]]).transpose(2, 0, 1) 403 | weights = data[:, 11] / np.sum(data[:, 11]) 404 | return means, cov_matrices, weights 405 | 406 | def gmm_pdf(x, means, covs, weights): 407 | pdf = np.zeros(x.shape[:-1]) 408 | for mean, cov, weight in zip(means, covs, weights): 409 | mvn = multivariate_normal(mean=mean, cov=cov) 410 | pdf += weight * mvn.pdf(x) 411 | return pdf 412 | def safe_norm(vmin, vmax): 413 | if vmin == vmax: 414 | return colors.Normalize(vmin, vmax + 1) 415 | elif vmin > vmax: 416 | return colors.Normalize(vmax, vmin) 417 | else: 418 | return colors.Normalize(vmin, vmax) 419 | def plot_gmm_2d(means, cov_matrices, weights, output_file, plane='xy'): 420 | fig, ax = plt.subplots(figsize=(10, 8)) 421 | idx1, idx2 = (0, 1) if plane == 'xy' else (0, 2) 422 | x = np.linspace(means[:, idx1].min() - 1, means[:, idx1].max() + 1, 200) 423 | y = np.linspace(means[:, idx2].min() - 1, means[:, idx2].max() + 1, 200) 424 | X, Y = np.meshgrid(x, y) 425 | pos = np.dstack((X, Y)) 426 | Z = gmm_pdf(pos, means[:, [idx1, idx2]], cov_matrices[:, [idx1, idx2]][:, :, [idx1, idx2]], weights) 427 | # Handle zero or negative values 428 | Z = np.maximum(Z, np.finfo(float).eps) # Replace zeros or negatives with a small positive value 429 | # Use logarithmic normalization for smoother visualization 430 | norm = colors.LogNorm(vmin=Z.min(), vmax=Z.max()) 431 | contour = ax.pcolormesh(X, Y, Z, norm=norm, cmap='viridis', shading='auto') 432 | # Add contour lines for better visualization of the distribution 433 | levels = np.logspace(np.log10(Z.min()), np.log10(Z.max()), 20) 434 | ax.contour(X, Y, Z, levels=levels, colors='k', alpha=0.3, linewidths=0.5) 435 | ax.set_xlabel(f'{"X" if idx1 == 0 else "Z"} (m)') 436 | ax.set_ylabel(f'{"Y" if idx2 == 1 else "Z"} (m)') 437 | ax.set_title(f'GMM Probability Density ({plane.upper()} plane)') 438 | cbar = plt.colorbar(contour) 439 | cbar.set_label('Probability Density') 440 | plt.grid(True) 441 | plt.tight_layout() 442 | plt.savefig(output_file, format='pdf', dpi=600, bbox_inches='tight') 443 | plt.show() 444 | plt.close() 445 | 446 | def plot_gmm_3d(means, cov_matrices, weights, output_file): 447 | fig = plt.figure(figsize=(12, 10)) 448 | ax = fig.add_subplot(111, projection='3d') 449 | x = np.linspace(means[:, 0].min() - 1, means[:, 0].max() + 1, 30) 450 | y = np.linspace(means[:, 1].min() - 1, means[:, 1].max() + 1, 30) 451 | z = np.linspace(means[:, 2].min() - 1, means[:, 2].max() + 1, 30) 452 | X, Y, Z = np.meshgrid(x, y, z) 453 | pos = np.stack((X, Y, Z), axis=-1) 454 | PDF = gmm_pdf(pos, means, cov_matrices, weights) 455 | # Use scatter plot with alpha based on PDF for 3D visualization 456 | points = pos.reshape(-1, 3) 457 | norm = safe_norm(PDF.min(), PDF.max()) 458 | colors_rgba = plt.cm.viridis(norm(PDF.flatten())) 459 | colors_rgba[:, 3] = norm(PDF.flatten()) # Set alpha channel 460 | ax.scatter(points[:, 0], points[:, 1], points[:, 2], c=colors_rgba) 461 | ax.set_xlabel('X (m)') 462 | ax.set_ylabel('Y (m)') 463 | ax.set_zlabel('Z (m)') 464 | ax.set_title('3D GMM Probability Density') 465 | # Add colorbar 466 | sm = plt.cm.ScalarMappable(cmap='viridis', norm=norm) 467 | sm.set_array([]) 468 | cbar = plt.colorbar(sm) 469 | cbar.set_label('Probability Density') 470 | plt.tight_layout() 471 | plt.savefig(output_file, format='pdf', dpi=600, bbox_inches='tight') 472 | plt.show() 473 | plt.close() 474 | 475 | def visualize_gmm(filename, output_prefix): 476 | means, cov_matrices, weights = read_data_cov(filename) 477 | plot_gmm_2d(means, cov_matrices, weights, f'{output_prefix}_xy.pdf', plane='xy') 478 | plot_gmm_2d(means, cov_matrices, weights, f'{output_prefix}_xz.pdf', plane='xz') 479 | plot_gmm_3d(means, cov_matrices, weights, f'{output_prefix}_3d.pdf') 480 | 481 | def plot_gaussian_comparison(data, output_file): 482 | # Find the voxel with the highest total point count (GT + EST) 483 | total_points = data[:, 10] + data[:, 11] 484 | max_index = np.argmax(total_points) 485 | # Extract data for the selected voxel 486 | est_mean = data[max_index, 6:9] 487 | gt_mean = data[max_index, 18:21] 488 | est_cov = np.array([[data[max_index, 12], data[max_index, 13], data[max_index, 14]], 489 | [data[max_index, 13], data[max_index, 15], data[max_index, 16]], 490 | [data[max_index, 14], data[max_index, 16], data[max_index, 17]]]) 491 | gt_cov = np.array([[data[max_index, 21], data[max_index, 22], data[max_index, 23]], 492 | [data[max_index, 22], data[max_index, 24], data[max_index, 25]], 493 | [data[max_index, 23], data[max_index, 25], data[max_index, 26]]]) 494 | # Create 2D plots for XY, XZ, and YZ planes 495 | planes = ['xy', 'xz', 'yz'] 496 | for plane in planes: 497 | fig, ax = plt.subplots(figsize=(10, 8)) 498 | if plane == 'xy': 499 | est_mean_2d, gt_mean_2d = est_mean[:2], gt_mean[:2] 500 | est_cov_2d, gt_cov_2d = est_cov[:2, :2], gt_cov[:2, :2] 501 | xlabel, ylabel = 'X', 'Y' 502 | elif plane == 'xz': 503 | est_mean_2d, gt_mean_2d = est_mean[::2], gt_mean[::2] 504 | est_cov_2d, gt_cov_2d = est_cov[::2, ::2], gt_cov[::2, ::2] 505 | xlabel, ylabel = 'X', 'Z' 506 | else: # yz 507 | est_mean_2d, gt_mean_2d = est_mean[1:], gt_mean[1:] 508 | est_cov_2d, gt_cov_2d = est_cov[1:, 1:], gt_cov[1:, 1:] 509 | xlabel, ylabel = 'Y', 'Z' 510 | # Create a meshgrid 511 | x = np.linspace(min(est_mean_2d[0], gt_mean_2d[0]) - 1, max(est_mean_2d[0], gt_mean_2d[0]) + 1, 100) 512 | y = np.linspace(min(est_mean_2d[1], gt_mean_2d[1]) - 1, max(est_mean_2d[1], gt_mean_2d[1]) + 1, 100) 513 | X, Y = np.meshgrid(x, y) 514 | pos = np.dstack((X, Y)) 515 | # Calculate PDF for EST and GT 516 | rv_est = multivariate_normal(est_mean_2d, est_cov_2d) 517 | rv_gt = multivariate_normal(gt_mean_2d, gt_cov_2d) 518 | Z_est = rv_est.pdf(pos) 519 | Z_gt = rv_gt.pdf(pos) 520 | # Plot contours 521 | levels = np.linspace(0, max(Z_est.max(), Z_gt.max()), 20) 522 | ax.contour(X, Y, Z_est, levels=levels, colors='r', alpha=0.5, linestyles='solid') 523 | ax.contour(X, Y, Z_gt, levels=levels, colors='b', alpha=0.5, linestyles='dashed') 524 | # Plot mean points 525 | ax.plot(est_mean_2d[0], est_mean_2d[1], 'ro', markersize=10, label='EST Mean') 526 | ax.plot(gt_mean_2d[0], gt_mean_2d[1], 'bo', markersize=10, label='GT Mean') 527 | ax.set_xlabel(f'{xlabel} (m)') 528 | ax.set_ylabel(f'{ylabel} (m)') 529 | ax.set_title(f'Gaussian Comparison ({plane.upper()} plane)') 530 | ax.legend() 531 | plt.grid(True) 532 | plt.tight_layout() 533 | plt.savefig(f'{output_file}_{plane}.pdf', format='pdf', dpi=600, bbox_inches='tight') 534 | plt.show() 535 | plt.close() 536 | 537 | 538 | def main(): 539 | data_file = 'voxel_errors.txt' # Replace with your actual file name 540 | data = read_data(data_file) 541 | errors = data[:, 9] # Assuming the error is in the 10th column now 542 | plot_error_histogram(errors, 'error_histogram.pdf') 543 | plot_planar_error(data, 'xy_planar_error.pdf', plane='xy') 544 | # plot_planar_error(data, 'xz_planar_error.pdf', plane='xz') 545 | 546 | # New combined plot 547 | # plot_planar_error_combined(data, 'combined_planar_error.pdf') 548 | # Example usage 549 | data2 = np.loadtxt(data_file, delimiter=' ') 550 | plot_combined_2x2(data2, 'combined_2x2_plot.pdf') 551 | 552 | plot_point_count_ratio(data, 'point_count_ratio.pdf') 553 | # Add the new 3D plot 554 | # plot_3d_error(data, '3d_voxel_error.pdf') 555 | 556 | # Add the new Gaussian comparison plot 557 | # plot_gaussian_comparison(data, 'gaussian_comparison') 558 | 559 | # Example usage 560 | # visualize_gmm('voxel_errors.txt', 'gmm_visualization') 561 | 562 | if __name__ == "__main__": 563 | main() -------------------------------------------------------------------------------- /map_eval/src/map_eval.h: -------------------------------------------------------------------------------- 1 | #ifndef SRC_POSE_SLAM_PRIOR_SRC_BENCHMARK_CLOUD_MAP_EVAL_H_ 2 | #define SRC_POSE_SLAM_PRIOR_SRC_BENCHMARK_CLOUD_MAP_EVAL_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "tic_toc.h" 18 | #include "voxel_calculator.hpp" 19 | 20 | 21 | #if __cplusplus >= 201703L 22 | #include 23 | namespace fs = std::filesystem; 24 | #else 25 | #include 26 | namespace fs = std::experimental::filesystem; 27 | #endif 28 | 29 | 30 | using namespace std; 31 | using namespace Eigen; 32 | using namespace open3d; 33 | typedef geometry::PointCloud PointCloud; 34 | typedef geometry::TriangleMesh Mesh; 35 | typedef Eigen::Matrix Vector8d; 36 | typedef Eigen::Matrix Vector5d; 37 | typedef Eigen::Matrix Vector5i; 38 | 39 | #include 40 | 41 | template 42 | struct hash_eigen { 43 | std::size_t operator()(const T &k) const { 44 | return std::hash()(k[0]) ^ std::hash()(k[1]) ^ std::hash()(k[2]); 45 | } 46 | }; 47 | 48 | struct Param { 49 | // Directories and file paths 50 | std::string evaluation_map_pcd_path_ = "/data/map_evaluation/canteen/"; // Path to the evaluation map PCD file 51 | std::string map_gt_path_ = "/data/map_evaluation/canteen/merged_scan.pcd"; // Path to the ground truth point cloud 52 | std::string result_path_ = "/home/hts/workspace/dataset/eva_results/"; // Directory to save results 53 | std::string pcd_file_name_ = "map.pcd"; // Name of the PCD file for the map 54 | 55 | // Experiment settings 56 | std::string name_; // Name of the experiment or dataset 57 | int evaluation_method_ = 2; // Method used for evaluation (e.g., 1: ICP, 2: Other method) 58 | double voxel_size_ = 1.0; // Voxel size for downsampling 59 | double icp_max_distance_ = 2.5; // Maximum distance for ICP registration 60 | double nn_radius_ = 0.2; // Radius for nearest neighbor search 61 | 62 | // Evaluation flags 63 | bool save_immediate_result_ = false; // Flag to save results immediately after processing 64 | bool evaluate_mme_ = true; // Flag to enable MME evaluation 65 | bool evaluate_gt_mme_ = true; // Flag to evaluate ground truth MME 66 | bool evaluate_using_initial_ = true; // Flag to evaluate using initial transformation matrix 67 | bool evaluate_noised_gt_ = false; // Flag to add noise to ground truth for simulation 68 | 69 | // Transformation and noise parameters 70 | Vector5d trunc_dist_; // Truncation distance (for point cloud processing) 71 | Eigen::Matrix4d initial_matrix_ = Eigen::Matrix4d::Identity(); // Initial transformation matrix 72 | double noise_std_dev_ = 0.1; // Standard deviation of Gaussian noise added to point cloud 73 | double vmd_voxel_size_ = 3.0; // Voxel size for VMD (Voxel-based Metric Distance) 74 | 75 | // Default constructor 76 | Param() {} 77 | 78 | // Parametrized constructor to initialize the parameters 79 | Param(std::string evaluation_map_pcd_path, std::string map_gt_path, 80 | std::string result_path, Eigen::Matrix4d initial_matrix, 81 | std::string name, int method, double icp_max_distance, 82 | Vector5d trunc_dist, bool save_immediate_result, bool evaluate_mme, bool evaluate_gt_mme, 83 | std::string pcd_file_name, bool evaluate_using_initial, bool evaluate_noised_gt, double noise_std_dev, double nn_radius, 84 | double vmd_voxel_size) 85 | : evaluation_map_pcd_path_(evaluation_map_pcd_path), 86 | map_gt_path_(map_gt_path), 87 | result_path_(result_path), 88 | initial_matrix_(initial_matrix), 89 | name_(name), 90 | evaluation_method_(method), 91 | icp_max_distance_(icp_max_distance), 92 | trunc_dist_(trunc_dist), 93 | save_immediate_result_(save_immediate_result), 94 | evaluate_mme_(evaluate_mme), evaluate_gt_mme_(evaluate_gt_mme), 95 | pcd_file_name_(pcd_file_name), 96 | evaluate_using_initial_(evaluate_using_initial), 97 | evaluate_noised_gt_(evaluate_noised_gt), 98 | noise_std_dev_(noise_std_dev), 99 | nn_radius_(nn_radius), 100 | vmd_voxel_size_(vmd_voxel_size) 101 | {} 102 | 103 | // Method to print out the parameters 104 | void printParam() const { 105 | std::cout 106 | << "\n[==================== Experiment Configuration ====================]\n" 107 | << "Experiment Name: " << name_ << "\n" 108 | << "Evaluation Map Path: " << evaluation_map_pcd_path_ << "\n" 109 | << "Ground Truth Map Path: " << map_gt_path_ << "\n" 110 | << "Result Save Path: " << result_path_ << "\n" 111 | << "Initial Transformation Matrix: \n" << initial_matrix_ << "\n" 112 | << "ICP Maximum Distance: " << icp_max_distance_ << "\n" 113 | << "Evaluation Method: " << evaluation_method_ << "\n" 114 | << "Truncation Distance: " << trunc_dist_.transpose() << "\n" 115 | << "Save Immediate Result: " << save_immediate_result_ << "\n" 116 | << "Evaluate MME: " << evaluate_mme_ << "\n" 117 | << "Evaluate Ground Truth MME: " << evaluate_gt_mme_ << "\n" 118 | << "Nearest Neighbor Radius: " << nn_radius_ << "\n" 119 | << "Use Initial Matrix for Evaluation: " << evaluate_using_initial_ << "\n" 120 | << "Evaluate with Noised Ground Truth: " << evaluate_noised_gt_ << "\n" 121 | << "Noise Standard Deviation: " << noise_std_dev_ << "\n" 122 | << "Voxel Size for VMD: " << vmd_voxel_size_ << "\n" 123 | << "[====================================================================]\n"; 124 | } 125 | }; 126 | 127 | 128 | class MapEval { 129 | public: 130 | MapEval(Param ¶m) 131 | : param_(param), map_3d_(new PointCloud), gt_3d_(new PointCloud), processed_points(9) { 132 | 133 | // Print configuration parameters for the evaluation 134 | param_.printParam(); 135 | 136 | // Initialize timing variables 137 | t1 = t2 = t3 = t4 = t5 = t6 = t7 = 0.0; 138 | 139 | // Initialize point cloud objects for rendering and processing 140 | map_3d_render_inlier.reset(new PointCloud()); 141 | map_3d_render_raw.reset(new PointCloud()); 142 | map_3d_entropy.reset(new PointCloud()); 143 | gt_3d_entropy.reset(new PointCloud()); 144 | corresponding_cloud_est.reset(new PointCloud()); 145 | corresponding_cloud_gt.reset(new PointCloud()); 146 | 147 | // Initialize mesh objects 148 | gt_mesh.reset(new Mesh()); 149 | est_mesh.reset(new Mesh()); 150 | gt_mesh_filtered.reset(new Mesh()); 151 | est_mesh_filtered.reset(new Mesh()); 152 | 153 | // Determine subfolder name based on the PCD file name 154 | if (param_.pcd_file_name_ == "merged_maps_all_trans.pcd") { 155 | subfolder = "merged_maps_all_results/"; 156 | } else if (param_.pcd_file_name_ == "merged_maps_s0_trans.pcd") { 157 | subfolder = "merged_maps_s0_results/"; 158 | } else if (param_.pcd_file_name_ == "merged_maps_s1_trans.pcd") { 159 | subfolder = "merged_maps_s1_results/"; 160 | } else { 161 | subfolder = "map_results/"; 162 | std::cerr << "ERROR: Invalid PCD file name: " << param_.pcd_file_name_ << std::endl; 163 | } 164 | 165 | // Set the directory path for saving results 166 | results_subfolder = param_.evaluation_map_pcd_path_ + subfolder; 167 | std::cout << "INFO: Saving results to: " << results_subfolder << std::endl; 168 | 169 | // Create the results directory if it doesn't exist 170 | if (!fs::exists(results_subfolder)) { 171 | fs::create_directory(results_subfolder); 172 | } 173 | 174 | // Define the result file path and open it for appending 175 | results_file_path = results_subfolder + "map_results.txt"; 176 | file_result.open(results_file_path, std::ios::app); 177 | 178 | // Check if file opened successfully 179 | if (!file_result.is_open()) { 180 | std::cerr << "ERROR: Failed to open results file at " << results_file_path << std::endl; 181 | } 182 | 183 | // Log the experiment name and current timestamp 184 | auto now = std::chrono::system_clock::now(); 185 | std::time_t now_c = std::chrono::system_clock::to_time_t(now); 186 | std::stringstream time_stream; 187 | time_stream << std::put_time(std::localtime(&now_c), "%Y-%m-%d %X"); // Example format: YYYY-MM-DD HH:MM:SS 188 | 189 | // Output experiment name, timestamp, and paths to the results file 190 | file_result << param_.name_ << " ===================== " << time_stream.str() << " ===================== " << std::endl; 191 | file_result << "Ground Truth Path: " << param_.map_gt_path_ << std::endl; 192 | file_result << "Evaluation Map Path: " << param_.evaluation_map_pcd_path_ + param_.pcd_file_name_ << std::endl; 193 | 194 | // Optional: Print to console for confirmation 195 | std::cout << "INFO: Evaluation details saved to " << results_file_path << std::endl; 196 | } 197 | 198 | 199 | ~MapEval() { 200 | file_result.close(); 201 | } 202 | 203 | int process(); 204 | 205 | void computeMME(); 206 | 207 | void computeMME(shared_ptr &cloud_, shared_ptr &cloud_gt_); 208 | 209 | void performRegistration(); 210 | 211 | void calculateVMD(); 212 | 213 | void saveMmeResults(); 214 | 215 | void saveRegistrationResults(); 216 | 217 | template 218 | map calculateError(vector &result_vec); 219 | 220 | vector computePointCloudDistance( 221 | shared_ptr &reference_points, 222 | shared_ptr &target_points); 223 | 224 | std::shared_ptr renderDistanceOnPointCloud( 225 | std::shared_ptr &reference_points, 226 | std::shared_ptr &target_points, const double &dis); 227 | 228 | std::shared_ptr ColorPointCloudByMME( 229 | const std::shared_ptr &pointcloud, 230 | const std::vector &entropies, 231 | double mean_entropy); 232 | 233 | std::shared_ptr ColorPointCloudByMME( 234 | const std::shared_ptr &pointcloud, 235 | const std::vector &entropies, 236 | double min_abs_entropy, double max_abs_entropy); 237 | 238 | std::shared_ptr ColorPointCloudByMME( 239 | const std::shared_ptr &pointcloud, 240 | const std::vector &entropies); 241 | 242 | 243 | std::shared_ptr renderDistanceOnMesh( 244 | std::shared_ptr &reference_points, 245 | std::shared_ptr &target_mesh, 246 | std::shared_ptr &target_points, double dis); 247 | 248 | shared_ptr createMeshFromPCD(shared_ptr &reference_points, 249 | double density_thres, int depth); 250 | 251 | void getDiffRegResult(std::vector &result, 252 | pipelines::registration::CorrespondenceSet &points_set, 253 | geometry::PointCloud &source, 254 | geometry::PointCloud &target); 255 | 256 | void getDiffRegResult(std::vector &result, 257 | pipelines::registration::CorrespondenceSet &points_set, 258 | geometry::PointCloud &source, 259 | geometry::PointCloud &target, 260 | geometry::PointCloud &source_set, 261 | geometry::PointCloud &target_set); 262 | 263 | void getDiffRegResult(std::vector &result, 264 | pipelines::registration::CorrespondenceSet &points_set, 265 | geometry::PointCloud &source, 266 | geometry::PointCloud &target, 267 | Eigen::MatrixXd &source_set, 268 | Eigen::MatrixXd &target_set); 269 | 270 | void getDiffRegResultWithCorrespondence( 271 | std::vector &result, 272 | pipelines::registration::CorrespondenceSet &points_set, 273 | geometry::PointCloud &source, 274 | geometry::PointCloud &target, 275 | geometry::PointCloud &source_set, 276 | geometry::PointCloud &target_set); 277 | 278 | void calculateMetrics(pipelines::registration::RegistrationResult ®istration_result); 279 | 280 | void calculateMetricsWithInitialMatrix(); 281 | 282 | void saveResults(); 283 | 284 | pipelines::registration::RegistrationResult performICPRegistration(); 285 | 286 | double computeChamferDistance(const geometry::PointCloud &cloud1, 287 | const geometry::PointCloud &cloud2); 288 | 289 | 290 | double ComputeEntropy(const Eigen::Matrix3d &covariance); 291 | 292 | double ComputeMeanMapEntropy(const std::shared_ptr &pointcloud, 293 | std::vector &entropies, double radius); 294 | 295 | double ComputeMeanMapEntropyUsingNormal(const std::shared_ptr &pointcloud, 296 | std::vector &entropies, double radius); 297 | 298 | void StartProcessing(int total); // 开始处理的函数声明 299 | 300 | void addGaussianNoise(std::shared_ptr &cloud, 301 | double noise_std_dev); 302 | 303 | // 非均匀密度变化 304 | void addNonUniformDensity(std::shared_ptr &cloud, 305 | double sparse_ratio, // 稀疏区域保留比例 306 | double dense_ratio, // 密集区域保留比例 307 | double region_size); // 区域大小 308 | // 添加离群点 309 | void addSparseOutliers(std::shared_ptr &cloud, 310 | double outlier_ratio, // 离群点比例 311 | double outlier_range); // 离群点最大距离 312 | // 局部几何变形 313 | void addLocalDeformation(std::shared_ptr &cloud, 314 | double deform_radius, // 变形区域半径 315 | double deform_strength, // 变形强度 316 | Eigen::Vector3d center); // 变形中心 317 | 318 | private: 319 | // 辅助函数：计算点到中心的距离 320 | double computeDistance(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2); 321 | 322 | public: 323 | Param param_; 324 | 325 | private: 326 | shared_ptr map_3d_, gt_3d_, noised_gt_3d_; 327 | double t1, t2, t3, t4, t5, t6, t7, t_fcd, t_acc; 328 | double t_vmd, t_v, t_cdf, t_scs; 329 | shared_ptr map_3d_render_inlier, map_3d_render_raw; 330 | shared_ptr map_3d_entropy, gt_3d_entropy; 331 | shared_ptr corresponding_cloud_est, corresponding_cloud_gt; 332 | std::vector est_gt_results, gt_est_results; 333 | Vector5d f1_vec = Vector5d::Zero(); 334 | Vector5d cd_vec = Vector5d::Zero(); 335 | Vector5d iou_vec = Vector5d::Zero(); 336 | Eigen::Matrix4d trans = Eigen::Matrix4d::Identity(); 337 | double vmd = 0.0; 338 | 339 | double full_chamfer_dist = 0.0; 340 | 341 | double scs_overall = 0.0; 342 | bool eva_mesh = false; 343 | shared_ptr gt_mesh, est_mesh; 344 | shared_ptr gt_mesh_filtered, est_mesh_filtered; 345 | 346 | std::string subfolder; 347 | std::string results_subfolder; 348 | std::ofstream file_result; 349 | std::string results_file_path; 350 | 351 | std::mutex file_mt; 352 | 353 | std::vector est_entropies, gt_entropies; 354 | std::vector valid_entropy_points; 355 | double mme_est = 0.0, mme_gt = 0.0; 356 | double max_abs_entropy = 0.0, min_abs_entropy = 0.0; 357 | double mean_entropy = 0.0; 358 | int valid_points = 0; 359 | std::atomic processed_points; 360 | std::chrono::steady_clock::time_point start_time; 361 | int total_points = 0; 362 | 363 | double lambda_ = 1.0; // Lamé 第一参数 364 | double mu_ = 0.5; // Lamé 第二参数 365 | double alpha_ = 0.1; // 平移-形变权重 366 | }; 367 | 368 | #endif// SRC_POSE_SLAM_PRIOR_SRC_BENCHMARK_CLOUD_MAP_EVAL_H_ 369 | -------------------------------------------------------------------------------- /map_eval/src/map_eval_main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file map_eval_node.cpp 3 | * @brief Evaluate the accuracy of Point Cloud Maps using various metrics (e.g., Chamfer Distance, F1 Score, IoU). 4 | * @author Xiangcheng Hu (xhubd@connect.ust.hk) 5 | * @date Feb 12, 2025 6 | * 7 | * The code is part of the MapEval project, which aims to provide a unified, robust, and efficient evaluation framework for SLAM maps. 8 | * The code is open-source and can be freely used for research purposes. 9 | * If you use this code in your research, please consider citing the following paper: 10 | * MapEval: Towards Unified, Robust and Efficient SLAM Map Evaluation Framework (https://arxiv.org/abs/2411.17928) 11 | * 12 | * In a normal scenario, we recommend using the AC, CD, or AWD metrics to evaluate the accuracy of the SLAM maps. 13 | * But when you do not have the ground truth map, you can only use the MME. 14 | * 15 | * We appreciate any contributions to the project, and you can pull requests to the GitHub repository. 16 | * For more information about the MapEval project, please visit the project website: https://github.com/JokerJohn/Cloud_Map_Evaluation 17 | * Copyright (C) 2025, Hong Kong University of Science and Technology. 18 | */ 19 | 20 | #include "map_eval.h" 21 | #include 22 | #include 23 | 24 | int main(int argc, char **argv) { 25 | std::cout << "Current working directory: " << std::filesystem::current_path() << std::endl; 26 | 27 | // load all the yaml parameters 28 | YAML::Node config = YAML::LoadFile("../config/config.yaml"); 29 | int method = config["registration_methods"].as(); 30 | double icp_max_distance = config["icp_max_distance"].as(); 31 | 32 | Vector5d accuracy_level = Vector5d::Zero(); 33 | for (int i = 0; i < 5; ++i) { 34 | accuracy_level[i] = config["accuracy_level"][i].as(); 35 | } 36 | Eigen::Matrix4d initial_matrix = Eigen::Matrix4d::Zero(); 37 | for (int i = 0; i < 4; ++i) { 38 | for (int j = 0; j < 4; ++j) { 39 | initial_matrix(i, j) = config["initial_matrix"][i][j].as(); 40 | } 41 | } 42 | // load all the settings 43 | bool save_immediate_result = config["save_immediate_result"].as(); 44 | bool evaluate_mme = config["evaluate_mme"].as(); 45 | bool evaluate_gt_mme = config["evaluate_gt_mme"].as(); 46 | bool evaluate_using_initial_ = config["evaluate_using_initial"].as(); 47 | double nn_radius = config["nn_radius"].as(); 48 | double vmd_voxel_size = config["vmd_voxel_size"].as(); 49 | 50 | // the path dir must end with '/' 51 | std::string est_path, gt_path, results_path, scene_name; 52 | est_path = config["estimate_map_path"].as(); 53 | gt_path = config["gt_map_path"].as(); 54 | scene_name = config["scene_name"].as(); 55 | results_path = est_path + "map_results/"; 56 | std::string pcd_file_name = "map.pcd"; 57 | 58 | Param param(est_path, gt_path, results_path, initial_matrix, scene_name, method, icp_max_distance, accuracy_level, 59 | save_immediate_result, evaluate_mme, evaluate_gt_mme, pcd_file_name, evaluate_using_initial_, false, 0.01, nn_radius, vmd_voxel_size); 60 | MapEval map_eval(param); 61 | map_eval.process(); 62 | 63 | return EXIT_SUCCESS; 64 | } 65 | -------------------------------------------------------------------------------- /map_eval/src/voxel_calculator.cpp: -------------------------------------------------------------------------------- 1 | #include "voxel_calculator.hpp" 2 | 3 | VoxelCalculator::VoxelCalculator(double voxel_size) { 4 | voxel_size_ = voxel_size; 5 | } 6 | 7 | std::vector VoxelCalculator::getNeighborIndices(const Eigen::Vector3i &index, int radius) { 8 | std::vector neighbors; 9 | for (int dx = -radius; dx <= radius; ++dx) { 10 | for (int dy = -radius; dy <= radius; ++dy) { 11 | for (int dz = -radius; dz <= radius; ++dz) { 12 | if (dx == 0 && dy == 0 && dz == 0) 13 | continue; // Skip the center voxel 14 | neighbors.emplace_back(index + Eigen::Vector3i(dx, dy, dz)); 15 | } 16 | } 17 | } 18 | return neighbors; 19 | } 20 | 21 | void VoxelCalculator::buildVoxelMap(const open3d::geometry::PointCloud &cloud) { 22 | voxel_map_.clear(); 23 | total_entropy_ = 0.0; 24 | total_energy_ = 0.0; 25 | for (const auto &point : cloud.points_) { 26 | Eigen::Vector3i voxel_index = getVoxelIndex(point); 27 | auto it = voxel_map_.find(voxel_index); 28 | if (it == voxel_map_.end()) { 29 | VoxelInfo voxel_info; 30 | voxel_info.num_points = 1; 31 | voxel_info.mu = point; 32 | voxel_info.sigma = Eigen::Matrix3d::Zero(); 33 | voxel_info.energy = voxel_info.sigma.trace(); 34 | voxel_info.active = 1; // All voxels are initially active 35 | voxel_map_.emplace(voxel_index, voxel_info); 36 | } else { 37 | VoxelInfo &voxel_info = it->second; 38 | voxel_info.num_points++; 39 | Eigen::Vector3d delta = point - voxel_info.mu; 40 | voxel_info.mu += delta / voxel_info.num_points; 41 | voxel_info.sigma += delta * (point - voxel_info.mu).transpose(); 42 | voxel_info.energy = voxel_info.sigma.trace(); 43 | } 44 | } 45 | for (auto &kv : voxel_map_) { 46 | VoxelInfo &voxel_info = kv.second; 47 | if (voxel_info.num_points > 10) { 48 | voxel_info.sigma /= (voxel_info.num_points - 1); 49 | computeVoxelEntropy(voxel_info); 50 | voxel_info.entropy_old = voxel_info.entropy; 51 | total_entropy_ += voxel_info.entropy; 52 | total_energy_ += voxel_info.energy; 53 | } 54 | } 55 | std::cout << "Build voxel map: " << total_entropy_ << " " << voxel_map_.size() << std::endl; 56 | } 57 | 58 | void VoxelCalculator::buildVoxelMap(const PCLPointCloud::Ptr &cloud) { 59 | voxel_map_.clear(); 60 | total_entropy_ = 0.0; 61 | for (const auto &point : cloud->points) { 62 | Eigen::Vector3d point_world = point.getVector3fMap().cast(); 63 | Eigen::Vector3i voxel_index = getVoxelIndex(point_world); 64 | auto it = voxel_map_.find(voxel_index); 65 | if (it == voxel_map_.end()) { 66 | VoxelInfo voxel_info; 67 | voxel_info.num_points = 1; 68 | voxel_info.mu = point_world; 69 | voxel_info.sigma = Eigen::Matrix3d::Zero(); 70 | voxel_info.energy = voxel_info.sigma.trace(); 71 | voxel_map_.emplace(voxel_index, voxel_info); 72 | } else { 73 | VoxelInfo &voxel_info = it->second; 74 | voxel_info.num_points++; 75 | Eigen::Vector3d delta = point_world - voxel_info.mu; 76 | voxel_info.mu += delta / voxel_info.num_points; 77 | voxel_info.sigma += delta * (point_world - voxel_info.mu).transpose(); 78 | voxel_info.energy = voxel_info.sigma.trace(); 79 | } 80 | } 81 | for (auto &kv : voxel_map_) { 82 | VoxelInfo &voxel_info = kv.second; 83 | if (voxel_info.num_points > 1) { 84 | voxel_info.sigma /= (voxel_info.num_points - 1); 85 | computeVoxelEntropy(voxel_info); 86 | voxel_info.entropy_old = voxel_info.entropy; 87 | total_entropy_ += kv.second.entropy; 88 | total_energy_ += kv.second.energy; 89 | } else { 90 | voxel_info.sigma = Eigen::Matrix3d::Identity() * 1e-6; 91 | voxel_info.energy = voxel_info.sigma.trace(); 92 | } 93 | } 94 | std::cout << "Build voxel map: " << total_entropy_ << " " << voxel_map_.size() << std::endl; 95 | } 96 | 97 | void VoxelCalculator::computeVoxelEntropy(VoxelInfo &voxel) { 98 | if (voxel.num_points < 2) { 99 | voxel.entropy = 0; 100 | voxel.energy = 0; 101 | } else { 102 | voxel.sigma /= (voxel.num_points - 1); 103 | double det = voxel.sigma.determinant(); 104 | if (det <= 0) { 105 | voxel.entropy = 0; 106 | voxel.energy = 0; 107 | } else { 108 | constexpr double PI = 3.141592653589793238463; 109 | voxel.entropy = 0.5 * std::log(std::pow(2 * PI * std::exp(1), 3) * det); 110 | voxel.energy = voxel.sigma.trace(); 111 | } 112 | } 113 | } 114 | 115 | double VoxelCalculator::computeWassersteinDistanceGaussian(const VoxelInfo &voxel1, const VoxelInfo &voxel2) { 116 | Eigen::Vector3d mu1 = voxel1.mu; 117 | Eigen::Vector3d mu2 = voxel2.mu; 118 | Eigen::Matrix3d sigma1 = Eigen::Matrix3d::Identity(); 119 | if (voxel1.num_points > 1) { 120 | sigma1 = voxel1.sigma / (voxel1.num_points - 1); 121 | sigma1 = (sigma1 + sigma1.transpose()) / 2; // Ensure symmetry 122 | Eigen::SelfAdjointEigenSolver eigensolver(sigma1); 123 | sigma1 = eigensolver.eigenvectors() * eigensolver.eigenvalues().cwiseMax(1e-6).asDiagonal() * 124 | eigensolver.eigenvectors().transpose(); 125 | } 126 | Eigen::Matrix3d sigma2 = Eigen::Matrix3d::Identity(); 127 | if (voxel2.num_points > 1) { 128 | sigma2 = voxel2.sigma / (voxel2.num_points - 1); 129 | sigma2 = (sigma2 + sigma2.transpose()) / 2; // Ensure symmetry 130 | Eigen::SelfAdjointEigenSolver eigensolver(sigma2); 131 | sigma2 = eigensolver.eigenvectors() * eigensolver.eigenvalues().cwiseMax(1e-6).asDiagonal() * 132 | eigensolver.eigenvectors().transpose(); 133 | } 134 | Eigen::Vector3d mu_diff = mu1 - mu2; 135 | Eigen::Matrix3d sigma_sum = sigma1 + sigma2; 136 | Eigen::Matrix3d sigma_sqrt = sigma1.llt().matrixL() * sigma2 * sigma1.llt().matrixL().transpose(); 137 | sigma_sqrt = sigma_sqrt.llt().matrixL(); 138 | double distance = mu_diff.dot(mu_diff) + sigma_sum.trace() - 2 * sigma_sqrt.trace(); 139 | return std::sqrt(std::max(0.0, distance)); // Ensure non-negative distance 140 | } 141 | 142 | void VoxelCalculator::updateVoxelMap(const VoxelMap >_map) { 143 | int old_area_num = 0, active_num = 0, new_area_num = 0; 144 | // First, mark all existing voxels as new 145 | for (auto &kv : voxel_map_) { 146 | kv.second.active = 2; // new area 147 | } 148 | // Compare with GT map and update labels 149 | for (const auto >_kv : gt_map) { 150 | const Eigen::Vector3i &index = gt_kv.first; 151 | auto it = voxel_map_.find(index); 152 | if (it != voxel_map_.end()) { 153 | it->second.active = 1; // active 154 | active_num++; 155 | } else { 156 | // GT voxel doesn't exist in EST map 157 | VoxelInfo voxel_info; 158 | voxel_info.active = 0; // old area 159 | voxel_map_[index] = voxel_info; 160 | old_area_num++; 161 | } 162 | } 163 | // Count remaining new areas 164 | for (const auto &kv : voxel_map_) { 165 | if (kv.second.active == 2) { 166 | new_area_num++; 167 | } 168 | } 169 | std::cout << "Update voxel map: " << voxel_map_.size() << std::endl; 170 | std::cout << "Update active/old/new voxel num: " << active_num << " " << old_area_num << " " << new_area_num 171 | << std::endl; 172 | } 173 | 174 | 175 | double VoxelCalculator::updateVoxelMap(const PCLPointCloud::Ptr &cloud) { 176 | voxel_map_tmp_ = voxel_map_; 177 | 178 | // Reset all voxels to old_area 179 | for (auto &kv : voxel_map_) { 180 | kv.second.active = 0; 181 | } 182 | 183 | for (const auto &point : cloud->points) { 184 | Eigen::Vector3d point_world = point.getVector3fMap().cast(); 185 | Eigen::Vector3i voxel_index = getVoxelIndex(point_world); 186 | auto it = voxel_map_.find(voxel_index); 187 | if (it == voxel_map_.end()) { 188 | // do not find the existing voxel, add new voxel 189 | VoxelInfo voxel_info; 190 | voxel_info.mu = point_world; 191 | voxel_info.num_points = 1; 192 | computeVoxelEntropy(voxel_info); 193 | voxel_map_.emplace(voxel_index, voxel_info); 194 | // set entropy_old for new voxel 195 | voxel_map_[voxel_index].entropy_old = voxel_info.entropy; 196 | // set entropy_old for new voxel 197 | // voxel_map_[voxel_index].entropy_old = 0.0; 198 | // mark as new area 199 | voxel_map_[voxel_index].active = 2; 200 | } else { 201 | // find the existing voxel 202 | VoxelInfo &voxel = it->second; 203 | Eigen::Vector3d old_mu = voxel.mu; 204 | 205 | // save old entropy 206 | auto it2 = voxel_map_tmp_.find(voxel_index); 207 | if (it2 != voxel_map_tmp_.end()) { 208 | voxel.entropy_old = it2->second.entropy; 209 | } 210 | // update gaussian params 211 | voxel.mu = (voxel.mu * voxel.num_points + point_world) / (voxel.num_points + 1); 212 | voxel.sigma = (voxel.num_points - 1.0) / voxel.num_points * voxel.sigma 213 | + (point_world - old_mu) * (point_world - voxel.mu).transpose(); 214 | voxel.num_points++; 215 | computeVoxelEntropy(voxel); 216 | // mark as active voxel if it's not new area 217 | if (voxel.active != 2) { 218 | voxel.active = 1; 219 | } 220 | } 221 | } 222 | // mark voxels that are not updated as old area 223 | old_area_num_ = active_num_ = new_area_num_ = 0; 224 | for (auto &kv : voxel_map_) { 225 | if (kv.second.active == 0) { 226 | //kv.second.entropy_old = kv.second.entropy; 227 | old_area_num_++; 228 | } else if (kv.second.active == 1) { 229 | active_num_++; 230 | } else if (kv.second.active == 2) { 231 | new_area_num_++; 232 | } 233 | } 234 | std::cout << "update voxel map: " << voxel_map_.size() << " " << voxel_map_tmp_.size() << std::endl; 235 | std::cout << "update active/old/new voxel num: " << active_num_ << " " << old_area_num_ << " " << new_area_num_ 236 | << std::endl; 237 | return total_entropy_; 238 | } 239 | 240 | 241 | Eigen::Vector3i VoxelCalculator::getVoxelIndex(const Eigen::Vector3d &point) { 242 | return Eigen::Vector3i(std::floor(point.x() / voxel_size_), 243 | std::floor(point.y() / voxel_size_), 244 | std::floor(point.z() / voxel_size_)); 245 | } 246 | -------------------------------------------------------------------------------- /map_eval/src/voxel_calculator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VOXEL_LIO_INFORMATIONGAINCALCULATOR_H 2 | #define VOXEL_LIO_INFORMATIONGAINCALCULATOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using PointType = pcl::PointXYZI; // PointCloud type definition with intensity 15 | using PCLPointCloud = pcl::PointCloud; // Define the PointCloud with PointType 16 | 17 | // Custom hash function for Eigen::Vector3i, used as a key in unordered_map 18 | struct VoxelHasher { 19 | std::size_t operator()(const Eigen::Vector3i &key) const { 20 | return std::hash()(key[0]) ^ std::hash()(key[1]) ^ std::hash()(key[2]); 21 | } 22 | }; 23 | 24 | // Voxel information structure to store mean, covariance, entropy, energy, and other properties 25 | struct VoxelInfo { 26 | Eigen::Vector3d mu; // Mean position of the points in the voxel 27 | Eigen::Matrix3d sigma; // Covariance matrix for the voxel 28 | int num_points; // Number of points inside the voxel 29 | double entropy; // Entropy for the voxel, a measure of uncertainty 30 | double energy; // Energy associated with the voxel, often related to the covariance 31 | 32 | int active; // Voxel status: 0 - old, 1 - active, 2 - new 33 | double entropy_old; // Previous entropy value to compare 34 | 35 | // Constructor to initialize voxel data 36 | VoxelInfo() : mu(Eigen::Vector3d::Zero()), sigma(Eigen::Matrix3d::Zero()), num_points(0), entropy(0), 37 | entropy_old(0), energy(0), active(0) {} 38 | }; 39 | 40 | // Voxel map structure to store VoxelInfo using Eigen::Vector3i as the key 41 | using VoxelMap = std::unordered_map; 42 | 43 | 44 | // VoxelCalculator class to handle voxel operations such as entropy calculation, Wasserstein distance, and map updates 45 | class VoxelCalculator { 46 | public: 47 | VoxelCalculator() = default; // Default constructor 48 | 49 | // Constructor to initialize with a specific voxel size 50 | VoxelCalculator(double voxel_size); 51 | 52 | // Build voxel map from different types of point clouds (Open3D or PCL) 53 | void buildVoxelMap(const open3d::geometry::PointCloud &cloud); 54 | void buildVoxelMap(const PCLPointCloud::Ptr &cloud); 55 | 56 | // Update the voxel map with a new point cloud and pose transformation 57 | double updateVoxelMap(const PCLPointCloud::Ptr &cloud, const Eigen::Isometry3d &pose); 58 | void updateVoxelMap(const VoxelMap &map); 59 | double updateVoxelMap(const PCLPointCloud::Ptr &cloud); 60 | 61 | 62 | // Compute total entropy of the voxel map 63 | Eigen::Vector2d computeTotalEntropy(); 64 | 65 | // Methods to compute Wasserstein distance, KLDivergence, and other metrics between voxel maps 66 | double computeWassersteinDistance(const VoxelMap &map1, const VoxelMap &map2); 67 | double computeWassersteinDistance2(const VoxelMap &map1, const VoxelMap &map2); 68 | double computeWassersteinDistance3(const VoxelMap &map1, const VoxelMap &map2); 69 | double computeWassersteinDistanceGaussian(const VoxelInfo &voxel1, const VoxelInfo &voxel2); 70 | 71 | double computeKLDivergenceGaussian(const VoxelInfo &voxel1, const VoxelInfo &voxel2); 72 | double computeKLDivergence(const VoxelMap &map1, const VoxelMap &map2); 73 | double computeKLDivergence2(const VoxelMap &map1, const VoxelMap &map2); 74 | 75 | double computeGaussianKernel(const Eigen::Vector3d &x, const Eigen::Vector3d &y, double sigma); 76 | double computeGaussianBhattacharyya(const VoxelInfo &voxel1, const VoxelInfo &voxel2, double sigma); 77 | 78 | double computeMMD(const VoxelMap &map1, const VoxelMap &map2, double sigma); 79 | double computeMMD2(const VoxelMap &map1, const VoxelMap &map2, double sigma); 80 | 81 | // Get the current voxel map 82 | const VoxelMap &getVoxelMap() const { return voxel_map_; } 83 | 84 | // Get the indices of neighboring voxels within a certain radius 85 | std::vector getNeighborIndices(const Eigen::Vector3i &index, int radius); 86 | 87 | private: 88 | // Method to compute the entropy for a single voxel 89 | void computeVoxelEntropy(VoxelInfo &voxel); 90 | 91 | // Method to compute the voxel index based on a 3D point 92 | Eigen::Vector3i getVoxelIndex(const Eigen::Vector3d &point); 93 | 94 | private: 95 | double voxel_size_; // Size of each voxel 96 | 97 | VoxelMap voxel_map_; // The main voxel map storing all voxel information 98 | VoxelMap voxel_map_tmp_; // Temporary voxel map for certain operations 99 | 100 | // Variables to track the number of different types of voxels 101 | int active_num_ = 0; 102 | int new_area_num_ = 0; 103 | int old_area_num_ = 0; 104 | 105 | // Entropy and energy values for different regions of the map 106 | double total_entropy_; 107 | double total_energy_; 108 | double active_entropy_old_; 109 | double active_entropy_; 110 | double new_area_entropy_; 111 | double new_area_entropy_old_; 112 | double old_area_entropy_; 113 | double old_area_entropy_old_; 114 | 115 | // Store divergences and distances for specific voxels 116 | std::vector> voxel_divergences; 117 | std::vector> voxel_distances; 118 | }; 119 | 120 | #endif //VOXEL_LIO_INFORMATIONGAINCALCULATOR_H 121 | --------------------------------------------------------------------------------