├── .gitignore ├── README.md ├── eskf_localization ├── 1_2D │ ├── results │ │ ├── result_manif.txt │ │ └── result_symforce.stx │ └── se2_localization_symforce.ipynb └── README.md ├── nonlinear_icp ├── 1_nonlinear_icp_SE3 │ ├── data │ │ └── 251371071.pcd │ └── nonlinear_icp_SE3.ipynb ├── 2_nonlinear_icp_Sim3 │ ├── data │ │ ├── budda.pcd │ │ ├── bunny.pcd │ │ ├── car6.pcd │ │ └── dragon.pcd │ ├── nonlinear_icp_Sim3.ipynb │ └── results │ │ ├── iter1.png │ │ ├── iter2.png │ │ ├── iter3.png │ │ ├── iter4.png │ │ ├── iter5.png │ │ ├── iter6.png │ │ ├── iter7.png │ │ ├── iter8.png │ │ └── iter9.png ├── 3_nonlinear_icp_Sim3_robust │ ├── data │ │ ├── budda.pcd │ │ ├── bunny.pcd │ │ ├── car6.pcd │ │ └── dragon.pcd │ ├── nonlinear_icp_Sim3_outlier_robust.ipynb │ ├── readmd.txt │ └── results │ │ ├── nonrobust │ │ ├── 0_given.png │ │ ├── 1_outliers50percent.png │ │ ├── 2_five_iters.png │ │ ├── 3_outliers50percent.png │ │ ├── 4_five_iters_again.png │ │ └── 5_diverged.png │ │ └── robust │ │ ├── 0_given.png │ │ ├── 1_outliers50percent.png │ │ └── 2_five_iters.png └── todo.txt └── pgo └── 1_pgo3d_toy └── pose_graph_opt_3d_toy.ipynb /.gitignore: -------------------------------------------------------------------------------- 1 | *DS_Store 2 | *.ipynb_checkpoints 3 | _dev 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # symforce-tutorials 2 | Learn symforce together :) 3 | 4 | ## Contents 5 | 6 | ### Nonlinear ICP 7 | - Results: see https://youtu.be/FG6bi5bAbTY 8 | -------------------------------------------------------------------------------- /eskf_localization/1_2D/results/result_manif.txt: -------------------------------------------------------------------------------- 1 | -- step 0 --------------------------------------------- 2 | 3 | current X is 4 | trans is [0.151 0.08 ] 5 | x is 0.151 6 | y is 0.080 7 | angle is 0.110 8 | 9 | e (calc by manif): [ 1.829 -0.283] 10 | H (calc by manif): 11 | [[-1. 0. -0.283] 12 | [ 0. -1. -1.829]] 13 | (2, 3) 14 | 15 | current X is 16 | trans is [0.103 0.035] 17 | x is 0.103 18 | y is 0.035 19 | angle is 0.030 20 | 21 | e (calc by manif): [1.924 0.909] 22 | H (calc by manif): 23 | [[-1. 0. 0.909] 24 | [ 0. -1. -1.924]] 25 | (2, 3) 26 | 27 | current X is 28 | trans is [ 0.098 -0.008] 29 | x is 0.098 30 | y is -0.008 31 | angle is 0.053 32 | 33 | e (calc by manif): [ 1.847 -1.092] 34 | H (calc by manif): 35 | [[-1. -0. -1.092] 36 | [-0. -1. -1.847]] 37 | (2, 3) 38 | 39 | X simulated : [0.1 0. 0.05] 40 | X estimated : [ 0.095 -0.004 0.049] 41 | X unfilterd : [0.155 0.072 0.11 ] 42 | ------------------------------------------------------- 43 | 44 | -- step 1 --------------------------------------------- 45 | 46 | current X is 47 | trans is [0.225 0.091] 48 | x is 0.225 49 | y is 0.091 50 | angle is 0.152 51 | 52 | e (calc by manif): [ 1.741 -0.358] 53 | H (calc by manif): 54 | [[-1. -0. -0.358] 55 | [-0. -1. -1.741]] 56 | (2, 3) 57 | 58 | current X is 59 | trans is [0.203 0.045] 60 | x is 0.203 61 | y is 0.045 62 | angle is 0.075 63 | 64 | e (calc by manif): [1.863 0.818] 65 | H (calc by manif): 66 | [[-1. 0. 0.818] 67 | [ 0. -1. -1.863]] 68 | (2, 3) 69 | 70 | current X is 71 | trans is [0.197 0.009] 72 | x is 0.197 73 | y is 0.009 74 | angle is 0.098 75 | 76 | e (calc by manif): [ 1.696 -1.18 ] 77 | H (calc by manif): 78 | [[-1. -0. -1.18 ] 79 | [-0. -1. -1.696]] 80 | (2, 3) 81 | 82 | X simulated : [ 0.2 -0. 0.1] 83 | X estimated : [ 0.199 -0.006 0.1 ] 84 | X unfilterd : [0.293 0.15 0.213] 85 | ------------------------------------------------------- 86 | 87 | -- step 2 --------------------------------------------- 88 | 89 | current X is 90 | trans is [0.359 0.12 ] 91 | x is 0.359 92 | y is 0.120 93 | angle is 0.248 94 | 95 | e (calc by manif): [ 1.561 -0.519] 96 | H (calc by manif): 97 | [[-1. -0. -0.519] 98 | [-0. -1. -1.561]] 99 | (2, 3) 100 | 101 | current X is 102 | trans is [0.309 0.047] 103 | x is 0.309 104 | y is 0.047 105 | angle is 0.130 106 | 107 | e (calc by manif): [1.801 0.726] 108 | H (calc by manif): 109 | [[-1. 0. 0.726] 110 | [ 0. -1. -1.801]] 111 | (2, 3) 112 | 113 | current X is 114 | trans is [0.3 0.005] 115 | x is 0.300 116 | y is 0.005 117 | angle is 0.158 118 | 119 | e (calc by manif): [ 1.521 -1.259] 120 | H (calc by manif): 121 | [[-1. 0. -1.259] 122 | [ 0. -1. -1.521]] 123 | (2, 3) 124 | 125 | X simulated : [ 0.3 -0. 0.15] 126 | X estimated : [ 0.296 -0.006 0.151] 127 | X unfilterd : [0.472 0.235 0.361] 128 | ------------------------------------------------------- 129 | 130 | -- step 3 --------------------------------------------- 131 | 132 | current X is 133 | trans is [0.476 0.105] 134 | x is 0.476 135 | y is 0.105 136 | angle is 0.243 137 | 138 | e (calc by manif): [ 1.454 -0.469] 139 | H (calc by manif): 140 | [[-1. -0. -0.469] 141 | [ 0. -1. -1.454]] 142 | (2, 3) 143 | 144 | current X is 145 | trans is [0.403 0.064] 146 | x is 0.403 147 | y is 0.064 148 | angle is 0.178 149 | 150 | e (calc by manif): [1.738 0.638] 151 | H (calc by manif): 152 | [[-1. 0. 0.638] 153 | [-0. -1. -1.738]] 154 | (2, 3) 155 | 156 | current X is 157 | trans is [0.397 0.028] 158 | x is 0.397 159 | y is 0.028 160 | angle is 0.203 161 | 162 | e (calc by manif): [ 1.363 -1.33 ] 163 | H (calc by manif): 164 | [[-1. 0. -1.33 ] 165 | [-0. -1. -1.363]] 166 | (2, 3) 167 | 168 | X simulated : [ 0.4 -0. 0.2] 169 | X estimated : [ 0.399 -0.01 0.202] 170 | X unfilterd : [0.667 0.3 0.452] 171 | ------------------------------------------------------- 172 | 173 | -- step 4 --------------------------------------------- 174 | 175 | current X is 176 | trans is [0.535 0.133] 177 | x is 0.535 178 | y is 0.133 179 | angle is 0.346 180 | 181 | e (calc by manif): [ 1.333 -0.622] 182 | H (calc by manif): 183 | [[-1. -0. -0.622] 184 | [ 0. -1. -1.333]] 185 | (2, 3) 186 | 187 | current X is 188 | trans is [0.501 0.063] 189 | x is 0.501 190 | y is 0.063 191 | angle is 0.244 192 | 193 | e (calc by manif): [1.681 0.546] 194 | H (calc by manif): 195 | [[-1. 0. 0.546] 196 | [-0. -1. -1.681]] 197 | (2, 3) 198 | 199 | current X is 200 | trans is [0.498 0.047] 201 | x is 0.498 202 | y is 0.047 203 | angle is 0.255 204 | 205 | e (calc by manif): [ 1.189 -1.392] 206 | H (calc by manif): 207 | [[-1. 0. -1.392] 208 | [-0. -1. -1.189]] 209 | (2, 3) 210 | 211 | X simulated : [0.5 0. 0.25] 212 | X estimated : [ 0.5 -0.011 0.252] 213 | X unfilterd : [0.837 0.351 0.597] 214 | ------------------------------------------------------- 215 | 216 | -- step 5 --------------------------------------------- 217 | 218 | current X is 219 | trans is [0.65 0.119] 220 | x is 0.650 221 | y is 0.119 222 | angle is 0.315 223 | 224 | e (calc by manif): [ 1.247 -0.531] 225 | H (calc by manif): 226 | [[-1. -0. -0.531] 227 | [ 0. -1. -1.247]] 228 | (2, 3) 229 | 230 | current X is 231 | trans is [0.592 0.102] 232 | x is 0.592 233 | y is 0.102 234 | angle is 0.288 235 | 236 | e (calc by manif): [1.605 0.462] 237 | H (calc by manif): 238 | [[-1. 0. 0.462] 239 | [-0. -1. -1.605]] 240 | (2, 3) 241 | 242 | current X is 243 | trans is [0.589 0.082] 244 | x is 0.589 245 | y is 0.082 246 | angle is 0.302 247 | 248 | e (calc by manif): [ 1.026 -1.452] 249 | H (calc by manif): 250 | [[-1. 0. -1.452] 251 | [-0. -1. -1.026]] 252 | (2, 3) 253 | 254 | X simulated : [ 0.6 -0. 0.3] 255 | X estimated : [0.595 0. 0.297] 256 | X unfilterd : [1.007 0.396 0.66 ] 257 | ------------------------------------------------------- 258 | 259 | -- step 6 --------------------------------------------- 260 | 261 | current X is 262 | trans is [0.694 0.145] 263 | x is 0.694 264 | y is 0.145 265 | angle is 0.413 266 | 267 | e (calc by manif): [ 1.138 -0.657] 268 | H (calc by manif): 269 | [[-1. -0. -0.657] 270 | [ 0. -1. -1.138]] 271 | (2, 3) 272 | 273 | current X is 274 | trans is [0.687 0.104] 275 | x is 0.687 276 | y is 0.104 277 | angle is 0.359 278 | 279 | e (calc by manif): [1.544 0.377] 280 | H (calc by manif): 281 | [[-1. -0. 0.377] 282 | [-0. -1. -1.544]] 283 | (2, 3) 284 | 285 | current X is 286 | trans is [0.688 0.111] 287 | x is 0.688 288 | y is 0.111 289 | angle is 0.355 290 | 291 | e (calc by manif): [ 0.844 -1.498] 292 | H (calc by manif): 293 | [[-1. -0. -1.498] 294 | [-0. -1. -0.844]] 295 | (2, 3) 296 | 297 | X simulated : [ 0.7 -0. 0.35] 298 | X estimated : [ 0.699 -0.008 0.352] 299 | X unfilterd : [1.147 0.395 0.775] 300 | ------------------------------------------------------- 301 | 302 | -- step 7 --------------------------------------------- 303 | 304 | current X is 305 | trans is [0.784 0.176] 306 | x is 0.784 307 | y is 0.176 308 | angle is 0.439 309 | 310 | e (calc by manif): [ 1.026 -0.676] 311 | H (calc by manif): 312 | [[-1. 0. -0.676] 313 | [ 0. -1. -1.026]] 314 | (2, 3) 315 | 316 | current X is 317 | trans is [0.772 0.149] 318 | x is 0.772 319 | y is 0.149 320 | angle is 0.404 321 | 322 | e (calc by manif): [1.464 0.3 ] 323 | H (calc by manif): 324 | [[-1. -0. 0.3 ] 325 | [-0. -1. -1.464]] 326 | (2, 3) 327 | 328 | current X is 329 | trans is [0.773 0.152] 330 | x is 0.773 331 | y is 0.152 332 | angle is 0.402 333 | 334 | e (calc by manif): [ 0.679 -1.54 ] 335 | H (calc by manif): 336 | [[-1. 0. -1.54 ] 337 | [-0. -1. -0.679]] 338 | (2, 3) 339 | 340 | X simulated : [ 0.8 -0. 0.4] 341 | X estimated : [ 0.792 -0.004 0.4 ] 342 | X unfilterd : [1.271 0.41 0.862] 343 | ------------------------------------------------------- 344 | 345 | -- step 8 --------------------------------------------- 346 | 347 | current X is 348 | trans is [0.922 0.294] 349 | x is 0.922 350 | y is 0.294 351 | angle is 0.524 352 | 353 | e (calc by manif): [ 0.786 -0.794] 354 | H (calc by manif): 355 | [[-1. -0. -0.794] 356 | [ 0. -1. -0.786]] 357 | (2, 3) 358 | 359 | current X is 360 | trans is [0.884 0.216] 361 | x is 0.884 362 | y is 0.216 363 | angle is 0.430 364 | 365 | e (calc by manif): [1.342 0.247] 366 | H (calc by manif): 367 | [[-1. -0. 0.247] 368 | [-0. -1. -1.342]] 369 | (2, 3) 370 | 371 | current X is 372 | trans is [0.874 0.191] 373 | x is 0.874 374 | y is 0.191 375 | angle is 0.454 376 | 377 | e (calc by manif): [ 0.489 -1.564] 378 | H (calc by manif): 379 | [[-1. 0. -1.564] 380 | [-0. -1. -0.489]] 381 | (2, 3) 382 | 383 | X simulated : [ 0.9 -0. 0.45] 384 | X estimated : [ 0.901 -0.004 0.45 ] 385 | X unfilterd : [1.467 0.477 0.986] 386 | ------------------------------------------------------- 387 | 388 | -- step 9 --------------------------------------------- 389 | 390 | current X is 391 | trans is [0.992 0.27 ] 392 | x is 0.992 393 | y is 0.270 394 | angle is 0.569 395 | 396 | e (calc by manif): [ 0.703 -0.771] 397 | H (calc by manif): 398 | [[-1. -0. -0.771] 399 | [-0. -1. -0.703]] 400 | (2, 3) 401 | 402 | current X is 403 | trans is [0.955 0.224] 404 | x is 0.955 405 | y is 0.224 406 | angle is 0.514 407 | 408 | e (calc by manif): [1.291 0.162] 409 | H (calc by manif): 410 | [[-1. 0. 0.162] 411 | [-0. -1. -1.291]] 412 | (2, 3) 413 | 414 | current X is 415 | trans is [0.958 0.234] 416 | x is 0.958 417 | y is 0.234 418 | angle is 0.505 419 | 420 | e (calc by manif): [ 0.314 -1.584] 421 | H (calc by manif): 422 | [[-1. -0. -1.584] 423 | [-0. -1. -0.314]] 424 | (2, 3) 425 | 426 | X simulated : [1. 0. 0.5] 427 | X estimated : [ 0.996 -0.01 0.502] 428 | X unfilterd : [1.637 0.469 1.105] 429 | ------------------------------------------------------- 430 | 431 | -------------------------------------------------------------------------------- /eskf_localization/1_2D/results/result_symforce.stx: -------------------------------------------------------------------------------- 1 | -- step 0 --------------------------------------------- 2 | 3 | current X is 4 | trans is [0.151 0.08 ] 5 | x is 0.151 6 | y is 0.080 7 | angle is 0.110 8 | 9 | e (calc by symforce): [ 1.829 -0.283] 10 | H (calc by symforce): 11 | [[-0.994 -0.11 -0.283] 12 | [ 0.11 -0.994 -1.829]] 13 | (2, 3) 14 | 15 | current X is 16 | trans is [0.109 0.03 ] 17 | x is 0.109 18 | y is 0.030 19 | angle is 0.030 20 | 21 | e (calc by symforce): [1.919 0.914] 22 | H (calc by symforce): 23 | [[-1. -0.03 0.914] 24 | [ 0.03 -1. -1.919]] 25 | (2, 3) 26 | 27 | current X is 28 | trans is [ 0.107 -0.024] 29 | x is 0.107 30 | y is -0.024 31 | angle is 0.061 32 | 33 | e (calc by symforce): [ 1.83 -1.088] 34 | H (calc by symforce): 35 | [[-0.998 -0.061 -1.088] 36 | [ 0.061 -0.998 -1.83 ]] 37 | (2, 3) 38 | 39 | X simulated : [0.1 0. 0.05] 40 | X estimated : [ 0.098 -0.004 0.048] 41 | X unfilterd : [0.155 0.072 0.11 ] 42 | ------------------------------------------------------- 43 | 44 | -- step 1 --------------------------------------------- 45 | 46 | current X is 47 | trans is [0.228 0.09 ] 48 | x is 0.228 49 | y is 0.090 50 | angle is 0.151 51 | 52 | e (calc by symforce): [ 1.739 -0.355] 53 | H (calc by symforce): 54 | [[-0.989 -0.15 -0.355] 55 | [ 0.15 -0.989 -1.739]] 56 | (2, 3) 57 | 58 | current X is 59 | trans is [0.21 0.042] 60 | x is 0.210 61 | y is 0.042 62 | angle is 0.074 63 | 64 | e (calc by symforce): [1.857 0.822] 65 | H (calc by symforce): 66 | [[-0.997 -0.074 0.822] 67 | [ 0.074 -0.997 -1.857]] 68 | (2, 3) 69 | 70 | current X is 71 | trans is [ 0.21 -0.008] 72 | x is 0.210 73 | y is -0.008 74 | angle is 0.106 75 | 76 | e (calc by symforce): [ 1.674 -1.176] 77 | H (calc by symforce): 78 | [[-0.994 -0.106 -1.176] 79 | [ 0.106 -0.994 -1.674]] 80 | (2, 3) 81 | 82 | X simulated : [ 0.2 -0. 0.1] 83 | X estimated : [ 0.204 -0.005 0.098] 84 | X unfilterd : [0.293 0.15 0.213] 85 | ------------------------------------------------------- 86 | 87 | -- step 2 --------------------------------------------- 88 | 89 | current X is 90 | trans is [0.364 0.121] 91 | x is 0.364 92 | y is 0.121 93 | angle is 0.246 94 | 95 | e (calc by symforce): [ 1.557 -0.516] 96 | H (calc by symforce): 97 | [[-0.97 -0.244 -0.516] 98 | [ 0.244 -0.97 -1.557]] 99 | (2, 3) 100 | 101 | current X is 102 | trans is [0.328 0.037] 103 | x is 0.328 104 | y is 0.037 105 | angle is 0.129 106 | 107 | e (calc by symforce): [1.782 0.74 ] 108 | H (calc by symforce): 109 | [[-0.992 -0.129 0.74 ] 110 | [ 0.129 -0.992 -1.782]] 111 | (2, 3) 112 | 113 | current X is 114 | trans is [ 0.332 -0.036] 115 | x is 0.332 116 | y is -0.036 117 | angle is 0.179 118 | 119 | e (calc by symforce): [ 1.47 -1.246] 120 | H (calc by symforce): 121 | [[-0.984 -0.178 -1.246] 122 | [ 0.178 -0.984 -1.47 ]] 123 | (2, 3) 124 | 125 | X simulated : [ 0.3 -0. 0.15] 126 | X estimated : [ 0.302 -0.006 0.146] 127 | X unfilterd : [0.472 0.235 0.361] 128 | ------------------------------------------------------- 129 | 130 | -- step 3 --------------------------------------------- 131 | 132 | current X is 133 | trans is [0.483 0.104] 134 | x is 0.483 135 | y is 0.104 136 | angle is 0.238 137 | 138 | e (calc by symforce): [ 1.45 -0.459] 139 | H (calc by symforce): 140 | [[-0.972 -0.235 -0.459] 141 | [ 0.235 -0.972 -1.45 ]] 142 | (2, 3) 143 | 144 | current X is 145 | trans is [0.413 0.049] 146 | x is 0.413 147 | y is 0.049 148 | angle is 0.177 149 | 150 | e (calc by symforce): [1.729 0.658] 151 | H (calc by symforce): 152 | [[-0.984 -0.176 0.658] 153 | [ 0.176 -0.984 -1.729]] 154 | (2, 3) 155 | 156 | current X is 157 | trans is [0.418 0.001] 158 | x is 0.418 159 | y is 0.001 160 | angle is 0.215 161 | 162 | e (calc by symforce): [ 1.332 -1.315] 163 | H (calc by symforce): 164 | [[-0.977 -0.213 -1.315] 165 | [ 0.213 -0.977 -1.332]] 166 | (2, 3) 167 | 168 | X simulated : [ 0.4 -0. 0.2] 169 | X estimated : [ 0.403 -0.012 0.198] 170 | X unfilterd : [0.667 0.3 0.452] 171 | ------------------------------------------------------- 172 | 173 | -- step 4 --------------------------------------------- 174 | 175 | current X is 176 | trans is [0.54 0.13] 177 | x is 0.540 178 | y is 0.130 179 | angle is 0.342 180 | 181 | e (calc by symforce): [ 1.331 -0.612] 182 | H (calc by symforce): 183 | [[-0.942 -0.336 -0.612] 184 | [ 0.336 -0.942 -1.331]] 185 | (2, 3) 186 | 187 | current X is 188 | trans is [0.525 0.053] 189 | x is 0.525 190 | y is 0.053 191 | angle is 0.244 192 | 193 | e (calc by symforce): [1.66 0.562] 194 | H (calc by symforce): 195 | [[-0.97 -0.242 0.562] 196 | [ 0.242 -0.97 -1.66 ]] 197 | (2, 3) 198 | 199 | current X is 200 | trans is [0.531 0.009] 201 | x is 0.531 202 | y is 0.009 203 | angle is 0.278 204 | 205 | e (calc by symforce): [ 1.135 -1.373] 206 | H (calc by symforce): 207 | [[-0.962 -0.274 -1.373] 208 | [ 0.274 -0.962 -1.135]] 209 | (2, 3) 210 | 211 | X simulated : [0.5 0. 0.25] 212 | X estimated : [ 0.504 -0.013 0.247] 213 | X unfilterd : [0.837 0.351 0.597] 214 | ------------------------------------------------------- 215 | 216 | -- step 5 --------------------------------------------- 217 | 218 | current X is 219 | trans is [0.655 0.115] 220 | x is 0.655 221 | y is 0.115 222 | angle is 0.310 223 | 224 | e (calc by symforce): [ 1.246 -0.52 ] 225 | H (calc by symforce): 226 | [[-0.952 -0.305 -0.52 ] 227 | [ 0.305 -0.952 -1.246]] 228 | (2, 3) 229 | 230 | current X is 231 | trans is [0.598 0.084] 232 | x is 0.598 233 | y is 0.084 234 | angle is 0.287 235 | 236 | e (calc by symforce): [1.603 0.482] 237 | H (calc by symforce): 238 | [[-0.959 -0.283 0.482] 239 | [ 0.283 -0.959 -1.603]] 240 | (2, 3) 241 | 242 | current X is 243 | trans is [0.602 0.062] 244 | x is 0.602 245 | y is 0.062 246 | angle is 0.309 247 | 248 | e (calc by symforce): [ 1.008 -1.438] 249 | H (calc by symforce): 250 | [[-0.953 -0.304 -1.438] 251 | [ 0.304 -0.953 -1.008]] 252 | (2, 3) 253 | 254 | X simulated : [ 0.6 -0. 0.3] 255 | X estimated : [ 0.595 -0.005 0.295] 256 | X unfilterd : [1.007 0.396 0.66 ] 257 | ------------------------------------------------------- 258 | 259 | -- step 6 --------------------------------------------- 260 | 261 | current X is 262 | trans is [0.695 0.139] 263 | x is 0.695 264 | y is 0.139 265 | angle is 0.410 266 | 267 | e (calc by symforce): [ 1.142 -0.648] 268 | H (calc by symforce): 269 | [[-0.917 -0.399 -0.648] 270 | [ 0.399 -0.917 -1.142]] 271 | (2, 3) 272 | 273 | current X is 274 | trans is [0.702 0.101] 275 | x is 0.702 276 | y is 0.101 277 | angle is 0.361 278 | 279 | e (calc by symforce): [1.532 0.382] 280 | H (calc by symforce): 281 | [[-0.936 -0.353 0.382] 282 | [ 0.353 -0.936 -1.532]] 283 | (2, 3) 284 | 285 | current X is 286 | trans is [0.704 0.093] 287 | x is 0.704 288 | y is 0.093 289 | angle is 0.369 290 | 291 | e (calc by symforce): [ 0.815 -1.487] 292 | H (calc by symforce): 293 | [[-0.933 -0.36 -1.487] 294 | [ 0.36 -0.933 -0.815]] 295 | (2, 3) 296 | 297 | X simulated : [ 0.7 -0. 0.35] 298 | X estimated : [ 0.699 -0.011 0.35 ] 299 | X unfilterd : [1.147 0.395 0.775] 300 | ------------------------------------------------------- 301 | 302 | -- step 7 --------------------------------------------- 303 | 304 | current X is 305 | trans is [0.784 0.172] 306 | x is 0.784 307 | y is 0.172 308 | angle is 0.437 309 | 310 | e (calc by symforce): [ 1.029 -0.671] 311 | H (calc by symforce): 312 | [[-0.906 -0.423 -0.671] 313 | [ 0.423 -0.906 -1.029]] 314 | (2, 3) 315 | 316 | current X is 317 | trans is [0.784 0.145] 318 | x is 0.784 319 | y is 0.145 320 | angle is 0.405 321 | 322 | e (calc by symforce): [1.455 0.307] 323 | H (calc by symforce): 324 | [[-0.919 -0.394 0.307] 325 | [ 0.394 -0.919 -1.455]] 326 | (2, 3) 327 | 328 | current X is 329 | trans is [0.785 0.137] 330 | x is 0.785 331 | y is 0.137 332 | angle is 0.413 333 | 334 | e (calc by symforce): [ 0.657 -1.529] 335 | H (calc by symforce): 336 | [[-0.916 -0.401 -1.529] 337 | [ 0.401 -0.916 -0.657]] 338 | (2, 3) 339 | 340 | X simulated : [ 0.8 -0. 0.4] 341 | X estimated : [ 0.792 -0.007 0.399] 342 | X unfilterd : [1.271 0.41 0.862] 343 | ------------------------------------------------------- 344 | 345 | -- step 8 --------------------------------------------- 346 | 347 | current X is 348 | trans is [0.923 0.291] 349 | x is 0.923 350 | y is 0.291 351 | angle is 0.523 352 | 353 | e (calc by symforce): [ 0.788 -0.79 ] 354 | H (calc by symforce): 355 | [[-0.866 -0.499 -0.79 ] 356 | [ 0.499 -0.866 -0.788]] 357 | (2, 3) 358 | 359 | current X is 360 | trans is [0.926 0.205] 361 | x is 0.926 362 | y is 0.205 363 | angle is 0.432 364 | 365 | e (calc by symforce): [1.308 0.273] 366 | H (calc by symforce): 367 | [[-0.908 -0.418 0.273] 368 | [ 0.418 -0.908 -1.308]] 369 | (2, 3) 370 | 371 | current X is 372 | trans is [0.936 0.141] 373 | x is 0.936 374 | y is 0.141 375 | angle is 0.494 376 | 377 | e (calc by symforce): [ 0.395 -1.509] 378 | H (calc by symforce): 379 | [[-0.88 -0.474 -1.509] 380 | [ 0.474 -0.88 -0.395]] 381 | (2, 3) 382 | 383 | X simulated : [ 0.9 -0. 0.45] 384 | X estimated : [ 0.91 -0.011 0.437] 385 | X unfilterd : [1.467 0.477 0.986] 386 | ------------------------------------------------------- 387 | 388 | -- step 9 --------------------------------------------- 389 | 390 | current X is 391 | trans is [1.005 0.258] 392 | x is 1.005 393 | y is 0.258 394 | angle is 0.556 395 | 396 | e (calc by symforce): [ 0.709 -0.744] 397 | H (calc by symforce): 398 | [[-0.849 -0.528 -0.744] 399 | [ 0.528 -0.849 -0.709]] 400 | (2, 3) 401 | 402 | current X is 403 | trans is [0.979 0.204] 404 | x is 0.979 405 | y is 0.204 406 | angle is 0.512 407 | 408 | e (calc by symforce): [1.28 0.194] 409 | H (calc by symforce): 410 | [[-0.872 -0.49 0.194] 411 | [ 0.49 -0.872 -1.28 ]] 412 | (2, 3) 413 | 414 | current X is 415 | trans is [0.977 0.2 ] 416 | x is 0.977 417 | y is 0.200 418 | angle is 0.527 419 | 420 | e (calc by symforce): [ 0.281 -1.552] 421 | H (calc by symforce): 422 | [[-0.865 -0.503 -1.552] 423 | [ 0.503 -0.865 -0.281]] 424 | (2, 3) 425 | 426 | X simulated : [1. 0. 0.5] 427 | X estimated : [ 0.988 -0.026 0.502] 428 | X unfilterd : [1.637 0.469 1.105] 429 | ------------------------------------------------------- 430 | 431 | -------------------------------------------------------------------------------- /eskf_localization/1_2D/se2_localization_symforce.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 181, 6 | "id": "5a7847d2", 7 | "metadata": {}, 8 | "outputs": [ 9 | { 10 | "name": "stdout", 11 | "output_type": "stream", 12 | "text": [ 13 | "symforce uses symengine as backend\n" 14 | ] 15 | } 16 | ], 17 | "source": [ 18 | "from manifpy import SE2, SE2Tangent\n", 19 | "\n", 20 | "import symforce\n", 21 | "import symforce.symbolic as sf\n", 22 | "from symforce.values import Values\n", 23 | "from symforce import ops\n", 24 | "from symforce.ops import StorageOps, GroupOps, LieGroupOps\n", 25 | "from symforce.notebook_util import display\n", 26 | "print(f\"symforce uses {symforce.get_symbolic_api()} as backend\")\n", 27 | "\n", 28 | "import numpy as np\n", 29 | "from numpy.linalg import inv\n", 30 | "np.set_printoptions(precision=3, suppress=True)\n", 31 | "\n", 32 | "# import pdb\n", 33 | "\n", 34 | "Vector = np.array\n", 35 | "\n", 36 | "def Covariance():\n", 37 | " return np.zeros((SE2.DoF, SE2.DoF))\n", 38 | "\n", 39 | "def Jacobian():\n", 40 | " return np.zeros((SE2.DoF, SE2.DoF))\n", 41 | "\n", 42 | "def disp_info(elm, name=''):\n", 43 | " print(\"=========INFO==========\")\n", 44 | " print(f\"The shape and equation of {name}:\")\n", 45 | " display(elm)\n", 46 | " display(elm.shape)\n", 47 | " print(\"=======================\\n\")\n" 48 | ] 49 | }, 50 | { 51 | "cell_type": "code", 52 | "execution_count": 192, 53 | "id": "503345c8", 54 | "metadata": { 55 | "scrolled": false 56 | }, 57 | "outputs": [ 58 | { 59 | "name": "stdout", 60 | "output_type": "stream", 61 | "text": [ 62 | "-- step 0 ---------------------------------------------\n", 63 | "\n", 64 | "current X is\n", 65 | " trans is [0.151 0.08 ]\n", 66 | " x is 0.151\n", 67 | " y is 0.080\n", 68 | " angle is 0.110\n", 69 | "\n", 70 | "e (calc by manif): [ 1.829 -0.283]\n", 71 | "H (calc by manif):\n", 72 | " [[-1. 0. -0.283]\n", 73 | " [ 0. -1. -1.829]]\n", 74 | "(2, 3)\n", 75 | "\n", 76 | "current X is\n", 77 | " trans is [0.103 0.035]\n", 78 | " x is 0.103\n", 79 | " y is 0.035\n", 80 | " angle is 0.030\n", 81 | "\n", 82 | "e (calc by manif): [1.924 0.909]\n", 83 | "H (calc by manif):\n", 84 | " [[-1. 0. 0.909]\n", 85 | " [ 0. -1. -1.924]]\n", 86 | "(2, 3)\n", 87 | "\n", 88 | "current X is\n", 89 | " trans is [ 0.098 -0.008]\n", 90 | " x is 0.098\n", 91 | " y is -0.008\n", 92 | " angle is 0.053\n", 93 | "\n", 94 | "e (calc by manif): [ 1.847 -1.092]\n", 95 | "H (calc by manif):\n", 96 | " [[-1. -0. -1.092]\n", 97 | " [-0. -1. -1.847]]\n", 98 | "(2, 3)\n", 99 | " \n", 100 | "X simulated : [0.1 0. 0.05]\n", 101 | "X estimated : [ 0.095 -0.004 0.049]\n", 102 | "X unfilterd : [0.155 0.072 0.11 ]\n", 103 | "-------------------------------------------------------\n", 104 | "\n", 105 | "-- step 1 ---------------------------------------------\n", 106 | "\n", 107 | "current X is\n", 108 | " trans is [0.225 0.091]\n", 109 | " x is 0.225\n", 110 | " y is 0.091\n", 111 | " angle is 0.152\n", 112 | "\n", 113 | "e (calc by manif): [ 1.741 -0.358]\n", 114 | "H (calc by manif):\n", 115 | " [[-1. -0. -0.358]\n", 116 | " [-0. -1. -1.741]]\n", 117 | "(2, 3)\n", 118 | "\n", 119 | "current X is\n", 120 | " trans is [0.203 0.045]\n", 121 | " x is 0.203\n", 122 | " y is 0.045\n", 123 | " angle is 0.075\n", 124 | "\n", 125 | "e (calc by manif): [1.863 0.818]\n", 126 | "H (calc by manif):\n", 127 | " [[-1. 0. 0.818]\n", 128 | " [ 0. -1. -1.863]]\n", 129 | "(2, 3)\n", 130 | "\n", 131 | "current X is\n", 132 | " trans is [0.197 0.009]\n", 133 | " x is 0.197\n", 134 | " y is 0.009\n", 135 | " angle is 0.098\n", 136 | "\n", 137 | "e (calc by manif): [ 1.696 -1.18 ]\n", 138 | "H (calc by manif):\n", 139 | " [[-1. -0. -1.18 ]\n", 140 | " [-0. -1. -1.696]]\n", 141 | "(2, 3)\n", 142 | " \n", 143 | "X simulated : [ 0.2 -0. 0.1]\n", 144 | "X estimated : [ 0.199 -0.006 0.1 ]\n", 145 | "X unfilterd : [0.293 0.15 0.213]\n", 146 | "-------------------------------------------------------\n", 147 | "\n", 148 | "-- step 2 ---------------------------------------------\n", 149 | "\n", 150 | "current X is\n", 151 | " trans is [0.359 0.12 ]\n", 152 | " x is 0.359\n", 153 | " y is 0.120\n", 154 | " angle is 0.248\n", 155 | "\n", 156 | "e (calc by manif): [ 1.561 -0.519]\n", 157 | "H (calc by manif):\n", 158 | " [[-1. -0. -0.519]\n", 159 | " [-0. -1. -1.561]]\n", 160 | "(2, 3)\n", 161 | "\n", 162 | "current X is\n", 163 | " trans is [0.309 0.047]\n", 164 | " x is 0.309\n", 165 | " y is 0.047\n", 166 | " angle is 0.130\n", 167 | "\n", 168 | "e (calc by manif): [1.801 0.726]\n", 169 | "H (calc by manif):\n", 170 | " [[-1. 0. 0.726]\n", 171 | " [ 0. -1. -1.801]]\n", 172 | "(2, 3)\n", 173 | "\n", 174 | "current X is\n", 175 | " trans is [0.3 0.005]\n", 176 | " x is 0.300\n", 177 | " y is 0.005\n", 178 | " angle is 0.158\n", 179 | "\n", 180 | "e (calc by manif): [ 1.521 -1.259]\n", 181 | "H (calc by manif):\n", 182 | " [[-1. 0. -1.259]\n", 183 | " [ 0. -1. -1.521]]\n", 184 | "(2, 3)\n", 185 | " \n", 186 | "X simulated : [ 0.3 -0. 0.15]\n", 187 | "X estimated : [ 0.296 -0.006 0.151]\n", 188 | "X unfilterd : [0.472 0.235 0.361]\n", 189 | "-------------------------------------------------------\n", 190 | "\n", 191 | "-- step 3 ---------------------------------------------\n", 192 | "\n", 193 | "current X is\n", 194 | " trans is [0.476 0.105]\n", 195 | " x is 0.476\n", 196 | " y is 0.105\n", 197 | " angle is 0.243\n", 198 | "\n", 199 | "e (calc by manif): [ 1.454 -0.469]\n", 200 | "H (calc by manif):\n", 201 | " [[-1. -0. -0.469]\n", 202 | " [ 0. -1. -1.454]]\n", 203 | "(2, 3)\n", 204 | "\n", 205 | "current X is\n", 206 | " trans is [0.403 0.064]\n", 207 | " x is 0.403\n", 208 | " y is 0.064\n", 209 | " angle is 0.178\n", 210 | "\n", 211 | "e (calc by manif): [1.738 0.638]\n", 212 | "H (calc by manif):\n", 213 | " [[-1. 0. 0.638]\n", 214 | " [-0. -1. -1.738]]\n", 215 | "(2, 3)\n", 216 | "\n", 217 | "current X is\n", 218 | " trans is [0.397 0.028]\n", 219 | " x is 0.397\n", 220 | " y is 0.028\n", 221 | " angle is 0.203\n", 222 | "\n", 223 | "e (calc by manif): [ 1.363 -1.33 ]\n", 224 | "H (calc by manif):\n", 225 | " [[-1. 0. -1.33 ]\n", 226 | " [-0. -1. -1.363]]\n", 227 | "(2, 3)\n", 228 | " \n", 229 | "X simulated : [ 0.4 -0. 0.2]\n", 230 | "X estimated : [ 0.399 -0.01 0.202]\n", 231 | "X unfilterd : [0.667 0.3 0.452]\n", 232 | "-------------------------------------------------------\n", 233 | "\n", 234 | "-- step 4 ---------------------------------------------\n", 235 | "\n", 236 | "current X is\n", 237 | " trans is [0.535 0.133]\n", 238 | " x is 0.535\n", 239 | " y is 0.133\n", 240 | " angle is 0.346\n", 241 | "\n", 242 | "e (calc by manif): [ 1.333 -0.622]\n", 243 | "H (calc by manif):\n", 244 | " [[-1. -0. -0.622]\n", 245 | " [ 0. -1. -1.333]]\n", 246 | "(2, 3)\n", 247 | "\n", 248 | "current X is\n", 249 | " trans is [0.501 0.063]\n", 250 | " x is 0.501\n", 251 | " y is 0.063\n", 252 | " angle is 0.244\n", 253 | "\n", 254 | "e (calc by manif): [1.681 0.546]\n", 255 | "H (calc by manif):\n", 256 | " [[-1. 0. 0.546]\n", 257 | " [-0. -1. -1.681]]\n", 258 | "(2, 3)\n", 259 | "\n", 260 | "current X is\n", 261 | " trans is [0.498 0.047]\n", 262 | " x is 0.498\n", 263 | " y is 0.047\n", 264 | " angle is 0.255\n", 265 | "\n", 266 | "e (calc by manif): [ 1.189 -1.392]\n", 267 | "H (calc by manif):\n", 268 | " [[-1. 0. -1.392]\n", 269 | " [-0. -1. -1.189]]\n", 270 | "(2, 3)\n", 271 | " \n", 272 | "X simulated : [0.5 0. 0.25]\n", 273 | "X estimated : [ 0.5 -0.011 0.252]\n", 274 | "X unfilterd : [0.837 0.351 0.597]\n", 275 | "-------------------------------------------------------\n", 276 | "\n", 277 | "-- step 5 ---------------------------------------------\n", 278 | "\n", 279 | "current X is\n", 280 | " trans is [0.65 0.119]\n", 281 | " x is 0.650\n", 282 | " y is 0.119\n", 283 | " angle is 0.315\n", 284 | "\n", 285 | "e (calc by manif): [ 1.247 -0.531]\n", 286 | "H (calc by manif):\n", 287 | " [[-1. -0. -0.531]\n", 288 | " [ 0. -1. -1.247]]\n", 289 | "(2, 3)\n", 290 | "\n", 291 | "current X is\n", 292 | " trans is [0.592 0.102]\n", 293 | " x is 0.592\n", 294 | " y is 0.102\n", 295 | " angle is 0.288\n", 296 | "\n", 297 | "e (calc by manif): [1.605 0.462]\n", 298 | "H (calc by manif):\n", 299 | " [[-1. 0. 0.462]\n", 300 | " [-0. -1. -1.605]]\n", 301 | "(2, 3)\n", 302 | "\n", 303 | "current X is\n", 304 | " trans is [0.589 0.082]\n", 305 | " x is 0.589\n", 306 | " y is 0.082\n", 307 | " angle is 0.302\n", 308 | "\n", 309 | "e (calc by manif): [ 1.026 -1.452]\n", 310 | "H (calc by manif):\n", 311 | " [[-1. 0. -1.452]\n", 312 | " [-0. -1. -1.026]]\n", 313 | "(2, 3)\n", 314 | " \n", 315 | "X simulated : [ 0.6 -0. 0.3]\n", 316 | "X estimated : [0.595 0. 0.297]\n", 317 | "X unfilterd : [1.007 0.396 0.66 ]\n", 318 | "-------------------------------------------------------\n", 319 | "\n", 320 | "-- step 6 ---------------------------------------------\n", 321 | "\n", 322 | "current X is\n", 323 | " trans is [0.694 0.145]\n", 324 | " x is 0.694\n", 325 | " y is 0.145\n", 326 | " angle is 0.413\n", 327 | "\n", 328 | "e (calc by manif): [ 1.138 -0.657]\n", 329 | "H (calc by manif):\n", 330 | " [[-1. -0. -0.657]\n", 331 | " [ 0. -1. -1.138]]\n", 332 | "(2, 3)\n", 333 | "\n", 334 | "current X is\n", 335 | " trans is [0.687 0.104]\n", 336 | " x is 0.687\n", 337 | " y is 0.104\n", 338 | " angle is 0.359\n", 339 | "\n", 340 | "e (calc by manif): [1.544 0.377]\n", 341 | "H (calc by manif):\n", 342 | " [[-1. -0. 0.377]\n", 343 | " [-0. -1. -1.544]]\n", 344 | "(2, 3)\n", 345 | "\n", 346 | "current X is\n", 347 | " trans is [0.688 0.111]\n", 348 | " x is 0.688\n", 349 | " y is 0.111\n", 350 | " angle is 0.355\n", 351 | "\n", 352 | "e (calc by manif): [ 0.844 -1.498]\n", 353 | "H (calc by manif):\n", 354 | " [[-1. -0. -1.498]\n", 355 | " [-0. -1. -0.844]]\n", 356 | "(2, 3)\n", 357 | " \n", 358 | "X simulated : [ 0.7 -0. 0.35]\n", 359 | "X estimated : [ 0.699 -0.008 0.352]\n", 360 | "X unfilterd : [1.147 0.395 0.775]\n", 361 | "-------------------------------------------------------\n", 362 | "\n", 363 | "-- step 7 ---------------------------------------------\n", 364 | "\n", 365 | "current X is\n", 366 | " trans is [0.784 0.176]\n", 367 | " x is 0.784\n", 368 | " y is 0.176\n", 369 | " angle is 0.439\n", 370 | "\n", 371 | "e (calc by manif): [ 1.026 -0.676]\n", 372 | "H (calc by manif):\n", 373 | " [[-1. 0. -0.676]\n", 374 | " [ 0. -1. -1.026]]\n", 375 | "(2, 3)\n", 376 | "\n", 377 | "current X is\n", 378 | " trans is [0.772 0.149]\n", 379 | " x is 0.772\n", 380 | " y is 0.149\n", 381 | " angle is 0.404\n", 382 | "\n", 383 | "e (calc by manif): [1.464 0.3 ]\n", 384 | "H (calc by manif):\n", 385 | " [[-1. -0. 0.3 ]\n", 386 | " [-0. -1. -1.464]]\n", 387 | "(2, 3)\n", 388 | "\n", 389 | "current X is\n", 390 | " trans is [0.773 0.152]\n", 391 | " x is 0.773\n", 392 | " y is 0.152\n", 393 | " angle is 0.402\n", 394 | "\n", 395 | "e (calc by manif): [ 0.679 -1.54 ]\n", 396 | "H (calc by manif):\n", 397 | " [[-1. 0. -1.54 ]\n", 398 | " [-0. -1. -0.679]]\n", 399 | "(2, 3)\n", 400 | " \n", 401 | "X simulated : [ 0.8 -0. 0.4]\n", 402 | "X estimated : [ 0.792 -0.004 0.4 ]\n", 403 | "X unfilterd : [1.271 0.41 0.862]\n", 404 | "-------------------------------------------------------\n", 405 | "\n", 406 | "-- step 8 ---------------------------------------------\n", 407 | "\n", 408 | "current X is\n", 409 | " trans is [0.922 0.294]\n", 410 | " x is 0.922\n", 411 | " y is 0.294\n", 412 | " angle is 0.524\n", 413 | "\n", 414 | "e (calc by manif): [ 0.786 -0.794]\n", 415 | "H (calc by manif):\n", 416 | " [[-1. -0. -0.794]\n", 417 | " [ 0. -1. -0.786]]\n", 418 | "(2, 3)\n", 419 | "\n", 420 | "current X is\n", 421 | " trans is [0.884 0.216]\n", 422 | " x is 0.884\n", 423 | " y is 0.216\n", 424 | " angle is 0.430\n", 425 | "\n", 426 | "e (calc by manif): [1.342 0.247]\n", 427 | "H (calc by manif):\n", 428 | " [[-1. -0. 0.247]\n", 429 | " [-0. -1. -1.342]]\n", 430 | "(2, 3)\n", 431 | "\n", 432 | "current X is\n", 433 | " trans is [0.874 0.191]\n", 434 | " x is 0.874\n", 435 | " y is 0.191\n", 436 | " angle is 0.454\n", 437 | "\n", 438 | "e (calc by manif): [ 0.489 -1.564]\n", 439 | "H (calc by manif):\n", 440 | " [[-1. 0. -1.564]\n", 441 | " [-0. -1. -0.489]]\n", 442 | "(2, 3)\n", 443 | " \n", 444 | "X simulated : [ 0.9 -0. 0.45]\n", 445 | "X estimated : [ 0.901 -0.004 0.45 ]\n", 446 | "X unfilterd : [1.467 0.477 0.986]\n", 447 | "-------------------------------------------------------\n", 448 | "\n", 449 | "-- step 9 ---------------------------------------------\n", 450 | "\n", 451 | "current X is\n", 452 | " trans is [0.992 0.27 ]\n", 453 | " x is 0.992\n", 454 | " y is 0.270\n", 455 | " angle is 0.569\n", 456 | "\n", 457 | "e (calc by manif): [ 0.703 -0.771]\n", 458 | "H (calc by manif):\n", 459 | " [[-1. -0. -0.771]\n", 460 | " [-0. -1. -0.703]]\n", 461 | "(2, 3)\n", 462 | "\n", 463 | "current X is\n", 464 | " trans is [0.955 0.224]\n", 465 | " x is 0.955\n", 466 | " y is 0.224\n", 467 | " angle is 0.514\n", 468 | "\n", 469 | "e (calc by manif): [1.291 0.162]\n", 470 | "H (calc by manif):\n", 471 | " [[-1. 0. 0.162]\n", 472 | " [-0. -1. -1.291]]\n", 473 | "(2, 3)\n", 474 | "\n", 475 | "current X is\n", 476 | " trans is [0.958 0.234]\n", 477 | " x is 0.958\n", 478 | " y is 0.234\n", 479 | " angle is 0.505\n", 480 | "\n", 481 | "e (calc by manif): [ 0.314 -1.584]\n", 482 | "H (calc by manif):\n", 483 | " [[-1. -0. -1.584]\n", 484 | " [-0. -1. -0.314]]\n", 485 | "(2, 3)\n", 486 | " \n", 487 | "X simulated : [1. 0. 0.5]\n", 488 | "X estimated : [ 0.996 -0.01 0.502]\n", 489 | "X unfilterd : [1.637 0.469 1.105]\n", 490 | "-------------------------------------------------------\n", 491 | "\n" 492 | ] 493 | } 494 | ], 495 | "source": [ 496 | "################################\n", 497 | "# START CONFIGURATION\n", 498 | "\n", 499 | "NUMBER_OF_LMKS_TO_MEASURE = 3\n", 500 | "\n", 501 | "# Define the robot pose element and its covariance\n", 502 | "X_simulation = SE2.Identity()\n", 503 | "X = SE2.Identity()\n", 504 | "X_unfiltered = SE2.Identity()\n", 505 | "P = Covariance()\n", 506 | "\n", 507 | "u_nom = Vector([0.1, 0.0, 0.05])\n", 508 | "u_sigmas = Vector([0.1, 0.1, 0.1])\n", 509 | "U = np.diagflat(np.square(u_sigmas))\n", 510 | "\n", 511 | "# Declare the Jacobians of the motion wrt robot and control\n", 512 | "J_x = Jacobian()\n", 513 | "J_u = Jacobian()\n", 514 | "\n", 515 | "# Define five landmarks in R^2\n", 516 | "landmarks = []\n", 517 | "landmarks.append(Vector([2.0, 0.0]))\n", 518 | "landmarks.append(Vector([2.0, 1.0]))\n", 519 | "landmarks.append(Vector([2.0, -1.0]))\n", 520 | "\n", 521 | "# Define the beacon's measurements\n", 522 | "measurements = [Vector([0, 0])] * NUMBER_OF_LMKS_TO_MEASURE\n", 523 | "\n", 524 | "y_sigmas = Vector([0.01, 0.01])\n", 525 | "R = np.diagflat(np.square(y_sigmas))\n", 526 | "\n", 527 | "# Declare some temporaries\n", 528 | "J_xi_x = Jacobian()\n", 529 | "J_e_xi = np.zeros((SE2.Dim, SE2.DoF))\n", 530 | "\n", 531 | "\n", 532 | "##############################\n", 533 | "# EXPERIEMENTS\n", 534 | "\n", 535 | "# vis_latex = True \n", 536 | "vis_latex = False \n", 537 | "\n", 538 | "# fix randomness of the noise below\n", 539 | "np.random.seed(0)\n", 540 | " \n", 541 | "# Experiment Summary: Make 10 steps. Measure up to three landmarks each time.\n", 542 | "for t in range(10):\n", 543 | " print(f'-- step {t} ---------------------------------------------')\n", 544 | "\n", 545 | " ###########\n", 546 | " # I. Generation of Simulation data\n", 547 | " ###########\n", 548 | " \n", 549 | " # simulate noise\n", 550 | " u_noise = u_sigmas * np.random.rand(SE2.DoF) # control noise\n", 551 | " u_noisy = u_nom + u_noise # noisy control\n", 552 | "\n", 553 | " u_simu = SE2Tangent(u_nom)\n", 554 | " u_est = SE2Tangent(u_noisy)\n", 555 | " u_unfilt = SE2Tangent(u_noisy)\n", 556 | "\n", 557 | " # first we move\n", 558 | " X_simulation = X_simulation + u_simu # overloaded X.rplus(u) = X * exp(u)\n", 559 | "\n", 560 | " # then we measure all landmarks\n", 561 | " for i in range(NUMBER_OF_LMKS_TO_MEASURE):\n", 562 | " b = landmarks[i] # lmk coordinates in world frame\n", 563 | "\n", 564 | " # simulate noise\n", 565 | " y_noise = y_sigmas * np.random.rand(SE2.Dim) # measurement noise\n", 566 | "\n", 567 | " y = X_simulation.inverse().act(b) # landmark measurement, before adding noise\n", 568 | "\n", 569 | " y = y + y_noise # landmark measurement, noisy\n", 570 | " measurements[i] = y # store for the estimator just below\n", 571 | "\n", 572 | " ###########\n", 573 | " # II. Estimation\n", 574 | " ###########\n", 575 | "\n", 576 | " # II-a. First we move\n", 577 | " X = X.plus(u_est, J_x, J_u) # state propagation, X * exp(u), with Jacobians\n", 578 | " P = J_x @ P @ J_x.transpose() + J_u @ U @ J_u.transpose() # covariance propagation \n", 579 | "\n", 580 | " # Then we correct using the measurements of each landmarks\n", 581 | " for i in range(NUMBER_OF_LMKS_TO_MEASURE):\n", 582 | "\n", 583 | " b = landmarks[i] # lmk coordinates in world frame\n", 584 | " y = measurements[i] # lmk measurement, noisy \n", 585 | " print(f\"\\ncurrent X is\")\n", 586 | " print(f\" trans is {X.translation()}\")\n", 587 | " print(f\" x is {X.x():.3f}\")\n", 588 | " print(f\" y is {X.y():.3f}\")\n", 589 | " print(f\" angle is {X.angle():.3f}\")\n", 590 | " \n", 591 | " # mode selection \n", 592 | " use_manif, use_symforce = True, False\n", 593 | "# use_manif, use_symforce = False, True \n", 594 | " \n", 595 | " # expectation (way 1, the original: manif ver.)\n", 596 | " if use_manif:\n", 597 | " # exactly the original manif example code\n", 598 | " e = X.inverse(J_xi_x).act(b, J_e_xi) # note: e = R.tr * ( b - t ), for X = (R,t).\n", 599 | " H = J_e_xi @ J_xi_x # Jacobian of the measurements wrt the robot pose. note: H = J_e_x = J_e_xi * J_xi_x\n", 600 | " E = H @ P @ H.transpose()\n", 601 | " \n", 602 | " print(f\"\\ne (calc by manif): {e}\")\n", 603 | " print(f\"H (calc by manif):\\n {H}\")\n", 604 | " print(H.shape)\n", 605 | "\n", 606 | " # expectation (way 2, symforce ver., which is the equivalent) \n", 607 | " if use_symforce:\n", 608 | " # i. build a equation \n", 609 | " trans_vec = sf.V2.symbolic(\"t\")\n", 610 | " rot_ang = sf.V1.symbolic(\"theta\")\n", 611 | " rot_mat = LieGroupOps.from_tangent(sf.Rot2, rot_ang)\n", 612 | " landmark = sf.V2.symbolic(\"L\")\n", 613 | "\n", 614 | " error = rot_mat.inverse() * (landmark - trans_vec) # note: e = R.tr * ( b - t ), for X = (R,t).\n", 615 | "\n", 616 | " Je_trans = error.jacobian(trans_vec)\n", 617 | " Je_rot_ang = error.jacobian(rot_ang)\n", 618 | " # note that we don't need to explicitly care \n", 619 | " # the seperated elements for the chain rule (e.g., J_e_xi, J_xi_x)\n", 620 | " # because SymForce takes care of it internally. \n", 621 | " \n", 622 | " # ii. inject the values to the error model \n", 623 | " def inject_values(model):\n", 624 | " val = model.subs({ \\\n", 625 | " trans_vec: sf.V2(X.x(), X.y()),\n", 626 | " rot_ang: sf.V1(X.angle()), \n", 627 | " landmark: sf.V2(b)\n", 628 | " })\n", 629 | " return val.to_numpy()\n", 630 | "\n", 631 | " e = inject_values(error).transpose().squeeze()\n", 632 | " H_trans = inject_values(Je_trans) \n", 633 | " H_theta = inject_values(Je_rot_ang) \n", 634 | " H = np.hstack((H_trans, H_theta))\n", 635 | " E = H @ P @ H.transpose()\n", 636 | "\n", 637 | " print(f\"\\ne (calc by symforce): {e}\")\n", 638 | " print(f\"H (calc by symforce):\\n {H}\")\n", 639 | " print(H.shape)\n", 640 | "\n", 641 | " if vis_latex:\n", 642 | " disp_info(error, \"error\")\n", 643 | " disp_info(Je_trans, \"Je_trans\")\n", 644 | " disp_info(Je_rot_ang, \"Je_rot_ang\")\n", 645 | " vis_latex = False \n", 646 | "\n", 647 | " # innovation\n", 648 | " z = y - e\n", 649 | " Z = E + R\n", 650 | "\n", 651 | " # Kalman gain\n", 652 | " K = P @ H.transpose() @ inv(Z) # K = P * H.tr * ( H * P * H.tr + R).inv\n", 653 | " # TODO: stacking H (i.e., gathering all information for all landmarks, and update once)\n", 654 | " \n", 655 | " # Correction step\n", 656 | " dx = K @ z # dx is in the tangent space at X\n", 657 | "\n", 658 | " # II-b. Update\n", 659 | " X = X + SE2Tangent(dx) # overloaded X.rplus(dx) = X * exp(dx)\n", 660 | " P = P - K @ Z @ K.transpose()\n", 661 | "\n", 662 | "\n", 663 | " # III. Unfiltered (for comparison)\n", 664 | "\n", 665 | " # move also an unfiltered version for comparison purposes\n", 666 | " X_unfiltered = X_unfiltered + u_unfilt\n", 667 | "\n", 668 | " # IV. Results\n", 669 | "\n", 670 | " # DEBUG\n", 671 | " print(' ')\n", 672 | " print('X simulated : ', X_simulation.log().coeffs().transpose())\n", 673 | " print('X estimated : ', X.log().coeffs().transpose())\n", 674 | " print('X unfilterd : ', X_unfiltered.log().coeffs().transpose())\n", 675 | " print('-------------------------------------------------------\\n')\n", 676 | " # END DEBUG" 677 | ] 678 | } 679 | ], 680 | "metadata": { 681 | "kernelspec": { 682 | "display_name": "Python 3 (ipykernel)", 683 | "language": "python", 684 | "name": "python3" 685 | }, 686 | "language_info": { 687 | "codemirror_mode": { 688 | "name": "ipython", 689 | "version": 3 690 | }, 691 | "file_extension": ".py", 692 | "mimetype": "text/x-python", 693 | "name": "python", 694 | "nbconvert_exporter": "python", 695 | "pygments_lexer": "ipython3", 696 | "version": "3.8.10" 697 | } 698 | }, 699 | "nbformat": 4, 700 | "nbformat_minor": 5 701 | } 702 | -------------------------------------------------------------------------------- /eskf_localization/README.md: -------------------------------------------------------------------------------- 1 | # NOTE 2 | - The original codebase was borrowed from manif, see https://github.com/artivis/manif/tree/devel/examples 3 | -------------------------------------------------------------------------------- /nonlinear_icp/1_nonlinear_icp_SE3/data/251371071.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/1_nonlinear_icp_SE3/data/251371071.pcd -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/data/budda.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/data/budda.pcd -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/data/bunny.pcd: -------------------------------------------------------------------------------- 1 | # .PCD v.5 - Point Cloud Data file format 2 | VERSION .5 3 | FIELDS x y z 4 | SIZE 4 4 4 5 | TYPE F F F 6 | COUNT 1 1 1 7 | WIDTH 397 8 | HEIGHT 1 9 | POINTS 397 10 | DATA ascii 11 | 0.0054216 0.11349 0.040749 12 | -0.0017447 0.11425 0.041273 13 | -0.010661 0.11338 0.040916 14 | 0.026422 0.11499 0.032623 15 | 0.024545 0.12284 0.024255 16 | 0.034137 0.11316 0.02507 17 | 0.02886 0.11773 0.027037 18 | 0.02675 0.12234 0.017605 19 | 0.03575 0.1123 0.019109 20 | 0.015982 0.12307 0.031279 21 | 0.0079813 0.12438 0.032798 22 | 0.018101 0.11674 0.035493 23 | 0.0086687 0.11758 0.037538 24 | 0.01808 0.12536 0.026132 25 | 0.0080861 0.12866 0.02619 26 | 0.02275 0.12146 0.029671 27 | -0.0018689 0.12456 0.033184 28 | -0.011168 0.12376 0.032519 29 | -0.0020063 0.11937 0.038104 30 | -0.01232 0.11816 0.037427 31 | -0.0016659 0.12879 0.026782 32 | -0.011971 0.12723 0.026219 33 | 0.016484 0.12828 0.01928 34 | 0.0070921 0.13103 0.018415 35 | 0.0014615 0.13134 0.017095 36 | -0.013821 0.12886 0.019265 37 | -0.01725 0.11202 0.040077 38 | -0.074556 0.13415 0.051046 39 | -0.065971 0.14396 0.04109 40 | -0.071925 0.14545 0.043266 41 | -0.06551 0.13624 0.042195 42 | -0.071112 0.13767 0.047518 43 | -0.079528 0.13416 0.051194 44 | -0.080421 0.14428 0.042793 45 | -0.082672 0.1378 0.046806 46 | -0.08813 0.13514 0.042222 47 | -0.066325 0.12347 0.050729 48 | -0.072399 0.12662 0.052364 49 | -0.066091 0.11973 0.050881 50 | -0.072012 0.11811 0.052295 51 | -0.062433 0.12627 0.043831 52 | -0.068326 0.12998 0.048875 53 | -0.063094 0.11811 0.044399 54 | -0.071301 0.11322 0.04841 55 | -0.080515 0.12741 0.052034 56 | -0.078179 0.1191 0.051116 57 | -0.085216 0.12609 0.049001 58 | -0.089538 0.12621 0.044589 59 | -0.082659 0.11661 0.04797 60 | -0.089536 0.11784 0.04457 61 | -0.0565 0.15248 0.030132 62 | -0.055517 0.15313 0.026915 63 | -0.03625 0.17198 0.00017688 64 | -0.03775 0.17198 0.00022189 65 | -0.03625 0.16935 0.00051958 66 | -0.033176 0.15711 0.0018682 67 | -0.051913 0.1545 0.011273 68 | -0.041707 0.16642 0.0030522 69 | -0.049468 0.16414 0.0041988 70 | -0.041892 0.15669 0.0054879 71 | -0.051224 0.15878 0.0080283 72 | -0.062417 0.15317 0.033161 73 | -0.07167 0.15319 0.033701 74 | -0.062543 0.15524 0.027405 75 | -0.07211 0.1555 0.027645 76 | -0.078663 0.15269 0.032268 77 | -0.081569 0.15374 0.026085 78 | -0.08725 0.1523 0.022135 79 | -0.05725 0.15568 0.010325 80 | -0.057888 0.1575 0.0073225 81 | -0.0885 0.15223 0.019215 82 | -0.056129 0.14616 0.03085 83 | -0.054705 0.13555 0.032127 84 | -0.054144 0.14714 0.026275 85 | -0.046625 0.13234 0.021909 86 | -0.05139 0.13694 0.025787 87 | -0.018278 0.12238 0.030773 88 | -0.021656 0.11643 0.035209 89 | -0.031921 0.11566 0.032851 90 | -0.021348 0.12421 0.024562 91 | -0.03241 0.12349 0.023293 92 | -0.024869 0.12094 0.028745 93 | -0.031747 0.12039 0.028229 94 | -0.052912 0.12686 0.034968 95 | -0.041672 0.11564 0.032998 96 | -0.052037 0.1168 0.034582 97 | -0.042495 0.12488 0.024082 98 | -0.047946 0.12736 0.028108 99 | -0.042421 0.12035 0.028633 100 | -0.047661 0.12024 0.028871 101 | -0.035964 0.1513 0.0005395 102 | -0.050598 0.1474 0.013881 103 | -0.046375 0.13293 0.018289 104 | -0.049125 0.13856 0.016269 105 | -0.042976 0.14915 0.0054003 106 | -0.047965 0.14659 0.0086783 107 | -0.022926 0.1263 0.018077 108 | -0.031583 0.1259 0.017804 109 | -0.041733 0.12796 0.01665 110 | -0.061482 0.14698 0.036168 111 | -0.071729 0.15026 0.038328 112 | -0.060526 0.1368 0.035999 113 | -0.082619 0.14823 0.035955 114 | -0.087824 0.14449 0.033779 115 | -0.089 0.13828 0.037774 116 | -0.085662 0.15095 0.028208 117 | -0.089601 0.14725 0.025869 118 | -0.090681 0.13748 0.02369 119 | -0.058722 0.12924 0.038992 120 | -0.060075 0.11512 0.037685 121 | -0.091812 0.12767 0.038703 122 | -0.091727 0.11657 0.039619 123 | -0.093164 0.12721 0.025211 124 | -0.093938 0.12067 0.024399 125 | -0.091583 0.14522 0.01986 126 | -0.090929 0.13667 0.019817 127 | -0.093094 0.11635 0.018959 128 | 0.024948 0.10286 0.041418 129 | 0.0336 0.092627 0.040463 130 | 0.02742 0.096386 0.043312 131 | 0.03392 0.086911 0.041034 132 | 0.028156 0.086837 0.045084 133 | 0.03381 0.078604 0.040854 134 | 0.028125 0.076874 0.045059 135 | 0.0145 0.093279 0.05088 136 | 0.0074817 0.09473 0.052315 137 | 0.017407 0.10535 0.043139 138 | 0.0079536 0.10633 0.042968 139 | 0.018511 0.097194 0.047253 140 | 0.0086436 0.099323 0.048079 141 | -0.0020197 0.095698 0.053906 142 | -0.011446 0.095169 0.053862 143 | -0.001875 0.10691 0.043455 144 | -0.011875 0.10688 0.043019 145 | -0.0017622 0.10071 0.046648 146 | -0.012498 0.10008 0.045916 147 | 0.016381 0.085894 0.051642 148 | 0.0081167 0.08691 0.055228 149 | 0.017644 0.076955 0.052372 150 | 0.008125 0.076853 0.055536 151 | 0.020575 0.088169 0.049006 152 | 0.022445 0.075721 0.049563 153 | -0.0017931 0.086849 0.056843 154 | -0.011943 0.086771 0.057009 155 | -0.0019567 0.076863 0.057803 156 | -0.011875 0.076964 0.057022 157 | 0.03325 0.067541 0.040033 158 | 0.028149 0.066829 0.042953 159 | 0.026761 0.057829 0.042588 160 | 0.023571 0.04746 0.040428 161 | 0.015832 0.067418 0.051639 162 | 0.0080431 0.066902 0.055006 163 | 0.013984 0.058886 0.050416 164 | 0.0080973 0.056888 0.05295 165 | 0.020566 0.065958 0.0483 166 | 0.018594 0.056539 0.047879 167 | 0.012875 0.052652 0.049689 168 | -0.0017852 0.066712 0.056503 169 | -0.011785 0.066885 0.055015 170 | -0.001875 0.056597 0.05441 171 | -0.01184 0.057054 0.052714 172 | -0.015688 0.052469 0.049615 173 | 0.0066154 0.04993 0.051259 174 | 0.018088 0.046655 0.043321 175 | 0.008841 0.045437 0.046623 176 | 0.017688 0.039719 0.043084 177 | 0.008125 0.039516 0.045374 178 | -0.0016111 0.049844 0.05172 179 | -0.01245 0.046773 0.050903 180 | -0.013851 0.039778 0.051036 181 | -0.0020294 0.044874 0.047587 182 | -0.011653 0.04686 0.048661 183 | -0.0018611 0.039606 0.047339 184 | -0.0091545 0.03958 0.049415 185 | 0.043661 0.094028 0.02252 186 | 0.034642 0.10473 0.031831 187 | 0.028343 0.1072 0.036339 188 | 0.036339 0.096552 0.034843 189 | 0.031733 0.099372 0.038505 190 | 0.036998 0.10668 0.026781 191 | 0.032875 0.11108 0.02959 192 | 0.040938 0.097132 0.026663 193 | 0.044153 0.086466 0.024241 194 | 0.05375 0.072221 0.020429 195 | 0.04516 0.076574 0.023594 196 | 0.038036 0.086663 0.035459 197 | 0.037861 0.076625 0.035658 198 | 0.042216 0.087237 0.028254 199 | 0.042355 0.076747 0.02858 200 | 0.043875 0.096228 0.015269 201 | 0.044375 0.096797 0.0086445 202 | 0.039545 0.1061 0.017655 203 | 0.042313 0.10009 0.017237 204 | 0.045406 0.087417 0.015604 205 | 0.055118 0.072639 0.017944 206 | 0.048722 0.07376 0.017434 207 | 0.045917 0.086298 0.0094211 208 | 0.019433 0.1096 0.039063 209 | 0.01097 0.11058 0.039648 210 | 0.046657 0.057153 0.031337 211 | 0.056079 0.066335 0.024122 212 | 0.048168 0.06701 0.026298 213 | 0.056055 0.057253 0.024902 214 | 0.051163 0.056662 0.029137 215 | 0.036914 0.067032 0.036122 216 | 0.033 0.06472 0.039903 217 | 0.038004 0.056507 0.033119 218 | 0.030629 0.054915 0.038484 219 | 0.041875 0.066383 0.028357 220 | 0.041434 0.06088 0.029632 221 | 0.044921 0.049904 0.031243 222 | 0.054635 0.050167 0.022044 223 | 0.04828 0.04737 0.025845 224 | 0.037973 0.048347 0.031456 225 | 0.028053 0.047061 0.035991 226 | 0.025595 0.040346 0.03415 227 | 0.038455 0.043509 0.028278 228 | 0.032031 0.043278 0.029253 229 | 0.036581 0.040335 0.025144 230 | 0.03019 0.039321 0.026847 231 | 0.059333 0.067891 0.017361 232 | 0.0465 0.071452 0.01971 233 | 0.059562 0.057747 0.01834 234 | 0.055636 0.049199 0.019173 235 | 0.0505 0.045064 0.019181 236 | 0.023 0.047803 0.039776 237 | 0.022389 0.03886 0.038795 238 | -0.019545 0.0939 0.052205 239 | -0.021462 0.10618 0.042059 240 | -0.031027 0.10395 0.041228 241 | -0.022521 0.097723 0.045194 242 | -0.031858 0.097026 0.043878 243 | -0.043262 0.10412 0.040891 244 | -0.052154 0.10404 0.040972 245 | -0.041875 0.096944 0.042424 246 | -0.051919 0.096967 0.043563 247 | -0.021489 0.086672 0.054767 248 | -0.027 0.083087 0.050284 249 | -0.02107 0.077249 0.054365 250 | -0.026011 0.089634 0.048981 251 | -0.031893 0.087035 0.044169 252 | -0.025625 0.074892 0.047102 253 | -0.03197 0.0769 0.042177 254 | -0.041824 0.086954 0.043295 255 | -0.051825 0.086844 0.044933 256 | -0.041918 0.076728 0.042564 257 | -0.051849 0.076877 0.042992 258 | -0.061339 0.10393 0.041164 259 | -0.072672 0.10976 0.044294 260 | -0.061784 0.096825 0.043327 261 | -0.070058 0.096203 0.041397 262 | -0.080439 0.11091 0.044343 263 | -0.061927 0.086724 0.04452 264 | -0.070344 0.087352 0.041908 265 | -0.06141 0.077489 0.042178 266 | -0.068579 0.080144 0.041024 267 | -0.019045 0.067732 0.052388 268 | -0.017742 0.058909 0.050809 269 | -0.023548 0.066382 0.045226 270 | -0.03399 0.067795 0.040929 271 | -0.02169 0.056549 0.045164 272 | -0.036111 0.060706 0.040407 273 | -0.041231 0.066951 0.041392 274 | -0.048588 0.070956 0.040357 275 | -0.0403 0.059465 0.040446 276 | -0.02192 0.044965 0.052258 277 | -0.029187 0.043585 0.051088 278 | -0.021919 0.039826 0.053521 279 | -0.030331 0.039749 0.052133 280 | -0.021998 0.049847 0.046725 281 | -0.031911 0.046848 0.045187 282 | -0.035276 0.039753 0.047529 283 | -0.042016 0.044823 0.041594 284 | -0.05194 0.044707 0.043498 285 | -0.041928 0.039327 0.043582 286 | -0.051857 0.039252 0.046212 287 | -0.059453 0.04424 0.042862 288 | -0.060765 0.039087 0.044363 289 | -0.024273 0.11038 0.039129 290 | -0.032379 0.10878 0.037952 291 | -0.041152 0.10853 0.037969 292 | -0.051698 0.10906 0.038258 293 | -0.062091 0.10877 0.038274 294 | -0.071655 0.10596 0.037516 295 | -0.074634 0.097746 0.038347 296 | -0.07912 0.10508 0.032308 297 | -0.080203 0.096758 0.033592 298 | -0.08378 0.10568 0.025985 299 | -0.087292 0.10314 0.020825 300 | -0.08521 0.097079 0.02781 301 | -0.088082 0.096456 0.022985 302 | -0.07516 0.08604 0.038816 303 | -0.064577 0.073455 0.03897 304 | -0.072279 0.076416 0.036413 305 | -0.076375 0.072563 0.02873 306 | -0.080031 0.087076 0.03429 307 | -0.078919 0.079371 0.032477 308 | -0.084834 0.086686 0.026974 309 | -0.087891 0.089233 0.022611 310 | -0.081048 0.077169 0.025829 311 | -0.086393 0.10784 0.018635 312 | -0.087672 0.10492 0.017264 313 | -0.089333 0.098483 0.01761 314 | -0.086375 0.083067 0.018607 315 | -0.089179 0.089186 0.018947 316 | -0.082879 0.076109 0.017794 317 | -0.0825 0.074674 0.0071175 318 | -0.026437 0.064141 0.039321 319 | -0.030035 0.06613 0.038942 320 | -0.026131 0.056531 0.038882 321 | -0.031664 0.056657 0.037742 322 | -0.045716 0.064541 0.039166 323 | -0.051959 0.066869 0.036733 324 | -0.042557 0.055545 0.039026 325 | -0.049406 0.056892 0.034344 326 | -0.0555 0.062391 0.029498 327 | -0.05375 0.058574 0.026313 328 | -0.03406 0.050137 0.038577 329 | -0.041741 0.04959 0.03929 330 | -0.050975 0.049435 0.036965 331 | -0.053 0.051065 0.029209 332 | -0.054145 0.054568 0.012257 333 | -0.055848 0.05417 0.0083272 334 | -0.054844 0.049295 0.011462 335 | -0.05615 0.050619 0.0092929 336 | -0.061451 0.068257 0.035376 337 | -0.069725 0.069958 0.032788 338 | -0.062823 0.063322 0.026886 339 | -0.071037 0.066787 0.025228 340 | -0.060857 0.060568 0.022643 341 | -0.067 0.061558 0.020109 342 | -0.0782 0.071279 0.021032 343 | -0.062116 0.045145 0.037802 344 | -0.065473 0.039513 0.037964 345 | -0.06725 0.03742 0.033413 346 | -0.072702 0.065008 0.018701 347 | -0.06145 0.059165 0.018731 348 | -0.0675 0.061479 0.019221 349 | -0.057411 0.054114 0.0038257 350 | -0.079222 0.070654 0.017735 351 | -0.062473 0.04468 0.01111 352 | -0.06725 0.042258 0.010414 353 | -0.066389 0.040515 0.01316 354 | -0.068359 0.038502 0.011958 355 | -0.061381 0.04748 0.007607 356 | -0.068559 0.043549 0.0081576 357 | -0.070929 0.03983 0.0085888 358 | -0.016625 0.18375 -0.019735 359 | -0.015198 0.17471 -0.018868 360 | -0.015944 0.16264 -0.0091037 361 | -0.015977 0.1607 -0.0088072 362 | -0.013251 0.16708 -0.015264 363 | -0.014292 0.16098 -0.011252 364 | -0.013986 0.184 -0.023739 365 | -0.011633 0.17699 -0.023349 366 | -0.0091029 0.16988 -0.021457 367 | -0.025562 0.18273 -0.0096247 368 | -0.02725 0.18254 -0.0094384 369 | -0.025736 0.17948 -0.0089653 370 | -0.031216 0.17589 -0.0051154 371 | -0.020399 0.1845 -0.014943 372 | -0.021339 0.17645 -0.014566 373 | -0.027125 0.17234 -0.010156 374 | -0.03939 0.1733 -0.0023575 375 | -0.022876 0.16406 -0.0078103 376 | -0.031597 0.16651 -0.0049292 377 | -0.0226 0.15912 -0.003799 378 | -0.030372 0.15767 -0.0012672 379 | -0.021158 0.16849 -0.012383 380 | -0.027 0.1712 -0.01022 381 | -0.041719 0.16813 -0.00074958 382 | -0.04825 0.16748 -0.00015191 383 | -0.03725 0.16147 -7.2628e-05 384 | -0.066429 0.15783 -0.0085673 385 | -0.071284 0.15839 -0.005998 386 | -0.065979 0.16288 -0.017792 387 | -0.071623 0.16384 -0.01576 388 | -0.066068 0.16051 -0.013567 389 | -0.073307 0.16049 -0.011832 390 | -0.077 0.16204 -0.019241 391 | -0.077179 0.15851 -0.01495 392 | -0.073691 0.17286 -0.037944 393 | -0.07755 0.17221 -0.039175 394 | -0.065921 0.16586 -0.025022 395 | -0.072095 0.16784 -0.024725 396 | -0.066 0.16808 -0.030916 397 | -0.073448 0.17051 -0.032045 398 | -0.07777 0.16434 -0.025938 399 | -0.077893 0.16039 -0.021299 400 | -0.078211 0.169 -0.034566 401 | -0.034667 0.15131 -0.00071029 402 | -0.066117 0.17353 -0.047453 403 | -0.071986 0.17612 -0.045384 404 | -0.06925 0.182 -0.055026 405 | -0.064992 0.17802 -0.054645 406 | -0.069935 0.17983 -0.051988 407 | -0.07793 0.17516 -0.0444 408 | -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/data/car6.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/data/car6.pcd -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/data/dragon.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/data/dragon.pcd -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter1.png -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter2.png -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter3.png -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter4.png -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter5.png -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter6.png -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter7.png -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter8.png -------------------------------------------------------------------------------- /nonlinear_icp/2_nonlinear_icp_Sim3/results/iter9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/2_nonlinear_icp_Sim3/results/iter9.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/data/budda.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/data/budda.pcd -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/data/bunny.pcd: -------------------------------------------------------------------------------- 1 | # .PCD v.5 - Point Cloud Data file format 2 | VERSION .5 3 | FIELDS x y z 4 | SIZE 4 4 4 5 | TYPE F F F 6 | COUNT 1 1 1 7 | WIDTH 397 8 | HEIGHT 1 9 | POINTS 397 10 | DATA ascii 11 | 0.0054216 0.11349 0.040749 12 | -0.0017447 0.11425 0.041273 13 | -0.010661 0.11338 0.040916 14 | 0.026422 0.11499 0.032623 15 | 0.024545 0.12284 0.024255 16 | 0.034137 0.11316 0.02507 17 | 0.02886 0.11773 0.027037 18 | 0.02675 0.12234 0.017605 19 | 0.03575 0.1123 0.019109 20 | 0.015982 0.12307 0.031279 21 | 0.0079813 0.12438 0.032798 22 | 0.018101 0.11674 0.035493 23 | 0.0086687 0.11758 0.037538 24 | 0.01808 0.12536 0.026132 25 | 0.0080861 0.12866 0.02619 26 | 0.02275 0.12146 0.029671 27 | -0.0018689 0.12456 0.033184 28 | -0.011168 0.12376 0.032519 29 | -0.0020063 0.11937 0.038104 30 | -0.01232 0.11816 0.037427 31 | -0.0016659 0.12879 0.026782 32 | -0.011971 0.12723 0.026219 33 | 0.016484 0.12828 0.01928 34 | 0.0070921 0.13103 0.018415 35 | 0.0014615 0.13134 0.017095 36 | -0.013821 0.12886 0.019265 37 | -0.01725 0.11202 0.040077 38 | -0.074556 0.13415 0.051046 39 | -0.065971 0.14396 0.04109 40 | -0.071925 0.14545 0.043266 41 | -0.06551 0.13624 0.042195 42 | -0.071112 0.13767 0.047518 43 | -0.079528 0.13416 0.051194 44 | -0.080421 0.14428 0.042793 45 | -0.082672 0.1378 0.046806 46 | -0.08813 0.13514 0.042222 47 | -0.066325 0.12347 0.050729 48 | -0.072399 0.12662 0.052364 49 | -0.066091 0.11973 0.050881 50 | -0.072012 0.11811 0.052295 51 | -0.062433 0.12627 0.043831 52 | -0.068326 0.12998 0.048875 53 | -0.063094 0.11811 0.044399 54 | -0.071301 0.11322 0.04841 55 | -0.080515 0.12741 0.052034 56 | -0.078179 0.1191 0.051116 57 | -0.085216 0.12609 0.049001 58 | -0.089538 0.12621 0.044589 59 | -0.082659 0.11661 0.04797 60 | -0.089536 0.11784 0.04457 61 | -0.0565 0.15248 0.030132 62 | -0.055517 0.15313 0.026915 63 | -0.03625 0.17198 0.00017688 64 | -0.03775 0.17198 0.00022189 65 | -0.03625 0.16935 0.00051958 66 | -0.033176 0.15711 0.0018682 67 | -0.051913 0.1545 0.011273 68 | -0.041707 0.16642 0.0030522 69 | -0.049468 0.16414 0.0041988 70 | -0.041892 0.15669 0.0054879 71 | -0.051224 0.15878 0.0080283 72 | -0.062417 0.15317 0.033161 73 | -0.07167 0.15319 0.033701 74 | -0.062543 0.15524 0.027405 75 | -0.07211 0.1555 0.027645 76 | -0.078663 0.15269 0.032268 77 | -0.081569 0.15374 0.026085 78 | -0.08725 0.1523 0.022135 79 | -0.05725 0.15568 0.010325 80 | -0.057888 0.1575 0.0073225 81 | -0.0885 0.15223 0.019215 82 | -0.056129 0.14616 0.03085 83 | -0.054705 0.13555 0.032127 84 | -0.054144 0.14714 0.026275 85 | -0.046625 0.13234 0.021909 86 | -0.05139 0.13694 0.025787 87 | -0.018278 0.12238 0.030773 88 | -0.021656 0.11643 0.035209 89 | -0.031921 0.11566 0.032851 90 | -0.021348 0.12421 0.024562 91 | -0.03241 0.12349 0.023293 92 | -0.024869 0.12094 0.028745 93 | -0.031747 0.12039 0.028229 94 | -0.052912 0.12686 0.034968 95 | -0.041672 0.11564 0.032998 96 | -0.052037 0.1168 0.034582 97 | -0.042495 0.12488 0.024082 98 | -0.047946 0.12736 0.028108 99 | -0.042421 0.12035 0.028633 100 | -0.047661 0.12024 0.028871 101 | -0.035964 0.1513 0.0005395 102 | -0.050598 0.1474 0.013881 103 | -0.046375 0.13293 0.018289 104 | -0.049125 0.13856 0.016269 105 | -0.042976 0.14915 0.0054003 106 | -0.047965 0.14659 0.0086783 107 | -0.022926 0.1263 0.018077 108 | -0.031583 0.1259 0.017804 109 | -0.041733 0.12796 0.01665 110 | -0.061482 0.14698 0.036168 111 | -0.071729 0.15026 0.038328 112 | -0.060526 0.1368 0.035999 113 | -0.082619 0.14823 0.035955 114 | -0.087824 0.14449 0.033779 115 | -0.089 0.13828 0.037774 116 | -0.085662 0.15095 0.028208 117 | -0.089601 0.14725 0.025869 118 | -0.090681 0.13748 0.02369 119 | -0.058722 0.12924 0.038992 120 | -0.060075 0.11512 0.037685 121 | -0.091812 0.12767 0.038703 122 | -0.091727 0.11657 0.039619 123 | -0.093164 0.12721 0.025211 124 | -0.093938 0.12067 0.024399 125 | -0.091583 0.14522 0.01986 126 | -0.090929 0.13667 0.019817 127 | -0.093094 0.11635 0.018959 128 | 0.024948 0.10286 0.041418 129 | 0.0336 0.092627 0.040463 130 | 0.02742 0.096386 0.043312 131 | 0.03392 0.086911 0.041034 132 | 0.028156 0.086837 0.045084 133 | 0.03381 0.078604 0.040854 134 | 0.028125 0.076874 0.045059 135 | 0.0145 0.093279 0.05088 136 | 0.0074817 0.09473 0.052315 137 | 0.017407 0.10535 0.043139 138 | 0.0079536 0.10633 0.042968 139 | 0.018511 0.097194 0.047253 140 | 0.0086436 0.099323 0.048079 141 | -0.0020197 0.095698 0.053906 142 | -0.011446 0.095169 0.053862 143 | -0.001875 0.10691 0.043455 144 | -0.011875 0.10688 0.043019 145 | -0.0017622 0.10071 0.046648 146 | -0.012498 0.10008 0.045916 147 | 0.016381 0.085894 0.051642 148 | 0.0081167 0.08691 0.055228 149 | 0.017644 0.076955 0.052372 150 | 0.008125 0.076853 0.055536 151 | 0.020575 0.088169 0.049006 152 | 0.022445 0.075721 0.049563 153 | -0.0017931 0.086849 0.056843 154 | -0.011943 0.086771 0.057009 155 | -0.0019567 0.076863 0.057803 156 | -0.011875 0.076964 0.057022 157 | 0.03325 0.067541 0.040033 158 | 0.028149 0.066829 0.042953 159 | 0.026761 0.057829 0.042588 160 | 0.023571 0.04746 0.040428 161 | 0.015832 0.067418 0.051639 162 | 0.0080431 0.066902 0.055006 163 | 0.013984 0.058886 0.050416 164 | 0.0080973 0.056888 0.05295 165 | 0.020566 0.065958 0.0483 166 | 0.018594 0.056539 0.047879 167 | 0.012875 0.052652 0.049689 168 | -0.0017852 0.066712 0.056503 169 | -0.011785 0.066885 0.055015 170 | -0.001875 0.056597 0.05441 171 | -0.01184 0.057054 0.052714 172 | -0.015688 0.052469 0.049615 173 | 0.0066154 0.04993 0.051259 174 | 0.018088 0.046655 0.043321 175 | 0.008841 0.045437 0.046623 176 | 0.017688 0.039719 0.043084 177 | 0.008125 0.039516 0.045374 178 | -0.0016111 0.049844 0.05172 179 | -0.01245 0.046773 0.050903 180 | -0.013851 0.039778 0.051036 181 | -0.0020294 0.044874 0.047587 182 | -0.011653 0.04686 0.048661 183 | -0.0018611 0.039606 0.047339 184 | -0.0091545 0.03958 0.049415 185 | 0.043661 0.094028 0.02252 186 | 0.034642 0.10473 0.031831 187 | 0.028343 0.1072 0.036339 188 | 0.036339 0.096552 0.034843 189 | 0.031733 0.099372 0.038505 190 | 0.036998 0.10668 0.026781 191 | 0.032875 0.11108 0.02959 192 | 0.040938 0.097132 0.026663 193 | 0.044153 0.086466 0.024241 194 | 0.05375 0.072221 0.020429 195 | 0.04516 0.076574 0.023594 196 | 0.038036 0.086663 0.035459 197 | 0.037861 0.076625 0.035658 198 | 0.042216 0.087237 0.028254 199 | 0.042355 0.076747 0.02858 200 | 0.043875 0.096228 0.015269 201 | 0.044375 0.096797 0.0086445 202 | 0.039545 0.1061 0.017655 203 | 0.042313 0.10009 0.017237 204 | 0.045406 0.087417 0.015604 205 | 0.055118 0.072639 0.017944 206 | 0.048722 0.07376 0.017434 207 | 0.045917 0.086298 0.0094211 208 | 0.019433 0.1096 0.039063 209 | 0.01097 0.11058 0.039648 210 | 0.046657 0.057153 0.031337 211 | 0.056079 0.066335 0.024122 212 | 0.048168 0.06701 0.026298 213 | 0.056055 0.057253 0.024902 214 | 0.051163 0.056662 0.029137 215 | 0.036914 0.067032 0.036122 216 | 0.033 0.06472 0.039903 217 | 0.038004 0.056507 0.033119 218 | 0.030629 0.054915 0.038484 219 | 0.041875 0.066383 0.028357 220 | 0.041434 0.06088 0.029632 221 | 0.044921 0.049904 0.031243 222 | 0.054635 0.050167 0.022044 223 | 0.04828 0.04737 0.025845 224 | 0.037973 0.048347 0.031456 225 | 0.028053 0.047061 0.035991 226 | 0.025595 0.040346 0.03415 227 | 0.038455 0.043509 0.028278 228 | 0.032031 0.043278 0.029253 229 | 0.036581 0.040335 0.025144 230 | 0.03019 0.039321 0.026847 231 | 0.059333 0.067891 0.017361 232 | 0.0465 0.071452 0.01971 233 | 0.059562 0.057747 0.01834 234 | 0.055636 0.049199 0.019173 235 | 0.0505 0.045064 0.019181 236 | 0.023 0.047803 0.039776 237 | 0.022389 0.03886 0.038795 238 | -0.019545 0.0939 0.052205 239 | -0.021462 0.10618 0.042059 240 | -0.031027 0.10395 0.041228 241 | -0.022521 0.097723 0.045194 242 | -0.031858 0.097026 0.043878 243 | -0.043262 0.10412 0.040891 244 | -0.052154 0.10404 0.040972 245 | -0.041875 0.096944 0.042424 246 | -0.051919 0.096967 0.043563 247 | -0.021489 0.086672 0.054767 248 | -0.027 0.083087 0.050284 249 | -0.02107 0.077249 0.054365 250 | -0.026011 0.089634 0.048981 251 | -0.031893 0.087035 0.044169 252 | -0.025625 0.074892 0.047102 253 | -0.03197 0.0769 0.042177 254 | -0.041824 0.086954 0.043295 255 | -0.051825 0.086844 0.044933 256 | -0.041918 0.076728 0.042564 257 | -0.051849 0.076877 0.042992 258 | -0.061339 0.10393 0.041164 259 | -0.072672 0.10976 0.044294 260 | -0.061784 0.096825 0.043327 261 | -0.070058 0.096203 0.041397 262 | -0.080439 0.11091 0.044343 263 | -0.061927 0.086724 0.04452 264 | -0.070344 0.087352 0.041908 265 | -0.06141 0.077489 0.042178 266 | -0.068579 0.080144 0.041024 267 | -0.019045 0.067732 0.052388 268 | -0.017742 0.058909 0.050809 269 | -0.023548 0.066382 0.045226 270 | -0.03399 0.067795 0.040929 271 | -0.02169 0.056549 0.045164 272 | -0.036111 0.060706 0.040407 273 | -0.041231 0.066951 0.041392 274 | -0.048588 0.070956 0.040357 275 | -0.0403 0.059465 0.040446 276 | -0.02192 0.044965 0.052258 277 | -0.029187 0.043585 0.051088 278 | -0.021919 0.039826 0.053521 279 | -0.030331 0.039749 0.052133 280 | -0.021998 0.049847 0.046725 281 | -0.031911 0.046848 0.045187 282 | -0.035276 0.039753 0.047529 283 | -0.042016 0.044823 0.041594 284 | -0.05194 0.044707 0.043498 285 | -0.041928 0.039327 0.043582 286 | -0.051857 0.039252 0.046212 287 | -0.059453 0.04424 0.042862 288 | -0.060765 0.039087 0.044363 289 | -0.024273 0.11038 0.039129 290 | -0.032379 0.10878 0.037952 291 | -0.041152 0.10853 0.037969 292 | -0.051698 0.10906 0.038258 293 | -0.062091 0.10877 0.038274 294 | -0.071655 0.10596 0.037516 295 | -0.074634 0.097746 0.038347 296 | -0.07912 0.10508 0.032308 297 | -0.080203 0.096758 0.033592 298 | -0.08378 0.10568 0.025985 299 | -0.087292 0.10314 0.020825 300 | -0.08521 0.097079 0.02781 301 | -0.088082 0.096456 0.022985 302 | -0.07516 0.08604 0.038816 303 | -0.064577 0.073455 0.03897 304 | -0.072279 0.076416 0.036413 305 | -0.076375 0.072563 0.02873 306 | -0.080031 0.087076 0.03429 307 | -0.078919 0.079371 0.032477 308 | -0.084834 0.086686 0.026974 309 | -0.087891 0.089233 0.022611 310 | -0.081048 0.077169 0.025829 311 | -0.086393 0.10784 0.018635 312 | -0.087672 0.10492 0.017264 313 | -0.089333 0.098483 0.01761 314 | -0.086375 0.083067 0.018607 315 | -0.089179 0.089186 0.018947 316 | -0.082879 0.076109 0.017794 317 | -0.0825 0.074674 0.0071175 318 | -0.026437 0.064141 0.039321 319 | -0.030035 0.06613 0.038942 320 | -0.026131 0.056531 0.038882 321 | -0.031664 0.056657 0.037742 322 | -0.045716 0.064541 0.039166 323 | -0.051959 0.066869 0.036733 324 | -0.042557 0.055545 0.039026 325 | -0.049406 0.056892 0.034344 326 | -0.0555 0.062391 0.029498 327 | -0.05375 0.058574 0.026313 328 | -0.03406 0.050137 0.038577 329 | -0.041741 0.04959 0.03929 330 | -0.050975 0.049435 0.036965 331 | -0.053 0.051065 0.029209 332 | -0.054145 0.054568 0.012257 333 | -0.055848 0.05417 0.0083272 334 | -0.054844 0.049295 0.011462 335 | -0.05615 0.050619 0.0092929 336 | -0.061451 0.068257 0.035376 337 | -0.069725 0.069958 0.032788 338 | -0.062823 0.063322 0.026886 339 | -0.071037 0.066787 0.025228 340 | -0.060857 0.060568 0.022643 341 | -0.067 0.061558 0.020109 342 | -0.0782 0.071279 0.021032 343 | -0.062116 0.045145 0.037802 344 | -0.065473 0.039513 0.037964 345 | -0.06725 0.03742 0.033413 346 | -0.072702 0.065008 0.018701 347 | -0.06145 0.059165 0.018731 348 | -0.0675 0.061479 0.019221 349 | -0.057411 0.054114 0.0038257 350 | -0.079222 0.070654 0.017735 351 | -0.062473 0.04468 0.01111 352 | -0.06725 0.042258 0.010414 353 | -0.066389 0.040515 0.01316 354 | -0.068359 0.038502 0.011958 355 | -0.061381 0.04748 0.007607 356 | -0.068559 0.043549 0.0081576 357 | -0.070929 0.03983 0.0085888 358 | -0.016625 0.18375 -0.019735 359 | -0.015198 0.17471 -0.018868 360 | -0.015944 0.16264 -0.0091037 361 | -0.015977 0.1607 -0.0088072 362 | -0.013251 0.16708 -0.015264 363 | -0.014292 0.16098 -0.011252 364 | -0.013986 0.184 -0.023739 365 | -0.011633 0.17699 -0.023349 366 | -0.0091029 0.16988 -0.021457 367 | -0.025562 0.18273 -0.0096247 368 | -0.02725 0.18254 -0.0094384 369 | -0.025736 0.17948 -0.0089653 370 | -0.031216 0.17589 -0.0051154 371 | -0.020399 0.1845 -0.014943 372 | -0.021339 0.17645 -0.014566 373 | -0.027125 0.17234 -0.010156 374 | -0.03939 0.1733 -0.0023575 375 | -0.022876 0.16406 -0.0078103 376 | -0.031597 0.16651 -0.0049292 377 | -0.0226 0.15912 -0.003799 378 | -0.030372 0.15767 -0.0012672 379 | -0.021158 0.16849 -0.012383 380 | -0.027 0.1712 -0.01022 381 | -0.041719 0.16813 -0.00074958 382 | -0.04825 0.16748 -0.00015191 383 | -0.03725 0.16147 -7.2628e-05 384 | -0.066429 0.15783 -0.0085673 385 | -0.071284 0.15839 -0.005998 386 | -0.065979 0.16288 -0.017792 387 | -0.071623 0.16384 -0.01576 388 | -0.066068 0.16051 -0.013567 389 | -0.073307 0.16049 -0.011832 390 | -0.077 0.16204 -0.019241 391 | -0.077179 0.15851 -0.01495 392 | -0.073691 0.17286 -0.037944 393 | -0.07755 0.17221 -0.039175 394 | -0.065921 0.16586 -0.025022 395 | -0.072095 0.16784 -0.024725 396 | -0.066 0.16808 -0.030916 397 | -0.073448 0.17051 -0.032045 398 | -0.07777 0.16434 -0.025938 399 | -0.077893 0.16039 -0.021299 400 | -0.078211 0.169 -0.034566 401 | -0.034667 0.15131 -0.00071029 402 | -0.066117 0.17353 -0.047453 403 | -0.071986 0.17612 -0.045384 404 | -0.06925 0.182 -0.055026 405 | -0.064992 0.17802 -0.054645 406 | -0.069935 0.17983 -0.051988 407 | -0.07793 0.17516 -0.0444 408 | -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/data/car6.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/data/car6.pcd -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/data/dragon.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/data/dragon.pcd -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/nonlinear_icp_Sim3_outlier_robust.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 3, 6 | "id": "03f90f35", 7 | "metadata": {}, 8 | "outputs": [ 9 | { 10 | "name": "stdout", 11 | "output_type": "stream", 12 | "text": [ 13 | "symforce uses symengine as backend\n" 14 | ] 15 | } 16 | ], 17 | "source": [ 18 | "import os \n", 19 | "import copy \n", 20 | "import numpy as np\n", 21 | "np.set_printoptions(suppress=True, precision=4)\n", 22 | "\n", 23 | "from scipy.spatial.transform import Rotation as R\n", 24 | "\n", 25 | "import open3d as o3d\n", 26 | "\n", 27 | "import symforce \n", 28 | "# symforce.set_log_level(\"warning\")\n", 29 | "symforce.set_log_level(\"ERROR\")\n", 30 | "print(f\"symforce uses {symforce.get_symbolic_api()} as backend\")\n", 31 | "\n", 32 | "from symforce.notebook_util import display\n", 33 | "import symforce.symbolic as sf\n", 34 | "from symforce.values import Values\n", 35 | "from symforce import ops\n", 36 | "from symforce.ops import StorageOps, GroupOps, LieGroupOps\n", 37 | "\n", 38 | "import symforce.opt.noise_models as nm" 39 | ] 40 | }, 41 | { 42 | "cell_type": "code", 43 | "execution_count": 4, 44 | "id": "5a6ec069", 45 | "metadata": {}, 46 | "outputs": [], 47 | "source": [ 48 | "from time import time\n", 49 | " \n", 50 | "# ref: https://www.geeksforgeeks.org/timing-functions-with-decorators-python/\n", 51 | "disp_timecost = True \n", 52 | "def timer(func):\n", 53 | " # This function shows the execution time of \n", 54 | " # the function object passed\n", 55 | " def wrap_func(*args, **kwargs):\n", 56 | " t1 = time()\n", 57 | " result = func(*args, **kwargs)\n", 58 | " t2 = time()\n", 59 | " \n", 60 | " if disp_timecost:\n", 61 | " print(f'Function {func.__name__} executed in {(t2-t1):.4f}s')\n", 62 | "\n", 63 | " return result\n", 64 | " return wrap_func\n", 65 | "\n", 66 | "def np2o3d(nx3mat):\n", 67 | " pcd = o3d.geometry.PointCloud()\n", 68 | " pcd.points = o3d.utility.Vector3dVector(nx3mat)\n", 69 | " return pcd\n", 70 | "\n", 71 | "def to_o3dlineset(points, corres_idxes):\n", 72 | " if len(points) == 0:\n", 73 | " return None\n", 74 | " \n", 75 | " return o3d.geometry.LineSet(\n", 76 | " points=o3d.utility.Vector3dVector(np.array(points)),\n", 77 | " lines=o3d.utility.Vector2iVector(np.array(corres_idxes)),\n", 78 | " )\n", 79 | "\n", 80 | "def probabilistic_false_corres_idx(init_idx, num_max, outlier_ratio=0.5):\n", 81 | " if np.random.rand(1) > outlier_ratio:\n", 82 | " # return ture_correspondenceness, corres_idx\n", 83 | " return init_idx, True\n", 84 | " else:\n", 85 | " return int(np.random.randint(num_max, size=(1)).squeeze()), False # if want to use random false corres\n", 86 | "# return 1, False # if want to use fixed false corres \n" 87 | ] 88 | }, 89 | { 90 | "cell_type": "code", 91 | "execution_count": 10, 92 | "id": "7f528c55", 93 | "metadata": { 94 | "scrolled": false 95 | }, 96 | "outputs": [ 97 | { 98 | "name": "stdout", 99 | "output_type": "stream", 100 | "text": [ 101 | "error_val type is \n" 102 | ] 103 | } 104 | ], 105 | "source": [ 106 | "\n", 107 | "# Model parameters (as symbolic)\n", 108 | "scale = sf.V1.symbolic(\"s\")\n", 109 | "transvec = sf.V3.symbolic(\"t\")\n", 110 | "rotvec = sf.V3.symbolic(\"Theta\") # i.e., angle-axis parametrization\n", 111 | "rotmat = LieGroupOps.from_tangent(sf.Rot3, rotvec) # for debug, display(rotmat.to_rotation_matrix())\n", 112 | "\n", 113 | "# Redisual (loss function)\n", 114 | "# note: the rotation 'matrix' is used to formulate the below constraint, \n", 115 | "# but it was parametrized as a 3-dim vector 'rotvec'!\n", 116 | "p_src = sf.V3.symbolic(\"p_src\") # p means a single 3D point \n", 117 | "p_tgt = sf.V3.symbolic(\"p_tgt\") \n", 118 | "\n", 119 | "# p_tgt_est = (rotmat * p_src)*scale + transvec # bug. sometimes converges but sometimes weired behaviors \n", 120 | "p_tgt_est = ((rotmat * p_src) + transvec)*scale # The constraint: see the eq149 of https://ethaneade.com/lie.pdf\n", 121 | " # for the Sim(3) details, see\n", 122 | " # Scale Drift-Aware Large Scale Monocular SLAM (RSS 2020)\n", 123 | " # and see the eq149 of https://ethaneade.com/lie.pdf\n", 124 | "\n", 125 | "error_val = p_tgt - p_tgt_est\n", 126 | "print(f\"error_val type is {type(error_val)}\")\n", 127 | "\n", 128 | "def robust_loss(error_V3: sf.V3, robustness=True):\n", 129 | " \"\"\"\n", 130 | " see the class BarronNoiseModel(ScalarNoiseModel) definition in noise_models.py\n", 131 | " alpha: Controls shape and convexity of the loss function. Notable values:\n", 132 | " alpha = 2 -> L2 loss\n", 133 | " alpha = 1 -> Pseudo-huber loss\n", 134 | " alpha = 0 -> Cauchy loss\n", 135 | " alpha = -2 -> Geman-McClure loss\n", 136 | " alpha = -inf -> Welsch loss\n", 137 | " delta: Determines the transition point from quadratic to robust. Similar to \"delta\" as used\n", 138 | " by the pseudo-huber loss function.\n", 139 | " scalar_information: Scalar representing the inverse of the variance of an element of the\n", 140 | " unwhitened residual. Conceptually, we use \"scalar_information\" to whiten (in a\n", 141 | " probabalistic sense) the unwhitened residual before passing it through the Barron loss.\n", 142 | " x_epsilon: Small value used for handling the singularity at x == 0.\n", 143 | " alpha_epsilon: Small value used for handling singularities around alpha.\n", 144 | " \"\"\"\n", 145 | "\n", 146 | " if robustness:\n", 147 | " alpha = 0\n", 148 | " else:\n", 149 | " alpha = 2 \n", 150 | " # then, this L2 loss would have equal effect of when we use the \n", 151 | " # robustified_error = error_V3.compute_AtA() #non robust loss \n", 152 | "\n", 153 | " delta = 0.2\n", 154 | " scalar_information = 5.0\n", 155 | " epsilon = 1.0e-6\n", 156 | "\n", 157 | " noise_model = nm.BarronNoiseModel(\n", 158 | " alpha=alpha, delta=delta, scalar_information=scalar_information, x_epsilon=epsilon\n", 159 | " )\n", 160 | "\n", 161 | " robustified_error = sf.V1(noise_model.error(error_V3))\n", 162 | "\n", 163 | " return robustified_error\n", 164 | "\n", 165 | "\n", 166 | "# Core part of this tutorial \n", 167 | "robustness = True ### Try True and False yourself. \n", 168 | "error_model = robust_loss(error_val, robustness) \n", 169 | "\n", 170 | "# residual jacobian\n", 171 | "# this is the powerful moment of symforce. It automatically generate the Jacobian equations explicitly. \n", 172 | "Je_trans_model = error_model.jacobian(transvec)\n", 173 | "Je_rot_model = error_model.jacobian(rotvec)\n", 174 | "Je_scale_model = error_model.jacobian(scale)\n", 175 | "\n", 176 | "# residual debug \n", 177 | "is_vis_jacobians = False \n", 178 | "\n", 179 | "def disp_info(elm, name=''):\n", 180 | " print(\"=========INFO==========\")\n", 181 | " print(f\"The shape and equation of {name}:\")\n", 182 | " display(elm.shape)\n", 183 | " display(elm)\n", 184 | " print(\"=======================\\n\")\n", 185 | "\n", 186 | "if is_vis_jacobians:\n", 187 | " disp_info(error_model, 'error_model')\n", 188 | " disp_info(Je_rot_model, 'Je_rot')\n", 189 | " disp_info(Je_trans_model, 'Je_trans')\n", 190 | " disp_info(Je_scale_model, 'scale')\n" 191 | ] 192 | }, 193 | { 194 | "cell_type": "code", 195 | "execution_count": 15, 196 | "id": "ad576453", 197 | "metadata": { 198 | "scrolled": false 199 | }, 200 | "outputs": [], 201 | "source": [ 202 | "\n", 203 | "# Sim(3) optimization state dimension \n", 204 | "ndim_state = 7\n", 205 | "ndim_loss = 1\n", 206 | "\n", 207 | "# The nonlinear icp alg. \n", 208 | "def evaluate_error_and_jacobian(src_pt: np.ndarray, tag_pt: np.ndarray, tf):\n", 209 | " # note: transformation is 6dim vector on the tangent space (i.e., [rotvec, trans]) == lie algebra, aka se(3) (note that \"small\" se)\n", 210 | " se3 = tf[:6] # [rotvec3dim, trans3dim]\n", 211 | " s = tf[-1] # scale \n", 212 | " \n", 213 | " def inject_values(model):\n", 214 | " model_evaluated = \\\n", 215 | " model.subs({rotvec: sf.V3(se3[:3]), \\\n", 216 | " transvec: sf.V3(se3[3:]), \\\n", 217 | " scale: sf.V1(s), \\\n", 218 | " p_src: sf.V3(src_pt), \\\n", 219 | " p_tgt: sf.V3(tag_pt)})\n", 220 | " return model_evaluated.to_numpy()\n", 221 | " \n", 222 | " error, Je_rot, Je_trans, Je_scale = \\\n", 223 | " [inject_values(x) for x in [error_model, Je_rot_model, Je_trans_model, Je_scale_model]]\n", 224 | "\n", 225 | " return error, Je_rot, Je_trans, Je_scale\n", 226 | " \n", 227 | "@timer\n", 228 | "def icp_once(src, tgt, tf_init, skip=100, outlier_ratio=0.5, num_iters=30, fix_scale=False, verbose=False):\n", 229 | "\n", 230 | " for _iter in range(num_iters):\n", 231 | " num_pts = src.shape[0]\n", 232 | "\n", 233 | " correct_corres_points = []\n", 234 | " correct_corres_indexes = []\n", 235 | " false_corres_points = []\n", 236 | " false_corres_indexes = []\n", 237 | "\n", 238 | " H = np.zeros((ndim_state, ndim_state))\n", 239 | " b = np.zeros((ndim_state, 1))\n", 240 | "\n", 241 | " # 1. gathering measurements\n", 242 | " # these should be parallelized with only locking the H++ and b++ block. C++ would be a choice for this job.\n", 243 | " for pt_idx in range(num_pts):\n", 244 | "\n", 245 | " # if \"true\" correspondence is given (this is an tutorial for education purpose), \n", 246 | " # using a few points okay ..\n", 247 | " if pt_idx % (skip+_iter) != 0:\n", 248 | " continue # to save time cost, ealry return\n", 249 | "\n", 250 | " # Here, we directly use the true-known pair (because this is a tutorial for educational purpose :)\n", 251 | " # In practice, (src_pt, tgt_pt) should be a correspondence (e.g., found by FPFH local featuer, see https://pcl.readthedocs.io/projects/tutorials/en/latest/fpfh_estimation.html)\n", 252 | " src_corres_idx = pt_idx \n", 253 | " tgt_corres_idx, is_true_corres \\\n", 254 | " = probabilistic_false_corres_idx(pt_idx, num_pts-1, outlier_ratio=outlier_ratio) # true \n", 255 | "\n", 256 | " src_pt, tgt_pt = src[src_corres_idx, :], tgt[tgt_corres_idx, :]\n", 257 | " \n", 258 | " e, Je_rot, Je_trans, Je_scale \\\n", 259 | " = evaluate_error_and_jacobian(src_pt, tgt_pt, tf_init)\n", 260 | " # ps. To understand the details of this nonlinear iterative update steps, see http://www.diag.uniroma1.it//~labrococo/tutorial_icra_2016/icra16_slam_tutorial_grisetti.pdf \n", 261 | " # however, in the above slide's example, the jacobian was generated by hand as well as Euler angle space was used, not angle-axis.\n", 262 | "\n", 263 | " if fix_scale:\n", 264 | " e[-1] = 0.000001\n", 265 | " Je_scale = np.array([[1]])\n", 266 | "\n", 267 | " J = np.hstack((Je_rot, Je_trans, Je_scale)) \n", 268 | " # this is 1x7 \n", 269 | " # 1 is observation error model's output dimension \n", 270 | " # (in this tutorial, the error model is a norm of 3-dim error-state vector)\n", 271 | " # 7 is the state dimension\n", 272 | "\n", 273 | " sqrtW = np.array([[1., 1., 1., 1., 1., 1., 1.]]) # trying change yourself :) \n", 274 | " J = sqrtW * J # whitened J. (element-wise multiplication)\n", 275 | " # i.e., J.t@W@J == (sqrtW@J).t @ (sqrtW@J) == ||sqrtW@J||2\n", 276 | " # In this case, make lower the sensitivity of the scale term will be benefit for the convergnece. \n", 277 | " # This is an engineering. you should apply your physical prior or domain knowledge, or even empirically find it.\n", 278 | " e[:3] *= 1.0 # this is also empirically important. \n", 279 | " e[-1] *= 0.3 # this is also empirically important. \n", 280 | " # because translation changes along a few meters, but rotation and scales are lives in [0, 1]\n", 281 | " \n", 282 | " H = H + (J.T @ J) # H: 7x1 * 1x7 => thus H is 7x7\n", 283 | " b = b + (J.T @ e) # b: 7x1 * 1x1 => thus b is 7x1\n", 284 | " # to understatand the update eq, see https://darkpgmr.tistory.com/142\n", 285 | "\n", 286 | " if verbose:\n", 287 | " print(\"\\n=================\")\n", 288 | " print(f\"{pt_idx} error is\\n{e.T}\")\n", 289 | " print(f\"{pt_idx} Je_rot is\\n{Je_rot}\")\n", 290 | " print(f\"{pt_idx} Je_trans is\\n{Je_trans}\")\n", 291 | " print(f\"{pt_idx} J is\\n{J}\")\n", 292 | " print(f\"{pt_idx} H is\\n{H}\")\n", 293 | " print(f\"{pt_idx} b is\\n{b}\")\n", 294 | "\n", 295 | " # debug \n", 296 | " if is_true_corres:\n", 297 | " correct_corres_points.append(src_pt)\n", 298 | " correct_corres_points.append(tgt_pt)\n", 299 | " correct_corres_indexes.append([len(correct_corres_indexes)*2, len(correct_corres_indexes)*2+1])\n", 300 | " else:\n", 301 | " false_corres_points.append(src_pt)\n", 302 | " false_corres_points.append(tgt_pt)\n", 303 | " false_corres_indexes.append([len(false_corres_indexes)*2, len(false_corres_indexes)*2+1])\n", 304 | "\n", 305 | "\n", 306 | " # 2. update once \n", 307 | " dtf = -np.linalg.solve(H, b).squeeze() # note the step direction is minus\n", 308 | "\n", 309 | " \n", 310 | " # debug\n", 311 | " correct_corres_line_set = to_o3dlineset(correct_corres_points, correct_corres_indexes)\n", 312 | " false_corres_line_set = to_o3dlineset(false_corres_points, false_corres_indexes)\n", 313 | " line_sets = {\"correct\": correct_corres_line_set, \n", 314 | " \"false\": false_corres_line_set}\n", 315 | "\n", 316 | " # update \n", 317 | " strange_update_alram_thres = 100.0\n", 318 | " if np.linalg.norm(dtf) > strange_update_alram_thres:\n", 319 | " # strange_update_alram_thres is just arbitrarily selected because this is a toy problem \n", 320 | " # if a weired update is detected, do not apply it. \n", 321 | " print(f\"dtf norm: {np.linalg.norm(dtf):.3f} is weired. Thus reject to update.\")\n", 322 | " break\n", 323 | "\n", 324 | " tf_init = tf_init + dtf # updated within the tangent space\n", 325 | " print(f\"the estimated relative tf for iter {_iter} is {tf_init}\")\n", 326 | "\n", 327 | " # final result \n", 328 | " tf = tf_init\n", 329 | "\n", 330 | " return tf, line_sets\n" 331 | ] 332 | }, 333 | { 334 | "cell_type": "code", 335 | "execution_count": 16, 336 | "id": "07d5d96d", 337 | "metadata": {}, 338 | "outputs": [ 339 | { 340 | "name": "stdout", 341 | "output_type": "stream", 342 | "text": [ 343 | "PointCloud with 5205 points.\n", 344 | " The datset metric scale min [-1.0758 0.5284 -0.4984]\n", 345 | " The datset metric scale max [0.9524 1.9634 0.4083]\n", 346 | "\n", 347 | "true_rot_diff is\n", 348 | " [[-0.4226 -0.9063 0. ]\n", 349 | " [ 0.9063 -0.4226 0. ]\n", 350 | " [ 0. 0. 1. ]]\n", 351 | "true_rot_diff_vec is\n", 352 | " [0. 0. 2.0071]\n", 353 | "true_trans_diff is\n", 354 | " [-0.1335 0.15 0.05 ]\n", 355 | "true_scale_diff is\n", 356 | " 5.0\n" 357 | ] 358 | } 359 | ], 360 | "source": [ 361 | "# Data generatation \n", 362 | "# source \n", 363 | "dataset_name = \"dragon\"\n", 364 | "pcd0 = o3d.io.read_point_cloud(f'data/{dataset_name}.pcd')\n", 365 | "\n", 366 | "scale_up = 10\n", 367 | "pcd0_points_scaled_up = np.array(pcd0.points) * scale_up\n", 368 | "pcd0 = np2o3d(pcd0_points_scaled_up)\n", 369 | "print(pcd0)\n", 370 | "print(f\" The datset metric scale min {np.min(np.array(pcd0.points), 0)}\")\n", 371 | "print(f\" The datset metric scale max {np.max(np.array(pcd0.points), 0)}\")\n", 372 | "\n", 373 | "# generate target \n", 374 | "def rpy2mat(rpy, deg=True):\n", 375 | " return R.from_euler('xyz', rpy, degrees=deg).as_matrix()\n", 376 | "\n", 377 | "def rpy2vec(rpy, deg=True):\n", 378 | " return R.from_euler('xyz', rpy, degrees=deg).as_rotvec()\n", 379 | "\n", 380 | "true_rot_diff_rpy = np.array([0, 0, 115]) # deg \n", 381 | "true_rot_diff = rpy2mat(true_rot_diff_rpy)\n", 382 | "true_rot_diff_vec = rpy2vec(true_rot_diff_rpy)\n", 383 | "true_trans_diff = np.array([-0.1335, 0.15, 0.05]) * (0.1*scale_up)\n", 384 | "true_scale_diff = 5.0\n", 385 | "\n", 386 | "print(f\"\\ntrue_rot_diff is\\n {true_rot_diff}\")\n", 387 | "print(f\"true_rot_diff_vec is\\n {true_rot_diff_vec}\")\n", 388 | "print(f\"true_trans_diff is\\n {true_trans_diff}\")\n", 389 | "print(f\"true_scale_diff is\\n {true_scale_diff}\")\n", 390 | "\n", 391 | "pcd1 = o3d.geometry.PointCloud()\n", 392 | "pcd1_Sim3_applied = true_scale_diff*(true_rot_diff @ np.array(pcd0.points).transpose()) + np.expand_dims(true_trans_diff, axis=-1)\n", 393 | "pcd1.points = o3d.utility.Vector3dVector(pcd1_Sim3_applied.transpose())\n", 394 | " \n", 395 | "# At the very first status \n", 396 | "is_viz = 1\n", 397 | "if is_viz:\n", 398 | " pcd0.paint_uniform_color([1, 0, 1])\n", 399 | " pcd1.paint_uniform_color([0, 0, 1])\n", 400 | " o3d.visualization.draw_geometries([pcd0, pcd1], window_name=\"initial status\")\n" 401 | ] 402 | }, 403 | { 404 | "cell_type": "code", 405 | "execution_count": 17, 406 | "id": "496d33a9", 407 | "metadata": { 408 | "scrolled": false 409 | }, 410 | "outputs": [ 411 | { 412 | "name": "stdout", 413 | "output_type": "stream", 414 | "text": [ 415 | "init_guess is [0.0196 0.0451 2.0547 0.2301 1.106 0.296 3.5001]\n", 416 | "\n", 417 | "======================================\n", 418 | " ========= refine 0 ==========\n", 419 | "======================================\n", 420 | "the estimated relative tf for iter 0 is [0.0101 0.0342 2.0629 0.1255 1.0749 0.2846 3.2138]\n", 421 | "the estimated relative tf for iter 1 is [0.0032 0.0095 2.0576 0.0322 1.0187 0.2883 2.9983]\n", 422 | "the estimated relative tf for iter 2 is [-0.0236 -0.0151 2.0722 -0.0748 1.0073 0.297 2.8517]\n", 423 | "the estimated relative tf for iter 3 is [-0.0185 -0.0356 2.1111 -0.1711 1.0005 0.29 2.7422]\n", 424 | "the estimated relative tf for iter 4 is [-0.0168 -0.0592 2.1052 -0.2412 0.9633 0.2847 2.6451]\n", 425 | "the estimated relative tf for iter 5 is [-0.0367 -0.0788 2.1037 -0.2919 0.8899 0.2823 2.6088]\n", 426 | "the estimated relative tf for iter 6 is [-0.0415 -0.0932 2.1117 -0.3441 0.8583 0.2738 2.5769]\n", 427 | "the estimated relative tf for iter 7 is [-0.0565 -0.1082 2.1196 -0.3997 0.8274 0.2679 2.5491]\n", 428 | "the estimated relative tf for iter 8 is [-0.0796 -0.1221 2.1198 -0.445 0.8059 0.2672 2.5269]\n", 429 | "the estimated relative tf for iter 9 is [-0.0966 -0.1489 2.1423 -0.4708 0.7677 0.2761 2.5778]\n", 430 | "the estimated relative tf for iter 10 is [-0.0887 -0.1566 2.1487 -0.4899 0.739 0.2553 2.5948]\n", 431 | "the estimated relative tf for iter 11 is [-0.0789 -0.1479 2.1598 -0.4934 0.6897 0.2334 2.6638]\n", 432 | "the estimated relative tf for iter 12 is [-0.0887 -0.1468 2.1439 -0.4862 0.6289 0.226 2.7077]\n", 433 | "the estimated relative tf for iter 13 is [-0.0836 -0.1378 2.1305 -0.4677 0.5867 0.2007 2.7704]\n", 434 | "the estimated relative tf for iter 14 is [-0.0783 -0.1334 2.1298 -0.4744 0.5598 0.1841 2.7968]\n", 435 | "the estimated relative tf for iter 15 is [-0.085 -0.1275 2.1378 -0.4637 0.5358 0.172 2.8947]\n", 436 | "the estimated relative tf for iter 16 is [-0.0732 -0.1144 2.1397 -0.4497 0.5083 0.152 2.9676]\n", 437 | "the estimated relative tf for iter 17 is [-0.0671 -0.1143 2.1488 -0.4278 0.4761 0.1459 3.0753]\n", 438 | "the estimated relative tf for iter 18 is [-0.0606 -0.101 2.1292 -0.3915 0.4278 0.1301 3.1758]\n", 439 | "the estimated relative tf for iter 19 is [-0.0534 -0.0931 2.1224 -0.378 0.3997 0.1169 3.2039]\n", 440 | "Function icp_once executed in 7.2368s\n", 441 | "\n", 442 | "==========estimation==========\n", 443 | "delta rotation:\n", 444 | "[[-0.5254 -0.8475 -0.0756]\n", 445 | " [ 0.8508 -0.5234 -0.0454]\n", 446 | " [-0.001 -0.0882 0.9961]]\n", 447 | "delta translation:\n", 448 | "[-0.378 0.3997 0.1169]\n", 449 | "delta scale: 3.204\n", 450 | "\n", 451 | "======================================\n", 452 | " ========= refine 1 ==========\n", 453 | "======================================\n", 454 | "the estimated relative tf for iter 0 is [ 0.0023 0.0209 -0.0069 -0.0162 -0.0494 -0.0246 1.0125]\n", 455 | "the estimated relative tf for iter 1 is [-0.005 0.0258 -0.0114 -0.0722 -0.0699 -0.0479 1.0248]\n", 456 | "the estimated relative tf for iter 2 is [-0.0131 0.0321 -0.0186 -0.0878 -0.1021 -0.0695 1.0393]\n", 457 | "the estimated relative tf for iter 3 is [-0.0213 0.0373 -0.03 -0.1119 -0.1391 -0.0845 1.0539]\n", 458 | "the estimated relative tf for iter 4 is [-0.0285 0.0385 -0.0365 -0.1567 -0.1627 -0.0964 1.0607]\n", 459 | "the estimated relative tf for iter 5 is [-0.0359 0.0415 -0.0452 -0.1508 -0.195 -0.0979 1.0782]\n", 460 | "the estimated relative tf for iter 6 is [-0.0356 0.0471 -0.0541 -0.1603 -0.2201 -0.1098 1.0921]\n", 461 | "the estimated relative tf for iter 7 is [-0.0399 0.0516 -0.0475 -0.2337 -0.2004 -0.1288 1.1028]\n", 462 | "the estimated relative tf for iter 8 is [-0.0442 0.0526 -0.046 -0.254 -0.1931 -0.1353 1.1142]\n", 463 | "the estimated relative tf for iter 9 is [-0.0422 0.0549 -0.0515 -0.266 -0.22 -0.1384 1.1247]\n", 464 | "the estimated relative tf for iter 10 is [-0.0456 0.0575 -0.0577 -0.279 -0.2377 -0.1481 1.1327]\n", 465 | "the estimated relative tf for iter 11 is [-0.0451 0.0567 -0.0589 -0.2791 -0.242 -0.135 1.1456]\n", 466 | "the estimated relative tf for iter 12 is [-0.0441 0.055 -0.0601 -0.2617 -0.2483 -0.1124 1.1637]\n", 467 | "the estimated relative tf for iter 13 is [-0.044 0.0567 -0.066 -0.2595 -0.2647 -0.1135 1.1764]\n", 468 | "the estimated relative tf for iter 14 is [-0.0452 0.0552 -0.0648 -0.2616 -0.2729 -0.1086 1.188 ]\n", 469 | "the estimated relative tf for iter 15 is [-0.0476 0.0615 -0.0668 -0.2487 -0.2797 -0.1258 1.2032]\n", 470 | "the estimated relative tf for iter 16 is [-0.0448 0.0633 -0.0695 -0.2394 -0.2988 -0.1292 1.2172]\n", 471 | "the estimated relative tf for iter 17 is [-0.0479 0.0614 -0.0701 -0.22 -0.3137 -0.1245 1.2335]\n", 472 | "the estimated relative tf for iter 18 is [-0.048 0.064 -0.0736 -0.2043 -0.3267 -0.124 1.2463]\n", 473 | "the estimated relative tf for iter 19 is [-0.0469 0.07 -0.0828 -0.1773 -0.3492 -0.1309 1.2612]\n", 474 | "Function icp_once executed in 7.0600s\n", 475 | "\n", 476 | "==========estimation==========\n", 477 | "delta rotation:\n", 478 | "[[ 0.9941 0.0809 0.0718]\n", 479 | " [-0.0842 0.9955 0.0439]\n", 480 | " [-0.0679 -0.0497 0.9965]]\n", 481 | "delta translation:\n", 482 | "[-0.1773 -0.3492 -0.1309]\n", 483 | "delta scale: 1.261\n", 484 | "\n", 485 | "======================================\n", 486 | " ========= refine 2 ==========\n", 487 | "======================================\n", 488 | "the estimated relative tf for iter 0 is [ 0.0076 0.0073 0.0063 -0.0046 -0.017 0.0077 1.0015]\n", 489 | "the estimated relative tf for iter 1 is [ 0.0074 0.0047 0.0032 0.0025 -0.0349 0.0117 1.0084]\n", 490 | "the estimated relative tf for iter 2 is [ 0.0065 0.0022 0.0008 0.0082 -0.0492 0.0143 1.0166]\n", 491 | "the estimated relative tf for iter 3 is [ 0.0067 -0.0001 -0.0019 0.0225 -0.0559 0.0196 1.0245]\n", 492 | "the estimated relative tf for iter 4 is [ 0.0035 -0.0013 -0.0054 0.0247 -0.0754 0.0168 1.0301]\n", 493 | "the estimated relative tf for iter 5 is [ 0.0036 -0.0012 -0.0095 0.0243 -0.091 0.0096 1.0346]\n", 494 | "the estimated relative tf for iter 6 is [ 0.002 -0.0021 -0.0104 0.0251 -0.1063 0.0076 1.0399]\n", 495 | "the estimated relative tf for iter 7 is [ 0.0031 0.003 -0.0155 0.0557 -0.1249 -0.0052 1.048 ]\n", 496 | "the estimated relative tf for iter 8 is [ 0.0006 0.0011 -0.0152 0.0675 -0.125 -0.0058 1.0545]\n", 497 | "the estimated relative tf for iter 9 is [ 0.0026 -0.0078 -0.0191 0.0731 -0.1373 0.0294 1.0589]\n", 498 | "the estimated relative tf for iter 10 is [-0.0012 -0.0061 -0.0216 0.0799 -0.1464 0.0122 1.0636]\n", 499 | "the estimated relative tf for iter 11 is [ 0.0001 -0.0047 -0.0249 0.0896 -0.1603 0.0131 1.0693]\n", 500 | "the estimated relative tf for iter 12 is [ 0.0053 -0.0059 -0.0226 0.081 -0.1605 0.0242 1.0727]\n", 501 | "the estimated relative tf for iter 13 is [ 0.002 -0.0037 -0.0214 0.0868 -0.1553 0.0134 1.0789]\n", 502 | "the estimated relative tf for iter 14 is [-0.0003 -0.003 -0.0252 0.0945 -0.1673 0.0102 1.083 ]\n", 503 | "the estimated relative tf for iter 15 is [ 0.0054 0.0035 -0.0271 0.0968 -0.1876 -0.0039 1.0871]\n", 504 | "the estimated relative tf for iter 16 is [ 0.0038 0.0115 -0.0248 0.101 -0.1741 -0.0395 1.0908]\n", 505 | "the estimated relative tf for iter 17 is [ 0.0022 0.0132 -0.0313 0.1229 -0.1952 -0.0627 1.095 ]\n", 506 | "the estimated relative tf for iter 18 is [ 0.0006 0.0178 -0.0296 0.1364 -0.1851 -0.0779 1.0994]\n", 507 | "the estimated relative tf for iter 19 is [ 0.0032 0.0204 -0.0311 0.1476 -0.189 -0.0848 1.104 ]\n", 508 | "Function icp_once executed in 6.5598s\n", 509 | "\n", 510 | "==========estimation==========\n", 511 | "delta rotation:\n", 512 | "[[ 0.9993 0.0312 0.0204]\n", 513 | " [-0.0311 0.9995 -0.0035]\n", 514 | " [-0.0205 0.0029 0.9998]]\n", 515 | "delta translation:\n", 516 | "[ 0.1476 -0.189 -0.0848]\n", 517 | "delta scale: 1.104\n", 518 | "\n", 519 | "======================================\n", 520 | " ========= refine 3 ==========\n", 521 | "======================================\n", 522 | "the estimated relative tf for iter 0 is [0.0099 0.0098 0.0089 0.0116 0.0031 0.008 1.0023]\n", 523 | "the estimated relative tf for iter 1 is [ 0.0097 0.0055 0.0063 0.0124 -0.0126 0.0245 1.0036]\n", 524 | "the estimated relative tf for iter 2 is [ 0.0091 -0.0017 0.0069 0.0107 -0.0153 0.0531 1.0051]\n", 525 | "the estimated relative tf for iter 3 is [ 0.0099 -0.0062 0.004 0.0159 -0.0332 0.0682 1.0061]\n", 526 | "the estimated relative tf for iter 4 is [ 0.0064 -0.0157 0.0049 0.0147 -0.0352 0.0945 1.0068]\n", 527 | "the estimated relative tf for iter 5 is [ 0.0048 -0.0058 0.0042 0.0262 -0.0362 0.0488 1.0104]\n", 528 | "the estimated relative tf for iter 6 is [ 0.0089 -0.0046 0.0036 0.0231 -0.0447 0.0528 1.0118]\n", 529 | "the estimated relative tf for iter 7 is [ 0.0097 -0.0087 0.0046 0.0165 -0.0443 0.0762 1.0135]\n", 530 | "the estimated relative tf for iter 8 is [ 0.006 -0.0187 0.0025 0.0143 -0.0622 0.1072 1.014 ]\n", 531 | "the estimated relative tf for iter 9 is [ 0.0054 -0.0125 -0. 0.0354 -0.0646 0.0728 1.018 ]\n", 532 | "the estimated relative tf for iter 10 is [ 0.0051 -0.0072 -0.0019 0.0495 -0.0687 0.0517 1.0212]\n", 533 | "the estimated relative tf for iter 11 is [-0.0018 -0.0075 -0.0027 0.0486 -0.075 0.0348 1.0221]\n" 534 | ] 535 | }, 536 | { 537 | "name": "stdout", 538 | "output_type": "stream", 539 | "text": [ 540 | "the estimated relative tf for iter 12 is [-0.0017 -0.0197 -0.0062 0.0388 -0.0932 0.0907 1.0214]\n", 541 | "the estimated relative tf for iter 13 is [-0.0037 -0.0313 -0.0074 0.0357 -0.0996 0.1414 1.0229]\n", 542 | "the estimated relative tf for iter 14 is [-0. -0.0302 -0.0113 0.0405 -0.1194 0.1446 1.0242]\n", 543 | "the estimated relative tf for iter 15 is [ 0.0024 -0.0178 -0.0113 0.049 -0.1242 0.097 1.0257]\n", 544 | "the estimated relative tf for iter 16 is [-0.0037 -0.0083 -0.0056 0.0379 -0.1012 0.0326 1.0263]\n", 545 | "the estimated relative tf for iter 17 is [-0.0002 -0.0156 -0.0039 0.0379 -0.098 0.0685 1.0273]\n", 546 | "the estimated relative tf for iter 18 is [ 0.0007 -0.0134 -0.0038 0.0446 -0.0973 0.0621 1.0291]\n", 547 | "the estimated relative tf for iter 19 is [-0.0035 -0.0169 0.0029 0.0335 -0.0694 0.0754 1.0306]\n", 548 | "Function icp_once executed in 6.2426s\n", 549 | "\n", 550 | "==========estimation==========\n", 551 | "delta rotation:\n", 552 | "[[ 0.9999 -0.0029 -0.0169]\n", 553 | " [ 0.0029 1. 0.0035]\n", 554 | " [ 0.0169 -0.0036 0.9999]]\n", 555 | "delta translation:\n", 556 | "[ 0.0335 -0.0694 0.0754]\n", 557 | "delta scale: 1.031\n", 558 | "\n", 559 | "======================================\n", 560 | " ========= refine 4 ==========\n", 561 | "======================================\n", 562 | "the estimated relative tf for iter 0 is [ 0.008 0.0025 0.0076 0.0076 -0.0106 0.0314 0.9995]\n", 563 | "the estimated relative tf for iter 1 is [ 0.0073 0.0047 0.0055 0.0069 -0.0149 0.0107 0.9995]\n", 564 | "the estimated relative tf for iter 2 is [ 0.0101 -0.0038 0.0028 0.0078 -0.0234 0.0561 0.9997]\n", 565 | "the estimated relative tf for iter 3 is [ 0.0123 0.0089 -0.0012 0.0175 -0.0402 -0.0023 0.9996]\n", 566 | "the estimated relative tf for iter 4 is [ 0.0075 0.0127 -0.0019 0.0266 -0.0369 -0.0306 1.0015]\n", 567 | "the estimated relative tf for iter 5 is [ 0.008 0.0104 -0.0031 0.0329 -0.0442 -0.024 1.0028]\n", 568 | "the estimated relative tf for iter 6 is [ 0.0067 0.0109 -0.0007 0.0334 -0.028 -0.0222 1.0051]\n", 569 | "the estimated relative tf for iter 7 is [ 0.0063 0.0066 -0.0008 0.0311 -0.0268 0.0011 1.0062]\n", 570 | "the estimated relative tf for iter 8 is [ 0.0073 0.0035 -0.0045 0.0404 -0.0415 0.0153 1.0067]\n", 571 | "the estimated relative tf for iter 9 is [ 0.0075 0.0026 -0.0054 0.0422 -0.0442 0.0208 1.0076]\n", 572 | "the estimated relative tf for iter 10 is [ 0.0012 0.0168 0.0006 0.0438 -0.0072 -0.0583 1.0106]\n", 573 | "the estimated relative tf for iter 11 is [ 0.0003 0.0168 -0.0027 0.0512 -0.0198 -0.0627 1.0112]\n", 574 | "the estimated relative tf for iter 12 is [-0.0032 0.0093 -0.0039 0.0485 -0.0221 -0.0392 1.0113]\n", 575 | "the estimated relative tf for iter 13 is [-0.0048 0.0196 -0.0096 0.0663 -0.0445 -0.0835 1.0126]\n", 576 | "the estimated relative tf for iter 14 is [-0.0028 0.017 -0.0065 0.0559 -0.0248 -0.0778 1.0127]\n", 577 | "the estimated relative tf for iter 15 is [-0.0028 0.0054 -0.0009 0.0492 0.0038 -0.0371 1.0131]\n", 578 | "the estimated relative tf for iter 16 is [-0.0027 0.014 0.001 0.07 0.014 -0.0847 1.0161]\n", 579 | "the estimated relative tf for iter 17 is [-0.0005 -0.0005 -0. 0.0566 -0.0001 -0.0099 1.0145]\n", 580 | "the estimated relative tf for iter 18 is [ 0.0026 0.0038 -0.0044 0.0583 -0.0258 -0.0265 1.0138]\n", 581 | "the estimated relative tf for iter 19 is [ 0.006 0.0028 -0.0093 0.0752 -0.049 -0.0224 1.0143]\n", 582 | "Function icp_once executed in 5.8048s\n", 583 | "\n", 584 | "==========estimation==========\n", 585 | "delta rotation:\n", 586 | "[[ 1. 0.0093 0.0028]\n", 587 | " [-0.0093 0.9999 -0.0061]\n", 588 | " [-0.0029 0.006 1. ]]\n", 589 | "delta translation:\n", 590 | "[ 0.0752 -0.049 -0.0224]\n", 591 | "delta scale: 1.014\n", 592 | "\n", 593 | "======================================\n", 594 | " ========= refine 5 ==========\n", 595 | "======================================\n", 596 | "the estimated relative tf for iter 0 is [0.0052 0.0084 0.0081 0.0148 0.012 0.0097 1.0002]\n", 597 | "the estimated relative tf for iter 1 is [0.005 0.0002 0.007 0.0117 0.0061 0.0473 1.0001]\n", 598 | "the estimated relative tf for iter 2 is [0.0019 0.0069 0.0092 0.0047 0.017 0.0035 1.0002]\n", 599 | "the estimated relative tf for iter 3 is [0.003 0.0056 0.0086 0.0019 0.0136 0.0137 1.0006]\n", 600 | "the estimated relative tf for iter 4 is [-0.0018 -0.0077 0.0079 -0.0017 0.0153 0.0637 1.0005]\n", 601 | "the estimated relative tf for iter 5 is [-0.0052 0.0004 0.0075 -0.0014 0.0148 0.0196 1.0007]\n", 602 | "the estimated relative tf for iter 6 is [-0.0085 0.0007 0.0054 0.0036 0.0134 0.0174 1.0019]\n", 603 | "the estimated relative tf for iter 7 is [-0.0107 0.0055 0.0067 -0.0086 0.0223 -0.0181 1.0008]\n", 604 | "the estimated relative tf for iter 8 is [-0.0074 0.0101 0.0041 -0.0044 0.0096 -0.0311 1.001 ]\n", 605 | "the estimated relative tf for iter 9 is [-0.0044 0.0083 0.0062 -0.014 0.0192 -0.0055 1.0013]\n", 606 | "the estimated relative tf for iter 10 is [-0.0042 0.0097 0.0044 -0.0098 0.0134 -0.0147 1.0019]\n", 607 | "the estimated relative tf for iter 11 is [-0.0057 0.0187 -0.0024 0.0003 -0.0124 -0.0632 1.0018]\n", 608 | "the estimated relative tf for iter 12 is [-0.0047 0.0167 -0.0052 0.0079 -0.0222 -0.0464 1.002 ]\n", 609 | "the estimated relative tf for iter 13 is [-0.0005 0.0064 -0.0113 0.0108 -0.0581 0.0142 0.9996]\n", 610 | "the estimated relative tf for iter 14 is [-0.0002 -0.0014 -0.0104 0.0054 -0.051 0.0432 0.9989]\n", 611 | "the estimated relative tf for iter 15 is [ 0.0026 -0.0014 -0.0046 -0.0008 -0.028 0.0409 0.9994]\n", 612 | "the estimated relative tf for iter 16 is [-0.0017 0.0084 -0.0035 0.005 -0.0177 -0.0204 1.0001]\n", 613 | "the estimated relative tf for iter 17 is [-0.0018 0.0047 -0.006 0.0098 -0.0267 0.007 1.0008]\n", 614 | "the estimated relative tf for iter 18 is [ 0.0007 0.0204 -0.006 0.0206 -0.0192 -0.0588 1.0031]\n", 615 | "the estimated relative tf for iter 19 is [-0.002 0.028 -0.0018 0.0156 0.0092 -0.1046 1.0047]\n", 616 | "Function icp_once executed in 5.6238s\n", 617 | "\n", 618 | "==========estimation==========\n", 619 | "delta rotation:\n", 620 | "[[ 0.9996 0.0018 0.028 ]\n", 621 | " [-0.0018 1. 0.002 ]\n", 622 | " [-0.028 -0.002 0.9996]]\n", 623 | "delta translation:\n", 624 | "[ 0.0156 0.0092 -0.1046]\n", 625 | "delta scale: 1.005\n", 626 | "\n", 627 | "======================================\n", 628 | " ========= refine 6 ==========\n", 629 | "======================================\n", 630 | "the estimated relative tf for iter 0 is [0.008 0.0059 0.0104 0.0046 0.0126 0.021 0.9997]\n", 631 | "the estimated relative tf for iter 1 is [ 0.0024 0.0049 0.0077 0.0088 -0.0033 -0.0039 0.999 ]\n", 632 | "the estimated relative tf for iter 2 is [ 0.0025 -0.0092 0.0093 -0.0138 -0.0077 0.0599 0.9963]\n", 633 | "the estimated relative tf for iter 3 is [-0.0001 -0.0076 0.0048 -0.0033 -0.0264 0.0412 0.9964]\n", 634 | "the estimated relative tf for iter 4 is [ 0.0072 -0.0225 0.0083 -0.0285 -0.0173 0.1279 0.994 ]\n", 635 | "the estimated relative tf for iter 5 is [ 0.0049 -0.0173 0.0041 -0.0232 -0.0329 0.0963 0.994 ]\n", 636 | "the estimated relative tf for iter 6 is [ 0.0063 -0.0132 0.0058 -0.0161 -0.0272 0.0685 0.9958]\n", 637 | "the estimated relative tf for iter 7 is [ 0.0026 -0.0124 0.0038 -0.0155 -0.0263 0.0603 0.9964]\n", 638 | "the estimated relative tf for iter 8 is [ 0.0034 -0.0154 0.004 -0.01 -0.0261 0.0752 0.9973]\n", 639 | "the estimated relative tf for iter 9 is [ 0.0097 -0.0079 0.0025 -0.0068 -0.0302 0.0671 0.999 ]\n", 640 | "the estimated relative tf for iter 10 is [ 0.0099 -0.0016 -0.0005 0.0121 -0.039 0.0326 0.9998]\n", 641 | "the estimated relative tf for iter 11 is [ 0.0077 -0.0202 -0.0017 -0.0226 -0.0578 0.119 0.9951]\n", 642 | "the estimated relative tf for iter 12 is [ 0.0019 -0.0235 -0.0007 -0.0222 -0.0535 0.109 0.9949]\n", 643 | "the estimated relative tf for iter 13 is [-0.0009 -0.0259 -0.0024 -0.007 -0.0507 0.1033 0.9959]\n", 644 | "the estimated relative tf for iter 14 is [ 0.0011 -0.0241 0.0052 -0.0296 -0.0244 0.0932 0.9958]\n", 645 | "the estimated relative tf for iter 15 is [-0.0002 -0.0248 0.0045 -0.0199 -0.0193 0.0849 0.9975]\n", 646 | "the estimated relative tf for iter 16 is [-0.0117 -0.0345 0.0092 -0.0432 0.021 0.1028 0.9983]\n", 647 | "the estimated relative tf for iter 17 is [-0.003 -0.0393 0.0049 -0.0362 -0.0081 0.1477 0.9976]\n", 648 | "the estimated relative tf for iter 18 is [ 0.0013 -0.0463 0.0011 -0.0345 -0.0342 0.1978 0.9978]\n", 649 | "the estimated relative tf for iter 19 is [-0.0005 -0.0399 0.0004 -0.0264 -0.0363 0.1663 0.9977]\n", 650 | "Function icp_once executed in 5.5092s\n", 651 | "\n", 652 | "==========estimation==========\n", 653 | "delta rotation:\n", 654 | "[[ 0.9992 -0.0004 -0.0399]\n", 655 | " [ 0.0004 1. 0.0005]\n", 656 | " [ 0.0399 -0.0005 0.9992]]\n", 657 | "delta translation:\n", 658 | "[-0.0264 -0.0363 0.1663]\n", 659 | "delta scale: 0.998\n", 660 | "\n", 661 | "======================================\n", 662 | " ========= refine 7 ==========\n", 663 | "======================================\n", 664 | "the estimated relative tf for iter 0 is [ 0.0085 0.0118 0.0103 0.0081 0.0185 -0.0022 1.0004]\n", 665 | "the estimated relative tf for iter 1 is [ 0.0072 0.0148 0.0041 0.0177 0.0073 -0.0178 1.0006]\n" 666 | ] 667 | }, 668 | { 669 | "name": "stdout", 670 | "output_type": "stream", 671 | "text": [ 672 | "the estimated relative tf for iter 2 is [0.0091 0.0115 0.0058 0.0124 0.0199 0.0017 1.001 ]\n", 673 | "the estimated relative tf for iter 3 is [ 0.0031 0.0134 0.0051 0.0151 0.0216 -0.0248 1.001 ]\n", 674 | "the estimated relative tf for iter 4 is [-0.0018 0.0109 0.0018 0.0248 0.0095 -0.0358 1.0008]\n", 675 | "the estimated relative tf for iter 5 is [ 0.0009 0.0266 -0.0028 0.0532 -0.0092 -0.1022 1.0024]\n", 676 | "the estimated relative tf for iter 6 is [ 0.0051 0.0204 -0.0016 0.0397 -0.0056 -0.066 1.0017]\n", 677 | "the estimated relative tf for iter 7 is [-0.0008 0.0227 -0. 0.0307 0.0069 -0.0893 1.0016]\n", 678 | "the estimated relative tf for iter 8 is [-0.004 0.0188 0.003 0.0198 0.0254 -0.0773 1.0022]\n", 679 | "the estimated relative tf for iter 9 is [-0.0033 0.0166 0.0041 0.0153 0.0325 -0.0635 1.003 ]\n", 680 | "the estimated relative tf for iter 10 is [-0.0065 0.0206 0.0037 0.0134 0.0367 -0.0907 1.0032]\n", 681 | "the estimated relative tf for iter 11 is [-0.0025 0.0181 0.003 0.0135 0.0308 -0.0654 1.0032]\n", 682 | "the estimated relative tf for iter 12 is [-0.0059 0.023 -0.001 0.0191 0.0145 -0.1015 1.0033]\n", 683 | "the estimated relative tf for iter 13 is [ 0.0043 0.0164 -0.0035 0.0281 -0.0003 -0.0445 1.0014]\n", 684 | "the estimated relative tf for iter 14 is [-0.0029 0.0157 -0.0047 0.0311 -0.0022 -0.0543 1.0014]\n", 685 | "the estimated relative tf for iter 15 is [ 0.0073 0.0191 -0.0081 0.0357 -0.0185 -0.0419 1.0009]\n", 686 | "the estimated relative tf for iter 16 is [ 0.0069 0.0292 -0.0074 0.0406 -0.001 -0.0852 1.0027]\n", 687 | "the estimated relative tf for iter 17 is [ 0.0079 0.0275 -0.0064 0.0383 -0.0073 -0.096 1.0014]\n", 688 | "the estimated relative tf for iter 18 is [ 0.0041 0.0327 0.0044 0.01 0.0483 -0.1386 1.0006]\n", 689 | "the estimated relative tf for iter 19 is [ 0.0014 0.0199 0.0025 0.0073 0.0278 -0.0937 0.9983]\n", 690 | "Function icp_once executed in 5.1629s\n", 691 | "\n", 692 | "==========estimation==========\n", 693 | "delta rotation:\n", 694 | "[[ 0.9998 -0.0025 0.0199]\n", 695 | " [ 0.0025 1. -0.0014]\n", 696 | " [-0.0199 0.0014 0.9998]]\n", 697 | "delta translation:\n", 698 | "[ 0.0073 0.0278 -0.0937]\n", 699 | "delta scale: 0.998\n", 700 | "\n", 701 | "======================================\n", 702 | " ========= refine 8 ==========\n", 703 | "======================================\n", 704 | "the estimated relative tf for iter 0 is [ 0.0075 0.0113 0.0088 0.0164 0.0146 -0.0021 1.0012]\n", 705 | "the estimated relative tf for iter 1 is [ 0.0026 0.0106 0.0083 0.0117 0.0173 -0.0095 1.0013]\n", 706 | "the estimated relative tf for iter 2 is [0.0017 0.0019 0.0058 0.0015 0.0069 0.0378 0.9996]\n", 707 | "the estimated relative tf for iter 3 is [-0.0002 0.0083 0.0051 0.011 0.0084 -0. 1.0009]\n", 708 | "the estimated relative tf for iter 4 is [-0.0011 0.0088 0.0062 0.0171 0.0203 -0.009 1.002 ]\n", 709 | "the estimated relative tf for iter 5 is [ 0.0032 -0.0021 0.0071 0.001 0.0169 0.0531 1.0005]\n", 710 | "the estimated relative tf for iter 6 is [ 0.0092 -0.0062 0.0067 0.0026 0.0132 0.0847 1.0006]\n" 711 | ] 712 | }, 713 | { 714 | "ename": "KeyboardInterrupt", 715 | "evalue": "", 716 | "output_type": "error", 717 | "traceback": [ 718 | "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", 719 | "\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", 720 | "Input \u001b[0;32mIn [17]\u001b[0m, in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[1;32m 73\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n\u001b[1;32m 74\u001b[0m fix_scale\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mFalse\u001b[39;00m\n\u001b[0;32m---> 75\u001b[0m tf_tangent, line_sets \u001b[38;5;241m=\u001b[39m \u001b[43micp_once\u001b[49m\u001b[43m(\u001b[49m\u001b[43msrc_pc\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtgt_pc\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43minit_guess\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mskip\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mpts_skip\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43m\\\u001b[49m\n\u001b[1;32m 76\u001b[0m \u001b[43m \u001b[49m\u001b[43moutlier_ratio\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43moutlier_ratio\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m 77\u001b[0m \u001b[43m \u001b[49m\u001b[43mnum_iters\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mnum_iters\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m 78\u001b[0m \u001b[43m \u001b[49m\u001b[43mfix_scale\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mfix_scale\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m 79\u001b[0m \u001b[43m \u001b[49m\u001b[43mverbose\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;28;43;01mFalse\u001b[39;49;00m\u001b[43m)\u001b[49m \u001b[38;5;66;03m# if \"true\" correspondence is given (this is an tutorial for education purpose), using a few iteration okay ..\u001b[39;00m\n\u001b[1;32m 81\u001b[0m \u001b[38;5;66;03m# 2. move the src to target and \u001b[39;00m\n\u001b[1;32m 82\u001b[0m est_rot3, est_trans3, est_scale \u001b[38;5;241m=\u001b[39m tf_tangent[:\u001b[38;5;241m3\u001b[39m], tf_tangent[\u001b[38;5;241m3\u001b[39m:\u001b[38;5;241m6\u001b[39m], tf_tangent[\u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m]\n", 721 | "Input \u001b[0;32mIn [4]\u001b[0m, in \u001b[0;36mtimer..wrap_func\u001b[0;34m(*args, **kwargs)\u001b[0m\n\u001b[1;32m 8\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mwrap_func\u001b[39m(\u001b[38;5;241m*\u001b[39margs, \u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39mkwargs):\n\u001b[1;32m 9\u001b[0m t1 \u001b[38;5;241m=\u001b[39m time()\n\u001b[0;32m---> 10\u001b[0m result \u001b[38;5;241m=\u001b[39m \u001b[43mfunc\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[43margs\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[43mkwargs\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 11\u001b[0m t2 \u001b[38;5;241m=\u001b[39m time()\n\u001b[1;32m 13\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m disp_timecost:\n", 722 | "Input \u001b[0;32mIn [15]\u001b[0m, in \u001b[0;36micp_once\u001b[0;34m(src, tgt, tf_init, skip, outlier_ratio, num_iters, fix_scale, verbose)\u001b[0m\n\u001b[1;32m 51\u001b[0m tgt_corres_idx, is_true_corres \\\n\u001b[1;32m 52\u001b[0m \u001b[38;5;241m=\u001b[39m probabilistic_false_corres_idx(pt_idx, num_pts\u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m, outlier_ratio\u001b[38;5;241m=\u001b[39moutlier_ratio) \u001b[38;5;66;03m# true \u001b[39;00m\n\u001b[1;32m 54\u001b[0m src_pt, tgt_pt \u001b[38;5;241m=\u001b[39m src[src_corres_idx, :], tgt[tgt_corres_idx, :]\n\u001b[1;32m 56\u001b[0m e, Je_rot, Je_trans, Je_scale \\\n\u001b[0;32m---> 57\u001b[0m \u001b[38;5;241m=\u001b[39m \u001b[43mevaluate_error_and_jacobian\u001b[49m\u001b[43m(\u001b[49m\u001b[43msrc_pt\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtgt_pt\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtf_init\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 58\u001b[0m \u001b[38;5;66;03m# ps. To understand the details of this nonlinear iterative update steps, see http://www.diag.uniroma1.it//~labrococo/tutorial_icra_2016/icra16_slam_tutorial_grisetti.pdf \u001b[39;00m\n\u001b[1;32m 59\u001b[0m \u001b[38;5;66;03m# however, in the above slide's example, the jacobian was generated by hand as well as Euler angle space was used, not angle-axis.\u001b[39;00m\n\u001b[1;32m 61\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m fix_scale:\n", 723 | "Input \u001b[0;32mIn [15]\u001b[0m, in \u001b[0;36mevaluate_error_and_jacobian\u001b[0;34m(src_pt, tag_pt, tf)\u001b[0m\n\u001b[1;32m 12\u001b[0m model_evaluated \u001b[38;5;241m=\u001b[39m \\\n\u001b[1;32m 13\u001b[0m model\u001b[38;5;241m.\u001b[39msubs({rotvec: sf\u001b[38;5;241m.\u001b[39mV3(se3[:\u001b[38;5;241m3\u001b[39m]), \\\n\u001b[1;32m 14\u001b[0m transvec: sf\u001b[38;5;241m.\u001b[39mV3(se3[\u001b[38;5;241m3\u001b[39m:]), \\\n\u001b[1;32m 15\u001b[0m scale: sf\u001b[38;5;241m.\u001b[39mV1(s), \\\n\u001b[1;32m 16\u001b[0m p_src: sf\u001b[38;5;241m.\u001b[39mV3(src_pt), \\\n\u001b[1;32m 17\u001b[0m p_tgt: sf\u001b[38;5;241m.\u001b[39mV3(tag_pt)})\n\u001b[1;32m 18\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m model_evaluated\u001b[38;5;241m.\u001b[39mto_numpy()\n\u001b[1;32m 20\u001b[0m error, Je_rot, Je_trans, Je_scale \u001b[38;5;241m=\u001b[39m \\\n\u001b[0;32m---> 21\u001b[0m [inject_values(x) \u001b[38;5;28;01mfor\u001b[39;00m x \u001b[38;5;129;01min\u001b[39;00m [error_model, Je_rot_model, Je_trans_model, Je_scale_model]]\n\u001b[1;32m 23\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m error, Je_rot, Je_trans, Je_scale\n", 724 | "Input \u001b[0;32mIn [15]\u001b[0m, in \u001b[0;36m\u001b[0;34m(.0)\u001b[0m\n\u001b[1;32m 12\u001b[0m model_evaluated \u001b[38;5;241m=\u001b[39m \\\n\u001b[1;32m 13\u001b[0m model\u001b[38;5;241m.\u001b[39msubs({rotvec: sf\u001b[38;5;241m.\u001b[39mV3(se3[:\u001b[38;5;241m3\u001b[39m]), \\\n\u001b[1;32m 14\u001b[0m transvec: sf\u001b[38;5;241m.\u001b[39mV3(se3[\u001b[38;5;241m3\u001b[39m:]), \\\n\u001b[1;32m 15\u001b[0m scale: sf\u001b[38;5;241m.\u001b[39mV1(s), \\\n\u001b[1;32m 16\u001b[0m p_src: sf\u001b[38;5;241m.\u001b[39mV3(src_pt), \\\n\u001b[1;32m 17\u001b[0m p_tgt: sf\u001b[38;5;241m.\u001b[39mV3(tag_pt)})\n\u001b[1;32m 18\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m model_evaluated\u001b[38;5;241m.\u001b[39mto_numpy()\n\u001b[1;32m 20\u001b[0m error, Je_rot, Je_trans, Je_scale \u001b[38;5;241m=\u001b[39m \\\n\u001b[0;32m---> 21\u001b[0m [\u001b[43minject_values\u001b[49m\u001b[43m(\u001b[49m\u001b[43mx\u001b[49m\u001b[43m)\u001b[49m \u001b[38;5;28;01mfor\u001b[39;00m x \u001b[38;5;129;01min\u001b[39;00m [error_model, Je_rot_model, Je_trans_model, Je_scale_model]]\n\u001b[1;32m 23\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m error, Je_rot, Je_trans, Je_scale\n", 725 | "Input \u001b[0;32mIn [15]\u001b[0m, in \u001b[0;36mevaluate_error_and_jacobian..inject_values\u001b[0;34m(model)\u001b[0m\n\u001b[1;32m 11\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21minject_values\u001b[39m(model):\n\u001b[1;32m 12\u001b[0m model_evaluated \u001b[38;5;241m=\u001b[39m \\\n\u001b[0;32m---> 13\u001b[0m \u001b[43mmodel\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msubs\u001b[49m\u001b[43m(\u001b[49m\u001b[43m{\u001b[49m\u001b[43mrotvec\u001b[49m\u001b[43m:\u001b[49m\u001b[43m \u001b[49m\u001b[43msf\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mV3\u001b[49m\u001b[43m(\u001b[49m\u001b[43mse3\u001b[49m\u001b[43m[\u001b[49m\u001b[43m:\u001b[49m\u001b[38;5;241;43m3\u001b[39;49m\u001b[43m]\u001b[49m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43m\\\u001b[49m\n\u001b[1;32m 14\u001b[0m \u001b[43m \u001b[49m\u001b[43mtransvec\u001b[49m\u001b[43m:\u001b[49m\u001b[43m \u001b[49m\u001b[43msf\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mV3\u001b[49m\u001b[43m(\u001b[49m\u001b[43mse3\u001b[49m\u001b[43m[\u001b[49m\u001b[38;5;241;43m3\u001b[39;49m\u001b[43m:\u001b[49m\u001b[43m]\u001b[49m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43m\\\u001b[49m\n\u001b[1;32m 15\u001b[0m \u001b[43m \u001b[49m\u001b[43mscale\u001b[49m\u001b[43m:\u001b[49m\u001b[43m \u001b[49m\u001b[43msf\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mV1\u001b[49m\u001b[43m(\u001b[49m\u001b[43ms\u001b[49m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43m\\\u001b[49m\n\u001b[1;32m 16\u001b[0m \u001b[43m \u001b[49m\u001b[43mp_src\u001b[49m\u001b[43m:\u001b[49m\u001b[43m \u001b[49m\u001b[43msf\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mV3\u001b[49m\u001b[43m(\u001b[49m\u001b[43msrc_pt\u001b[49m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43m\\\u001b[49m\n\u001b[1;32m 17\u001b[0m \u001b[43m \u001b[49m\u001b[43mp_tgt\u001b[49m\u001b[43m:\u001b[49m\u001b[43m \u001b[49m\u001b[43msf\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mV3\u001b[49m\u001b[43m(\u001b[49m\u001b[43mtag_pt\u001b[49m\u001b[43m)\u001b[49m\u001b[43m}\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 18\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m model_evaluated\u001b[38;5;241m.\u001b[39mto_numpy()\n", 726 | "File \u001b[0;32m~/.local/lib/python3.8/site-packages/symforce/ops/interfaces/storage.py:76\u001b[0m, in \u001b[0;36mStorage.subs\u001b[0;34m(self, *args, **kwargs)\u001b[0m\n\u001b[1;32m 72\u001b[0m \u001b[38;5;124;03m\"\"\"\u001b[39;00m\n\u001b[1;32m 73\u001b[0m \u001b[38;5;124;03mSubstitute given values of each scalar element into a new instance.\u001b[39;00m\n\u001b[1;32m 74\u001b[0m \u001b[38;5;124;03m\"\"\"\u001b[39;00m\n\u001b[1;32m 75\u001b[0m \u001b[38;5;66;03m# TODO(hayk): If this is slow, compute the subs dict once.\u001b[39;00m\n\u001b[0;32m---> 76\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mfrom_storage([sf\u001b[38;5;241m.\u001b[39mS(s)\u001b[38;5;241m.\u001b[39msubs(\u001b[38;5;241m*\u001b[39margs, \u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39mkwargs) \u001b[38;5;28;01mfor\u001b[39;00m s \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mto_storage()])\n", 727 | "File \u001b[0;32m~/.local/lib/python3.8/site-packages/symforce/ops/interfaces/storage.py:76\u001b[0m, in \u001b[0;36m\u001b[0;34m(.0)\u001b[0m\n\u001b[1;32m 72\u001b[0m \u001b[38;5;124;03m\"\"\"\u001b[39;00m\n\u001b[1;32m 73\u001b[0m \u001b[38;5;124;03mSubstitute given values of each scalar element into a new instance.\u001b[39;00m\n\u001b[1;32m 74\u001b[0m \u001b[38;5;124;03m\"\"\"\u001b[39;00m\n\u001b[1;32m 75\u001b[0m \u001b[38;5;66;03m# TODO(hayk): If this is slow, compute the subs dict once.\u001b[39;00m\n\u001b[0;32m---> 76\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mfrom_storage([\u001b[43msf\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mS\u001b[49m\u001b[43m(\u001b[49m\u001b[43ms\u001b[49m\u001b[43m)\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msubs\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[43margs\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[43mkwargs\u001b[49m\u001b[43m)\u001b[49m \u001b[38;5;28;01mfor\u001b[39;00m s \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mto_storage()])\n", 728 | "File \u001b[0;32msymengine_wrapper.pyx:958\u001b[0m, in \u001b[0;36msymengine.lib.symengine_wrapper.Basic.subs\u001b[0;34m()\u001b[0m\n", 729 | "File \u001b[0;32m~/.local/lib/python3.8/site-packages/symforce/internal/symbolic.py:643\u001b[0m, in \u001b[0;36m\u001b[0;34m(*args, **kwargs)\u001b[0m\n\u001b[1;32m 640\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01msymengine\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mlib\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01msymengine_wrapper\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mwrapper\u001b[39;00m \u001b[38;5;66;03m# pylint: disable=no-name-in-module\u001b[39;00m\n\u001b[1;32m 642\u001b[0m original_get_dict \u001b[38;5;241m=\u001b[39m wrapper\u001b[38;5;241m.\u001b[39mget_dict\n\u001b[0;32m--> 643\u001b[0m wrapper\u001b[38;5;241m.\u001b[39mget_dict \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mlambda\u001b[39;00m \u001b[38;5;241m*\u001b[39margs, \u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39mkwargs: original_get_dict(\u001b[43m_get_subs_dict\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[43margs\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[43mkwargs\u001b[49m\u001b[43m)\u001b[49m)\n\u001b[1;32m 644\u001b[0m \u001b[38;5;28;01melif\u001b[39;00m sympy\u001b[38;5;241m.\u001b[39m__package__ \u001b[38;5;241m==\u001b[39m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124msympy\u001b[39m\u001b[38;5;124m\"\u001b[39m:\n\u001b[1;32m 645\u001b[0m original_subs \u001b[38;5;241m=\u001b[39m sympy\u001b[38;5;241m.\u001b[39mBasic\u001b[38;5;241m.\u001b[39msubs\n", 730 | "File \u001b[0;32m~/.local/lib/python3.8/site-packages/symforce/internal/symbolic.py:633\u001b[0m, in \u001b[0;36m_get_subs_dict\u001b[0;34m(dont_flatten_args, *args, **kwargs)\u001b[0m\n\u001b[1;32m 630\u001b[0m subs_pairs \u001b[38;5;241m=\u001b[39m args[\u001b[38;5;241m0\u001b[39m]\n\u001b[1;32m 632\u001b[0m \u001b[38;5;28;01massert\u001b[39;00m \u001b[38;5;28misinstance\u001b[39m(subs_pairs, T\u001b[38;5;241m.\u001b[39mSequence)\n\u001b[0;32m--> 633\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[43m_flatten_storage_type_subs\u001b[49m\u001b[43m(\u001b[49m\u001b[43msubs_pairs\u001b[49m\u001b[43m)\u001b[49m\n", 731 | "File \u001b[0;32m~/.local/lib/python3.8/site-packages/symforce/internal/symbolic.py:587\u001b[0m, in \u001b[0;36m_flatten_storage_type_subs\u001b[0;34m(subs_pairs)\u001b[0m\n\u001b[1;32m 583\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01msymforce\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m python_util \u001b[38;5;66;03m# pylint: disable=cyclic-import\u001b[39;00m\n\u001b[1;32m 585\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m key, value \u001b[38;5;129;01min\u001b[39;00m subs_pairs:\n\u001b[0;32m--> 587\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[43mpython_util\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mscalar_like\u001b[49m\u001b[43m(\u001b[49m\u001b[43mkey\u001b[49m\u001b[43m)\u001b[49m:\n\u001b[1;32m 588\u001b[0m \u001b[38;5;28;01massert\u001b[39;00m python_util\u001b[38;5;241m.\u001b[39mscalar_like(value)\n\u001b[1;32m 589\u001b[0m new_subs_dict[key] \u001b[38;5;241m=\u001b[39m value\n", 732 | "File \u001b[0;32m~/.local/lib/python3.8/site-packages/symforce/python_util.py:201\u001b[0m, in \u001b[0;36mscalar_like\u001b[0;34m(a)\u001b[0m\n\u001b[1;32m 184\u001b[0m \u001b[38;5;66;03m# NOTE(brad): Each of these classes is automatically registered with ScalarLieGroupOps. (see\u001b[39;00m\n\u001b[1;32m 185\u001b[0m \u001b[38;5;66;03m# ops/__init__.py)\u001b[39;00m\n\u001b[1;32m 186\u001b[0m SCALAR_TYPES \u001b[38;5;241m=\u001b[39m (\n\u001b[1;32m 187\u001b[0m \u001b[38;5;28mfloat\u001b[39m,\n\u001b[1;32m 188\u001b[0m np\u001b[38;5;241m.\u001b[39mfloat16,\n\u001b[0;32m (...)\u001b[0m\n\u001b[1;32m 197\u001b[0m np\u001b[38;5;241m.\u001b[39mint64,\n\u001b[1;32m 198\u001b[0m )\n\u001b[0;32m--> 201\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mscalar_like\u001b[39m(a: T\u001b[38;5;241m.\u001b[39mAny) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28mbool\u001b[39m:\n\u001b[1;32m 202\u001b[0m \u001b[38;5;124;03m\"\"\"\u001b[39;00m\n\u001b[1;32m 203\u001b[0m \u001b[38;5;124;03m Returns whether the element is scalar-like (an int, float, or sympy expression).\u001b[39;00m\n\u001b[1;32m 204\u001b[0m \n\u001b[1;32m 205\u001b[0m \u001b[38;5;124;03m This method does not rely on the value of a, only the type.\u001b[39;00m\n\u001b[1;32m 206\u001b[0m \u001b[38;5;124;03m \"\"\"\u001b[39;00m\n\u001b[1;32m 207\u001b[0m a_type \u001b[38;5;241m=\u001b[39m get_type(a)\n", 733 | "\u001b[0;31mKeyboardInterrupt\u001b[0m: " 734 | ] 735 | } 736 | ], 737 | "source": [ 738 | "##########\n", 739 | "# MAIN \n", 740 | "##########\n", 741 | "\n", 742 | "# At the very first status \n", 743 | "is_viz = 1\n", 744 | "if is_viz:\n", 745 | " pcd0.paint_uniform_color([1, 0, 1])\n", 746 | " pcd1.paint_uniform_color([0, 0, 1])\n", 747 | " o3d.visualization.draw_geometries([pcd0, pcd1], window_name=\"initial status\")\n", 748 | "\n", 749 | "# Initial condition \n", 750 | "def gen_noisy_but_reliable_inital():\n", 751 | " rot_init = R.from_euler('xyz', true_rot_diff_rpy, degrees=True).as_rotvec() + 0.05*np.random.rand(3)\n", 752 | " trans_init = true_trans_diff + np.random.rand(3)*(0.1*scale_up)\n", 753 | " scale_init = 0.7* true_scale_diff\n", 754 | " \n", 755 | " eps = 0.0001 # +eps means: because zero initial should be avoided (see the symbolic equation of Je_rot!)\n", 756 | " initial_state_vector = np.hstack((rot_init, trans_init, scale_init)) + eps \n", 757 | " return initial_state_vector\n", 758 | "\n", 759 | "def identity_inital():\n", 760 | " eps = 0.01\n", 761 | " return np.array([eps, eps, eps, eps, eps, eps, 1.0])\n", 762 | " # because after the update, the registered_src is expected to be equal to the target \n", 763 | " # thus, the translation and rotataion would be zero and the relative scale must be 1.0\n", 764 | "\n", 765 | "init_guess = gen_noisy_but_reliable_inital() \n", 766 | "# init_guess = gen_noisy_but_reliable_inital() \n", 767 | " # at the very first step, a moderate (i.e., not-identity) initial value is required \n", 768 | " # because the cost function is highly nonlinear\n", 769 | " # ps. try yourself using init_guess = identity_inital() rather than gen_noisy_but_reliable_inital()\n", 770 | " # the convergence speed would be deteriorated. (test yourself!)\n", 771 | "print(f\"init_guess is {init_guess}\")\n", 772 | "\n", 773 | "# NOTE\n", 774 | "# The number of correspondences and their spatial distirbution would affect the results\n", 775 | "# for example, \n", 776 | "# in the below example, \n", 777 | "# for the dragon dataset, it will converge (when we use the robust loss) even under 50% outliers while using skip=20\n", 778 | "# however, for the bunny dataset, which has the more smaller number of points, would not converge when we use skip=20 (skip=1 is then okay. try yourself!)\n", 779 | " # ps. for the production level code, you also adaptively conclude when num_iters should be stopped in the icp_once (e.g., by tracking the df or residuals)\n", 780 | " # by doing so, you need to prevent the solution from divergent. \n", 781 | "# therefore, the what I want to say is for parameter tuning, we should well understand your dataset's characteristics (e.g., density, spatial distribution, etc.)\n", 782 | "\n", 783 | "# the robust loss parameter is also important\n", 784 | "# for dragon, we will use \n", 785 | "\"\"\"\n", 786 | " alpha = 0\n", 787 | " delta = 0.1-0.2\n", 788 | " scalar_information = 5-10\n", 789 | " epsilon = 1.0e-6\n", 790 | " \n", 791 | " , and pts_skip = 20\n", 792 | "\"\"\"\n", 793 | "\n", 794 | "# ICP starts \n", 795 | "max_iter = 25\n", 796 | "src_pc, tgt_pc = [np.array(pc.points) for pc in [pcd0, pcd1]]\n", 797 | "for _iter in range(max_iter):\n", 798 | " print(f\"\\n======================================\")\n", 799 | " print(f\" ========= refine {_iter} ==========\")\n", 800 | " print(f\"======================================\")\n", 801 | " # 1. optimize once \n", 802 | " src_pc_before_updated = copy.deepcopy(src_pc)\n", 803 | "\n", 804 | " outlier_ratio = 0.5 # test yourself up to 0.00 (no outlier) to 0.99\n", 805 | " pts_skip = (10 + _iter) # for bunny (num points are small), use skip = 1 and for the dragon, use skip=20 is okay\n", 806 | " num_iters = 20\n", 807 | "\n", 808 | " if _iter < 0:\n", 809 | " fix_scale=True \n", 810 | " else:\n", 811 | " fix_scale=False\n", 812 | " tf_tangent, line_sets = icp_once(src_pc, tgt_pc, init_guess, skip=pts_skip, \\\n", 813 | " outlier_ratio=outlier_ratio,\n", 814 | " num_iters=num_iters,\n", 815 | " fix_scale=fix_scale,\n", 816 | " verbose=False) # if \"true\" correspondence is given (this is an tutorial for education purpose), using a few iteration okay ..\n", 817 | "\n", 818 | " # 2. move the src to target and \n", 819 | " est_rot3, est_trans3, est_scale = tf_tangent[:3], tf_tangent[3:6], tf_tangent[-1]\n", 820 | " est_rotmat3x3 = rotmat.subs({rotvec: sf.V3(est_rot3)}).to_rotation_matrix().to_numpy()\n", 821 | " src_pc_updated = est_scale*(est_rotmat3x3 @ src_pc.transpose()) + np.array([est_trans3]).transpose()\n", 822 | " src_pc = src_pc_updated.transpose()\n", 823 | " \n", 824 | " init_guess = identity_inital()\n", 825 | " # note: we explicitly update the source point cloud (i.e., registered), \n", 826 | " # thus from the next step, we will use init_guess always equal to identity (toy example assumption)\n", 827 | " # because, as already mentioned, after the update, the registered_src is expected to be equal to the target \n", 828 | " # thus, the translation and rotataion would be zero and the relative scale must be 1.0\n", 829 | " # in real world example, we need to use a domain knowledge to update the better initial (e.g., constant motion model, the prior knowledge of the object's metric scale, etc.)\n", 830 | "\n", 831 | " # 3. re-correspondence\n", 832 | " # here, we can use the known true-correspondence because this is just a tutorial and affine transformation does not change the true correspondences \n", 833 | " # but in real world applications, kd-tree-like nearest neighbor search to find a newaly updated correspondences is required. \n", 834 | "\n", 835 | " # 4. debug: Verify the result visually \n", 836 | " if is_viz:\n", 837 | " pcd0_Sim3_before_update = np2o3d(src_pc_before_updated)\n", 838 | " pcd0_Sim3_before_update.paint_uniform_color([0.5, 0.5, 0.5])\n", 839 | " \n", 840 | " pcd0_Sim3_after_update = np2o3d(src_pc)\n", 841 | " pcd0_Sim3_after_update.paint_uniform_color([25./255, 158./255, 243./255])\n", 842 | " \n", 843 | " pcd1.paint_uniform_color([0, 0, 1])\n", 844 | " \n", 845 | " line_sets[\"correct\"].paint_uniform_color([0, 0.737, 0.354])\n", 846 | " if line_sets[\"false\"] is None:\n", 847 | " line_sets[\"false\"] = copy.deepcopy(line_sets[\"correct\"])\n", 848 | " else:\n", 849 | " line_sets[\"false\"].paint_uniform_color([1.0, 0.0, 0.0])\n", 850 | " \n", 851 | " # draw before \n", 852 | " o3d.visualization.draw_geometries([pcd0_Sim3_before_update, pcd1, \\\n", 853 | " line_sets[\"correct\"], line_sets[\"false\"]], \\\n", 854 | " window_name=f\"iteration {_iter} (gray: before update, blue: target)\")\n", 855 | " \n", 856 | " # draw after \n", 857 | " o3d.visualization.draw_geometries([pcd0_Sim3_after_update, pcd1], \\\n", 858 | " window_name=f\"iteration {_iter} (sky: after updated, blue: target)\")\n", 859 | "\n", 860 | " # 5. if tf_tangent is smaller than a threshold, stop \n", 861 | " print(\"\\n==========estimation==========\")\n", 862 | " print(f\"delta rotation:\\n{est_rotmat3x3}\")\n", 863 | " print(f\"delta translation:\\n{est_trans3}\")\n", 864 | " print(f\"delta scale: {est_scale:.3f}\")\n", 865 | " # TBA, e.g., if delta_translation < 0.01, break;\n" 866 | ] 867 | }, 868 | { 869 | "cell_type": "code", 870 | "execution_count": null, 871 | "id": "052b376f", 872 | "metadata": {}, 873 | "outputs": [], 874 | "source": [ 875 | "# @ Future works \n", 876 | "\n", 877 | "# 1: \n", 878 | "# Here, the false residual and its Hessian is directly incorporated within the normal equation \n", 879 | "# we can say this deweighting method implictily handles the outliers. \n", 880 | "# The next step you can do is to \"explictly remove\" the false correspondences from the pairs \n", 881 | "# e.g., using RANSAC \n", 882 | "\n", 883 | "# 2:\n", 884 | "# using parallel implmentation, boost the above per-point iterations faster \n", 885 | "\n", 886 | "# 3:\n", 887 | "# using not a point-to-point L2 distance, using point-to-plane (using normal and dot product) loss\n", 888 | "\n" 889 | ] 890 | } 891 | ], 892 | "metadata": { 893 | "kernelspec": { 894 | "display_name": "Python 3 (ipykernel)", 895 | "language": "python", 896 | "name": "python3" 897 | }, 898 | "language_info": { 899 | "codemirror_mode": { 900 | "name": "ipython", 901 | "version": 3 902 | }, 903 | "file_extension": ".py", 904 | "mimetype": "text/x-python", 905 | "name": "python", 906 | "nbconvert_exporter": "python", 907 | "pygments_lexer": "ipython3", 908 | "version": "3.8.10" 909 | } 910 | }, 911 | "nbformat": 4, 912 | "nbformat_minor": 5 913 | } 914 | -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/readmd.txt: -------------------------------------------------------------------------------- 1 | scale term 은 다른 rotation 등에 영향을 주어 highly nonlinear 해지므로 2 | initial value 가 더욱 중요하다 3 | 따라서 gen_noisy_but_reliable_inital 에서 처음 이니셜을 줄 때 4 | 너무 0.3 배, 와 같은, true 에서 멀리 떨어진 값을 주면 5 | 아무리 robust loss 를 사용하더라도 수렴이 어렵다. 6 | 0.7 정도는 충분히 실험적으로 극복하는 듯 하다 (이 토이 데이터셋에서) 7 | 하지만 이런 세부적인 initial guess 수준과 robust loss parameter tuning 은 8 | variable space 의 metric scale, domain knowledge 등을 고려하여 실험적으로 결정되어야 할 것이다. 9 | 10 | 아무튼 이니셜을 11 | def gen_noisy_but_reliable_inital(): 12 | rot_init = R.from_euler('xyz', true_rot_diff_rpy, degrees=True).as_rotvec() + 0.05*np.random.rand(3) 13 | trans_init = true_trans_diff + np.random.rand(3)*(0.1*scale_up) 14 | # scale_init = np.random.rand(1) * true_scale_diff 15 | scale_init = 0.7* true_scale_diff 16 | 17 | eps = 0.0001 # +eps means: because zero initial should be avoided (see the symbolic equation of Je_rot!) 18 | initial_state_vector = np.hstack((rot_init, trans_init, scale_init)) + eps 19 | return initial_state_vector 20 | 21 | 정도를 사용하면, 22 | 23 | outlier_ratio 를 무려 0.5 24 | 로 세팅하더라도 (50%) 25 | 26 | 로버스트 로스 를 사용하면 27 | robustified_error = sf.V1(noise_model.error(error_V3)) # robust loss 28 | 아웃라이어가 절반이더라도 잘 수렴해나가는 것을 볼 수 있다. 29 | 30 | 반면에 안 로버스트 로스 를 사용하면 31 | 앞서 로버스트로스 사용예와 달리 해가 발산함을 알 수 있다. 32 | 33 | 그리고 empirical 한 error scaling 을 적용해야 해가 오실레이션하는 것을 막을 수 있다. 34 | 그래서 scale term (e[-1]) 의 에러 스케일을 translation 대비 0.1-0.3 정도로 하면 안정적으로 수렴하는 것을 볼 수 있다. 35 | 36 | -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/0_given.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/0_given.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/1_outliers50percent.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/1_outliers50percent.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/2_five_iters.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/2_five_iters.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/3_outliers50percent.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/3_outliers50percent.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/4_five_iters_again.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/4_five_iters_again.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/5_diverged.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/nonrobust/5_diverged.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/robust/0_given.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/robust/0_given.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/robust/1_outliers50percent.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/robust/1_outliers50percent.png -------------------------------------------------------------------------------- /nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/robust/2_five_iters.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/symforce-tutorials/3fbc9cd9e53c19978a5967995421facb007ed70b/nonlinear_icp/3_nonlinear_icp_Sim3_robust/results/robust/2_five_iters.png -------------------------------------------------------------------------------- /nonlinear_icp/todo.txt: -------------------------------------------------------------------------------- 1 | the bug should be fixed 2 | - the next step's initial would be identity, because in this example, the source cloud explicitly adjusted 3 | --------------------------------------------------------------------------------