diff --git a/examples/Data/VO_camera_poses_small.txt b/examples/Data/VO_camera_poses_small.txt deleted file mode 100644 index c0de67ee6..000000000 --- a/examples/Data/VO_camera_poses_small.txt +++ /dev/null @@ -1,3 +0,0 @@ -1 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1 -2 0.999997 -0.00240146 0.00061075 0.00314304 0.0024019 0.999997 -0.00071977 0.00414596 -0.00060902 0.000721235 1 0.95998 0 0 0 1 -3 0.999993 -0.00371801 0.000595621 0.00280572 0.00371612 0.999988 0.00314729 0.00981461 -0.000607315 -0.00314506 0.999995 1.91967 0 0 0 1 diff --git a/examples/Data/VO_stereo_factors_small.txt b/examples/Data/VO_stereo_factors_small.txt deleted file mode 100644 index 8c7e977a3..000000000 --- a/examples/Data/VO_stereo_factors_small.txt +++ /dev/null @@ -1,244 +0,0 @@ -1 3 209.979 185.87 61.5418 -8.90263 -2.48003 16.0758 -2 3 183.871 158.526 58.5288 -9.02175 -2.42293 15.2918 -3 3 154.533 127.498 45.2523 -9.04073 -2.53526 14.3359 -1 7 402.088 390.052 9.90739 -9.25908 -7.27203 32.201 -2 7 394.391 382.151 5.65911 -9.4424 -7.33715 31.6638 -1 8 363.621 350.3 19.013 -9.91743 -6.20361 29.0959 -2 8 354.573 340.708 15.0627 -9.87834 -6.11295 27.9529 -1 11 328.245 314.688 36.8183 -11.1456 -5.38969 28.5871 -2 11 317.629 303.492 33.1041 -11.0929 -5.31025 27.4172 -1 13 354.416 341.486 73.4162 -10.5993 -4.13089 29.9745 -2 13 345.335 332.443 71.2318 -11.0089 -4.23409 30.0629 -1 14 368.212 355.083 79.6458 -9.87405 -3.81336 29.5197 -2 14 359.443 346.409 77.3278 -10.308 -3.9369 29.7366 -1 19 408.795 393 106.946 -6.82736 -2.24133 24.5372 -2 19 401.89 386.918 105.831 -7.45048 -2.40457 25.8863 -3 19 393.846 380.541 101.098 -8.70882 -2.89695 29.1301 -1 22 301.371 288.22 123.147 -12.5881 -2.0303 29.4716 -2 22 290.134 277.334 122.566 -13.4053 -2.1104 30.2806 -3 22 279.457 266.441 118.448 -13.6226 -2.24522 29.7763 -1 24 286.858 267.821 24.4042 -9.10534 -4.18865 20.3589 -2 24 270.266 249.809 17.1811 -8.90883 -4.08751 18.9454 -1 25 309.828 295.159 40.0603 -10.9759 -4.86279 26.4221 -2 25 297.643 283.051 37.0473 -11.4821 -4.99925 26.5609 -3 25 284.476 269.313 30.9841 -11.5164 -5.0259 25.5613 -1 26 363.837 351.125 88.2117 -10.3829 -3.57654 30.4884 -2 26 355.143 341.918 86.1881 -10.3332 -3.51996 29.3055 -1 27 370.625 357.942 91.0858 -10.1195 -3.46309 30.5589 -2 27 362.093 348.932 89.118 -10.0997 -3.41747 29.4477 -3 27 353.648 339.913 83.786 -10.0087 -3.48344 28.2193 -1 31 363.348 350.145 50.9786 -10.0172 -4.95853 29.356 -2 31 354.105 340.884 47.9673 -10.3787 -5.07395 29.315 -3 31 345.08 331.083 40.9019 -10.1498 -5.06386 27.6901 -1 34 320.828 308.065 68.3594 -12.1511 -4.3976 30.3656 -2 34 309.484 299.53 65.6399 -16.1932 -5.78567 38.9369 -3 34 300.833 288.357 60.6176 -13.2921 -4.83229 31.0655 -1 35 381.449 368.573 103.554 -9.51642 -2.8911 30.1014 -2 35 373.428 360.047 102.291 -9.47902 -2.83262 28.9646 -3 35 364.836 351.587 97.66 -9.92143 -3.04847 29.2522 -1 36 412.916 399.447 45.4646 -7.8421 -5.08028 28.7748 -2 36 405.446 391.636 42.1742 -7.93906 -5.08285 28.0645 -3 36 397.898 383.772 34.6847 -8.04846 -5.25392 27.4366 -1 37 382.101 369.058 62.9139 -9.36758 -4.52773 29.7155 -2 37 373.575 360.402 60.3917 -9.62259 -4.58581 29.4217 -3 37 365.323 351.075 53.9171 -9.20802 -4.48407 27.2029 -1 38 368.271 355.646 103.567 -10.2656 -2.94784 30.6979 -2 38 359.673 346.474 102.328 -10.1689 -2.87003 29.3625 -3 38 351.187 337.872 97.0015 -10.4237 -3.06016 29.1094 -1 39 368.271 355.646 103.567 -10.2656 -2.94784 30.6979 -2 39 359.673 346.474 102.328 -10.1689 -2.87003 29.3625 -3 39 351.187 337.872 97.0015 -10.4237 -3.06016 29.1094 -1 40 399.549 387.359 100.33 -9.25423 -3.19583 31.7949 -2 40 392.339 379.894 99.3608 -9.3758 -3.17216 31.1435 -3 40 385.548 371.692 93.9091 -8.68421 -3.06044 27.9717 -1 41 314.778 303.477 61.5898 -14.0112 -5.28847 34.2952 -2 41 305.745 293.424 58.7429 -13.2453 -4.97487 31.4567 -3 41 295.126 280.156 52.0137 -11.2824 -4.33594 25.8899 -1 42 322.734 309.225 27.9559 -11.4054 -5.76176 28.6914 -2 42 311.74 297.908 23.4169 -11.5653 -5.80315 28.0198 -1 43 413.858 400.794 69.7088 -8.04664 -4.24101 29.6674 -2 43 406.585 392.833 66.8017 -7.92805 -4.14233 28.1828 -3 43 399.186 384.6 60.1878 -7.74727 -4.14907 26.5715 -1 44 429.786 416.985 10.8113 -7.54372 -6.79969 30.2774 -2 44 422.927 409.032 6.22854 -7.21468 -6.44127 27.8926 -1 45 429.786 416.985 10.8113 -7.54372 -6.79969 30.2774 -2 45 422.927 409.032 6.22854 -7.21468 -6.44127 27.8926 -1 47 476.875 462.803 22.9268 -5.06458 -5.72276 27.5413 -2 47 471.267 456.98 18.7569 -5.19929 -5.79349 27.1272 -1 49 534.551 521.959 30.0889 -3.19959 -6.08989 30.7784 -2 49 531.222 518.03 26.2453 -3.18982 -5.96976 29.3803 -1 51 438.047 425.403 38.834 -7.28628 -5.69352 30.6528 -2 51 431.401 418.237 35.3691 -7.2697 -5.61002 29.4421 -1 52 536.646 523.91 44.453 -3.07516 -5.41537 30.4312 -2 52 533.182 520.347 41.0937 -3.1965 -5.51432 30.1972 -1 56 433.895 420.782 74.4585 -7.19587 -4.03064 29.5568 -2 56 427.255 413.599 72.1036 -7.17079 -3.96294 28.3811 -3 56 420.734 406.674 65.7893 -7.21386 -4.09028 27.5655 -1 59 427.689 414.944 90.0841 -7.66535 -3.48853 30.4109 -2 59 421.003 407.495 88.2374 -7.49811 -3.36485 28.6925 -3 59 414.164 400.243 82.4393 -7.5395 -3.48873 27.8411 -1 61 476.203 463.728 119.273 -5.74205 -2.3071 31.068 -2 61 471.246 458.29 118.352 -5.73435 -2.2596 29.9143 -3 61 466.603 453.213 113.763 -5.73513 -2.3706 28.9466 -1 62 549.824 538.317 13.0527 -2.7883 -7.45917 33.6798 -2 62 547.191 535.348 8.33458 -2.8288 -7.46196 32.7262 -1 63 494.158 482.317 49.1257 -5.23515 -5.6129 32.7324 -2 63 490.361 477.694 46.2753 -5.05459 -5.36754 30.5967 -3 63 486.219 472.742 39.6488 -4.91601 -5.30919 28.7585 -1 65 555.494 543.327 74.0345 -2.38699 -4.36286 31.8557 -2 65 552.415 540.886 71.3792 -2.66241 -4.72779 33.617 -3 65 550.653 538.6 65.114 -2.6252 -4.80152 32.1559 -1 66 456.271 443.352 85.912 -6.37377 -3.61506 30.0017 -2 66 450.683 437.004 83.9661 -6.239 -3.49059 28.3344 -1 67 480.016 466.663 91.5877 -5.21084 -3.26892 29.0237 -2 67 474.718 461.005 89.4507 -5.2816 -3.26684 28.262 -3 67 469.735 455.72 83.7122 -5.35873 -3.41635 27.6528 -1 68 495.411 483.037 98.2553 -4.95513 -3.2383 31.3217 -2 68 490.935 478.057 96.7302 -4.94811 -3.1753 30.097 -1 69 444.061 431.758 124.931 -7.22597 -2.09239 31.5037 -2 69 437.98 425.397 124.035 -7.32459 -2.08407 30.8019 -3 69 432.417 419.076 119.726 -7.1323 -2.13909 29.0514 -1 74 477.818 464.863 47.7721 -5.46253 -5.18639 29.9178 -2 74 472.705 459.099 44.6283 -5.40304 -5.06238 28.4865 -1 75 477.818 464.863 47.7721 -5.46253 -5.18639 29.9178 -2 75 472.705 459.099 44.6283 -5.40304 -5.06238 28.4865 -1 76 422.265 408.875 55.2521 -7.51344 -4.71768 28.945 -2 76 415.112 401.023 52.1342 -7.41364 -4.60265 27.5099 -3 76 408.035 393.207 45.1294 -7.30047 -4.62698 26.1386 -1 79 451.106 438.208 77.5776 -6.59924 -3.96805 30.0504 -2 79 445.036 431.574 75.2566 -6.56457 -3.89418 28.7897 -1 80 461.33 448.183 77.9245 -6.05615 -3.8785 29.4796 -2 80 455.329 441.776 75.6553 -6.11249 -3.85221 28.5962 -3 80 449.704 435.837 69.5686 -6.19193 -4.00072 27.9485 -1 83 524.861 512.387 99.0037 -3.64712 -3.18001 31.0695 -2 83 521.387 508.737 97.3619 -3.74378 -3.2054 30.6365 -3 83 518.54 505.038 91.9327 -3.62084 -3.21914 28.7036 -1 86 451.632 438.453 41.5387 -6.43651 -5.35192 29.4072 -2 86 445.619 432.262 37.972 -6.59294 -5.42433 29.0169 -3 86 439.939 425.582 30.5923 -6.34627 -5.32265 26.996 -1 87 519.097 508.425 56.748 -4.55329 -5.84403 36.3176 -2 87 515.739 502.233 53.8001 -3.73134 -4.73491 28.6964 -3 87 512.106 497.923 47.0277 -3.69077 -4.76531 27.3262 -1 88 534.071 522.238 63.8388 -3.42661 -4.94851 32.7527 -2 88 531.165 518.834 61.4406 -3.41475 -4.85304 31.4294 -1 89 534.071 522.238 63.8388 -3.42661 -4.94851 32.7527 -2 89 531.165 518.834 61.4406 -3.41475 -4.85304 31.4294 -1 92 435.312 422.269 78.4345 -7.1758 -3.88835 29.7142 -2 92 428.787 415.335 76.4018 -7.21856 -3.8515 28.8123 -3 92 422.68 408.458 70.1035 -7.05831 -3.88082 27.252 -1 93 450.491 437.056 88.6618 -6.35949 -3.36598 28.8469 -2 93 444.629 430.861 86.4587 -6.43476 -3.37071 28.1507 -3 93 438.781 424.649 80.6644 -6.49125 -3.50411 27.4255 -1 94 455.793 443.363 111.853 -6.64468 -2.63603 31.1797 -2 94 450 436.592 110.47 -6.39219 -2.4992 28.906 -3 94 444.45 431.11 105.548 -6.64812 -2.71008 29.0528 -1 96 516.839 504.605 114.884 -4.07104 -2.54525 31.6802 -2 96 513.501 500.595 113.958 -3.99767 -2.45111 30.0285 -1 97 545.632 534.021 117.895 -2.95751 -2.5426 33.3807 -2 97 543.43 531.117 117.068 -2.88466 -2.4335 31.4748 -1 98 455.088 441.992 55.321 -6.3357 -4.82066 29.5942 -2 98 448.718 435.522 52.1503 -6.54678 -4.91306 29.3691 -3 98 443.134 428.998 45.1421 -6.3241 -4.85301 27.4182 -1 100 427.579 414.128 66.7824 -7.26719 -4.23585 28.8138 -2 100 420.199 406.208 64.0832 -7.26993 -4.17594 27.7013 -3 100 413.155 398.753 57.4381 -7.32551 -4.3048 26.912 -1 101 427.579 414.128 66.7824 -7.26719 -4.23585 28.8138 -2 101 420.199 406.208 64.0832 -7.26993 -4.17594 27.7013 -3 101 413.155 398.753 57.4381 -7.32551 -4.3048 26.912 -1 103 542.696 530.061 22.2283 -2.84256 -6.40353 30.6746 -2 103 539.496 526.632 18.0308 -2.92561 -6.46493 30.1291 -3 103 538.168 524.086 12.1824 -2.72324 -6.12886 27.5232 -1 104 535.956 523.649 89.7316 -3.21258 -3.62806 31.4931 -2 104 533.076 520.355 87.8891 -3.22957 -3.58772 30.4676 -3 104 530.458 517.095 82.3061 -3.17958 -3.6397 29.0032 -1 105 535.956 523.649 89.7316 -3.21258 -3.62806 31.4931 -2 105 533.076 520.355 87.8891 -3.22957 -3.58772 30.4676 -3 105 530.458 517.095 82.3061 -3.17958 -3.6397 29.0032 -1 107 566.9 555.395 21.5232 -1.99174 -7.06559 33.6884 -2 107 564.716 552.833 17.392 -2.02712 -7.02754 32.6166 -1 112 658.214 646.669 88.116 2.26369 -3.9425 33.5701 -2 112 658.897 647.085 85.9733 2.24356 -3.95075 32.8107 -1 113 675.96 664.671 103.316 3.15958 -3.30888 34.3333 -2 113 677.522 665.531 101.535 3.04463 -3.195 32.324 -1 114 658.454 646.975 119.766 2.28803 -2.48428 33.7645 -2 114 659.332 647.379 118.527 2.23661 -2.44124 32.423 -3 114 660.767 648.4 113.81 2.22421 -2.56458 31.3399 -1 115 606.492 600.958 15.8831 -0.297771 -15.2365 70.0367 -2 115 605.534 599.882 14.2559 -0.382556 -15.0733 68.5755 -1 116 670.748 663.186 44.3523 4.34633 -9.12761 51.2516 -2 116 671.336 663.146 42.0262 4.05203 -8.58126 47.3271 -3 116 672.087 664.058 35.3488 4.18339 -9.1997 48.2741 -1 120 683.092 671.693 102.585 3.46491 -3.31111 33.9993 -2 120 684.634 672.737 100.791 3.38946 -3.25346 32.5757 -3 120 686.62 674.585 95.205 3.43928 -3.46554 32.2029 -1 123 661.897 656.37 19.4334 5.08714 -14.9123 70.1326 -2 123 661.865 656.089 16.9831 4.86425 -14.4954 67.1001 -3 123 662.228 656.907 12.0509 5.3169 -16.2332 72.8397 -1 125 559.802 547.566 36.2278 -2.18429 -5.9978 31.675 -2 125 557.384 544.657 32.6228 -2.20213 -5.9186 30.4532 -3 125 555.609 542.186 24.9967 -2.15891 -5.91676 28.8736 -1 128 617.818 606.749 109.389 0.400776 -3.07971 35.0133 -2 128 617.902 605.737 108.604 0.368359 -2.8369 31.8587 -1 129 653.913 642.678 111.773 2.1206 -2.92037 34.4978 -2 129 654.56 642.954 110.278 2.08274 -2.89618 33.3948 -3 129 655.858 643.64 105.386 2.03546 -2.96612 31.7211 -1 137 602.716 594.449 115.366 -0.444616 -3.73531 46.8819 -2 137 601.999 593.818 114.699 -0.496396 -3.81847 47.3762 -3 137 601.865 593.596 110.463 -0.499836 -4.05281 46.8702 -1 138 822.785 817.07 56.4723 20.0396 -10.9379 67.8124 -2 138 825.065 819.162 54.809 19.6106 -10.7418 65.6584 -3 138 828.256 822.273 49.8178 19.6342 -11.046 64.7783 -1 142 823.365 817.614 40.2749 19.9696 -12.383 67.3921 -2 142 825.659 819.737 38.3195 19.6034 -12.2042 65.454 -3 142 828.962 822.701 32.8848 18.8242 -12.009 61.9061 -1 144 815.798 810.929 59.8465 22.7486 -12.465 79.5871 -2 144 817.787 812.929 58.1828 23.0257 -12.6803 79.7871 -3 144 821.963 815.798 53.8181 18.5089 -10.3729 62.8752 -1 145 815.798 810.929 59.8465 22.7486 -12.465 79.5871 -2 145 817.787 812.929 58.1828 23.0257 -12.6803 79.7871 -1 146 777.011 765.364 70.4036 7.72293 -4.72504 33.2775 -2 146 781.246 769.121 67.4527 7.60553 -4.66916 31.9633 -3 146 786.571 774.015 60.9102 7.57279 -4.78911 30.8683 -1 148 764.595 753.222 100.942 7.32281 -3.39666 34.0805 -2 148 768.801 756.5 98.7651 6.95374 -3.2353 31.508 -1 151 769.692 758.18 119.424 7.47199 -2.4931 33.6678 -2 151 774.14 761.647 117.907 7.07619 -2.36246 31.0226 -1 152 769.692 758.18 119.424 7.47199 -2.4931 33.6678 -2 152 774.14 761.647 117.907 7.07619 -2.36246 31.0226 -1 153 769.692 758.18 119.424 7.47199 -2.4931 33.6678 -2 153 774.14 761.647 117.907 7.07619 -2.36246 31.0226 -3 153 779.344 766.681 112.851 7.2019 -2.54521 30.606 -1 155 829.479 823.269 46.3632 19.0214 -10.9405 62.4074 -2 155 832.007 825.885 44.4532 19.5195 -11.267 63.314 -1 158 819.357 810.23 71.9845 12.3472 -5.93645 42.4645 -2 158 824.128 814.918 69.5689 12.5149 -6.02419 42.0843 -3 158 830.206 820.885 63.3726 12.7145 -6.30875 41.5779 -1 159 788.924 777.307 74.6425 8.29323 -4.54096 33.3615 -2 159 793.683 781.418 71.6578 8.0637 -4.43188 31.5997 -3 159 799.1 786.642 65.4656 8.17271 -4.63044 31.1117 -1 160 700.033 688.956 119.498 4.38707 -2.58723 34.9873 -2 160 702.16 690.617 117.977 4.30911 -2.55363 33.5761 -3 160 704.427 692.419 114.085 4.2437 -2.62889 32.2765 -1 170 863.862 853.773 32.7773 13.5388 -7.45754 38.414 -2 170 869.634 859.099 29.2066 13.2603 -7.32407 36.7887 -3 170 876.662 865.763 21.815 13.1645 -7.44417 35.562 -1 171 863.862 853.773 32.7773 13.5388 -7.45754 38.414 -2 171 869.634 859.099 29.2066 13.2603 -7.32407 36.7887 -3 171 876.662 865.763 21.815 13.1645 -7.44417 35.562 -1 179 1082.33 1035.83 56.9512 5.46202 -1.33906 8.33616 -2 179 1143.34 1090.25 40.9695 5.3999 -1.33418 7.29928 -3 179 1223.79 1163.7 16.1894 5.49146 -1.40065 6.45087 -1 180 1057.33 1012.89 113.1 5.41204 -0.722226 8.72091 -2 180 1112.37 1062.54 104.89 5.41996 -0.732605 7.77771 -1 186 1032 985.776 48.4555 4.90898 -1.44557 8.38463 -2 186 1085.42 1033.06 31.4657 4.88201 -1.45056 7.40254 -1 187 1051.8 1005.3 52.7114 5.10833 -1.38778 8.33454 -2 187 1108.05 1055.56 36.3784 5.10147 -1.39666 7.38407 -1 188 1034.71 988.427 55.8301 4.93464 -1.35829 8.37485 -2 188 1088.88 1036.23 39.7023 4.88985 -1.35836 7.36084 -3 188 1160.46 1100.97 15.2843 4.97444 -1.42279 6.51521 -1 190 1085.46 1039.22 62.8393 5.52829 -1.27798 8.38169 -2 190 1146.8 1094.21 47.6122 5.48765 -1.27929 7.3702 -3 190 1228.58 1168.42 24.1453 5.52629 -1.32759 6.44148 -1 200 988.3 953.944 70.9989 5.92163 -1.59251 11.2813 -2 200 1023.27 984.786 60.7993 5.77461 -1.56408 10.0713 \ No newline at end of file diff --git a/matlab/examples/StereoVOExample.m b/matlab/examples/StereoVOExample.m new file mode 100644 index 000000000..b3c834a93 --- /dev/null +++ b/matlab/examples/StereoVOExample.m @@ -0,0 +1,77 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Basic VO Example with 3 landmarks and two cameras +% @author Chris Beall +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Assumptions +% - For simplicity this example is in the camera's coordinate frame +% - X: right, Y: down, Z: forward +% - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis) +% - x1 is fixed with a constraint, x2 is initialized with noisy values +% - No noise on measurements + +%% Create keys for variables +x1 = symbol('x',1); x2 = symbol('x',2); +l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3); + +%% Create graph container and add factors to it +graph = visualSLAMGraph; + +%% add a constraint on the starting pose +first_pose = gtsamPose3(); +graph.addPoseConstraint(x1, first_pose); + +%% Create realistic calibration and measurement noise model +% format: fx fy skew cx cy baseline +K = gtsamCal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); +stereo_model = gtsamSharedNoiseModel_Sigmas([1.0; 1.0; 1.0]); + +%% Add measurements +% pose 1 +graph.addStereoMeasurement(gtsamStereoPoint2(520, 480, 440), stereo_model, x1, l1, K); +graph.addStereoMeasurement(gtsamStereoPoint2(120, 80, 440), stereo_model, x1, l2, K); +graph.addStereoMeasurement(gtsamStereoPoint2(320, 280, 140), stereo_model, x1, l3, K); + +%pose 2 +graph.addStereoMeasurement(gtsamStereoPoint2(570, 520, 490), stereo_model, x2, l1, K); +graph.addStereoMeasurement(gtsamStereoPoint2( 70, 20, 490), stereo_model, x2, l2, K); +graph.addStereoMeasurement(gtsamStereoPoint2(320, 270, 115), stereo_model, x2, l3, K); + + +%% Create initial estimate for camera poses and landmarks +initialEstimate = visualSLAMValues; +initialEstimate.insertPose(x1, first_pose); +% noisy estimate for pose 2 +initialEstimate.insertPose(x2, gtsamPose3(gtsamRot3(), gtsamPoint3(0.1,-.1,1.1))); +initialEstimate.insertPoint(l1, gtsamPoint3( 1, 1, 5)); +initialEstimate.insertPoint(l2, gtsamPoint3(-1, 1, 5)); +initialEstimate.insertPoint(l3, gtsamPoint3( 0,-.5, 5)); + +%% optimize +fprintf(1,'Optimizing\n'); tic +result = graph.optimize(initialEstimate); +toc + +%% visualize initial trajectory, final trajectory, and final points +figure(1); clf; hold on; + +% initial trajectory in red (rotated so Z is up) +plot3(initialEstimate.zs(),-initialEstimate.xs(),-initialEstimate.ys(), '-*r','LineWidth',2); + +% final trajectory in green (rotated so Z is up) +plot3(result.zs(),-result.xs(),-result.ys(), '-*g','LineWidth',2); +xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); + +% optimized 3D points (rotated so Z is up) +points = result.points(); +plot3(points(:,3),-points(:,1),-points(:,2),'.'); + +axis equal +view(3) diff --git a/matlab/examples/StereoVOExample_small.m b/matlab/examples/StereoVOExample_small.m deleted file mode 100644 index 95926ee0d..000000000 --- a/matlab/examples/StereoVOExample_small.m +++ /dev/null @@ -1,79 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% GTSAM Copyright 2010, Georgia Tech Research Corporation, -% Atlanta, Georgia 30332-0415 -% All Rights Reserved -% Authors: Frank Dellaert, et al. (see THANKS for the full author list) -% -% See LICENSE for the license information -% -% @brief Read Stereo Visual Odometry from file and optimize -% @author Chris Beall -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% Load calibration -% format: fx fy skew cx cy baseline -calib = dlmread('../../examples/Data/VO_calibration.txt'); -K = gtsamCal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); -stereo_model = gtsamSharedNoiseModel_Sigmas([1.0; 1.0; 1.0]); - -%% create empty graph and values -graph = visualSLAMGraph; -initial = visualSLAMValues; - - -%% load the initial poses from VO -% row format: camera_id 4x4 pose (row, major) -fprintf(1,'Reading data\n'); -cameras = dlmread('../../examples/Data/VO_camera_poses_small.txt'); -for i=1:size(cameras,1) - pose = gtsamPose3(reshape(cameras(i,2:17),4,4)'); - initial.insertPose(symbol('x',cameras(i,1)),pose); -end - -%% load stereo measurements and initialize landmarks -% camera_id landmark_id uL uR v X Y Z -measurements = dlmread('../../examples/Data/VO_stereo_factors_small.txt'); - -fprintf(1,'Creating Graph\n'); tic -for i=1:size(measurements,1) - sf = measurements(i,:); - graph.addStereoMeasurement(gtsamStereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ... - symbol('x', sf(1)), symbol('l', sf(2)), K); - - if ~initial.exists(symbol('l',sf(2))) - % 3D landmarks are stored in camera coordinates: transform - % to world coordinates using the respective initial pose - pose = initial.pose(symbol('x', sf(1))); - world_point = pose.transform_from(gtsamPoint3(sf(6),sf(7),sf(8))); - initial.insertPoint(symbol('l',sf(2)), world_point); - end -end -toc - -%% add a constraint on the starting pose -key = symbol('x',1); -first_pose = initial.pose(key); -graph.addPoseConstraint(key, first_pose); - -%% optimize -fprintf(1,'Optimizing\n'); tic -result = graph.optimize(initial); -toc - -%% visualize initial trajectory, final trajectory, and final points -figure(1); clf; hold on; - -% initial trajectory in red (rotated so Z is up) -plot3(initial.zs(),-initial.xs(),-initial.ys(), '-*r','LineWidth',2); - -% final trajectory in green (rotated so Z is up) -plot3(result.zs(),-result.xs(),-result.ys(), '-*g','LineWidth',2); -xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); - - -% optimized 3D points (rotated so Z is up) -points = result.points(); -plot3(points(:,3),-points(:,1),-points(:,2),'.'); - -axis equal -view(3)