|
53 | 53 | }, |
54 | 54 | { |
55 | 55 | "cell_type": "code", |
56 | | - "execution_count": 1, |
| 56 | + "execution_count": null, |
57 | 57 | "metadata": {}, |
58 | 58 | "outputs": [], |
59 | 59 | "source": [ |
60 | 60 | "import requests\n", |
61 | 61 | "from io import StringIO\n", |
62 | 62 | "import numpy as np\n", |
63 | 63 | "import gtsam\n", |
64 | | - "from gtsam.utils.plot import plot_pose3\n", |
65 | | - "import matplotlib.pyplot as plt\n", |
66 | | - "import gtsam.utils.plot as gtsam_plot\n", |
67 | 64 | "\n", |
68 | 65 | "# For shorthand for common GTSAM types (like X for poses, L for landmarks)\n", |
69 | 66 | "from gtsam.symbol_shorthand import X, L" |
|
80 | 77 | }, |
81 | 78 | { |
82 | 79 | "cell_type": "code", |
83 | | - "execution_count": 2, |
| 80 | + "execution_count": null, |
84 | 81 | "metadata": {}, |
85 | 82 | "outputs": [], |
86 | 83 | "source": [ |
|
103 | 100 | }, |
104 | 101 | { |
105 | 102 | "cell_type": "code", |
106 | | - "execution_count": 3, |
| 103 | + "execution_count": null, |
107 | 104 | "metadata": {}, |
108 | | - "outputs": [ |
109 | | - { |
110 | | - "name": "stdout", |
111 | | - "output_type": "stream", |
112 | | - "text": [ |
113 | | - "K: 721.538 0 609.559\n", |
114 | | - " 0 721.538 172.854\n", |
115 | | - " 0 0 1\n", |
116 | | - "Baseline: 0.537151\n", |
117 | | - "\n" |
118 | | - ] |
119 | | - } |
120 | | - ], |
| 105 | + "outputs": [], |
121 | 106 | "source": [ |
122 | 107 | "# Retrieve Calibration file\n", |
123 | 108 | "url = \"https://raw.githubusercontent.com/borglab/gtsam/refs/heads/develop/examples/Data/VO_calibration.txt\"\n", |
|
151 | 136 | }, |
152 | 137 | { |
153 | 138 | "cell_type": "code", |
154 | | - "execution_count": 4, |
| 139 | + "execution_count": null, |
155 | 140 | "metadata": {}, |
156 | 141 | "outputs": [], |
157 | 142 | "source": [ |
|
184 | 169 | }, |
185 | 170 | { |
186 | 171 | "cell_type": "code", |
187 | | - "execution_count": 5, |
| 172 | + "execution_count": null, |
188 | 173 | "metadata": {}, |
189 | 174 | "outputs": [], |
190 | 175 | "source": [ |
|
224 | 209 | }, |
225 | 210 | { |
226 | 211 | "cell_type": "code", |
227 | | - "execution_count": 6, |
| 212 | + "execution_count": null, |
228 | 213 | "metadata": {}, |
229 | 214 | "outputs": [], |
230 | 215 | "source": [ |
|
244 | 229 | }, |
245 | 230 | { |
246 | 231 | "cell_type": "code", |
247 | | - "execution_count": 7, |
| 232 | + "execution_count": null, |
248 | 233 | "metadata": {}, |
249 | 234 | "outputs": [], |
250 | 235 | "source": [ |
|
266 | 251 | }, |
267 | 252 | { |
268 | 253 | "cell_type": "code", |
269 | | - "execution_count": 9, |
| 254 | + "execution_count": null, |
270 | 255 | "metadata": {}, |
271 | | - "outputs": [ |
272 | | - { |
273 | | - "name": "stdout", |
274 | | - "output_type": "stream", |
275 | | - "text": [ |
276 | | - "Final result sample:\n", |
277 | | - "Values with 26 values:\n", |
278 | | - "Value x1: (gtsam::Pose3)\n", |
279 | | - "R: [\n", |
280 | | - "\t1, 0, 0;\n", |
281 | | - "\t0, 1, 0;\n", |
282 | | - "\t0, 0, 1\n", |
283 | | - "]\n", |
284 | | - "t: 0 0 0\n", |
285 | | - "\n", |
286 | | - "Value x2: (gtsam::Pose3)\n", |
287 | | - "R: [\n", |
288 | | - "\t0.999997, -0.00234524, 0.000667659;\n", |
289 | | - "\t0.00234572, 0.999997, -0.000714904;\n", |
290 | | - "\t-0.000665981, 0.000716469, 1\n", |
291 | | - "]\n", |
292 | | - "t: 0.00254935 0.00430119 0.959176\n", |
293 | | - "\n", |
294 | | - "Value x3: (gtsam::Pose3)\n", |
295 | | - "R: [\n", |
296 | | - "\t0.999993, -0.00363586, 0.000733503;\n", |
297 | | - "\t0.00363353, 0.999988, 0.00316275;\n", |
298 | | - "\t-0.000744993, -0.00316007, 0.999995\n", |
299 | | - "]\n", |
300 | | - "t: 0.00112241 0.00989265 1.9179\n", |
301 | | - "\n", |
302 | | - "Value x4: (gtsam::Pose3)\n", |
303 | | - "R: [\n", |
304 | | - "\t0.999982, -0.00599989, 8.41316e-05;\n", |
305 | | - "\t0.00599887, 0.99994, 0.00918689;\n", |
306 | | - "\t-0.000139247, -0.00918622, 0.999958\n", |
307 | | - "]\n", |
308 | | - "t: 0.00127606 0.0117263 2.87189\n", |
309 | | - "\n", |
310 | | - "Value x5: (gtsam::Pose3)\n", |
311 | | - "R: [\n", |
312 | | - "\t0.999952, -0.00962861, -0.00135944;\n", |
313 | | - "\t0.00964083, 0.999911, 0.00925616;\n", |
314 | | - "\t0.0012702, -0.00926882, 0.999957\n", |
315 | | - "]\n", |
316 | | - "t: 0.00314102 0.0166897 3.81369\n", |
317 | | - "\n", |
318 | | - "Value x6: (gtsam::Pose3)\n", |
319 | | - "R: [\n", |
320 | | - "\t0.999922, -0.0122294, -0.00243049;\n", |
321 | | - "\t0.012245, 0.999903, 0.00654308;\n", |
322 | | - "\t0.00235024, -0.00657234, 0.999976\n", |
323 | | - "]\n", |
324 | | - "t: 0.00124453 0.021009 4.75581\n", |
325 | | - "\n", |
326 | | - "Value x7: (gtsam::Pose3)\n", |
327 | | - "R: [\n", |
328 | | - "\t0.999912, -0.0128882, -0.0032083;\n", |
329 | | - "\t0.0128983, 0.999912, 0.0031073;\n", |
330 | | - "\t0.00316797, -0.00314841, 0.99999\n", |
331 | | - "]\n", |
332 | | - "t: -0.00450171 0.0259461 5.69136\n", |
333 | | - "\n", |
334 | | - "Value x8: (gtsam::Pose3)\n", |
335 | | - "R: [\n", |
336 | | - "\t0.999915, -0.0124045, -0.00404068;\n", |
337 | | - "\t0.0124148, 0.999919, 0.0025552;\n", |
338 | | - "\t0.00400866, -0.00260514, 0.999989\n", |
339 | | - "]\n", |
340 | | - "t: -0.0125223 0.0290088 6.62678\n", |
341 | | - "\n", |
342 | | - "Value x9: (gtsam::Pose3)\n", |
343 | | - "R: [\n", |
344 | | - "\t0.999914, -0.0120563, -0.00520066;\n", |
345 | | - "\t0.0120792, 0.999917, 0.00438779;\n", |
346 | | - "\t0.00514734, -0.00445023, 0.999977\n", |
347 | | - "]\n", |
348 | | - "t: -0.019421 0.0332204 7.56075\n", |
349 | | - "\n", |
350 | | - "Value x10: (gtsam::Pose3)\n", |
351 | | - "R: [\n", |
352 | | - "\t0.99991, -0.0119415, -0.00609504;\n", |
353 | | - "\t0.0119864, 0.999901, 0.00739178;\n", |
354 | | - "\t0.00600617, -0.00746417, 0.999954\n", |
355 | | - "]\n", |
356 | | - "t: -0.0263364 0.0374578 8.49318\n", |
357 | | - "\n", |
358 | | - "Value x11: (gtsam::Pose3)\n", |
359 | | - "R: [\n", |
360 | | - "\t0.999902, -0.0122664, -0.00674184;\n", |
361 | | - "\t0.0123243, 0.999887, 0.00861252;\n", |
362 | | - "\t0.00663543, -0.00869477, 0.99994\n", |
363 | | - "]\n", |
364 | | - "t: -0.0435016 0.046867 9.42584\n", |
365 | | - "\n", |
366 | | - "Value x12: (gtsam::Pose3)\n", |
367 | | - "R: [\n", |
368 | | - "\t0.999878, -0.0136837, -0.00747182;\n", |
369 | | - "\t0.0137518, 0.999864, 0.00914289;\n", |
370 | | - "\t0.0073457, -0.00924453, 0.99993\n", |
371 | | - "]\n", |
372 | | - "t: -0.049959 0.0593662 10.3537\n", |
373 | | - "\n", |
374 | | - "Value x13: (gtsam::Pose3)\n", |
375 | | - "R: [\n", |
376 | | - "\t0.999869, -0.0138033, -0.00839008;\n", |
377 | | - "\t0.0138795, 0.999863, 0.00909613;\n", |
378 | | - "\t0.0082634, -0.0092114, 0.999924\n", |
379 | | - "]\n", |
380 | | - "t: -0.0577887 0.0663218 11.273\n", |
381 | | - "\n", |
382 | | - "Value x14: (gtsam::Pose3)\n", |
383 | | - "R: [\n", |
384 | | - "\t0.999859, -0.0141667, -0.00902236;\n", |
385 | | - "\t0.0142364, 0.999869, 0.00770711;\n", |
386 | | - "\t0.00891194, -0.00783446, 0.99993\n", |
387 | | - "]\n", |
388 | | - "t: -0.0701502 0.0714193 12.1859\n", |
389 | | - "\n", |
390 | | - "Value x15: (gtsam::Pose3)\n", |
391 | | - "R: [\n", |
392 | | - "\t0.999839, -0.0145645, -0.0105222;\n", |
393 | | - "\t0.0146258, 0.999876, 0.00576628;\n", |
394 | | - "\t0.0104369, -0.00591925, 0.999928\n", |
395 | | - "]\n", |
396 | | - "t: -0.0818121 0.0774193 13.0933\n", |
397 | | - "\n", |
398 | | - "Value x16: (gtsam::Pose3)\n", |
399 | | - "R: [\n", |
400 | | - "\t0.999798, -0.0161243, -0.012061;\n", |
401 | | - "\t0.0161784, 0.99986, 0.00440646;\n", |
402 | | - "\t0.0119882, -0.0046007, 0.999918\n", |
403 | | - "]\n", |
404 | | - "t: -0.0946674 0.0835267 14.0007\n", |
405 | | - "\n", |
406 | | - "Value x17: (gtsam::Pose3)\n", |
407 | | - "R: [\n", |
408 | | - "\t0.999779, -0.0159184, -0.0136876;\n", |
409 | | - "\t0.0159703, 0.999865, 0.00368892;\n", |
410 | | - "\t0.013627, -0.00390671, 0.9999\n", |
411 | | - "]\n", |
412 | | - "t: -0.113811 0.0882549 14.909\n", |
413 | | - "\n", |
414 | | - "Value x18: (gtsam::Pose3)\n", |
415 | | - "R: [\n", |
416 | | - "\t0.999777, -0.0147969, -0.0150562;\n", |
417 | | - "\t0.0148422, 0.999886, 0.00290648;\n", |
418 | | - "\t0.0150114, -0.0031293, 0.999882\n", |
419 | | - "]\n", |
420 | | - "t: -0.137146 0.0884828 15.811\n", |
421 | | - "\n", |
422 | | - "Value x19: (gtsam::Pose3)\n", |
423 | | - "R: [\n", |
424 | | - "\t0.999772, -0.013353, -0.0166407;\n", |
425 | | - "\t0.0134016, 0.999906, 0.0028127;\n", |
426 | | - "\t0.0166015, -0.00303508, 0.999858\n", |
427 | | - "]\n", |
428 | | - "t: -0.160148 0.0925047 16.7112\n", |
429 | | - "\n", |
430 | | - "Value x20: (gtsam::Pose3)\n", |
431 | | - "R: [\n", |
432 | | - "\t0.999738, -0.0139189, -0.0181207;\n", |
433 | | - "\t0.0139833, 0.999896, 0.00343832;\n", |
434 | | - "\t0.0180709, -0.00369081, 0.99983\n", |
435 | | - "]\n", |
436 | | - "t: -0.182263 0.0978961 17.6102\n", |
437 | | - "\n", |
438 | | - "Value x21: (gtsam::Pose3)\n", |
439 | | - "R: [\n", |
440 | | - "\t0.999719, -0.0134212, -0.0195276;\n", |
441 | | - "\t0.0135153, 0.999897, 0.00469594;\n", |
442 | | - "\t0.0194626, -0.00495854, 0.999798\n", |
443 | | - "]\n", |
444 | | - "t: -0.205531 0.101463 18.5001\n", |
445 | | - "\n", |
446 | | - "Value x22: (gtsam::Pose3)\n", |
447 | | - "R: [\n", |
448 | | - "\t0.999701, -0.0130668, -0.0207031;\n", |
449 | | - "\t0.0131757, 0.9999, 0.00513278;\n", |
450 | | - "\t0.0206341, -0.00540402, 0.999773\n", |
451 | | - "]\n", |
452 | | - "t: -0.232978 0.107104 19.384\n", |
453 | | - "\n", |
454 | | - "Value x23: (gtsam::Pose3)\n", |
455 | | - "R: [\n", |
456 | | - "\t0.999654, -0.0142711, -0.0220968;\n", |
457 | | - "\t0.0143815, 0.999885, 0.00484618;\n", |
458 | | - "\t0.0220251, -0.00516229, 0.999744\n", |
459 | | - "]\n", |
460 | | - "t: -0.258803 0.112105 20.2666\n", |
461 | | - "\n", |
462 | | - "Value x24: (gtsam::Pose3)\n", |
463 | | - "R: [\n", |
464 | | - "\t0.999616, -0.0143263, -0.0237284;\n", |
465 | | - "\t0.0144326, 0.999886, 0.00431834;\n", |
466 | | - "\t0.0236639, -0.00465915, 0.999709\n", |
467 | | - "]\n", |
468 | | - "t: -0.283633 0.116787 21.1431\n", |
469 | | - "\n", |
470 | | - "Value x25: (gtsam::Pose3)\n", |
471 | | - "R: [\n", |
472 | | - "\t0.999588, -0.0141709, -0.0249476;\n", |
473 | | - "\t0.014296, 0.999886, 0.00484451;\n", |
474 | | - "\t0.0248761, -0.00519917, 0.999677\n", |
475 | | - "]\n", |
476 | | - "t: -0.309037 0.119298 22.0121\n", |
477 | | - "\n", |
478 | | - "Value x26: (gtsam::Pose3)\n", |
479 | | - "R: [\n", |
480 | | - "\t0.999559, -0.0141483, -0.0261242;\n", |
481 | | - "\t0.0143302, 0.999874, 0.00678903;\n", |
482 | | - "\t0.0260249, -0.0071604, 0.999636\n", |
483 | | - "]\n", |
484 | | - "t: -0.334408 0.124848 22.874\n", |
485 | | - "\n", |
486 | | - "\n" |
487 | | - ] |
488 | | - } |
489 | | - ], |
| 256 | + "outputs": [], |
490 | 257 | "source": [ |
491 | 258 | "print(\"Final result sample:\")\n", |
492 | 259 | "\n", |
|
0 commit comments