├── .flake8 ├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── assets ├── ground_truth.slam ├── ground_truth_raw.csv └── individual_clouds │ ├── cloud_1583840261_039995392.pcd │ ├── cloud_1583840267_239448320.pcd │ ├── cloud_1583840272_839619840.pcd │ ├── cloud_1583840278_538413056.pcd │ ├── cloud_1583840283_939608320.pcd │ ├── cloud_1583840289_537910016.pcd │ ├── cloud_1583840294_837452544.pcd │ ├── cloud_1583840299_937714432.pcd │ ├── cloud_1583840304_938342400.pcd │ ├── cloud_1583840310_039396096.pcd │ ├── cloud_1583840321_837331200.pcd │ ├── cloud_1583840327_638376448.pcd │ ├── cloud_1583840333_039096064.pcd │ ├── cloud_1583840338_937465600.pcd │ ├── cloud_1583840344_338319104.pcd │ ├── cloud_1583840349_438621952.pcd │ ├── cloud_1583840354_338373632.pcd │ ├── cloud_1583840359_338098176.pcd │ ├── cloud_1583840364_237831424.pcd │ ├── cloud_1583840369_137761792.pcd │ ├── cloud_1583840373_938645760.pcd │ ├── cloud_1583840378_937544448.pcd │ ├── cloud_1583840384_136391424.pcd │ ├── cloud_1583840389_435392512.pcd │ ├── cloud_1583840394_636034816.pcd │ ├── cloud_1583840400_036679936.pcd │ ├── cloud_1583840405_236971008.pcd │ ├── cloud_1583840410_236916224.pcd │ ├── cloud_1583840415_136673536.pcd │ ├── cloud_1583840419_836455680.pcd │ ├── cloud_1583840424_635875328.pcd │ ├── cloud_1583840429_335477760.pcd │ ├── cloud_1583840434_135571968.pcd │ ├── cloud_1583840439_035764480.pcd │ ├── cloud_1583840443_935761920.pcd │ ├── cloud_1583840449_236556288.pcd │ ├── cloud_1583840454_036222464.pcd │ ├── cloud_1583840458_735444992.pcd │ ├── cloud_1583840463_434758144.pcd │ ├── cloud_1583840468_334895360.pcd │ ├── cloud_1583840473_235074816.pcd │ ├── cloud_1583840478_034745856.pcd │ ├── cloud_1583840482_734428928.pcd │ ├── cloud_1583840487_534158336.pcd │ ├── cloud_1583840492_233600256.pcd │ ├── cloud_1583840497_133344256.pcd │ ├── cloud_1583840501_933006592.pcd │ ├── cloud_1583840506_732880384.pcd │ ├── cloud_1583840511_332471296.pcd │ └── cloud_1583840516_132663040.pcd ├── notebooks ├── preprocess_groundtruth.ipynb └── slam_tutorial.ipynb ├── requirements.txt ├── setup.py └── slam_tutorial ├── __init__.py ├── io.py ├── pose_graph.py └── visualization.py /.flake8: -------------------------------------------------------------------------------- 1 | # setup.cfg 2 | 3 | [flake8] 4 | # Recommend matching the black line length (default 88), 5 | # rather than using the flake8 default of 79: 6 | max-line-length = 88 7 | 8 | max-complexity = 18 9 | exclude=_*,.vscode,.git 10 | 11 | ignore=BLK100 #E402,E501,W503 12 | #extend-ignore = E203 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | 162 | # Other 163 | *.dot 164 | *.pdf -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # .pre-commit-config.yaml 2 | # From https://sbarnea.com/lint/black/ 3 | --- 4 | repos: 5 | - repo: https://github.com/python/black.git 6 | rev: 22.12.0 7 | hooks: 8 | - id: black 9 | language_version: python3 10 | - repo: https://github.com/pycqa/flake8 11 | rev: 3.7.9 12 | hooks: 13 | - id: flake8 14 | additional_dependencies: 15 | - flake8-black>=0.1.1 16 | language_version: python3 17 | - repo: local 18 | hooks: 19 | - id: jupyter-nb-clear-output 20 | name: jupyter-nb-clear-output 21 | files: \.ipynb$ 22 | stages: [commit] 23 | language: system 24 | entry: jupyter nbconvert --ClearOutputPreprocessor.enabled=True --inplace -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Oxford Dynamic Robot Systems Group 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # slam-tutorial 2 | 3 | ORIentate tutorial: SLAM and factor graphs 4 | Slides: [link](https://docs.google.com/presentation/d/1JN-fBMJgrXs6wE3bcKmvP25KfUHAOUQuBsK460e5oYQ/edit#slide=id.g8caa8b3668_0_0) 5 | 6 | ## Installation 7 | 8 | Use a virtual environment (`env` in the example), isolated from the system dependencies: 9 | 10 | ```sh 11 | python3 -m venv env && source env/bin/activate 12 | ``` 13 | 14 | Install the dependencies: 15 | 16 | ```sh 17 | pip install -r requirements.txt 18 | ``` 19 | 20 | Install the automatic formatting pre-commit hooks (black and flake8), which will check the code before each commit: 21 | 22 | ```sh 23 | pre-commit install 24 | ``` 25 | -------------------------------------------------------------------------------- /assets/ground_truth.slam: -------------------------------------------------------------------------------- 1 | VERTEX_SE3:QUAT_TIME 0 -0.415299 4.07653 0.625911 -0.050034780689368 -0.015304194093435483 -0.6793587378054545 0.7319382824872053 1583840261 39995392 2 | VERTEX_SE3:QUAT_TIME 1 3.1814 0.472834 0.593847 -0.020618496319379478 -0.026015595355939992 -0.35019293748684227 0.9360891671018259 1583840267 239448320 3 | VERTEX_SE3:QUAT_TIME 2 8.07914 -0.866458 0.556637 -0.03131001355813704 -0.044696419354836044 -0.37398816194763834 0.9258265990902617 1583840272 839619840 4 | VERTEX_SE3:QUAT_TIME 3 12.5035 -3.37622 0.523242 -0.0313403113833312 -0.046432016864893906 -0.5105881854542654 0.8579986883602504 1583840278 538413056 5 | VERTEX_SE3:QUAT_TIME 4 15.9937 -7.00808 0.397473 -0.054536898758380005 -0.03720029915307551 -0.6318159856156955 0.7723020175826941 1583840283 939608320 6 | VERTEX_SE3:QUAT_TIME 5 18.5475 -11.3208 0.475479 -0.04731928687019862 -0.01858369484353551 -0.7530847910396713 0.655956182009759 1583840289 537910016 7 | VERTEX_SE3:QUAT_TIME 6 20.0497 -16.1507 0.386255 -0.04514891384868697 -0.033934610408883785 -0.779138238987844 0.6243028085053198 1583840294 837452544 8 | VERTEX_SE3:QUAT_TIME 7 21.1097 -21.0758 0.423336 -0.05858748508072459 -0.01870949523563587 -0.7921287982847756 0.6072471546351618 1583840299 937714432 9 | VERTEX_SE3:QUAT_TIME 8 21.5313 -26.0723 0.44355 -0.03766187933848148 0.014378292111988196 -0.8413065384545083 0.5390522957269545 1583840304 938342400 10 | VERTEX_SE3:QUAT_TIME 9 20.7895 -31.029 0.284436 0.0607426 0.0005757370000000002 0.914205 -0.400674 1583840310 39396096 11 | VERTEX_SE3:QUAT_TIME 10 18.6105 -35.6291 0.309098 0.04052999999999999 -0.025348499999999996 0.976342 -0.21087999999999998 1583840321 837331200 12 | VERTEX_SE3:QUAT_TIME 11 14.9668 -39.0651 0.284729 0.0166839 -0.0321872 0.998989 -0.0265643 1583840327 638376448 13 | VERTEX_SE3:QUAT_TIME 12 10.3468 -41.1182 0.184932 0.045102699999999996 -0.0227551 0.989402 0.136131 1583840333 39096064 14 | VERTEX_SE3:QUAT_TIME 13 5.31239 -40.7375 0.298862 0.0222363 -0.015434 0.918471 0.39456099999999994 1583840338 937465600 15 | VERTEX_SE3:QUAT_TIME 14 0.727175 -38.5154 0.302019 0.006616494908416215 -0.03837412846767011 0.8343976189940231 0.5497865921430513 1583840344 338319104 16 | VERTEX_SE3:QUAT_TIME 15 -2.96084 -35.0021 0.311487 0.020944125917192823 -0.0452596560063111 0.7263378988028172 0.6855271516971665 1583840349 438621952 17 | VERTEX_SE3:QUAT_TIME 16 -5.58528 -30.745 0.364995 0.01918611637861182 -0.045110138509171574 0.6705865724596339 0.7402103681040816 1583840354 338373632 18 | VERTEX_SE3:QUAT_TIME 17 -7.6629 -26.1095 0.400762 0.03123190485718131 -0.047420107374768206 0.6109570950159586 0.7896248771976349 1583840359 338098176 19 | VERTEX_SE3:QUAT_TIME 18 -9.01349 -21.2191 0.432432 0.03214609705251922 -0.06474549406347528 0.5311459512991428 0.844191077404008 1583840364 237831424 20 | VERTEX_SE3:QUAT_TIME 19 -9.67431 -16.1669 0.456118 0.03436710101023031 -0.05436810159816518 0.48370601421867027 0.8728639743419246 1583840369 137761792 21 | VERTEX_SE3:QUAT_TIME 20 -8.86096 -11.1598 0.458233 0.029862209095811866 -0.06148211872700654 0.31255709520261965 0.9474367114175754 1583840373 938645760 22 | VERTEX_SE3:QUAT_TIME 21 -6.88593 -6.55384 0.527255 0.025125300283980873 -0.06362840071916549 0.22951100259406793 0.9708989890263301 1583840378 937544448 23 | VERTEX_SE3:QUAT_TIME 22 -3.45972 -2.90283 0.512864 0.011071304601970665 -0.05521082294929069 0.03992911659719698 0.9976145853248438 1583840384 136391424 24 | VERTEX_SE3:QUAT_TIME 23 0.999288 -0.450799 0.538047 0.003248109716156355 -0.04566699600928303 -0.14273798752650801 0.9887010863999431 1583840389 435392512 25 | VERTEX_SE3:QUAT_TIME 24 6.01057 -0.324909 0.567258 -0.0034748090779316405 -0.048383887160948845 -0.3732139009647085 0.9264762458478112 1583840394 636034816 26 | VERTEX_SE3:QUAT_TIME 25 10.6898 -2.18421 0.553274 -0.023727917169571344 -0.02257031633192891 -0.5628954073123141 0.8258794023914751 1583840400 36679936 27 | VERTEX_SE3:QUAT_TIME 26 14.4503 -5.61141 0.449397 -0.035027863027324545 -0.02787517057714785 -0.6515213123050067 0.7573077993541001 1583840405 236971008 28 | VERTEX_SE3:QUAT_TIME 27 17.6795 -9.46711 0.47905 -0.04248837095596871 -0.014223790276958132 -0.7558524833173718 0.6532064465159542 1583840410 236916224 29 | VERTEX_SE3:QUAT_TIME 28 19.4303 -14.2069 0.379235 -0.05060176454197796 -0.01858172370077827 -0.8174800426865422 0.5734302685952496 1583840415 136673536 30 | VERTEX_SE3:QUAT_TIME 29 20.6501 -19.1115 0.388281 -0.031412008234884096 -0.005946251558852656 -0.8411112205033616 0.5399168584568715 1583840419 836455680 31 | VERTEX_SE3:QUAT_TIME 30 21.2656 -24.1778 0.363369 0.0394579 0.003678190000000001 0.8837209999999999 -0.46633300000000005 1583840424 635875328 32 | VERTEX_SE3:QUAT_TIME 31 20.9861 -29.2168 0.374488 0.0424647 -0.004476409999999998 0.921784 -0.385345 1583840429 335477760 33 | VERTEX_SE3:QUAT_TIME 32 19.3756 -33.9757 0.334051 0.0421569 0.0046675300000000005 0.9753 -0.21677299999999997 1583840434 135571968 34 | VERTEX_SE3:QUAT_TIME 33 16.1763 -37.9258 0.274857 0.046582599999999995 -0.0014299999999999998 0.998591 -0.0253778 1583840439 35764480 35 | VERTEX_SE3:QUAT_TIME 34 11.7134 -40.2104 0.2164 0.05205139999999999 0.00123407 0.994448 0.0914443 1583840443 935761920 36 | VERTEX_SE3:QUAT_TIME 35 6.72229 -40.936 0.262735 0.0558307 -0.010620699999999999 0.898043 0.4362210000000001 1583840449 236556288 37 | VERTEX_SE3:QUAT_TIME 36 2.08157 -38.9823 0.275967 0.05934957227172552 -0.020468090437254935 0.8246846147052038 0.5620972626131798 1583840454 36222464 38 | VERTEX_SE3:QUAT_TIME 37 -1.98977 -35.8735 0.348744 0.05082176949761309 -0.024496285297734042 0.7415665549239981 0.6685024012238624 1583840458 735444992 39 | VERTEX_SE3:QUAT_TIME 38 -5.07358 -31.917 0.414948 0.033213988276972035 -0.026345490701239444 0.6922187556782472 0.720441254282922 1583840463 434758144 40 | VERTEX_SE3:QUAT_TIME 39 -7.53765 -27.4732 0.427733 0.03052680007350957 -0.02417880005822337 0.5890510014184548 0.8071569980563386 1583840468 334895360 41 | VERTEX_SE3:QUAT_TIME 40 -8.77089 -22.5171 0.415438 0.03233310212328869 -0.028373901863291216 0.5342110350812072 0.8442559445583905 1583840473 235074816 42 | VERTEX_SE3:QUAT_TIME 41 -9.3745 -17.5195 0.474494 0.021374105452581737 -0.030497607780007423 0.47107112017128816 0.8813087751761184 1583840478 34745856 43 | VERTEX_SE3:QUAT_TIME 42 -9.0135 -12.5079 0.493971 0.01580340125553415 -0.03237570257215518 0.36456202896339035 0.930481926075918 1583840482 734428928 44 | VERTEX_SE3:QUAT_TIME 43 -7.48437 -7.74432 0.500241 0.022952708335534228 -0.03943951432290328 0.18781606820751795 0.9811436436865195 1583840487 534158336 45 | VERTEX_SE3:QUAT_TIME 44 -4.32839 -3.85225 0.537509 0.027576486880967102 -0.03754378213920014 0.07811446283840606 0.9958554737606558 1583840492 233600256 46 | VERTEX_SE3:QUAT_TIME 45 -0.324397 -0.841453 0.583457 0.02893569572765469 -0.05289859218954144 -0.143820978764883 0.9877651458431553 1583840497 133344256 47 | VERTEX_SE3:QUAT_TIME 46 4.67751 -0.248991 0.617343 0.014863003379878592 -0.040130809125831376 -0.35013607962168947 0.9357207872150538 1583840501 933006592 48 | VERTEX_SE3:QUAT_TIME 47 9.47624 -1.71057 0.607516 0.002258890010232819 -0.04844600021946135 -0.5314280024073794 0.8457139961688986 1583840506 732880384 49 | VERTEX_SE3:QUAT_TIME 48 13.4929 -4.69373 0.508408 -0.009897679894155614 -0.04271139954325033 -0.6398369931576737 0.7672590082049592 1583840511 332471296 50 | VERTEX_SE3:QUAT_TIME 49 16.8555 -8.47085 0.483653 -0.012845715364684013 -0.04319625166678061 -0.6796538129298975 0.7321481242809204 1583840516 132663040 51 | EDGE_SE3:QUAT 0 1 3.850503804506989 3.324863117478348 -0.2068392323561744 0.04406006063370249 -0.0012012465141188462 0.3786346914703428 0.9244959538588301 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 52 | EDGE_SE3:QUAT 1 2 4.564125266118404 2.2074220035550507 -0.28105056060614886 -0.004296962630203673 -0.021007369792626143 -0.025975299450811465 0.9994326279103771 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 53 | EDGE_SE3:QUAT 2 3 4.8964687930299 1.2742807906955118 -0.5251616248644584 -0.007608181306102858 -0.00037291125647964305 -0.15188775002581734 0.988368524806859 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 54 | EDGE_SE3:QUAT 3 4 4.812863044753934 1.337886913432646 -0.6588954058419043 -0.032930864543407094 -0.0041028614411605345 -0.1464027202502826 0.9886683474429837 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 55 | EDGE_SE3:QUAT 4 5 4.708987508436367 1.6558619511392312 -0.4594452275501221 -0.01704448147863338 0.021223366737844862 -0.16641864301262024 0.9856793283920413 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 56 | EDGE_SE3:QUAT 5 6 4.552077707755545 2.1599884934054074 -0.45334766334676024 0.011002246378411585 -0.007790447428920132 -0.04169398376924055 0.9990394792558991 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 57 | EDGE_SE3:QUAT 6 7 4.551004775015082 2.108927201107864 -0.4715733397259453 -0.02146310264131449 -0.0009577008082493992 -0.020255661666700776 0.9995639817315812 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 58 | EDGE_SE3:QUAT 7 8 4.690417765828974 1.7137716288557066 -0.4540082389226765 -0.018418259294898078 0.03827356538860971 -0.08233533051940217 0.995698974650006 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 59 | EDGE_SE3:QUAT 8 9 4.80215503031126 1.4123294095488141 -0.2984449034594554 -0.004024207452941341 -0.022743837998291584 -0.1566095737096629 0.9873905530566639 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 60 | EDGE_SE3:QUAT 9 10 4.835735625964917 1.5254496552543984 -0.4452763462304443 -0.027165736987201955 0.03253071546503658 -0.1968438747782572 0.9795182220907533 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 61 | EDGE_SE3:QUAT 10 11 4.727925627281832 1.6346787955143398 -0.24024405733199794 -0.0085444841920122 0.030314098235866133 -0.18384913936456618 0.9824489152227444 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 62 | EDGE_SE3:QUAT 11 12 4.718916082866377 1.812206744280111 -0.13122491441691686 0.005644673656786249 -0.023563870866765056 -0.16334809845622128 0.9862704106420264 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 63 | EDGE_SE3:QUAT 12 13 4.93998001985543 0.9965591203803328 -0.3265966867040566 -0.009139247684364828 0.026302111498277943 -0.2651569438980142 0.9638031458009153 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 64 | EDGE_SE3:QUAT 13 14 4.762231561839581 1.797235690978905 -0.23028529891685778 -0.03198203180551783 0.005821335037415774 -0.17499157385151667 0.9840333087630662 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 65 | EDGE_SE3:QUAT 14 15 4.680281037910106 2.00596446133758 -0.12621248752099667 -0.002912944126135758 -0.01124652643837674 -0.17317585287074705 0.9848228836802253 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 66 | EDGE_SE3:QUAT 15 16 4.391744693259557 2.3781067426401608 -0.2658894106577261 -0.0047651306996589985 0.002686674366618008 -0.07786087651135122 0.9969497504801205 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 67 | EDGE_SE3:QUAT 16 17 4.396565986950691 2.528058348614365 -0.29123249507208016 0.0037294288206829503 -0.008702584102959973 -0.07777382878065787 0.9969263196568839 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 68 | EDGE_SE3:QUAT 17 18 4.371365332002785 2.5368448326919357 -0.44350805364836354 -0.01535205365595808 -0.014144214695134664 -0.09586064031031244 0.9951758784620511 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 69 | EDGE_SE3:QUAT 18 19 4.230642319858937 2.786160367182277 -0.5485809584766099 0.0033936160950206323 0.00791226022249567 -0.05575528571791964 0.9984073320299757 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 70 | EDGE_SE3:QUAT 19 20 4.6375909235461865 1.9624028346298827 -0.611816748211027 -0.01924114236002947 -0.005858034955121119 -0.1849716201573854 0.9825380555101954 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 71 | EDGE_SE3:QUAT 20 21 4.294534431431424 2.52209246296183 -0.5624981948163885 -0.01096524056466008 -0.0015903437770632277 -0.08565887667887068 0.9962629415770301 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 72 | EDGE_SE3:QUAT 21 22 4.651020282355833 1.7235891907813128 -0.6828227004612648 -0.0244471130747375 0.008334733826109378 -0.18951364602246823 0.9815383100590324 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 73 | EDGE_SE3:QUAT 22 23 4.612752036848299 2.083366170733505 -0.5272067736444139 -0.017409973877334266 0.007318949621320255 -0.18154919680765438 0.9832004958123528 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 74 | EDGE_SE3:QUAT 23 24 4.753219981514242 1.5342757005514363 -0.4272544374444521 -0.016582183573637162 -0.0072360428190919 -0.23643780383107837 0.9714781332720944 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 75 | EDGE_SE3:QUAT 24 25 4.637674224515668 1.895771900493743 -0.500409142909935 -0.037925080313481366 0.01214877217822337 -0.21220973503164683 0.9764125077245617 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 76 | EDGE_SE3:QUAT 25 26 4.549814645369686 2.2503312051597106 -0.3648001944388982 -0.00997372203795146 -0.010186659877290732 -0.1116644240597489 0.9936436021560213 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 77 | EDGE_SE3:QUAT 26 27 4.282611520174028 2.6195021697270366 -0.3040216094668773 -0.021098772362946605 0.006230278905114742 -0.14614879533655056 0.9890175354977926 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 78 | EDGE_SE3:QUAT 27 28 4.415915842821397 2.4274849859432024 -0.3845031522216509 -0.006271833399872141 -0.007495396789080832 -0.10062310385308894 0.9948767285536831 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 79 | EDGE_SE3:QUAT 28 29 4.17859651243732 2.8213792939717095 -0.3497179106695021 -0.001460224691633141 0.02350581231366991 -0.0406652109758447 0.9988955688756397 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 80 | EDGE_SE3:QUAT 29 30 4.3427136894952785 2.672045271879675 -0.2188143172771968 -0.00881659106248971 -0.004642013135397552 -0.08477889865144518 0.9963499145409269 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 81 | EDGE_SE3:QUAT 30 31 4.3096437879076 2.6164473000051807 -0.22564822220393677 -0.011944180969286396 0.002349584290676993 -0.08898801723885076 0.9959581130102746 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 82 | EDGE_SE3:QUAT 31 32 4.505954157623617 2.20355647544131 -0.2883981070229138 0.001628551446364798 -0.00021270904711840767 -0.17639590959069076 0.9843176471231194 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 83 | EDGE_SE3:QUAT 32 33 4.550960456833127 2.2251903955417873 -0.42374881209122495 -0.015083633501608731 -0.0029060718062945087 -0.1914388987448472 0.981384127671445 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 84 | EDGE_SE3:QUAT 33 34 4.5484460566399925 2.056349682882761 -0.4726049051699004 -0.0029262711492962965 -0.005554638813934468 -0.1166842710297934 0.9931490925096847 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 85 | EDGE_SE3:QUAT 34 35 4.753316281549463 1.6211251826153301 -0.4666199793428656 -0.029270489855840093 -0.010285863215864775 -0.3510563683601588 0.9358399741231532 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 86 | EDGE_SE3:QUAT 35 36 4.375471873192602 2.432130969221646 -0.5416401295303697 -0.015115230728930363 -0.010214503196826255 -0.14452998915726492 0.9893320733774916 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 87 | EDGE_SE3:QUAT 36 37 4.353384188134335 2.6451300288016455 -0.5450279243209026 -0.01613181409915942 0.002013398775725371 -0.13405744862680474 0.9908399484143171 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 88 | EDGE_SE3:QUAT 37 38 4.231688630384387 2.65183360239934 -0.47824620850336386 -0.016990627928316116 0.010585522117153615 -0.07098007758374651 0.9972766007801549 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 89 | EDGE_SE3:QUAT 38 39 4.3263218614046615 2.6374853664740696 -0.38177847985572777 -0.00603432600099256 0.0022790572861539605 -0.1343538954385089 0.9909123297419958 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 90 | EDGE_SE3:QUAT 39 40 4.32860683987521 2.681786226638936 -0.39388430886263714 -0.0034716371905454788 -0.005227189484456944 -0.06603326302419822 0.9977977052005913 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 91 | EDGE_SE3:QUAT 40 41 4.245523307818178 2.681717313624258 -0.3574426043154717 -0.013376265114619248 0.003071291617076015 -0.07272070138763256 0.9972579697795417 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 92 | EDGE_SE3:QUAT 41 42 4.356239285947847 2.482752375062578 -0.32552073356472677 -0.010093529549769336 0.00019214159607945706 -0.11682138338314575 0.9931016849035308 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 93 | EDGE_SE3:QUAT 42 43 4.346828198150895 2.4560427540967895 -0.3208001425734066 -0.0024457990423834694 -0.010332091836372811 -0.18304809754509938 0.9830466448641368 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 94 | EDGE_SE3:QUAT 43 44 4.354086307555598 2.4456637747175796 -0.41288168355974664 0.00022838621385182724 -0.0009461569300775169 -0.11062201913031268 0.9938620830167696 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 95 | EDGE_SE3:QUAT 44 45 4.4096652859828875 2.340491104247599 -0.41943105617210785 -0.007955049961760162 -0.021821286911359025 -0.2200112498366549 0.9752207868766938 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 96 | EDGE_SE3:QUAT 45 46 4.600103544578339 1.9752559024103342 -0.5555573064092717 -0.025144629413563026 0.0018646802645156113 -0.2109009447408953 0.9771822279451465 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 97 | EDGE_SE3:QUAT 46 47 4.5654916837454556 2.0355699595087478 -0.4205531365833416 -0.014820101427466929 -0.018500442220952124 -0.20052387044351297 0.9794020263213796 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 98 | EDGE_SE3:QUAT 47 48 4.403319088879123 2.305956036111236 -0.579630469904045 -0.018403269574806157 -0.005656224363762349 -0.13280020013484498 0.9909558164949794 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 99 | EDGE_SE3:QUAT 48 49 4.300495745822101 2.619666695853871 -0.4662716788522727 -0.003999817917563129 -0.0033637744054064167 -0.05289340226814981 0.9985867843618225 1000.0 0.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 1000.0 0.0 0.0 0.0 1000.0 0.0 0.0 1000.0 0.0 1000.0 100 | -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840261_039995392.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840261_039995392.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840267_239448320.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840267_239448320.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840272_839619840.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840272_839619840.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840278_538413056.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840278_538413056.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840283_939608320.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840283_939608320.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840289_537910016.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840289_537910016.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840294_837452544.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840294_837452544.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840299_937714432.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840299_937714432.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840304_938342400.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840304_938342400.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840310_039396096.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840310_039396096.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840321_837331200.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840321_837331200.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840327_638376448.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840327_638376448.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840333_039096064.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840333_039096064.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840338_937465600.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840338_937465600.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840344_338319104.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840344_338319104.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840349_438621952.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840349_438621952.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840354_338373632.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840354_338373632.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840359_338098176.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840359_338098176.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840364_237831424.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840364_237831424.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840369_137761792.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840369_137761792.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840373_938645760.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840373_938645760.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840378_937544448.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840378_937544448.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840384_136391424.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840384_136391424.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840389_435392512.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840389_435392512.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840394_636034816.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840394_636034816.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840400_036679936.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840400_036679936.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840405_236971008.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840405_236971008.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840410_236916224.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840410_236916224.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840415_136673536.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840415_136673536.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840419_836455680.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840419_836455680.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840424_635875328.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840424_635875328.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840429_335477760.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840429_335477760.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840434_135571968.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840434_135571968.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840439_035764480.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840439_035764480.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840443_935761920.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840443_935761920.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840449_236556288.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840449_236556288.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840454_036222464.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840454_036222464.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840458_735444992.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840458_735444992.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840463_434758144.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840463_434758144.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840468_334895360.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840468_334895360.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840473_235074816.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840473_235074816.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840478_034745856.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840478_034745856.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840482_734428928.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840482_734428928.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840487_534158336.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840487_534158336.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840492_233600256.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840492_233600256.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840497_133344256.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840497_133344256.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840501_933006592.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840501_933006592.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840506_732880384.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840506_732880384.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840511_332471296.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840511_332471296.pcd -------------------------------------------------------------------------------- /assets/individual_clouds/cloud_1583840516_132663040.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/slam_tutorial/c5d7365f4ab2025193ae9826c77af065cd9cba9d/assets/individual_clouds/cloud_1583840516_132663040.pcd -------------------------------------------------------------------------------- /notebooks/preprocess_groundtruth.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# SLAM tutorial\n", 8 | "\n", 9 | "SLAM and factor graphs tutorial prepared for the ORIentate Seminars\n", 10 | "\n", 11 | "Author: Matias Mattamala (matias@robots.ox.ac.uk, [mmattamala@github](https://github.com/mmattamala))\n", 12 | "\n", 13 | "Date: 23/11/2023\n", 14 | "\n", 15 | "\n", 16 | "> ⚠️ **Warning**: Before proceeding, make sure you installed all the required dependencies in the [`requirements.txt`](../requirements.txt) file!\n" 17 | ] 18 | }, 19 | { 20 | "cell_type": "markdown", 21 | "metadata": {}, 22 | "source": [ 23 | "## Preliminaries" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": { 30 | "tags": [] 31 | }, 32 | "outputs": [], 33 | "source": [ 34 | "import slam_tutorial\n", 35 | "import slam_tutorial.io as io\n", 36 | "\n", 37 | "graph = io.load_ground_truth_file_as_pose_graph(\n", 38 | " slam_tutorial.ASSETS_DIR + \"/ground_truth_raw.csv\", distance_thr=5.0, num_nodes=50\n", 39 | ")\n", 40 | "io.write_pose_graph(graph, slam_tutorial.ASSETS_DIR + \"/ground_truth.slam\")" 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "metadata": {}, 47 | "outputs": [], 48 | "source": [ 49 | "graph2 = io.load_pose_graph(slam_tutorial.ASSETS_DIR + \"/ground_truth.slam\")\n", 50 | "io.write_pose_graph(graph, slam_tutorial.ASSETS_DIR + \"/ground_truth2.slam\")" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "metadata": {}, 57 | "outputs": [], 58 | "source": [ 59 | "INPUT_CLOUDS = (\n", 60 | " \"/home/matias/rosbags/2020-03-10-newer-college-02-long-experiment/raw_format/ouster\"\n", 61 | ")\n", 62 | "OUTPUT_FOLDER = \"/home/matias/git/slam-tutorial/assets/individual_clouds\"\n", 63 | "\n", 64 | "import shutil\n", 65 | "\n", 66 | "for i, node in enumerate(graph.nodes):\n", 67 | " sec, nsec = node[\"stamp\"]\n", 68 | " shutil.copy2(f\"{INPUT_CLOUDS}/cloud_{sec}_{int(nsec):09}.pcd\", OUTPUT_FOLDER)" 69 | ] 70 | } 71 | ], 72 | "metadata": { 73 | "kernelspec": { 74 | "display_name": "Python 3 (ipykernel)", 75 | "language": "python", 76 | "name": "python3" 77 | }, 78 | "language_info": { 79 | "codemirror_mode": { 80 | "name": "ipython", 81 | "version": 3 82 | }, 83 | "file_extension": ".py", 84 | "mimetype": "text/x-python", 85 | "name": "python", 86 | "nbconvert_exporter": "python", 87 | "pygments_lexer": "ipython3", 88 | "version": "3.8.10" 89 | } 90 | }, 91 | "nbformat": 4, 92 | "nbformat_minor": 4 93 | } 94 | -------------------------------------------------------------------------------- /notebooks/slam_tutorial.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# SLAM tutorial\n", 8 | "\n", 9 | "SLAM and factor graphs tutorial prepared for the ORIentate Seminars\n", 10 | "\n", 11 | "Author: Matias Mattamala (matias@robots.ox.ac.uk, [mmattamala@github](https://github.com/mmattamala))\n", 12 | "\n", 13 | "Date: 23/11/2023\n", 14 | "\n", 15 | "\n", 16 | "> ⚠️ **Warning**: Before proceeding, make sure you installed all the required dependencies in the [`requirements.txt`](../requirements.txt) file!\n" 17 | ] 18 | }, 19 | { 20 | "cell_type": "markdown", 21 | "metadata": {}, 22 | "source": [ 23 | "## Preliminaries" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": { 30 | "tags": [] 31 | }, 32 | "outputs": [], 33 | "source": [ 34 | "%pip install -e ../" 35 | ] 36 | }, 37 | { 38 | "cell_type": "code", 39 | "execution_count": null, 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "import numpy as np\n", 44 | "import open3d as o3d\n", 45 | "\n", 46 | "import slam_tutorial\n", 47 | "import slam_tutorial.visualization as vis\n", 48 | "import slam_tutorial.pose_graph as pg\n", 49 | "from slam_tutorial.io import load_pose_graph" 50 | ] 51 | }, 52 | { 53 | "cell_type": "markdown", 54 | "metadata": { 55 | "tags": [] 56 | }, 57 | "source": [ 58 | "## Part 1: Inspecting a perfect SLAM graph\n", 59 | "In this first part we will check how a pose graph looks like. For that, we will use some data avaialble in the [assets](../assets/) folder." 60 | ] 61 | }, 62 | { 63 | "cell_type": "code", 64 | "execution_count": null, 65 | "metadata": { 66 | "tags": [] 67 | }, 68 | "outputs": [], 69 | "source": [ 70 | "graph = load_pose_graph(\n", 71 | " slam_tutorial.ASSETS_DIR + \"/ground_truth.slam\",\n", 72 | " clouds_path=slam_tutorial.ASSETS_DIR + \"/individual_clouds\",\n", 73 | ")" 74 | ] 75 | }, 76 | { 77 | "cell_type": "markdown", 78 | "metadata": {}, 79 | "source": [ 80 | "The `graph` object stores the **nodes** and **edges** of the pose graph.\n", 81 | "\n", 82 | "Each **node** is defined by:\n", 83 | "- A _pose_ $\\mathbf{T}_{\\mathtt{WB}} \\in $ SE(3) ($4\\times4$ rigid body matrices), representing the pose of the _base_ in the _world_ frame.\n", 84 | "- An _id_\n", 85 | "- A _time stamp_ indicating when the node was created.\n", 86 | "\n", 87 | "On the other side, each **edge** is defined by:\n", 88 | "- A _parent id_, which indicates what is the origin node for the edge\n", 89 | "- A _child id_ representing the node where the edge lands\n", 90 | "- A _relative pose_ $\\textbf{T}_{\\text{parent, child}}$ indicating the rigid body transformation between from the child frame to the parent frame\n", 91 | "- A _type_ indicating if the edge was produced by an odometry system (`odometry`) or a loop candidate (`loop`)" 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": null, 97 | "metadata": {}, 98 | "outputs": [], 99 | "source": [ 100 | "import time\n", 101 | "\n", 102 | "visualizer = o3d.visualization.Visualizer()\n", 103 | "visualizer.create_window(\"Building a Pose Graph\")\n", 104 | "\n", 105 | "# Manually run optimizer\n", 106 | "for i in range(graph.size):\n", 107 | " visualizer.clear_geometries()\n", 108 | " geometry = vis.graph_to_geometries(\n", 109 | " graph,\n", 110 | " show_frames=True,\n", 111 | " show_edges=True,\n", 112 | " show_nodes=True,\n", 113 | " show_clouds=True,\n", 114 | " odometry_color=vis.GRAY,\n", 115 | " loop_color=vis.RED,\n", 116 | " up_to_node=i,\n", 117 | " )\n", 118 | " for g in geometry:\n", 119 | " visualizer.add_geometry(g)\n", 120 | "\n", 121 | " ctr = visualizer.get_view_control()\n", 122 | " ctr.set_front([-0.35, -0.57, 0.7])\n", 123 | " ctr.set_up([0.4, 0.5, 0.6])\n", 124 | " ctr.set_zoom(0.24)\n", 125 | " ctr.set_lookat([1.4, -18, 2.0])\n", 126 | "\n", 127 | " r = visualizer.get_render_option()\n", 128 | " r.point_size = 1\n", 129 | "\n", 130 | " for g in geometry:\n", 131 | " visualizer.update_geometry(g)\n", 132 | "\n", 133 | " visualizer.poll_events()\n", 134 | " visualizer.update_renderer()\n", 135 | " time.sleep(0.1)\n", 136 | "\n", 137 | "visualizer.run()" 138 | ] 139 | }, 140 | { 141 | "cell_type": "markdown", 142 | "metadata": {}, 143 | "source": [ 144 | "#### Static view" 145 | ] 146 | }, 147 | { 148 | "cell_type": "code", 149 | "execution_count": null, 150 | "metadata": {}, 151 | "outputs": [], 152 | "source": [ 153 | "geometries = vis.graph_to_geometries(\n", 154 | " graph,\n", 155 | " show_frames=True,\n", 156 | " show_edges=True,\n", 157 | " show_nodes=True,\n", 158 | " show_clouds=True,\n", 159 | " odometry_color=vis.GRAY,\n", 160 | " loop_color=vis.RED,\n", 161 | " up_to_node=4\n", 162 | ")\n", 163 | "\n", 164 | "o3d.visualization.draw_geometries(\n", 165 | " geometries,\n", 166 | " window_name=\"Perfect Pose Graph\",\n", 167 | " zoom=0.54,\n", 168 | " front=[-0.35, -0.57, 0.7],\n", 169 | " lookat=[1.4, -18, 2.0],\n", 170 | " up=[0.4, 0.5, 0.6],\n", 171 | ")" 172 | ] 173 | }, 174 | { 175 | "cell_type": "markdown", 176 | "metadata": {}, 177 | "source": [ 178 | "### A more realistic pose graph\n", 179 | "\n", 180 | "The previous pose graph was generated with ground truth poses, which is not usually the case.\n", 181 | "\n", 182 | "In general, the graph will be built using an **odometry estimator**, which provides the pose of the robot in some fixed frame.\n", 183 | "\n", 184 | "The odometry estimator will provide a smooth estimate fo the robot's pose, but it **will very likely drift** over time." 185 | ] 186 | }, 187 | { 188 | "cell_type": "code", 189 | "execution_count": null, 190 | "metadata": {}, 191 | "outputs": [], 192 | "source": [ 193 | "graph_with_drift = pg.add_odometry_drift(\n", 194 | " graph, noise_per_m=0.1, axis=\"xyz\", drift_type=\"random_walk\"\n", 195 | ")" 196 | ] 197 | }, 198 | { 199 | "cell_type": "code", 200 | "execution_count": null, 201 | "metadata": {}, 202 | "outputs": [], 203 | "source": [ 204 | "geometries = vis.graph_to_geometries(\n", 205 | " graph_with_drift,\n", 206 | " show_frames=True,\n", 207 | " show_edges=True,\n", 208 | " show_nodes=True,\n", 209 | " show_clouds=True,\n", 210 | " odometry_color=vis.GRAY,\n", 211 | " loop_color=vis.RED,\n", 212 | ")\n", 213 | "\n", 214 | "o3d.visualization.draw_geometries(\n", 215 | " geometries,\n", 216 | " window_name=\"Realistic Pose Graph\",\n", 217 | " zoom=0.54,\n", 218 | " front=[-0.35, -0.57, 0.7],\n", 219 | " lookat=[1.4, -18, 2.0],\n", 220 | " up=[0.4, 0.5, 0.6],\n", 221 | ")" 222 | ] 223 | }, 224 | { 225 | "cell_type": "markdown", 226 | "metadata": {}, 227 | "source": [ 228 | "## Part 2: Solving a pose graph with factor graphs" 229 | ] 230 | }, 231 | { 232 | "cell_type": "code", 233 | "execution_count": null, 234 | "metadata": {}, 235 | "outputs": [], 236 | "source": [ 237 | "initial_graph = pg.create_test_pose_graph(\n", 238 | " use_wrong_init=True,\n", 239 | " use_noisy_odom=True,\n", 240 | " init_noise_per_m=0.1,\n", 241 | " init_drift_axis=\"xy\",\n", 242 | " init_drift_type=\"random_walk\",\n", 243 | " odo_noise_per_m=0.01,\n", 244 | " odo_drift_axis=\"xyz\",\n", 245 | " odo_drift_type=\"random_walk\",\n", 246 | ")\n", 247 | "\n", 248 | "geometries = vis.graph_to_geometries(\n", 249 | " initial_graph,\n", 250 | " show_frames=True,\n", 251 | " show_edges=True,\n", 252 | " show_nodes=True,\n", 253 | " show_clouds=True,\n", 254 | " odometry_color=vis.GRAY,\n", 255 | " loop_color=vis.RED,\n", 256 | ")\n", 257 | "o3d.visualization.draw_geometries(\n", 258 | " geometries,\n", 259 | " window_name=\"Initial Pose Graph\",\n", 260 | " zoom=0.24,\n", 261 | " front=[-0.35, -0.57, 0.7],\n", 262 | " lookat=[1.4, -18, 2.0],\n", 263 | " up=[0.4, 0.5, 0.6],\n", 264 | ")" 265 | ] 266 | }, 267 | { 268 | "cell_type": "markdown", 269 | "metadata": {}, 270 | "source": [ 271 | "### Prepare factor graph" 272 | ] 273 | }, 274 | { 275 | "cell_type": "code", 276 | "execution_count": null, 277 | "metadata": {}, 278 | "outputs": [], 279 | "source": [ 280 | "import gtsam\n", 281 | "import numpy as np\n", 282 | "\n", 283 | "# Create a factor graph container and add factors to it\n", 284 | "factor_graph = gtsam.NonlinearFactorGraph()" 285 | ] 286 | }, 287 | { 288 | "cell_type": "markdown", 289 | "metadata": {}, 290 | "source": [ 291 | "#### Add prior factor" 292 | ] 293 | }, 294 | { 295 | "cell_type": "code", 296 | "execution_count": null, 297 | "metadata": {}, 298 | "outputs": [], 299 | "source": [ 300 | "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(1000 * np.ones(6))\n", 301 | "factor_graph.add(gtsam.PriorFactorPose3(0, initial_graph.get_node_pose(0), PRIOR_NOISE))" 302 | ] 303 | }, 304 | { 305 | "cell_type": "markdown", 306 | "metadata": {}, 307 | "source": [ 308 | "#### Add odometry factors" 309 | ] 310 | }, 311 | { 312 | "cell_type": "code", 313 | "execution_count": null, 314 | "metadata": {}, 315 | "outputs": [], 316 | "source": [ 317 | "for e in initial_graph.edges:\n", 318 | " if e[\"type\"] == \"odometry\":\n", 319 | " ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(0.01 * np.ones(6))\n", 320 | " \n", 321 | " factor_graph.add(\n", 322 | " gtsam.BetweenFactorPose3(\n", 323 | " e[\"parent_id\"], e[\"child_id\"], e[\"pose\"], ODOMETRY_NOISE\n", 324 | " )\n", 325 | " )" 326 | ] 327 | }, 328 | { 329 | "cell_type": "markdown", 330 | "metadata": {}, 331 | "source": [ 332 | "#### Initialize state values" 333 | ] 334 | }, 335 | { 336 | "cell_type": "code", 337 | "execution_count": null, 338 | "metadata": {}, 339 | "outputs": [], 340 | "source": [ 341 | "initial_estimate = gtsam.Values()\n", 342 | "for i, node in enumerate(initial_graph.nodes):\n", 343 | " initial_estimate.insert(i, graph.get_node_pose(i))" 344 | ] 345 | }, 346 | { 347 | "cell_type": "markdown", 348 | "metadata": {}, 349 | "source": [ 350 | "#### Visualize factor graph" 351 | ] 352 | }, 353 | { 354 | "cell_type": "code", 355 | "execution_count": null, 356 | "metadata": {}, 357 | "outputs": [], 358 | "source": [ 359 | "factor_graph.saveGraph(\"test.dot\", initial_estimate)\n", 360 | "\n", 361 | "from graphviz import Source\n", 362 | "s = Source.from_file(\"test.dot\")\n", 363 | "s.view()" 364 | ] 365 | }, 366 | { 367 | "cell_type": "markdown", 368 | "metadata": {}, 369 | "source": [ 370 | "#### Optimize" 371 | ] 372 | }, 373 | { 374 | "cell_type": "code", 375 | "execution_count": null, 376 | "metadata": {}, 377 | "outputs": [], 378 | "source": [ 379 | "# copy graph\n", 380 | "import copy\n", 381 | "\n", 382 | "optimized_graph = copy.deepcopy(initial_graph)\n", 383 | "\n", 384 | "# Setup optimizer\n", 385 | "parameters = gtsam.GaussNewtonParams()\n", 386 | "optimizer = gtsam.GaussNewtonOptimizer(factor_graph, initial_estimate, parameters)\n", 387 | "\n", 388 | "result = optimizer.optimize()\n", 389 | "for n, _ in enumerate(optimized_graph.nodes):\n", 390 | " optimized_graph.set_node_pose(n, result.atPose3(n))" 391 | ] 392 | }, 393 | { 394 | "cell_type": "markdown", 395 | "metadata": {}, 396 | "source": [ 397 | "#### Visualize solution" 398 | ] 399 | }, 400 | { 401 | "cell_type": "code", 402 | "execution_count": null, 403 | "metadata": {}, 404 | "outputs": [], 405 | "source": [ 406 | "geometries = vis.graph_to_geometries(\n", 407 | " optimized_graph,\n", 408 | " show_frames=True,\n", 409 | " show_edges=True,\n", 410 | " show_nodes=True,\n", 411 | " show_clouds=True,\n", 412 | " odometry_color=vis.GRAY,\n", 413 | " loop_color=vis.RED,\n", 414 | ")\n", 415 | "o3d.visualization.draw_geometries(\n", 416 | " geometries,\n", 417 | " window_name=\"Optimized Pose Graph\",\n", 418 | " zoom=0.24,\n", 419 | " front=[-0.35, -0.57, 0.7],\n", 420 | " lookat=[1.4, -18, 2.0],\n", 421 | " up=[0.4, 0.5, 0.6],\n", 422 | ")" 423 | ] 424 | }, 425 | { 426 | "cell_type": "markdown", 427 | "metadata": { 428 | "jp-MarkdownHeadingCollapsed": true 429 | }, 430 | "source": [ 431 | "#### Step-by-step optimization" 432 | ] 433 | }, 434 | { 435 | "cell_type": "code", 436 | "execution_count": null, 437 | "metadata": {}, 438 | "outputs": [], 439 | "source": [ 440 | "import copy\n", 441 | "import open3d as o3d\n", 442 | "import time\n", 443 | "\n", 444 | "# copy graph\n", 445 | "optimized_graph = copy.deepcopy(initial_graph)\n", 446 | "\n", 447 | "# Setup optimizer\n", 448 | "parameters = gtsam.GaussNewtonParams()\n", 449 | "optimizer = gtsam.GaussNewtonOptimizer(factor_graph, initial_estimate, parameters)\n", 450 | "\n", 451 | "visualizer = o3d.visualization.Visualizer()\n", 452 | "visualizer.create_window(\"Step-by-step optimization\")\n", 453 | "\n", 454 | "# Manually run optimizer\n", 455 | "for i in range(10):\n", 456 | " print(f\"iter: {i}\")\n", 457 | " visualizer.clear_geometries()\n", 458 | " geometry = vis.graph_to_geometries(\n", 459 | " optimized_graph,\n", 460 | " show_frames=True,\n", 461 | " show_edges=True,\n", 462 | " show_nodes=True,\n", 463 | " show_clouds=True,\n", 464 | " odometry_color=vis.GRAY,\n", 465 | " loop_color=vis.RED,\n", 466 | " )\n", 467 | " for g in geometry:\n", 468 | " visualizer.add_geometry(g)\n", 469 | "\n", 470 | " # ctr = visualizer.get_view_control()\n", 471 | " # ctr.set_front([-0.35, -0.57, 0.7])\n", 472 | " # ctr.set_up([0.4, 0.5, 0.6])\n", 473 | " # ctr.set_zoom(0.24)\n", 474 | " # ctr.set_lookat([1.4, -18, 2.0])\n", 475 | "\n", 476 | " r = visualizer.get_render_option()\n", 477 | " r.point_size = 1\n", 478 | "\n", 479 | " # for g in geometry:\n", 480 | " # visualizer.update_geometry(g)\n", 481 | "\n", 482 | " visualizer.poll_events()\n", 483 | " visualizer.update_renderer()\n", 484 | "\n", 485 | " optimizer.iterate()\n", 486 | " result = optimizer.values()\n", 487 | " for n, _ in enumerate(optimized_graph.nodes):\n", 488 | " optimized_graph.set_node_pose(n, result.atPose3(n))\n", 489 | " time.sleep(1)\n", 490 | "\n", 491 | "visualizer.run()" 492 | ] 493 | }, 494 | { 495 | "cell_type": "markdown", 496 | "metadata": {}, 497 | "source": [ 498 | "## Part 3: Adding loops" 499 | ] 500 | }, 501 | { 502 | "cell_type": "code", 503 | "execution_count": null, 504 | "metadata": {}, 505 | "outputs": [], 506 | "source": [ 507 | "initial_graph = pg.create_test_pose_graph(\n", 508 | " use_wrong_init=True,\n", 509 | " use_noisy_odom=True,\n", 510 | " use_true_loops=True,\n", 511 | " use_false_loops=True,\n", 512 | " init_noise_per_m=0.1,\n", 513 | " odo_noise_per_m=0.1,\n", 514 | " loop_candidate_id_distance=10,\n", 515 | " true_loop_dist_thr=5,\n", 516 | " true_loop_num=2,\n", 517 | " false_loop_dist_thr=10,\n", 518 | " false_loop_num=10,\n", 519 | " loop_info=1000,\n", 520 | ")\n", 521 | "\n", 522 | "geometries = vis.graph_to_geometries(\n", 523 | " initial_graph,\n", 524 | " show_frames=True,\n", 525 | " show_edges=True,\n", 526 | " show_nodes=True,\n", 527 | " show_clouds=True,\n", 528 | " odometry_color=vis.GRAY,\n", 529 | " loop_color=vis.RED,\n", 530 | ")\n", 531 | "o3d.visualization.draw_geometries(\n", 532 | " geometries,\n", 533 | " window_name=\"Initial Pose Graph\",\n", 534 | " zoom=0.24,\n", 535 | " front=[-0.35, -0.57, 0.7],\n", 536 | " lookat=[1.4, -18, 2.0],\n", 537 | " up=[0.4, 0.5, 0.6],\n", 538 | ")" 539 | ] 540 | }, 541 | { 542 | "cell_type": "markdown", 543 | "metadata": {}, 544 | "source": [ 545 | "### Setup factor graph" 546 | ] 547 | }, 548 | { 549 | "cell_type": "code", 550 | "execution_count": null, 551 | "metadata": {}, 552 | "outputs": [], 553 | "source": [ 554 | "import gtsam\n", 555 | "import numpy as np\n", 556 | "\n", 557 | "# Create a factor graph container and add factors to it\n", 558 | "factor_graph = gtsam.NonlinearFactorGraph()\n", 559 | "\n", 560 | "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(0.001 * np.ones(6))\n", 561 | "factor_graph.add(gtsam.PriorFactorPose3(0, initial_graph.get_node_pose(0), PRIOR_NOISE))\n", 562 | "\n", 563 | "for e in initial_graph.edges:\n", 564 | " if e[\"type\"] == \"odometry\":\n", 565 | " ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Information(e[\"info\"])\n", 566 | " factor_graph.add(\n", 567 | " gtsam.BetweenFactorPose3(\n", 568 | " e[\"parent_id\"], e[\"child_id\"], e[\"pose\"], ODOMETRY_NOISE\n", 569 | " )\n", 570 | " )" 571 | ] 572 | }, 573 | { 574 | "cell_type": "markdown", 575 | "metadata": {}, 576 | "source": [ 577 | "### Add loop candidate factors" 578 | ] 579 | }, 580 | { 581 | "cell_type": "code", 582 | "execution_count": null, 583 | "metadata": {}, 584 | "outputs": [], 585 | "source": [ 586 | "for e in initial_graph.edges:\n", 587 | " if e[\"type\"] == \"loop\":\n", 588 | " LOOP_NOISE = gtsam.noiseModel.Diagonal.Information(1000 * np.eye(6))\n", 589 | " ROBUST_MODEL = gtsam.noiseModel.Robust.Create(\n", 590 | " gtsam.noiseModel.mEstimator.DCS.Create(1.0), LOOP_NOISE\n", 591 | " )\n", 592 | "\n", 593 | " factor_graph.add(\n", 594 | " gtsam.BetweenFactorPose3(\n", 595 | " e[\"parent_id\"], e[\"child_id\"], e[\"pose\"], ROBUST_MODEL\n", 596 | " )\n", 597 | " )" 598 | ] 599 | }, 600 | { 601 | "cell_type": "markdown", 602 | "metadata": {}, 603 | "source": [ 604 | "### Initialize state values" 605 | ] 606 | }, 607 | { 608 | "cell_type": "code", 609 | "execution_count": null, 610 | "metadata": {}, 611 | "outputs": [], 612 | "source": [ 613 | "initial_estimate = gtsam.Values()\n", 614 | "for i, node in enumerate(initial_graph.nodes):\n", 615 | " initial_estimate.insert(i, graph.get_node_pose(i))" 616 | ] 617 | }, 618 | { 619 | "cell_type": "markdown", 620 | "metadata": {}, 621 | "source": [ 622 | "### Visualize factor graph" 623 | ] 624 | }, 625 | { 626 | "cell_type": "code", 627 | "execution_count": null, 628 | "metadata": {}, 629 | "outputs": [], 630 | "source": [ 631 | "factor_graph.saveGraph(\"test.dot\", initial_estimate)\n", 632 | "\n", 633 | "from graphviz import Source\n", 634 | "\n", 635 | "s = Source.from_file(\"test.dot\")\n", 636 | "s.view()" 637 | ] 638 | }, 639 | { 640 | "cell_type": "markdown", 641 | "metadata": {}, 642 | "source": [ 643 | "### Optimize" 644 | ] 645 | }, 646 | { 647 | "cell_type": "code", 648 | "execution_count": null, 649 | "metadata": {}, 650 | "outputs": [], 651 | "source": [ 652 | "import copy\n", 653 | "optimized_graph = copy.deepcopy(initial_graph)\n", 654 | "\n", 655 | "# Setup optimizer\n", 656 | "parameters = gtsam.GaussNewtonParams()\n", 657 | "optimizer = gtsam.GaussNewtonOptimizer(factor_graph, initial_estimate, parameters)\n", 658 | "\n", 659 | "result = optimizer.optimize()\n", 660 | "for n, _ in enumerate(optimized_graph.nodes):\n", 661 | " optimized_graph.set_node_pose(n, result.atPose3(n))" 662 | ] 663 | }, 664 | { 665 | "cell_type": "markdown", 666 | "metadata": {}, 667 | "source": [ 668 | "### Visualize solution" 669 | ] 670 | }, 671 | { 672 | "cell_type": "code", 673 | "execution_count": null, 674 | "metadata": {}, 675 | "outputs": [], 676 | "source": [ 677 | "geometries = vis.graph_to_geometries(\n", 678 | " optimized_graph,\n", 679 | " show_frames=True,\n", 680 | " show_edges=True,\n", 681 | " show_nodes=True,\n", 682 | " show_clouds=True,\n", 683 | " odometry_color=vis.GRAY,\n", 684 | " loop_color=vis.RED,\n", 685 | ")\n", 686 | "o3d.visualization.draw_geometries(\n", 687 | " geometries,\n", 688 | " window_name=\"Optimized Pose Graph\",\n", 689 | " zoom=0.24,\n", 690 | " front=[-0.35, -0.57, 0.7],\n", 691 | " lookat=[1.4, -18, 2.0],\n", 692 | " up=[0.4, 0.5, 0.6],\n", 693 | ")" 694 | ] 695 | }, 696 | { 697 | "cell_type": "code", 698 | "execution_count": null, 699 | "metadata": {}, 700 | "outputs": [], 701 | "source": [] 702 | } 703 | ], 704 | "metadata": { 705 | "kernelspec": { 706 | "display_name": "Python 3 (ipykernel)", 707 | "language": "python", 708 | "name": "python3" 709 | }, 710 | "language_info": { 711 | "codemirror_mode": { 712 | "name": "ipython", 713 | "version": 3 714 | }, 715 | "file_extension": ".py", 716 | "mimetype": "text/x-python", 717 | "name": "python", 718 | "nbconvert_exporter": "python", 719 | "pygments_lexer": "ipython3", 720 | "version": "3.10.13" 721 | } 722 | }, 723 | "nbformat": 4, 724 | "nbformat_minor": 4 725 | } 726 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pip>=23.0.0 2 | pre-commit 3 | pandas 4 | gtsam 5 | jupyter 6 | jupyterlab 7 | graphviz 8 | open3d==0.16.0 9 | ipywidgets 10 | jupyterlab-widgets 11 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from setuptools import find_packages 3 | 4 | setup( 5 | name="slam_tutorial", 6 | version="0.1.0", 7 | author="Matias Mattamala", 8 | author_email="matias@robots.ox.ac.uk", 9 | packages=find_packages(), 10 | package_dir={"": "."}, 11 | python_requires=">=3.8", 12 | description="SLAM tutorial presented in the ORIentate Seminar", 13 | ) 14 | -------------------------------------------------------------------------------- /slam_tutorial/__init__.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | """Absolute path to the slam_tutorial repository.""" 4 | PKG_DIR = os.path.dirname(os.path.dirname(os.path.realpath(__file__))) 5 | ASSETS_DIR = PKG_DIR + "/assets" 6 | -------------------------------------------------------------------------------- /slam_tutorial/io.py: -------------------------------------------------------------------------------- 1 | # Author: Matias Mattamala (matias@robots.ox.ac.uk) 2 | 3 | import numpy as np 4 | import open3d as o3d 5 | import gtsam 6 | 7 | from slam_tutorial.pose_graph import PoseGraph 8 | 9 | NCD_BASE_LIDAR = gtsam.Pose3( 10 | gtsam.Rot3.Ypr(3 / 4 * np.pi, 0.0, 0.0), np.array([-0.04, -0.0845, -0.06]) 11 | ).matrix() 12 | 13 | 14 | def read_cloud(stamp, clouds_path): 15 | sec, nsec = stamp 16 | cloud_file = clouds_path + f"/cloud_{sec}_{int(nsec):09}.pcd" 17 | cloud = o3d.io.read_point_cloud(cloud_file) 18 | return cloud 19 | 20 | 21 | def read_pose_gt(tokens): 22 | sec = tokens[0] 23 | nsec = tokens[1] 24 | pos = [float(i) for i in tokens[2:5]] 25 | q = [float(i) for i in tokens[5:9]] 26 | quat = gtsam.Rot3.Quaternion(q[3], q[0], q[1], q[2]) 27 | pose = gtsam.Pose3(quat, pos) 28 | 29 | return pose, (sec, nsec) 30 | 31 | 32 | def read_pose_slam(tokens): 33 | pose_id = int(tokens[0]) 34 | pose_stamp = (tokens[8], tokens[9]) 35 | pos = [float(i) for i in tokens[1:4]] 36 | q = [float(i) for i in tokens[4:8]] 37 | quat = gtsam.Rot3.Quaternion(q[3], q[0], q[1], q[2]) 38 | pose = gtsam.Pose3(quat, pos) 39 | 40 | return pose, pose_stamp, pose_id 41 | 42 | 43 | def read_pose_edge_slam(tokens): 44 | parent_id = int(tokens[0]) 45 | child_id = int(tokens[1]) 46 | pos = [float(i) for i in tokens[2:5]] 47 | q = [float(i) for i in tokens[5:9]] 48 | quat = gtsam.Rot3.Quaternion(q[3], q[0], q[1], q[2]) 49 | relative_pose = gtsam.Pose3(quat, pos) 50 | upper_triangular = [float(i) for i in tokens[9:]] 51 | relative_info = np.eye(6, dtype=np.float64) 52 | relative_info[0, 0:6] = relative_info[0:6, 0] = upper_triangular[0:6] 53 | relative_info[1, 1:6] = relative_info[1:6, 1] = upper_triangular[6:11] 54 | relative_info[2, 2:6] = relative_info[2:6, 2] = upper_triangular[11:15] 55 | relative_info[3, 3:6] = relative_info[3:6, 3] = upper_triangular[15:18] 56 | relative_info[4, 4:6] = relative_info[4:6, 4] = upper_triangular[18:20] 57 | relative_info[5, 5:6] = relative_info[5:6, 5] = upper_triangular[20:21] 58 | 59 | return relative_pose, relative_info, parent_id, child_id 60 | 61 | 62 | def load_ground_truth_file_as_pose_graph( 63 | path: str, distance_thr: float = 0.0, num_nodes: int = np.inf 64 | ): 65 | graph = PoseGraph() 66 | 67 | with open(path, "r") as file: 68 | lines = file.readlines() 69 | 70 | # Load nodes 71 | n = 0 72 | last_pose = None 73 | for line in lines: 74 | tokens = line.strip().split(",") 75 | if tokens[0][0] == "#": 76 | continue 77 | 78 | # Parse line 79 | pose, stamp = read_pose_gt(tokens) 80 | if last_pose is None: 81 | last_pose = pose 82 | continue 83 | 84 | # Just add nodes farther apart 85 | if ( 86 | np.linalg.norm(pose.translation() - last_pose.translation()) 87 | >= distance_thr 88 | ): 89 | graph.add_node(n, stamp, pose) 90 | last_pose = pose 91 | n += 1 92 | 93 | # Just add num_nodes nodes 94 | if n >= num_nodes: 95 | break 96 | 97 | # Add odometry edges 98 | for n, node in enumerate(graph.nodes): 99 | if n == 0: 100 | continue 101 | if n == graph.size: 102 | break 103 | 104 | ni = graph.nodes[n - 1] 105 | nj = graph.nodes[n] 106 | delta_pose = ni["pose"].inverse() * nj["pose"] 107 | graph.add_edge(n - 1, n, "odometry", delta_pose, np.eye(6) * 1000) 108 | 109 | # Return complete graph 110 | return graph 111 | 112 | 113 | def load_pose_graph(path: str, clouds_path: str = None, T_base_lidar=NCD_BASE_LIDAR): 114 | graph = PoseGraph() 115 | with open(path, "r") as file: 116 | lines = file.readlines() 117 | for line in lines: 118 | tokens = line.strip().split(" ") 119 | 120 | # Parse lines 121 | if tokens[0] == "#": 122 | continue 123 | 124 | elif tokens[0] == "PLATFORM_ID": 125 | continue 126 | 127 | elif tokens[0] == "VERTEX_SE3:QUAT_TIME": 128 | pose, pose_stamp, pose_id = read_pose_slam(tokens[1:]) 129 | graph.add_node(pose_id, pose_stamp, pose) 130 | 131 | if clouds_path is not None: 132 | cloud = read_cloud(pose_stamp, clouds_path) 133 | cloud.transform(T_base_lidar) 134 | graph.add_clouds(pose_id, cloud) 135 | 136 | elif tokens[0] == "EDGE_SE3:QUAT": 137 | relative_pose, relative_info, parent_id, child_id = read_pose_edge_slam( 138 | tokens[1:] 139 | ) 140 | edge_type = "odometry" if (parent_id == child_id - 1) else "loop" 141 | graph.add_edge( 142 | parent_id, child_id, edge_type, relative_pose, relative_info 143 | ) 144 | 145 | return graph 146 | 147 | 148 | def write_graph_node(node): 149 | id = node["id"] 150 | sec, nsec = node["stamp"] 151 | x = node["pose"].translation()[0] 152 | y = node["pose"].translation()[1] 153 | z = node["pose"].translation()[2] 154 | 155 | qx = node["pose"].rotation().toQuaternion().x() 156 | qy = node["pose"].rotation().toQuaternion().y() 157 | qz = node["pose"].rotation().toQuaternion().z() 158 | qw = node["pose"].rotation().toQuaternion().w() 159 | 160 | stream = f"VERTEX_SE3:QUAT_TIME {id} {x} {y} {z} {qx} {qy} {qz} {qw} {sec} {nsec}\n" 161 | return stream 162 | 163 | 164 | def write_graph_edge(edge): 165 | parent_id = edge["parent_id"] 166 | child_id = edge["child_id"] 167 | 168 | x = edge["pose"].translation()[0] 169 | y = edge["pose"].translation()[1] 170 | z = edge["pose"].translation()[2] 171 | 172 | qx = edge["pose"].rotation().toQuaternion().x() 173 | qy = edge["pose"].rotation().toQuaternion().y() 174 | qz = edge["pose"].rotation().toQuaternion().z() 175 | qw = edge["pose"].rotation().toQuaternion().w() 176 | 177 | info = "" 178 | for i in range(6): 179 | for j in range(i, 6): 180 | info += f"{edge['info'][i][j]} " 181 | info = info[:-1] 182 | 183 | stream = ( 184 | f"EDGE_SE3:QUAT {parent_id} {child_id} {x} {y} {z} {qx} {qy} {qz} {qw} {info}\n" 185 | ) 186 | return stream 187 | 188 | 189 | def write_pose_graph(pose_graph: PoseGraph, path: str): 190 | with open(path, "w") as file: 191 | for node in pose_graph.nodes: 192 | file.write(write_graph_node(node)) 193 | for edge in pose_graph.edges: 194 | file.write(write_graph_edge(edge)) 195 | -------------------------------------------------------------------------------- /slam_tutorial/pose_graph.py: -------------------------------------------------------------------------------- 1 | import gtsam 2 | import numpy as np 3 | import open3d as o3d 4 | 5 | 6 | class PoseGraph: 7 | def __init__(self): 8 | self._nodes = [] 9 | self._edges = [] 10 | self._adjacency = {} 11 | self._clouds = {} 12 | 13 | @property 14 | def size(self): 15 | return len(self._nodes) 16 | 17 | @property 18 | def nodes(self): 19 | return self._nodes 20 | 21 | @property 22 | def edges(self): 23 | return self._edges 24 | 25 | @nodes.setter 26 | def nodes(self, nodes): 27 | self._nodes = nodes 28 | 29 | @edges.setter 30 | def edges(self, edges): 31 | self._edges = edges 32 | 33 | def get_node_pose(self, id): 34 | return self._nodes[id]["pose"] 35 | 36 | def get_node_cloud(self, id): 37 | try: 38 | return self._clouds[id] 39 | except Exception: 40 | return o3d.geometry.PointCloud() 41 | 42 | def set_node_pose(self, id, pose): 43 | self._nodes[id]["pose"] = pose 44 | 45 | def get_odometry_edge(self, parent_id, child_id): 46 | try: 47 | edge_idx = self._adjacency[parent_id][child_id] 48 | return self._edges[edge_idx]["pose"] 49 | except Exception: 50 | return gtsam.Pose3.Identity() 51 | 52 | def _is_valid_id(self, id): 53 | return id >= 0 and id < self.size 54 | 55 | def add_node(self, id, stamp, pose): 56 | assert isinstance(pose, gtsam.Pose3) 57 | self._nodes.append({"pose": pose, "stamp": stamp, "id": id}) 58 | self._adjacency[id] = {} 59 | 60 | def add_edge(self, parent_id, child_id, edge_type, relative_pose, relative_info): 61 | # This adds directed edges, even though they should be an undirected graph 62 | # We do this to simplify the API 63 | assert isinstance(relative_pose, gtsam.Pose3) 64 | assert isinstance(relative_info, np.ndarray) 65 | assert relative_info.shape == (6, 6) 66 | 67 | if not self._is_valid_id(parent_id): 68 | raise KeyError( 69 | f"Node parent [{parent_id}] not in graph. Cannot add the edge" 70 | ) 71 | if not self._is_valid_id(child_id): 72 | raise KeyError(f"Node child [{child_id}] not in graph. Cannot add the edge") 73 | 74 | self._edges.append( 75 | { 76 | "parent_id": parent_id, 77 | "child_id": child_id, 78 | "pose": relative_pose, 79 | "info": relative_info, 80 | "type": edge_type, 81 | } 82 | ) 83 | # Save reference to edge in adjacency matrix 84 | self._adjacency[parent_id][child_id] = len(self._edges) - 1 85 | 86 | def add_clouds(self, id, scan): 87 | assert isinstance(scan, o3d.geometry.PointCloud) 88 | self._clouds[id] = scan 89 | 90 | 91 | def initialize_from_odometry(graph): 92 | node_initialized = [False] * graph.size 93 | node_initialized[0] = True 94 | 95 | for n, node in enumerate(graph.nodes): 96 | if node_initialized[n]: 97 | continue 98 | 99 | # Get previous pose 100 | last_pose = graph.nodes[n - 1]["pose"] 101 | 102 | # Get odometry from previous 103 | relative_pose = graph.get_odometry_edge(n - 1, n) 104 | pose = last_pose * relative_pose 105 | 106 | # Update current node pose 107 | graph.set_node_pose(n, pose) 108 | node_initialized[n] = True 109 | 110 | 111 | def add_odometry_drift( 112 | graph, 113 | noise_per_m=0.0, 114 | axis="", 115 | rot_axis="", 116 | drift_type="constant", 117 | reset_node_poses=True, 118 | ): 119 | noise_mask = np.zeros(6) 120 | if "x" in axis: 121 | noise_mask[3] = 1.0 122 | if "y" in axis: 123 | noise_mask[4] = 1.0 124 | if "z" in axis: 125 | noise_mask[5] = 1.0 126 | 127 | if "x" in rot_axis: 128 | noise_mask[0] = 1.0 129 | if "y" in rot_axis: 130 | noise_mask[1] = 1.0 131 | if "z" in rot_axis: 132 | noise_mask[2] = 1.0 133 | 134 | import copy 135 | 136 | new_graph = copy.deepcopy(graph) 137 | for i, edge in enumerate(new_graph.edges): 138 | if edge["type"] != "odometry": 139 | continue 140 | 141 | # Get original odometry 142 | relative_pose = edge["pose"] 143 | 144 | # Drift model 145 | if drift_type == "constant": 146 | noise = np.ones(6) 147 | elif drift_type == "random_walk": 148 | noise = np.random.standard_normal(6) 149 | 150 | relative_distance = np.linalg.norm(relative_pose.translation()) 151 | noise = noise * noise_mask * relative_distance * noise_per_m 152 | 153 | # Apply drift to odometry measurement 154 | new_relative_pose = relative_pose * gtsam.Pose3.Expmap(noise) 155 | 156 | # Update odometry measurement 157 | new_graph.edges[i]["pose"] = new_relative_pose 158 | 159 | if reset_node_poses: 160 | initialize_from_odometry(new_graph) 161 | return new_graph 162 | 163 | 164 | def create_test_pose_graph( 165 | use_wrong_init=True, 166 | use_noisy_odom=False, 167 | use_true_loops=False, 168 | use_false_loops=False, 169 | init_noise_per_m=0.1, 170 | init_drift_axis="xy", 171 | init_drift_type="random_walk", 172 | odo_noise_per_m=0.1, 173 | odo_drift_axis="xy", 174 | odo_drift_type="random_walk", 175 | loop_candidate_id_distance=10, 176 | true_loop_dist_thr=5, 177 | false_loop_dist_thr=10, 178 | true_loop_num=2, 179 | false_loop_num=10, 180 | loop_info=1000, 181 | ): 182 | import slam_tutorial.io as io 183 | import slam_tutorial 184 | import copy 185 | 186 | # Read ground truth data 187 | graph_ground_truth = io.load_pose_graph( 188 | slam_tutorial.ASSETS_DIR + "/ground_truth.slam", 189 | clouds_path=slam_tutorial.ASSETS_DIR + "/individual_clouds", 190 | ) 191 | out_graph = copy.deepcopy(graph_ground_truth) 192 | 193 | # Create a graph with odometry drift 194 | if use_wrong_init: 195 | graph_init = add_odometry_drift( 196 | graph_ground_truth, 197 | noise_per_m=init_noise_per_m, 198 | axis=init_drift_axis, 199 | drift_type=init_drift_type, 200 | reset_node_poses=True, 201 | ) 202 | out_graph.nodes = graph_init.nodes 203 | 204 | if use_noisy_odom: 205 | graph_odo = add_odometry_drift( 206 | graph_ground_truth, 207 | noise_per_m=odo_noise_per_m, 208 | axis=odo_drift_axis, 209 | drift_type=odo_drift_type, 210 | reset_node_poses=False, 211 | ) 212 | out_graph.edges = graph_odo.edges 213 | 214 | if use_true_loops or use_false_loops: 215 | import random 216 | 217 | true_loop_candidates = [] 218 | false_loop_candidates = [] 219 | for i in range(graph_ground_truth.size): 220 | for j in range(i, graph_ground_truth.size): 221 | if abs(i - j) > loop_candidate_id_distance: 222 | posei = graph_ground_truth.get_node_pose(i) 223 | posej = graph_ground_truth.get_node_pose(j) 224 | relative_pose = posei.inverse() * posej 225 | relative_distance = np.linalg.norm(relative_pose.translation()) 226 | 227 | if relative_distance < true_loop_dist_thr: 228 | true_loop_candidates.append( 229 | {"parent_id": i, "child_id": j, "pose": relative_pose} 230 | ) 231 | if relative_distance > false_loop_dist_thr: 232 | eye = gtsam.Pose3.Identity() 233 | false_loop_candidates.append( 234 | {"parent_id": i, "child_id": j, "pose": eye} 235 | ) 236 | 237 | # Add true loop candidates 238 | if use_true_loops: 239 | random.shuffle(true_loop_candidates) 240 | for c in true_loop_candidates[:true_loop_num]: 241 | out_graph.add_edge( 242 | c["parent_id"], 243 | c["child_id"], 244 | "loop", 245 | c["pose"], 246 | loop_info * np.eye(6), 247 | ) 248 | 249 | # Add false loop candidates 250 | if use_false_loops: 251 | random.shuffle(false_loop_candidates) 252 | for c in false_loop_candidates[:false_loop_num]: 253 | out_graph.add_edge( 254 | c["parent_id"], 255 | c["child_id"], 256 | "loop", 257 | c["pose"], 258 | loop_info * np.eye(6), 259 | ) 260 | 261 | return out_graph 262 | -------------------------------------------------------------------------------- /slam_tutorial/visualization.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import numpy as np 3 | import open3d as o3d 4 | import open3d.visualization.gui as gui 5 | 6 | # colors 7 | RED = [1, 0, 0] 8 | GRAY = [0.3, 0.3, 0.3] 9 | LIGHT_GRAY = [0.7, 0.7, 0.7] 10 | 11 | 12 | def graph_to_geometries( 13 | graph, 14 | show_frames=True, 15 | show_edges=True, 16 | show_nodes=True, 17 | show_clouds=False, 18 | odometry_color=GRAY, 19 | loop_color=RED, 20 | up_to_node=np.inf, 21 | ): 22 | pose_graph = copy.deepcopy(graph) 23 | geometries = [] 24 | 25 | node_centers = [] 26 | frames_vis = o3d.geometry.TriangleMesh() 27 | nodes_vis = o3d.geometry.TriangleMesh() 28 | # clouds_vis = o3d.t.geometry.PointCloud() 29 | 30 | for n, node in enumerate(pose_graph.nodes): 31 | if n > up_to_node: 32 | break 33 | pose = node["pose"].matrix() 34 | pos = pose[0:3, 3] 35 | rot = pose[0:3, 0:3] 36 | node_centers.append(pos) 37 | 38 | if show_frames: 39 | frame_mesh = o3d.geometry.TriangleMesh.create_coordinate_frame( 40 | size=1.0, origin=pos 41 | ) 42 | frame_mesh.rotate(rot, center=pos) 43 | frames_vis += frame_mesh 44 | 45 | if show_nodes: 46 | node_mesh = o3d.geometry.TriangleMesh.create_sphere(radius=0.3) 47 | node_mesh.translate(pos) 48 | nodes_vis += node_mesh 49 | 50 | if show_clouds: 51 | cloud = pose_graph.get_node_cloud(n) 52 | cloud.transform(pose) 53 | cloud.paint_uniform_color(LIGHT_GRAY) 54 | geometries.append(cloud) 55 | 56 | # vis.add_geometry("clouds", clouds_vis) 57 | geometries.append(frames_vis) 58 | geometries.append(nodes_vis) 59 | 60 | if show_edges: 61 | edges = [] 62 | edge_colors = [] 63 | for e in pose_graph.edges: 64 | if e["parent_id"] > up_to_node or e["child_id"] > up_to_node: 65 | continue 66 | 67 | edges.append([e["parent_id"], e["child_id"]]) 68 | edge_colors.append( 69 | odometry_color if e["type"] == "odometry" else loop_color 70 | ) 71 | 72 | line_set = o3d.geometry.LineSet( 73 | points=o3d.utility.Vector3dVector(node_centers), 74 | lines=o3d.utility.Vector2iVector(edges), 75 | ) 76 | line_set.colors = o3d.utility.Vector3dVector(edge_colors) 77 | geometries.append(line_set) 78 | 79 | return geometries 80 | 81 | 82 | def show_pose_graph( 83 | graph, 84 | window_name="Pose Graph", 85 | show_ids=True, 86 | show_frames=True, 87 | show_edges=True, 88 | show_nodes=True, 89 | show_clouds=False, 90 | odometry_color=GRAY, 91 | loop_color=RED, 92 | up_to_node=np.inf, 93 | ): 94 | pose_graph = copy.deepcopy(graph) 95 | 96 | app = gui.Application.instance 97 | app.initialize() 98 | 99 | vis = o3d.visualization.O3DVisualizer(window_name, 1024, 768) 100 | vis.show_settings = True 101 | vis.show_skybox(False) 102 | 103 | node_centers = [] 104 | frames_vis = o3d.geometry.TriangleMesh() 105 | nodes_vis = o3d.geometry.TriangleMesh() 106 | # clouds_vis = o3d.t.geometry.PointCloud() 107 | 108 | for n, node in enumerate(pose_graph.nodes): 109 | if n > up_to_node: 110 | break 111 | pose = node["pose"].matrix() 112 | pos = pose[0:3, 3] 113 | rot = pose[0:3, 0:3] 114 | node_centers.append(pos) 115 | 116 | if show_ids: 117 | vis.add_3d_label(pos + np.array([0.1, 0.1, 0.1]), f"{n}") 118 | 119 | if show_frames: 120 | frame_mesh = o3d.geometry.TriangleMesh.create_coordinate_frame( 121 | size=1.0, origin=pos 122 | ) 123 | frame_mesh.rotate(rot, center=pos) 124 | frames_vis += frame_mesh 125 | # vis.add_geometry(f"frame_{n}", frame_mesh) 126 | 127 | if show_nodes: 128 | node_mesh = o3d.geometry.TriangleMesh.create_sphere(radius=0.3) 129 | node_mesh.translate(pos) 130 | nodes_vis += node_mesh 131 | # vis.add_geometry(f"node_{n}", frame_mesh) 132 | 133 | if show_clouds: 134 | cloud = pose_graph.get_node_cloud(n) 135 | cloud.transform(pose) 136 | # clouds_vis += cloud 137 | vis.add_geometry(f"cloud_{n}", cloud) 138 | 139 | vis.add_geometry("frames", frames_vis) 140 | vis.add_geometry("nodes", nodes_vis) 141 | # vis.add_geometry("clouds", clouds_vis) 142 | 143 | if show_edges: 144 | edges = [] 145 | edge_colors = [] 146 | for e in pose_graph.edges: 147 | edges.append([e["parent_id"], e["child_id"]]) 148 | edge_colors.append( 149 | odometry_color if e["type"] == "odometry" else loop_color 150 | ) 151 | 152 | line_set = o3d.geometry.LineSet( 153 | points=o3d.utility.Vector3dVector(node_centers), 154 | lines=o3d.utility.Vector2iVector(edges), 155 | ) 156 | line_set.colors = o3d.utility.Vector3dVector(edge_colors) 157 | vis.add_geometry("edges", line_set) 158 | 159 | vis.reset_camera_to_default() 160 | app.add_window(vis) 161 | app.run() 162 | --------------------------------------------------------------------------------