12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

small three pose VO example

release/4.3a0
Chris Beall 2012-06-14 05:10:12 +00:00
parent 2524c632df
commit a6ea70da7b
3 changed files with 326 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/Data/VO_camera_poses_small.txt Normal file
View File

@ -0,0 +1,3 @@
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

244
examples/Data/VO_stereo_factors_small.txt Normal file
View File

@ -0,0 +1,244 @@
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

79
matlab/examples/StereoVOExample_small.m Normal file
View File

@ -0,0 +1,79 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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)