├── .gitignore ├── LICENSE ├── README.md ├── dataset_loaders ├── CameraParameters.py └── IclTumCompatLoader.py ├── eval_icl.ipynb ├── eval_synthetic.ipynb ├── metrics ├── ape.py └── rpe.py ├── perturbation └── perturbation.py ├── plane_backends ├── BaseBackend.py ├── EFBackend.py ├── backend_impls │ ├── BaregBackend.py │ ├── EFAlternatingBackend.py │ ├── EFDenseBackend.py │ ├── LandmarkBackend.py │ └── PiFBackend.py └── solver_factory.py ├── plane_extractor └── plane_extractor.py ├── requirements.txt └── scripts └── enough_planes ├── EnoughPlanesDetector.py ├── Pcd.py └── Plane.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | .idea 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | build/ 14 | develop-eggs/ 15 | dist/ 16 | downloads/ 17 | eggs/ 18 | .eggs/ 19 | lib/ 20 | lib64/ 21 | parts/ 22 | sdist/ 23 | var/ 24 | wheels/ 25 | pip-wheel-metadata/ 26 | share/python-wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | MANIFEST 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .nox/ 46 | .coverage 47 | .coverage.* 48 | .cache 49 | nosetests.xml 50 | coverage.xml 51 | *.cover 52 | *.py,cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | local_settings.py 63 | db.sqlite3 64 | db.sqlite3-journal 65 | 66 | # Flask stuff: 67 | instance/ 68 | .webassets-cache 69 | 70 | # Scrapy stuff: 71 | .scrapy 72 | 73 | # Sphinx documentation 74 | docs/_build/ 75 | 76 | # PyBuilder 77 | target/ 78 | 79 | # Jupyter Notebook 80 | .ipynb_checkpoints 81 | 82 | # IPython 83 | profile_default/ 84 | ipython_config.py 85 | 86 | # pyenv 87 | .python-version 88 | 89 | # pipenv 90 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 91 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 92 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 93 | # install all needed dependencies. 94 | #Pipfile.lock 95 | 96 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 97 | __pypackages__/ 98 | 99 | # Celery stuff 100 | celerybeat-schedule 101 | celerybeat.pid 102 | 103 | # SageMath parsed files 104 | *.sage.py 105 | 106 | # Environments 107 | .env 108 | .venv 109 | env/ 110 | venv/ 111 | ENV/ 112 | env.bak/ 113 | venv.bak/ 114 | 115 | # Spyder project settings 116 | .spyderproject 117 | .spyproject 118 | 119 | # Rope project settings 120 | .ropeproject 121 | 122 | # mkdocs documentation 123 | /site 124 | 125 | # mypy 126 | .mypy_cache/ 127 | .dmypy.json 128 | dmypy.json 129 | 130 | # Pyre type checker 131 | .pyre/ 132 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EF Plane SLAM Back-end Benchmark 2 | 3 | This repository contains all required scripts and notebooks to compare quality of different SLAM Backends 4 | using planes as landmarks. 5 | 6 | ## Data 7 | As the data source we provide two variants: [EVOPS dataset](https://evops.netlify.app) 8 | and synthetic planar poses generator from [mrob](https://github.com/prime-slam/mrob) library. 9 | 10 | ## Supported backends 11 | For comparison needs two algorithms except origin EF backend are implemented in **mrob** library. They are: 12 | * [Pi-Factor](https://www.cs.cmu.edu/~kaess/pub/Zhou21ral2.pdf) 13 | * [Bareg](https://arxiv.org/abs/2108.02976) 14 | * [EF]() Eigen Factors 15 | 16 | ## Evaluations 17 | You can see examples of backend comparisons in two Python notebooks: 18 | * `eval_icl.ipynb` --- example of backend comparison on data from EVOPS dataset (sequence based on [ICL NUIM](https://www.doc.ic.ac.uk/~ahanda/VaFRIC/iclnuim.html) Living Room kt0 trajectory) 19 | * `eval_synthetic.ipynb` --- example of backend comparison on synthetic data generated by `CreatePoints` method from **mrob** library 20 | 21 | ## Metrics 22 | To compare SLAM backends predicted trajectories on perturbed origin poses are compared with ground truth poses using classic `ape` and `rpe` metrics. 23 | -------------------------------------------------------------------------------- /dataset_loaders/CameraParameters.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import open3d as o3d 16 | 17 | 18 | class CameraParameters: 19 | def __init__(self, width, height, cx, cy, fx, fy, scale): 20 | self.width = width 21 | self.height = height 22 | self.cx = cx 23 | self.cy = cy 24 | self.fx = fx 25 | self.fy = fy 26 | self.scale = scale 27 | 28 | def to_o3d_intrinsics(self): 29 | intrinsics = o3d.camera.PinholeCameraIntrinsic() 30 | intrinsics.width, intrinsics.height = self.width, self.height 31 | intrinsics.intrinsic_matrix = [ 32 | [self.fx, 0, self.cx], 33 | [0, self.fy, self.cy], 34 | [0, 0, 1], 35 | ] 36 | 37 | return intrinsics 38 | -------------------------------------------------------------------------------- /dataset_loaders/IclTumCompatLoader.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import numpy as np 16 | import open3d as o3d 17 | 18 | 19 | class IclTumCompatLoader: 20 | def __init__(self, camera): 21 | self.camera = camera 22 | self.color_to_plane_id = {} 23 | self.id_counter = 0 24 | 25 | def load_point_cloud(self, depth_path, label_path): 26 | color_raw = o3d.io.read_image(str(label_path)) 27 | depth_raw = o3d.io.read_image(str(depth_path)) 28 | rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth( 29 | color_raw, 30 | depth_raw, 31 | convert_rgb_to_intensity=False, 32 | depth_scale=self.camera.scale, 33 | depth_trunc=100 34 | ) 35 | 36 | intrinsic = o3d.camera.PinholeCameraIntrinsic() 37 | intrinsic.width, intrinsic.height = self.camera.width, self.camera.height 38 | intrinsic.intrinsic_matrix = [ 39 | [float(self.camera.fx), 0, float(self.camera.cx)], 40 | [0, float(self.camera.fy), float(self.camera.cy)], 41 | [0, 0, 1], 42 | ] 43 | 44 | point_cloud = o3d.geometry.PointCloud() 45 | point_cloud = point_cloud.create_from_rgbd_image(rgbd_image, intrinsic).transform(np.asarray([ 46 | [1, 0, 0, 0], 47 | [0, 1, 0, 0], 48 | [0, 0, 1, 0], 49 | [0, 0, 0, 1]])) 50 | 51 | return point_cloud 52 | -------------------------------------------------------------------------------- /eval_synthetic.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "outputs": [], 7 | "source": [ 8 | "# Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova\n", 9 | "#\n", 10 | "# Licensed under the Apache License, Version 2.0 (the \"License\");\n", 11 | "# you may not use this file except in compliance with the License.\n", 12 | "# You may obtain a copy of the License at\n", 13 | "#\n", 14 | "# http://www.apache.org/licenses/LICENSE-2.0\n", 15 | "#\n", 16 | "# Unless required by applicable law or agreed to in writing, software\n", 17 | "# distributed under the License is distributed on an \"AS IS\" BASIS,\n", 18 | "# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n", 19 | "# See the License for the specific language governing permissions and\n", 20 | "# limitations under the License." 21 | ], 22 | "metadata": { 23 | "collapsed": false, 24 | "pycharm": { 25 | "name": "#%%\n" 26 | } 27 | } 28 | }, 29 | { 30 | "cell_type": "code", 31 | "execution_count": 7, 32 | "outputs": [], 33 | "source": [ 34 | "import mrob\n", 35 | "import numpy as np\n", 36 | "import os\n", 37 | "import pandas as pd\n", 38 | "import open3d as o3d\n", 39 | "import pickle\n", 40 | "\n", 41 | "from matplotlib import pyplot as plt\n", 42 | "from tqdm import tqdm" 43 | ], 44 | "metadata": { 45 | "collapsed": false, 46 | "pycharm": { 47 | "name": "#%%\n" 48 | } 49 | } 50 | }, 51 | { 52 | "cell_type": "code", 53 | "execution_count": 8, 54 | "outputs": [], 55 | "source": [ 56 | "from plane_backends.solver_factory import create_solver_by_name\n", 57 | "\n", 58 | "from metrics.ape import ape\n", 59 | "from metrics.rpe import rpe\n", 60 | "\n", 61 | "from perturbation.perturbation import Perturbation, generate_random_pose_shift" 62 | ], 63 | "metadata": { 64 | "collapsed": false, 65 | "pycharm": { 66 | "name": "#%%\n" 67 | } 68 | } 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": 9, 73 | "outputs": [], 74 | "source": [ 75 | "def save_generated_data(generated_data, perturbed_poses, folder_name):\n", 76 | " os.mkdir(folder_name)\n", 77 | " for i, _ in enumerate(perturbed_poses):\n", 78 | " points = np.vstack(generated_data.get_point_cloud(i))\n", 79 | " pcd = o3d.geometry.PointCloud()\n", 80 | " pcd.points = o3d.utility.Vector3dVector(points)\n", 81 | " o3d.io.write_point_cloud(os.path.join(folder_name, f'{i}.pcd'), pcd)\n", 82 | "\n", 83 | " gt_poses = [T.T() for T in generated_data.get_trajectory()]\n", 84 | " with open(os.path.join(folder_name, 'gt_poses.pkl'), 'wb') as f:\n", 85 | " pickle.dump(gt_poses, f)\n", 86 | "\n", 87 | " with open(os.path.join(folder_name, 'pert_poses.pkl'), 'wb') as f:\n", 88 | " pickle.dump(perturbed_poses, f)" 89 | ], 90 | "metadata": { 91 | "collapsed": false, 92 | "pycharm": { 93 | "name": "#%%\n" 94 | } 95 | } 96 | }, 97 | { 98 | "cell_type": "code", 99 | "execution_count": 10, 100 | "outputs": [], 101 | "source": [ 102 | "def generated_data_to_observations(generated_data, poses_count):\n", 103 | " observations = []\n", 104 | " for i in range(poses_count):\n", 105 | " observation = {}\n", 106 | " points = np.vstack(generated_data.get_point_cloud(i))\n", 107 | " labels = generated_data.get_point_plane_ids(i)\n", 108 | " for label in np.unique(labels):\n", 109 | " point_indices = np.where(labels == label)[0]\n", 110 | " observation[label] = points[point_indices]\n", 111 | "\n", 112 | " observations.append(observation)\n", 113 | " return observations" 114 | ], 115 | "metadata": { 116 | "collapsed": false, 117 | "pycharm": { 118 | "name": "#%%\n" 119 | } 120 | } 121 | }, 122 | { 123 | "cell_type": "code", 124 | "execution_count": 23, 125 | "outputs": [ 126 | { 127 | "name": "stderr", 128 | "output_type": "stream", 129 | "text": [ 130 | "100%|██████████| 1/1 [00:52<00:00, 52.47s/it]\n", 131 | "100%|██████████| 1/1 [02:13<00:00, 133.57s/it]\n", 132 | "100%|██████████| 1/1 [04:06<00:00, 246.04s/it]\n", 133 | "100%|██████████| 1/1 [05:55<00:00, 355.86s/it]\n", 134 | "100%|██████████| 1/1 [06:51<00:00, 411.78s/it]\n" 135 | ] 136 | } 137 | ], 138 | "source": [ 139 | "planes_count_list = [5] # [5, 10, 25, 50, 100]\n", 140 | "poses_count_list = [5, 10, 25, 50, 75]\n", 141 | "point_noise_list = [0.005] # [0.005, 0.01, 0.02, 0.04, 0.08]\n", 142 | "points_count_list = [5] # [5, 10, 25, 50, 100, 200]\n", 143 | "perturbations = [\n", 144 | " Perturbation(rotation_shift=1, translation_shift=0.01),\n", 145 | " Perturbation(rotation_shift=5, translation_shift=0.05),\n", 146 | " Perturbation(rotation_shift=10, translation_shift=0.1),\n", 147 | " Perturbation(rotation_shift=15, translation_shift=0.15),\n", 148 | " Perturbation(rotation_shift=20, translation_shift=0.2),\n", 149 | "]\n", 150 | "bias_noise = 0.01\n", 151 | "T0 = mrob.geometry.SE3(np.eye(4))\n", 152 | "solvers = [\n", 153 | " 'bareg',\n", 154 | " 'pi-factor',\n", 155 | " 'ef-centered',\n", 156 | "]\n", 157 | "df_stat = pd.DataFrame()\n", 158 | "\n", 159 | "SAMPLES_COUNT = 100\n", 160 | "\n", 161 | "ind = 0\n", 162 | "for perturbation in perturbations:\n", 163 | " for points_count in points_count_list:\n", 164 | " for point_noise in point_noise_list:\n", 165 | " for planes_count in tqdm(planes_count_list):\n", 166 | " for poses_count in poses_count_list:\n", 167 | " for sample_index in range(SAMPLES_COUNT):\n", 168 | " # Generate synthetic data\n", 169 | " total_points_count = planes_count * points_count\n", 170 | " generated_data = mrob.registration.CreatePoints(\n", 171 | " total_points_count,\n", 172 | " planes_count,\n", 173 | " poses_count,\n", 174 | " point_noise,\n", 175 | " bias_noise,\n", 176 | " T0\n", 177 | " )\n", 178 | " name = '_'.join(\n", 179 | " [\n", 180 | " str(points_count),\n", 181 | " str(planes_count),\n", 182 | " str(poses_count),\n", 183 | " str(point_noise),\n", 184 | " str(sample_index),\n", 185 | " str(perturbation.rotation_shift),\n", 186 | " str(perturbation.translation_shift)\n", 187 | " ]\n", 188 | " )\n", 189 | " observations = generated_data_to_observations(generated_data, poses_count)\n", 190 | " gt_poses = [T.T() for T in generated_data.get_trajectory()]\n", 191 | "\n", 192 | " # Apply perturbations\n", 193 | " perturbed_poses = []\n", 194 | " for i, gt_pose in enumerate(gt_poses):\n", 195 | " perturbed_poses.append(\n", 196 | " gt_pose @ generate_random_pose_shift(\n", 197 | " perturbation.rotation_shift / 180 * np.pi,\n", 198 | " perturbation.translation_shift\n", 199 | " )\n", 200 | " )\n", 201 | "\n", 202 | " # save_generated_data(generated_data, perturbed_poses, name)\n", 203 | "\n", 204 | " # Estimate trajectory with each backend\n", 205 | " initial_trajectory = perturbed_poses\n", 206 | " for solver_name in solvers:\n", 207 | " solver = create_solver_by_name(solver_name, iterations_count=300)\n", 208 | " refined_poses, iterations_used_count, optimization_time = solver.solve(\n", 209 | " observations,\n", 210 | " initial_trajectory\n", 211 | " )\n", 212 | " ape_translation, ape_rotation = ape(gt_poses[:poses_count], refined_poses[:poses_count])\n", 213 | " rpe_translation, rpe_rotation = rpe(gt_poses[:poses_count], refined_poses[:poses_count])\n", 214 | " stat = {\n", 215 | " 'sample': sample_index,\n", 216 | " 'pose_perturbation': '{}'.format(str(perturbation)),\n", 217 | " 'point_noise': point_noise,\n", 218 | " 'planes_count': planes_count,\n", 219 | " 'poses_count': poses_count,\n", 220 | " 'points_count': points_count,\n", 221 | " 'time': optimization_time,\n", 222 | " 'solver': solver_name,\n", 223 | " 'iterations': iterations_used_count,\n", 224 | " 'ape_rotation': ape_rotation,\n", 225 | " 'rpe_rotation': rpe_rotation,\n", 226 | " 'ape_translation': ape_translation,\n", 227 | " 'rpe_translation': rpe_translation\n", 228 | " }\n", 229 | "\n", 230 | " df_stat = pd.concat([df_stat, pd.DataFrame(stat, index=[ind])])\n", 231 | " ind += 1" 232 | ], 233 | "metadata": { 234 | "collapsed": false, 235 | "pycharm": { 236 | "name": "#%%\n" 237 | } 238 | } 239 | }, 240 | { 241 | "cell_type": "code", 242 | "execution_count": 24, 243 | "outputs": [ 244 | { 245 | "name": "stdout", 246 | "output_type": "stream", 247 | "text": [ 248 | "[5, 10, 25, 50, 75]\n", 249 | "[0.07795214 0.10587338 0.12453548 0.13431448 0.15047763]\n", 250 | "[5, 10, 25, 50, 75]\n", 251 | "[0.01248174 0.01384102 0.01419566 0.0154653 0.01837304]\n", 252 | "[5, 10, 25, 50, 75]\n", 253 | "[0.10144651 0.11953621 0.15279705 0.1662112 0.17449846]\n" 254 | ] 255 | }, 256 | { 257 | "name": "stderr", 258 | "output_type": "stream", 259 | "text": [ 260 | "C:\\Users\\dimaj\\AppData\\Local\\Temp\\ipykernel_8268\\4065899866.py:2: UserWarning: Boolean Series key will be reindexed to match DataFrame index.\n", 261 | " median_stat = df_stat[df_stat['solver'] == solver_name][df_stat['planes_count'] == 5].groupby(\n", 262 | "C:\\Users\\dimaj\\AppData\\Local\\Temp\\ipykernel_8268\\4065899866.py:2: FutureWarning: The default value of numeric_only in DataFrameGroupBy.median is deprecated. In a future version, numeric_only will default to False. Either specify numeric_only or select only columns which should be valid for the function.\n", 263 | " median_stat = df_stat[df_stat['solver'] == solver_name][df_stat['planes_count'] == 5].groupby(\n", 264 | "C:\\Users\\dimaj\\AppData\\Local\\Temp\\ipykernel_8268\\4065899866.py:2: UserWarning: Boolean Series key will be reindexed to match DataFrame index.\n", 265 | " median_stat = df_stat[df_stat['solver'] == solver_name][df_stat['planes_count'] == 5].groupby(\n", 266 | "C:\\Users\\dimaj\\AppData\\Local\\Temp\\ipykernel_8268\\4065899866.py:2: FutureWarning: The default value of numeric_only in DataFrameGroupBy.median is deprecated. In a future version, numeric_only will default to False. Either specify numeric_only or select only columns which should be valid for the function.\n", 267 | " median_stat = df_stat[df_stat['solver'] == solver_name][df_stat['planes_count'] == 5].groupby(\n", 268 | "C:\\Users\\dimaj\\AppData\\Local\\Temp\\ipykernel_8268\\4065899866.py:2: UserWarning: Boolean Series key will be reindexed to match DataFrame index.\n", 269 | " median_stat = df_stat[df_stat['solver'] == solver_name][df_stat['planes_count'] == 5].groupby(\n", 270 | "C:\\Users\\dimaj\\AppData\\Local\\Temp\\ipykernel_8268\\4065899866.py:2: FutureWarning: The default value of numeric_only in DataFrameGroupBy.median is deprecated. In a future version, numeric_only will default to False. Either specify numeric_only or select only columns which should be valid for the function.\n", 271 | " median_stat = df_stat[df_stat['solver'] == solver_name][df_stat['planes_count'] == 5].groupby(\n" 272 | ] 273 | }, 274 | { 275 | "data": { 276 | "text/plain": "[]" 277 | }, 278 | "execution_count": 24, 279 | "metadata": {}, 280 | "output_type": "execute_result" 281 | }, 282 | { 283 | "data": { 284 | "text/plain": "
", 285 | "image/png": "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\n" 286 | }, 287 | "metadata": {}, 288 | "output_type": "display_data" 289 | } 290 | ], 291 | "source": [ 292 | "for solver_name in solvers:\n", 293 | " median_stat = df_stat[df_stat['solver'] == solver_name][df_stat['planes_count'] == 5].groupby(\n", 294 | " ['planes_count', 'poses_count', 'points_count']\n", 295 | " ).median()\n", 296 | " indices = list(median_stat.index)\n", 297 | " print(poses_count_list)\n", 298 | " print(median_stat['ape_translation'].values)\n", 299 | " plt.plot(poses_count_list, median_stat['ape_translation'].values)\n", 300 | "\n", 301 | "plt.legend(['BAREG', 'Pi-factor', 'EF-Centered'])\n", 302 | "plt.semilogy()" 303 | ], 304 | "metadata": { 305 | "collapsed": false, 306 | "pycharm": { 307 | "name": "#%%\n" 308 | } 309 | } 310 | } 311 | ], 312 | "metadata": { 313 | "kernelspec": { 314 | "display_name": "Python 3 (ipykernel)", 315 | "language": "python", 316 | "name": "python3" 317 | }, 318 | "language_info": { 319 | "codemirror_mode": { 320 | "name": "ipython", 321 | "version": 3 322 | }, 323 | "file_extension": ".py", 324 | "mimetype": "text/x-python", 325 | "name": "python", 326 | "nbconvert_exporter": "python", 327 | "pygments_lexer": "ipython3", 328 | "version": "3.9.13" 329 | } 330 | }, 331 | "nbformat": 4, 332 | "nbformat_minor": 5 333 | } -------------------------------------------------------------------------------- /metrics/ape.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import copy 16 | 17 | import mrob 18 | import numpy as np 19 | 20 | 21 | def ape(traj_gt_, traj_est_): 22 | traj_gt = [np.linalg.inv(traj_gt_[0]) @ T for T in copy.deepcopy(traj_gt_)] 23 | traj_est = [np.linalg.inv(traj_est_[0]) @ T for T in copy.deepcopy(traj_est_)] 24 | dTs = [np.linalg.inv(traj_gt[i]) @ traj_est[i] for i in range(len(traj_gt))] 25 | 26 | transl_err = [] 27 | rot_err = [] 28 | for dT in dTs: 29 | transl_err.append(mrob.geometry.SE3(dT).distance_trans() ** 2) 30 | rot_err.append(mrob.geometry.SE3(dT).distance_rotation() ** 2) 31 | return np.mean(transl_err) ** 0.5, np.mean(rot_err) ** 0.5 32 | 33 | -------------------------------------------------------------------------------- /metrics/rpe.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import mrob 16 | import numpy as np 17 | 18 | 19 | def rpe(traj_gt, traj_est): 20 | dT_traj_gt = [np.linalg.inv(traj_gt[i]) @ traj_gt[i + 1] for i in range(len(traj_gt) - 1)] 21 | dT_traj_est = [np.linalg.inv(traj_est[i]) @ traj_est[i + 1] for i in range(len(traj_est) - 1)] 22 | dTs = [np.linalg.inv(dT_traj_gt[i]) @ dT_traj_est[i] for i in range(len(dT_traj_gt))] 23 | 24 | transl_err = [] 25 | rot_err = [] 26 | for dT in dTs: 27 | transl_err.append(mrob.geometry.SE3(dT).distance_trans() ** 2) 28 | rot_err.append(mrob.geometry.SE3(dT).distance_rotation() ** 2) 29 | return np.mean(transl_err) ** 0.5, np.mean(rot_err) ** 0.5 -------------------------------------------------------------------------------- /perturbation/perturbation.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import mrob 16 | import numpy as np 17 | 18 | 19 | class Perturbation: 20 | def __init__(self, rotation_shift, translation_shift): 21 | self.rotation_shift = rotation_shift 22 | self.translation_shift = translation_shift 23 | 24 | def __str__(self): 25 | return f"{self.rotation_shift}_{self.translation_shift}" 26 | 27 | 28 | def generate_uniform_vector(r): 29 | theta = np.random.uniform(0, 2 * np.pi) 30 | phi = np.random.uniform(0, np.pi) 31 | x = r * np.sin(phi) * np.cos(theta) 32 | y = r * np.sin(phi) * np.sin(theta) 33 | z = r * np.cos(phi) 34 | return np.array([x, y, z]) 35 | 36 | 37 | def generate_random_pose_shift(rotation_shift, translation_shift): 38 | shift_se3 = np.hstack([generate_uniform_vector(rotation_shift), generate_uniform_vector(translation_shift)]) 39 | return mrob.geometry.SE3(shift_se3).T() -------------------------------------------------------------------------------- /plane_backends/BaseBackend.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import abc 16 | import datetime 17 | import mrob 18 | import numpy as np 19 | 20 | 21 | class BaseBackend(abc.ABC): 22 | def __init__(self, iterations_count): 23 | self.graph = mrob.FGraph() 24 | self.iterations_count = iterations_count 25 | 26 | def solve(self, observations, Ts_init=None): 27 | if Ts_init is None: 28 | Ts_init = [np.eye(4) for _ in observations] 29 | 30 | self._init_poses(Ts_init) 31 | self.__add_observations(observations) 32 | timestamp_before_optimization = datetime.datetime.now() 33 | used_iterations_count = self._optimize() 34 | timestamp_after_optimization = datetime.datetime.now() 35 | return ( 36 | self.__get_trajectory(), 37 | used_iterations_count, 38 | (timestamp_after_optimization - timestamp_before_optimization).microseconds 39 | ) 40 | 41 | def _init_poses(self, Ts_init): 42 | # Add nodes for poses 43 | for i, Ts in enumerate(Ts_init): 44 | node_mode = mrob.NODE_ANCHOR if i == 0 else mrob.NODE_STANDARD 45 | self.graph.add_node_pose_3d(mrob.geometry.SE3(Ts), node_mode) 46 | 47 | def __get_trajectory(self): 48 | return self.graph.get_estimated_state() 49 | 50 | def __add_observations(self, observations): 51 | label_id_to_graph_id = {} 52 | for pose_id, observation in enumerate(observations): 53 | for plane_id in observation: 54 | if plane_id in label_id_to_graph_id: 55 | # This landmark already exists in the graph 56 | graph_plane_id = label_id_to_graph_id[plane_id] 57 | else: 58 | # This landmarks doesn't exist in the graph 59 | graph_plane_id = self._add_node_to_graph() 60 | label_id_to_graph_id[plane_id] = graph_plane_id 61 | 62 | self._register_observation(observation, pose_id, plane_id, graph_plane_id) 63 | 64 | @abc.abstractmethod 65 | def _add_node_to_graph(self): 66 | pass 67 | 68 | @abc.abstractmethod 69 | def _register_observation(self, observation, pose_id, plane_id, graph_plane_id): 70 | pass 71 | 72 | @abc.abstractmethod 73 | def _optimize(self): 74 | pass 75 | -------------------------------------------------------------------------------- /plane_backends/EFBackend.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from plane_backends.BaseBackend import BaseBackend 16 | 17 | 18 | class EFBackend(BaseBackend): 19 | def _add_node_to_graph(self): 20 | return self.graph.add_eigen_factor_plane() 21 | 22 | def _register_observation(self, observation, pose_id, plane_id, graph_plane_id): 23 | self.graph.eigen_factor_plane_add_points_array( 24 | planeEigenId=graph_plane_id, 25 | nodePoseId=pose_id, 26 | pointsArray=observation[plane_id], 27 | W=1.0 28 | ) 29 | -------------------------------------------------------------------------------- /plane_backends/backend_impls/BaregBackend.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import mrob 16 | 17 | from plane_backends.BaseBackend import BaseBackend 18 | 19 | 20 | class BaregBackend(BaseBackend): 21 | def _add_node_to_graph(self): 22 | return self.graph.add_bareg_plane() 23 | 24 | def _register_observation(self, observation, pose_id, plane_id, graph_plane_id): 25 | self.graph.eigen_factor_plane_add_points_array( 26 | planeEigenId=graph_plane_id, 27 | nodePoseId=pose_id, 28 | pointsArray=observation[plane_id], 29 | W=1.0 30 | ) 31 | 32 | def _optimize(self): 33 | return self.graph.solve(mrob.LM_ELLIPS, self.iterations_count) -------------------------------------------------------------------------------- /plane_backends/backend_impls/EFAlternatingBackend.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import mrob 16 | 17 | from plane_backends.EFBackend import EFBackend 18 | 19 | 20 | class EFAlternatingBackend(EFBackend): 21 | def _add_node_to_graph(self): 22 | return self.graph.add_eigen_factor_plane_alternating() 23 | 24 | def _optimize(self): 25 | return self.graph.solve(mrob.LM_ELLIPS, self.iterations_count) 26 | -------------------------------------------------------------------------------- /plane_backends/backend_impls/EFDenseBackend.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import mrob 16 | 17 | from plane_backends.EFBackend import EFBackend 18 | 19 | 20 | class EFDenseBackend(EFBackend): 21 | def _add_node_to_graph(self): 22 | return self.graph.add_eigen_factor_plane_dense() 23 | 24 | def _optimize(self): 25 | print('Here') 26 | return self.graph.solve(mrob.LM_ELLIPS, 1000, lambdaParam=0.1) 27 | -------------------------------------------------------------------------------- /plane_backends/backend_impls/LandmarkBackend.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import numpy as np 16 | import mrob 17 | 18 | from plane_backends.BaseBackend import BaseBackend 19 | 20 | 21 | class LandmarkBackend(BaseBackend): 22 | def _add_node_to_graph(self): 23 | return self.graph.add_node_plane_4d(np.array([1, 0, 0, 0])) 24 | 25 | def _register_observation(self, observation, pose_id, plane_id, graph_plane_id): 26 | w_z = np.identity(4) 27 | plane_equation = self.__get_plane_equation(observation[plane_id]) 28 | self.graph.add_factor_1pose_1plane_4d( 29 | plane_equation, pose_id, graph_plane_id, w_z 30 | ) 31 | 32 | @staticmethod 33 | def __get_plane_equation(points): 34 | c = np.mean(points, axis=0) 35 | A = np.array(points) - c 36 | eigvals, eigvects = np.linalg.eig(A.T @ A) 37 | min_index = np.argmin(eigvals) 38 | n = eigvects[:, min_index] 39 | 40 | d = -np.dot(n, c) 41 | normal = int(np.sign(d)) * n 42 | d *= np.sign(d) 43 | return np.asarray([normal[0], normal[1], normal[2], d]) 44 | 45 | def _optimize(self): 46 | return self.graph.solve(mrob.LM_ELLIPS, self.iterations_count) 47 | -------------------------------------------------------------------------------- /plane_backends/backend_impls/PiFBackend.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import mrob 16 | import numpy as np 17 | 18 | from plane_backends.BaseBackend import BaseBackend 19 | 20 | 21 | class PiFBackend(BaseBackend): 22 | def _add_node_to_graph(self): 23 | return self.graph.add_node_plane_4d(np.array([1, 1, 1, 1])) 24 | 25 | def _register_observation(self, observation, pose_id, plane_id, graph_plane_id): 26 | S = mrob.registration.estimate_matrix_S(observation[plane_id]) 27 | self.graph.add_pi_factor_plane_4d( 28 | S, pose_id, graph_plane_id 29 | ) 30 | 31 | def _optimize(self): 32 | return self.graph.solve(mrob.LM_ELLIPS, self.iterations_count) -------------------------------------------------------------------------------- /plane_backends/solver_factory.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from plane_backends.backend_impls.BaregBackend import BaregBackend 16 | from plane_backends.backend_impls.EFDenseBackend import EFDenseBackend 17 | from plane_backends.backend_impls.EFAlternatingBackend import EFAlternatingBackend 18 | from plane_backends.backend_impls.LandmarkBackend import LandmarkBackend 19 | from plane_backends.backend_impls.PiFBackend import PiFBackend 20 | 21 | solvers = { 22 | 'ef-dense': EFDenseBackend, 23 | 'ef-alternating': EFAlternatingBackend, 24 | 'bareg': BaregBackend, 25 | 'pi-factor': PiFBackend, 26 | 'landmark': LandmarkBackend, 27 | } 28 | 29 | 30 | def create_solver_by_name(solver_name, iterations_count): 31 | return solvers[solver_name](iterations_count) 32 | -------------------------------------------------------------------------------- /plane_extractor/plane_extractor.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import numpy as np 16 | 17 | 18 | def extract_planes_from_pcd_colors(point_cloud): 19 | points = np.asarray(point_cloud.points) 20 | colors = np.asarray(point_cloud.colors) 21 | 22 | colors_unique = np.unique(colors, axis=0) 23 | unique_colors_without_black = list( 24 | filter(lambda x: (x != [0, 0, 0]).all(axis=0), colors_unique) 25 | ) 26 | 27 | color_to_points = {} 28 | 29 | for color in unique_colors_without_black: 30 | indices = np.where((colors == color).all(axis=1))[0] 31 | if len(indices) > 1000: 32 | color_to_points[tuple(color)] = points[indices] 33 | 34 | return color_to_points 35 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | mrob==0.0.11 2 | numpy 3 | open3d 4 | pandas 5 | matplotlib 6 | tqdm 7 | -------------------------------------------------------------------------------- /scripts/enough_planes/EnoughPlanesDetector.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from typing import List 16 | 17 | import numpy as np 18 | 19 | from scripts.enough_planes.Pcd import Pcd 20 | from scripts.enough_planes.Plane import Plane 21 | 22 | 23 | class EnoughPlanesDetector: 24 | @staticmethod 25 | def has_enough_planes(pcd: Pcd) -> bool: 26 | return abs(EnoughPlanesDetector.__check_planes(pcd.planes)) > 0.1 27 | 28 | @staticmethod 29 | def __check_planes(planes: List[Plane]): 30 | matrix = [] 31 | for plane in planes: 32 | matrix.append(plane.equation[:-1]) 33 | matrix = np.asarray(matrix) 34 | covarience = matrix.T @ matrix 35 | eigvals, eigvects = np.linalg.eig(covarience) 36 | det = np.linalg.det(eigvects * eigvals) 37 | print("Det was {}".format(det)) 38 | return det 39 | -------------------------------------------------------------------------------- /scripts/enough_planes/Pcd.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import numpy as np 16 | 17 | 18 | class Pcd: 19 | """ 20 | A class to represent a point cloud 21 | :attribute planes: planes of an image 22 | :attribute points: all points of an image 23 | """ 24 | 25 | def __init__(self, points: np.array): 26 | self.planes = [] 27 | self.points = points 28 | -------------------------------------------------------------------------------- /scripts/enough_planes/Plane.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, Gonzalo Ferrer, Dmitrii Iarosh, Anastasiia Kornilova 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import numpy as np 16 | 17 | 18 | class Plane: 19 | """ 20 | A class to represent a plane 21 | :attribute equation: equation of a plane 22 | :attribute track: track of a plane 23 | :attribute color: color to use on planes of this track 24 | :attribute plane_indices: indices of points that belong to a plane 25 | """ 26 | 27 | def __init__(self, equation, track: int, color, indices): 28 | self.equation = equation 29 | self.track = track 30 | self.color = color 31 | self.plane_indices = indices 32 | 33 | @staticmethod 34 | def get_equation(points): 35 | """ 36 | :param points: all points of a plane 37 | :return: equation of a plane 38 | """ 39 | c = np.mean(points, axis=0) 40 | A = np.array(points) - c 41 | eigvals, eigvects = np.linalg.eig(A.T @ A) 42 | min_index = np.argmin(eigvals) 43 | n = eigvects[:, min_index] 44 | 45 | d = -np.dot(n, c) 46 | normal = int(np.sign(d)) * n 47 | d *= np.sign(d) 48 | return np.asarray([normal[0], normal[1], normal[2], d]) 49 | --------------------------------------------------------------------------------