├── docs ├── robust_pgo.png └── dataset_download_links.txt ├── README.md ├── LICENSE └── robust_pgo └── robust_pgo.ipynb /docs/robust_pgo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/modern-slam-tutorial-python/HEAD/docs/robust_pgo.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # modern-slam-tutorial-python 2 | - Learning and feeling SLAM together with hands-on-experiments :grinning: :smiley: :laughing: 3 | 4 | ## Dependencies 5 | - Most of the examples are based on GTSAM. use `$ pip install gtsam` and I prefer using conda environment. 6 | - Also, I'll (want to) also use Pytorch to study recent differentiable factor-graph optimization works. 7 | 8 | ## Contents 9 | 1. [robust_pgo](https://github.com/gisbi-kim/modern-slam-tutorial-python/tree/main/robust_pgo): a robust pose-graph optimization 10 | - [Tutorial video](https://youtu.be/zOr9HreMthY) 11 | - Dependencies: numpy, GTSAM (python) 12 | - The datasets are available from https://lucacarlone.mit.edu/datasets/ 13 | 14 | *To be continued ...* 15 | 16 | ## Contact 17 | - `gisbi.kim@gmail.com` 18 | 19 | ## Plan 20 | - Other geometric optimization for SLAM 21 | - Non-rigid ICP 22 | - Rotation initialization 23 | - ... 24 | - Trying GTSAM integration with Open3D, scipy, Pytorch, etc ... 25 | 26 | 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2021, Giseop Kim 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /docs/dataset_download_links.txt: -------------------------------------------------------------------------------- 1 | # The below lines are from the SLAM++ paper's README.txt (2017 IJRR SLAM++ - A highly efficient and temporally scalable incremental SLAM framework) 2 | 3 | 4 | # Related papers for the data sequences 5 | [1] G. Grisetti, C. Stachniss, S. Grzonka, and W. Burgard, "A tree parameterization for efficiently computing maximum likelihood maps using gradient descent," in Robotics: Science and Systems (RSS), June 2007. 6 | [2] E. Olson, "Robust and efficient robot mapping," Ph.D. dissertation, Massachusetts Institute of Technology, 2008. 7 | [3] M. Kaess, A. Ranganathan, and F. Dellaert, "iSAM: Fast incremental smoothing and mapping with efficient data association," in IEEE Intl. Conf. on Robotics and Automation (ICRA), Rome, Italy, April 2007, pp. 1670-1677. 8 | [4] A. Howard and N. Roy, "The robotics data set repository (Radish)," 2003. [Online]. Available: http://radish.sourceforge.net/ 9 | [5] M. Bosse, P. Newman, J. Leonard, and S. Teller, "Simultaneous localization and map building in large-scale cyclic environments using the Atlas framework," Intl. J. of Robotics Research, vol. 23, no. 12, pp. 1113-1139, Dec 2004. 10 | [6] R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard: "g2o: A General Framework for Graph Optimization", IEEE Intl. Conf. on Robotics and Automation (ICRA), 2011 11 | [7] H. Kim and A. Hilton, "Influence of Colour and Feature Geometry on Multi-modal 3D Point Clouds Data Registration," Proc. 3DV, 2014. 12 | 13 | --------------------------------------------------- 14 | - Datasets 15 | --------------------------------------------------- 16 | 17 | The following text contains information about sources of dataset files 18 | available to download. 19 | 20 | Guildford cathedral [7] 21 | --------------------------------------------------- 22 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/cathedral.zip/download 23 | 24 | Guildford cathedral - incremental version [7] 25 | --------------------------------------------------- 26 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/cathedral_inc.zip/download 27 | 28 | Venice - incremental version [6] 29 | --------------------------------------------------- 30 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/venice_inc.zip/download 31 | 32 | intel [4] 33 | --------------------------------------------------- 34 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/intel.txt/download 35 | 36 | manhattanOlson3500 [2] 37 | --------------------------------------------------- 38 | modification: the information values have been squared 39 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/manhattanOlson3500.txt/download 40 | 41 | 10kHog-man [1] 42 | --------------------------------------------------- 43 | modification: order of information values has been changed 44 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/10kHog-man.txt/download 45 | 46 | 10k [1] 47 | --------------------------------------------------- 48 | modification: order of information values has been changed 49 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/10k.txt/download 50 | 51 | 100k [1] 52 | --------------------------------------------------- 53 | modification: order of information values has been changed 54 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/100k.txt/download 55 | 56 | city10k [3] 57 | --------------------------------------------------- 58 | modification: the information values have been squared 59 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/city10k.txt/download 60 | 61 | cityTrees10k [3] 62 | --------------------------------------------------- 63 | modification: the information values have been squared 64 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/cityTrees10k.txt/download 65 | 66 | Victoria park [4] 67 | --------------------------------------------------- 68 | modification: the information values have been squared 69 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/victoria-park.txt/download 70 | 71 | Killian court [5] 72 | --------------------------------------------------- 73 | modification: the information values have been squared and the order changed 74 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/killian-court.txt/download 75 | 76 | sphere2500.txt [3] 77 | --------------------------------------------------- 78 | modification: the information values have been converted from RPY to axis angle 79 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/sphere2500.txt/download 80 | 81 | parking-garage.txt [6] 82 | --------------------------------------------------- 83 | modification: the information values have been converted from RPY to axis angle 84 | download: http://sourceforge.net/projects/slam-plus-plus/files/data/parking-garage.txt/download 85 | 86 | sphere2500a [6] 87 | --------------------------------------------------- 88 | https://github.com/RainerKuemmerle/g2o/tree/master/g2o/examples/sphere 89 | Being a popular shape in robotics, there is another sphere2500 dataset generated by g2o (https://github.com/RainerKuemmerle/g2o/tree/master/g2o/examples/sphere) which is sometimes called sphere2500a. It is more connected than the iSAM sphere and thus more difficult to solve. There is however another sphere which was created by Giorgio Grisetti while implementing TORO in its 3D variant. It's called sphere_bignoise_vertex3.g2o and can be found inside the repository at http://www.openslam.org/g2o.html. 90 | -------------------------------------------------------------------------------- /robust_pgo/robust_pgo.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "id": "9a50067a-5fbe-47f0-a458-8ba75b17b16c", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import os\n", 11 | "\n", 12 | "import numpy as np\n", 13 | "import gtsam # in conda env, $pip install gtsam \n", 14 | "\n", 15 | "from pytictoc import TicToc" 16 | ] 17 | }, 18 | { 19 | "cell_type": "code", 20 | "execution_count": 2, 21 | "id": "e4dd4380-c511-4da2-87d1-286f58e39e8c", 22 | "metadata": {}, 23 | "outputs": [], 24 | "source": [ 25 | "\"\"\"\n", 26 | " helper functions \n", 27 | "\"\"\"\n", 28 | "\n", 29 | "def vector6(x, y, z, a, b, c):\n", 30 | " \"\"\"Create 6d double numpy array.\"\"\"\n", 31 | " return np.array([x, y, z, a, b, c], dtype=float)\n", 32 | "\n", 33 | "def info2mat(info):\n", 34 | " mat = np.zeros((6,6))\n", 35 | " ix = 0\n", 36 | " for i in range(mat.shape[0]):\n", 37 | " mat[i,i:] = info[ix:ix+(6-i)]\n", 38 | " mat[i:,i] = info[ix:ix+(6-i)]\n", 39 | " ix += (6-i)\n", 40 | "\n", 41 | " return mat\n", 42 | "\n", 43 | "def read_g2o(fn):\n", 44 | " verticies, edges = [], []\n", 45 | " with open(fn) as f:\n", 46 | " for line in f:\n", 47 | " line = line.split()\n", 48 | " if line[0] == 'VERTEX_SE3:QUAT':\n", 49 | " v = int(line[1])\n", 50 | " pose = np.array(line[2:], dtype=np.float32)\n", 51 | " verticies.append([v, pose])\n", 52 | "\n", 53 | " elif line[0] == 'EDGE_SE3:QUAT':\n", 54 | " u = int(line[1])\n", 55 | " v = int(line[2])\n", 56 | " pose = np.array(line[3:10], dtype=np.float32)\n", 57 | " info = np.array(line[10:], dtype=np.float32)\n", 58 | "\n", 59 | " info = info2mat(info)\n", 60 | " edges.append([u, v, pose, info, line])\n", 61 | "\n", 62 | " return verticies, edges\n", 63 | "\n", 64 | "def write_g2o(pose_graph, fn):\n", 65 | " import csv\n", 66 | " verticies, edges = pose_graph\n", 67 | " with open(fn, 'w') as f:\n", 68 | " writer = csv.writer(f, delimiter=' ')\n", 69 | " for (v, pose) in verticies:\n", 70 | " row = ['VERTEX_SE3:QUAT', v] + pose.tolist()\n", 71 | " writer.writerow(row)\n", 72 | " for edge in edges:\n", 73 | " writer.writerow(edge[-1])\n", 74 | "\n", 75 | "def write_xyz(pose_graph, fn):\n", 76 | " import csv\n", 77 | " verticies, edges = pose_graph\n", 78 | " with open(fn, 'w') as f:\n", 79 | " writer = csv.writer(f, delimiter=' ')\n", 80 | " for (v, pose) in verticies:\n", 81 | " row = pose.tolist()\n", 82 | " writer.writerow(row)" 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": 3, 88 | "id": "17d4f841-b369-4010-b372-cdce5559dfb6", 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "\"\"\"\n", 93 | " noise models \n", 94 | "\"\"\"\n", 95 | "\n", 96 | "# constants (user variables) \n", 97 | "priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))\n", 98 | "odomModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2))\n", 99 | "loopModel = gtsam.noiseModel.Diagonal.Variances(vector6(0.5, 0.5, 0.5, 0.5, 0.5, 0.5))\n", 100 | "robustLoopModel = gtsam.noiseModel.Robust.Create(gtsam.noiseModel.mEstimator.Cauchy.Create(1), loopModel)\n", 101 | "\n", 102 | "\n", 103 | "# and helper functions \n", 104 | "def consecutive(node_idx_to, node_idx_from):\n", 105 | " return abs(node_idx_to - node_idx_from) == 1\n", 106 | "\n", 107 | "def robustifyNoiseModel(noise_model, use_fixed=False):\n", 108 | " if use_fixed:\n", 109 | " return robustLoopModel\n", 110 | " else: \n", 111 | " return gtsam.noiseModel.Robust.Create(gtsam.noiseModel.mEstimator.Cauchy.Create(1), noise_model)" 112 | ] 113 | }, 114 | { 115 | "cell_type": "code", 116 | "execution_count": 4, 117 | "id": "24f50b71-94c0-474d-8c02-07851fdfb8ca", 118 | "metadata": {}, 119 | "outputs": [], 120 | "source": [ 121 | "def optimize_pgo(in_file, out_file, add_random_false_loops=[False, 0], use_robust_loops=True, use_fixed_noise=True):\n", 122 | "\n", 123 | " # load an initial graph and reconstruct it \n", 124 | " is3D = True\n", 125 | " initial_graph, initial = gtsam.readG2o(in_file, is3D)\n", 126 | "\n", 127 | " graph = gtsam.NonlinearFactorGraph()\n", 128 | " for factor_idx in range(initial_graph.size()):\n", 129 | " factor = initial_graph.at(factor_idx)\n", 130 | "\n", 131 | " node_idx_from = factor.keys()[0]\n", 132 | " node_idx_to = factor.keys()[1]\n", 133 | "\n", 134 | " measurement = factor.measured()\n", 135 | " \n", 136 | " noise_model = factor.noiseModel()\n", 137 | " if use_fixed_noise:\n", 138 | " noise_model = odomModel \n", 139 | " \n", 140 | " if use_robust_loops and not consecutive(node_idx_to, node_idx_from):\n", 141 | " noise_model = robustifyNoiseModel(noise_model, use_fixed_noise)\n", 142 | "\n", 143 | " graph.add( gtsam.BetweenFactorPose3(node_idx_from, node_idx_to, measurement, noise_model) )\n", 144 | "\n", 145 | " # add noise loops \n", 146 | " if add_random_false_loops[0]:\n", 147 | " for ii in range(add_random_false_loops[1]):\n", 148 | " random_node_idx_from = np.random.randint(initial.size(), size=1)\n", 149 | " random_node_idx_to = np.random.randint(initial.size(), size=1)\n", 150 | " \n", 151 | " random_factor_idx = np.random.randint(initial_graph.size(), size=1)\n", 152 | " random_factor = graph.at(factor_idx)\n", 153 | " random_measurement = random_factor.measured()\n", 154 | "\n", 155 | " random_noise_model = random_factor.noiseModel()\n", 156 | " if use_fixed_noise:\n", 157 | " random_noise_model = odomModel \n", 158 | "\n", 159 | " if use_robust_loops and not consecutive(node_idx_to, node_idx_from):\n", 160 | " random_noise_model = robustifyNoiseModel(random_noise_model, use_fixed_noise)\n", 161 | "\n", 162 | " graph.add( gtsam.BetweenFactorPose3(random_node_idx_from, random_node_idx_to, random_measurement, random_noise_model) )\n", 163 | " \n", 164 | " # add prior factor to avoid gauge problem \n", 165 | " graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))\n", 166 | " \n", 167 | " # optimizer \n", 168 | " params = gtsam.LevenbergMarquardtParams()\n", 169 | " params.setVerbosity(\"Termination\") # this will show info about stopping conds\n", 170 | " optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)\n", 171 | "\n", 172 | " # run opt \n", 173 | " result = optimizer.optimize()\n", 174 | " print(\"Optimization complete\")\n", 175 | " print(\"initial error = \", graph.error(initial))\n", 176 | " print(\"final error = \", graph.error(result))\n", 177 | "\n", 178 | " # save the optimized result \n", 179 | " save_graph = gtsam.NonlinearFactorGraph()\n", 180 | " for factor_idx in range(graph.size()-1):\n", 181 | " factor = graph.at(factor_idx)\n", 182 | "\n", 183 | " node_idx_from = factor.keys()[0]\n", 184 | " node_idx_to = factor.keys()[1]\n", 185 | "\n", 186 | " measurement = factor.measured()\n", 187 | " noise_model = odomModel # because gtsam.writeG2o does not support to save robust factor, so this is a just, naive solution to save the node values.\n", 188 | " \n", 189 | " save_graph.add( gtsam.BetweenFactorPose3(node_idx_from, node_idx_to, measurement, noise_model) )\n", 190 | " \n", 191 | " print(\"Writing results to file: \", out_file)\n", 192 | " gtsam.writeG2o(save_graph, result, out_file)\n", 193 | " save_graph = read_g2o(out_file)\n", 194 | " write_xyz(save_graph, out_file + '.xyz')\n", 195 | " print (\"Done!\")\n", 196 | "\n" 197 | ] 198 | }, 199 | { 200 | "cell_type": "code", 201 | "execution_count": 7, 202 | "id": "48b1de69-dabc-4539-9991-52ade8c76462", 203 | "metadata": { 204 | "tags": [] 205 | }, 206 | "outputs": [ 207 | { 208 | "name": "stdout", 209 | "output_type": "stream", 210 | "text": [ 211 | "\n", 212 | "Robust version starts\n", 213 | "converged\n", 214 | "errorThreshold: 68.7457271009 ? 100\n", 218 | "Optimization complete\n", 219 | "initial error = 655.286328639572\n", 220 | "final error = 68.74572710087696\n", 221 | "Writing results to file: ./data/cubicle/robust.g2o\n", 222 | "Done!\n", 223 | "Elapsed time is 2.737819 seconds.\n", 224 | "\n", 225 | "Non-robust version starts\n", 226 | "converged\n", 227 | "errorThreshold: 13664.3463552 ? 100\n", 231 | "Optimization complete\n", 232 | "initial error = 3306626.4190220158\n", 233 | "final error = 13664.346355194586\n", 234 | "Writing results to file: ./data/cubicle/nonrobust.g2o\n", 235 | "Done!\n", 236 | "Elapsed time is 28.404976 seconds.\n" 237 | ] 238 | } 239 | ], 240 | "source": [ 241 | "# tutorial datasets (see https://lucacarlone.mit.edu/datasets/)\n", 242 | "data_dir = './data/'\n", 243 | "seq_names = ['cubicle', 'grid3D', 'parking-garage', 'rim', 'sphere_bignoise_vertex3', 'torus3D']\n", 244 | "\n", 245 | "# choose a sequence!\n", 246 | "seq_name = seq_names[0] # change this \n", 247 | "in_file = data_dir + seq_name + '.g2o'\n", 248 | "\n", 249 | "seq_save_dir = data_dir + seq_name\n", 250 | "os.makedirs(seq_save_dir, exist_ok=True)\n", 251 | "\n", 252 | "# save the initial one \n", 253 | "write_xyz(read_g2o(in_file), os.path.join(seq_save_dir, seq_name + '.xyz'))\n", 254 | "\n", 255 | "\"\"\"\n", 256 | " main experiments \n", 257 | "\"\"\"\n", 258 | "use_false_loops = True \n", 259 | "num_false_loops = 10\n", 260 | "\n", 261 | "t = TicToc()\n", 262 | "\n", 263 | "# Robust version\n", 264 | "t.tic()\n", 265 | "print('\\nRobust version starts')\n", 266 | "optimize_pgo(in_file=in_file,\n", 267 | " out_file=os.path.join(seq_save_dir, 'robust.g2o'), \n", 268 | " add_random_false_loops=[use_false_loops, num_false_loops],\n", 269 | " use_robust_loops=True,\n", 270 | " use_fixed_noise=True)\n", 271 | "t.toc()\n", 272 | "\n", 273 | "# Non-robust version\n", 274 | "t.tic()\n", 275 | "print('\\nNon-robust version starts')\n", 276 | "optimize_pgo(in_file=in_file,\n", 277 | " out_file=os.path.join(seq_save_dir, 'nonrobust.g2o'), \n", 278 | " add_random_false_loops=[use_false_loops, num_false_loops],\n", 279 | " use_robust_loops=False,\n", 280 | " use_fixed_noise=True)\n", 281 | "t.toc()\n" 282 | ] 283 | }, 284 | { 285 | "cell_type": "code", 286 | "execution_count": null, 287 | "id": "e31278cf-1e59-4594-86e3-7e359d8e9488", 288 | "metadata": {}, 289 | "outputs": [], 290 | "source": [] 291 | } 292 | ], 293 | "metadata": { 294 | "kernelspec": { 295 | "display_name": "Python 3 (ipykernel)", 296 | "language": "python", 297 | "name": "python3" 298 | }, 299 | "language_info": { 300 | "codemirror_mode": { 301 | "name": "ipython", 302 | "version": 3 303 | }, 304 | "file_extension": ".py", 305 | "mimetype": "text/x-python", 306 | "name": "python", 307 | "nbconvert_exporter": "python", 308 | "pygments_lexer": "ipython3", 309 | "version": "3.8.11" 310 | } 311 | }, 312 | "nbformat": 4, 313 | "nbformat_minor": 5 314 | } 315 | --------------------------------------------------------------------------------